kino-dome/1- Kinect Process

## 1- Kinect Process
Kinect depth image filtering methods and 3D position extraction of blobs using OpenCV, compatible with libcinder and KCB2 block for Cinder
Read blog post at http://kino.holescapes.com/2018/03/04/kinect-process-some-useful-methods-for-processing-kinect-data/

## Body.cpp
#include "Body.h"

using namespace ci;
using namespace ci::app;
using namespace std;

void Body::calc3dData(const Kinect2::DeviceRef & aDevice, const ci::Channel16uRef & aDepthChannel)
{
	//////// calc pointcloud
	if (mScreenPositions.size() > 0) {
		mPointCloud = aDevice->mapDepthToCamera(mScreenPositions, aDepthChannel);
	}

	/////// calc the 3D outline
	//we need ivec2 instead of vec2 for the mapDepthToCamera method
	vector<ivec2> outline2d;
	for (auto& point : mPolyLine.getPoints()) {
		outline2d.push_back(point);
	}
	if (outline2d.size() > 0) {
		mOutline = aDevice->mapDepthToCamera(outline2d, aDepthChannel);
	}

	//calc the center
	mCenter = aDevice->mapDepthToCamera(mCentroid, aDepthChannel);
}

## Body.h
#pragma once

#include "cinder/app/App.h"
#include "cinder/Vector.h"
#include "cinder/Cinder.h"
#include "cinder/Path2d.h"
#include "cinder/PolyLine.h"

#include "CinderOpenCV.h"
#include "Kinect2.h"

typedef std::shared_ptr<class Body> BodyRef;
class Body {
public:
	static BodyRef			 	create() { return std::make_shared<Body>(); }
	Body() {}
	~Body() {}

	// calculating the Camera Space data, the DeviceRef and the 16-bit Depth Channel are needed to be able to use the mapDepthToCamera() method
	void					calc3dData(const Kinect2::DeviceRef& aDevice, const ci::Channel16uRef& aDepthChannel);
public:
	//3d
	std::vector<ci::vec3>                	mPointCloud; // a pointcloud is basically is vector of 3D vectors
	std::vector<ci::vec3>                 	mOutline; // keeping the 3D outline in another vector
	ci::vec3                              	mCenter; // blob's centroid translated to the 3D space
	//2d
	std::vector<cv::Point>                	mContour;
	cv::Moments                           	mMoments;
	ci::PolyLine2                         	mPolyLine;
	ci::Rectf                             	mBoundingRect;
	ci::vec2                              	mCentroid;
	std::vector<ci::ivec2>                	mScreenPositions; // these are all the pixels that are inside the contour in the 2d space
};

## KinectUtils.cpp
#include "KinectUtils.h"

using namespace ci;
using namespace ci::app;
using namespace std;

ci::Channel16uRef filterDepthByRange(const ci::Channel16uRef& aDepthChannel, float aMin, float aMax, const ci::Area& aArea) const
{
	// make the result channel the same size as the input and make iterators for walking the source and dest pixels
	auto resultChannel = Channel16u::create(aDepthChannel->getWidth(), aDepthChannel->getHeight());
	Channel16u::Iter srcIter = aDepthChannel->getIter();
	Channel16u::Iter dstIter = resultChannel->getIter();

	// actual filtering process
	while (srcIter.line() && dstIter.line()) {
		while (srcIter.pixel() && dstIter.pixel()) {
			auto& depthInMilliMeters = srcIter.v();
			if (depthInMilliMeters >= aMin && depthInMilliMeters < aMax && aArea.contains(srcIter.getPos())) {
				dstIter.v() = depthInMilliMeters;
			}
			else {
				dstIter.v() = 0;
			}
		}
	}

	return resultChannel;
}

ci::Channel8uRef binarizeDepthByRange(const ci::Channel16uRef& aDepthChannel, float aMin, float aMax, const ci::Area& aArea) const
{
	// make the result channel the same size as the input and make iterators for walking the source and dest pixels
	auto resultChannel = Channel8u::create(aDepthChannel->getWidth(), aDepthChannel->getHeight());
	Channel16u::Iter srcIter = aDepthChannel->getIter();
	Channel8u::Iter dstIter = resultChannel->getIter();

	// actual filtering process
	while (srcIter.line() && dstIter.line()) {
		while (srcIter.pixel() && dstIter.pixel()) {
			auto& depthInMilliMeters = srcIter.v();
			if (depthInMilliMeters >= aMin && depthInMilliMeters < aMax && aArea.contains(srcIter.getPos())) {
				dstIter.v() = 255;
			}
			else {
				dstIter.v() = 0;
			}
		}
	}

	return resultChannel;
}

ci::Channel8uRef binarizeBodyIndex(const ci::Channel8uRef& aBodyIndexChannel)
{
	// make the result channel the same size as the input and make iterators for walking the source and dest pixels
	auto resultChannel = Channel8u::create(aBodyIndexChannel->getWidth(), aBodyIndexChannel->getHeight());
	Channel8u::Iter srcIter = aBodyIndexChannel->getIter();
	Channel8u::Iter dstIter = resultChannel->getIter();

	// if a user exists in the pixel position make it 255 otherwise it should be 0
	while (srcIter.line() && dstIter.line()) {
		while (srcIter.pixel() && dstIter.pixel()) {
			if (srcIter.v() >= 0 && srcIter.v() < 6) {
				dstIter.v() = 255;
			}
			else {
				dstIter.v() = 0;
			}
		}
	}

	return resultChannel;
}

## KinectUtils.h
#pragma once

#include "cinder/Area.h"
#include "cinder/Channel.h"

#include "Kinect2.h"

// kinect 2 depth frame's dimensions
const int DEPTH_FRAME_WIDTH = 512;
const int DEPTH_FRAME_HEIGHT = 424;

// filters and returns the depth frame by aMin and aMax distance from the kinect (in millimeters) and optionally imposes a ROI filter via aArea
ci::Channel16uRef                       filterDepthByRange(const ci::Channel16uRef& aDepthChannel, float aMin, float aMax, const ci::Area& aArea = ci::Area(0, 0, DEPTH_FRAME_WIDTH, DEPTH_FRAME_HEIGHT)) const;

// filters and returns the depth frame by aMin and aMax distance from the kinect (in millimeters) and optionally imposes a ROI filter via aArea
// good for blob detection using OpenCV, returns the filtered channel with binary values ( passed is 255, cut is 0)
ci::Channel8uRef                        binarizeDepthByRange(const ci::Channel16uRef& aDepthChannel, float aMin, float aMax, const ci::Area& aArea = ci::Area(0, 0, DEPTH_FRAME_WIDTH, DEPTH_FRAME_HEIGHT)) const;

// normalizes body Index channel so that all users have the pixel value of 255 , perfect for blob detection using OpenCV
ci::Channel8uRef                        binarizeBodyIndex(const ci::Channel8uRef& aBodyIndexChannel);

## main.cpp
#include "Body.h"
#include "KinectUtils.h"
#include "OcvUtils.h"

std::vector<BodyRef> bodies;
ci::Channel8uRef filteredDepthChannel;

	if (shouldFilterByDepth) {
		filteredDepthChannel = binarizeDepthByRange(mDepthChannel, 0, 8000);
	}
	else { // should use body index and binarize
		filteredDepthChannel = binarizeBodyIndex(mBodyIndexChannel);
	}

	//process depth source channel
	if (filteredDepthChannel) {
		//////// preprocess
		cv::Mat processedDepthMat = toOcv(*mFilteredDepthChannel);
    		// blur and denoise the input kinect image
		cv::blur(processedDepthMat, processedDepthMat, cv::Size2f(13, 13));
		// threshold
		cv::threshold(processedDepthMat, processedDepthMat, 0, 255, cv::THRESH_OTSU);
    		// this is optional, I liked the result more with one step of erosion
		cv::erode(processedDepthMat, processedDepthMat, cv::Mat(), cv::Point(-1, -1), 1);

		//contour and body creation
		cv::Mat contourMat, labels, stats, centroids;
    		// the processed mat is copied because the following cv processes modify the original image, if you don't need it this could be skipped
		processedDepthMat.copyTo(contourMat);

		// https://stackoverflow.com/questions/39770382/opencv-how-to-find-the-pixels-inside-a-contour-in-c
		int nLabels = connectedComponentsWithStats(contourMat, labels, stats, centroids);
		for (int label = 1; label < nLabels; label++) {  //0 is background
			//filter by area here
			int area = stats.at<int>(label, cv::CC_STAT_AREA);

			// extracting data
			int x1 = stats.at<int>(label, cv::CC_STAT_LEFT);
			int y1 = stats.at<int>(label, cv::CC_STAT_TOP);
			int width = stats.at<int>(label, cv::CC_STAT_WIDTH);
			int height = stats.at<int>(label, cv::CC_STAT_HEIGHT);
			int x2 = x1 + width;
			int y2 = y1 + height;
			// find screenPositions
			vector<ivec2> screenPositions;
			for (int i = x1; i < x2; i++)
			{
				for (int j = y1; j < y2; j++)
				{
					cv::Point p(i, j);
					if (label == labels.at<int>(p))
					{
						screenPositions.push_back(fromOcv(p));
					}
				}

			}
			// find contour
			// Get the mask for the i-th contour
			cv::Mat1b mask = labels == label;

			// Compute the contour
			vector<vector<cv::Point>> contours;
			findContours(mask, contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_NONE);
			mContours.push_back(contours[0]);

			auto body = Body::create();
			body->mBoundingRect = Rectf(x1, y1, x2, y2);
			body->mScreenPositions = std::move(screenPositions);
			auto moments = cv::moments(contours[0]);
			body->mCentroid = fromOcv(cv::Point(int(moments.m10 / moments.m00), int(moments.m01 / moments.m00)));
			body->mMoments = std::move(moments);
			body->mPolyLine = makePolylineFromContour(contours[0]);
			body->mContour = std::move(contours[0]);
			body->calc3dData(mDevice, mDepthChannel);

      			bodies.push_back(body);
		}
	}

## OcvUtils.cpp
#include "OcvUtils.h"

using namespace ci;
using namespace ci::app;
using namespace std;

void filterContoursByAreaSize(std::vector<std::vector<cv::Point>>& aContours, float aMin, float aMax)
{
	if (aContours.size() > 0) {
		auto newEnd = std::remove_if(aContours.begin(), aContours.end(), [aMin, aMax](const std::vector<cv::Point>& contour) {
			float area = cv::contourArea(contour);
			return area < aMin || area > aMax;
		});
		aContours.erase(newEnd, aContours.end());
	}

}

ci::Path2d makePath2dFromContour(const std::vector<cv::Point>& aContour)
{
	Path2d path;
	path.moveTo(fromOcv(aContour[0]));
	for (int i = 1; i < aContour.size(); i++) {
		path.lineTo(fromOcv(aContour[i]));
	}
	path.close();
	return path;
}

ci::PolyLine2 makePolylineFromContour(const std::vector<cv::Point>& aContour)
{
	PolyLine2 poly;
	for (int i = 0; i < aContour.size(); i++) {
		poly.push_back(fromOcv(aContour[i]));
	}
	poly.setClosed(true);
	return poly;
}

## OcvUtils.h
#pragma once
#include "cinder/Path2d.h"
#include "cinder/PolyLine.h"

#include "CinderOpenCV.h"

// filters the input vector of contours by considering their area size and aMin and aMax (in pixels)
void                            filterContoursByAreaSize(std::vector<std::vector<cv::Point>>& aContours, float aMin, float aMax);
// make a Cinder Path2d from an OpenCV contour
ci::Path2d                      makePath2dFromContour(const std::vector<cv::Point>& aContour);
// make a Cinder Polyline2 from an OpenCV contour
ci::PolyLine2                   makePolylineFromContour(const std::vector<cv::Point>& aContour);
	Kinect depth image filtering methods and 3D position extraction of blobs using OpenCV, compatible with libcinder and KCB2 block for Cinder
	Read blog post at http://kino.holescapes.com/2018/03/04/kinect-process-some-useful-methods-for-processing-kinect-data/
	#include "Body.h"

	using namespace ci;
	using namespace ci::app;
	using namespace std;

	void Body::calc3dData(const Kinect2::DeviceRef & aDevice, const ci::Channel16uRef & aDepthChannel)
	{
	//////// calc pointcloud
	if (mScreenPositions.size() > 0) {
	mPointCloud = aDevice->mapDepthToCamera(mScreenPositions, aDepthChannel);
	}

	/////// calc the 3D outline
	//we need ivec2 instead of vec2 for the mapDepthToCamera method
	vector<ivec2> outline2d;
	for (auto& point : mPolyLine.getPoints()) {
	outline2d.push_back(point);
	}
	if (outline2d.size() > 0) {
	mOutline = aDevice->mapDepthToCamera(outline2d, aDepthChannel);
	}

	//calc the center
	mCenter = aDevice->mapDepthToCamera(mCentroid, aDepthChannel);
	}
	#pragma once

	#include "cinder/app/App.h"
	#include "cinder/Vector.h"
	#include "cinder/Cinder.h"
	#include "cinder/Path2d.h"
	#include "cinder/PolyLine.h"

	#include "CinderOpenCV.h"
	#include "Kinect2.h"

	typedef std::shared_ptr<class Body> BodyRef;
	class Body {
	public:
	static BodyRef create() { return std::make_shared<Body>(); }
	Body() {}
	~Body() {}

	// calculating the Camera Space data, the DeviceRef and the 16-bit Depth Channel are needed to be able to use the mapDepthToCamera() method
	void calc3dData(const Kinect2::DeviceRef& aDevice, const ci::Channel16uRef& aDepthChannel);
	public:
	//3d
	std::vector<ci::vec3> mPointCloud; // a pointcloud is basically is vector of 3D vectors
	std::vector<ci::vec3> mOutline; // keeping the 3D outline in another vector
	ci::vec3 mCenter; // blob's centroid translated to the 3D space
	//2d
	std::vector<cv::Point> mContour;
	cv::Moments mMoments;
	ci::PolyLine2 mPolyLine;
	ci::Rectf mBoundingRect;
	ci::vec2 mCentroid;
	std::vector<ci::ivec2> mScreenPositions; // these are all the pixels that are inside the contour in the 2d space
	};
	#include "KinectUtils.h"

	using namespace ci;
	using namespace ci::app;
	using namespace std;

	ci::Channel16uRef filterDepthByRange(const ci::Channel16uRef& aDepthChannel, float aMin, float aMax, const ci::Area& aArea) const
	{
	// make the result channel the same size as the input and make iterators for walking the source and dest pixels
	auto resultChannel = Channel16u::create(aDepthChannel->getWidth(), aDepthChannel->getHeight());
	Channel16u::Iter srcIter = aDepthChannel->getIter();
	Channel16u::Iter dstIter = resultChannel->getIter();

	// actual filtering process
	while (srcIter.line() && dstIter.line()) {
	while (srcIter.pixel() && dstIter.pixel()) {
	auto& depthInMilliMeters = srcIter.v();
	if (depthInMilliMeters >= aMin && depthInMilliMeters < aMax && aArea.contains(srcIter.getPos())) {
	dstIter.v() = depthInMilliMeters;
	}
	else {
	dstIter.v() = 0;
	}
	}
	}

	return resultChannel;
	}

	ci::Channel8uRef binarizeDepthByRange(const ci::Channel16uRef& aDepthChannel, float aMin, float aMax, const ci::Area& aArea) const
	{
	// make the result channel the same size as the input and make iterators for walking the source and dest pixels
	auto resultChannel = Channel8u::create(aDepthChannel->getWidth(), aDepthChannel->getHeight());
	Channel16u::Iter srcIter = aDepthChannel->getIter();
	Channel8u::Iter dstIter = resultChannel->getIter();

	// actual filtering process
	while (srcIter.line() && dstIter.line()) {
	while (srcIter.pixel() && dstIter.pixel()) {
	auto& depthInMilliMeters = srcIter.v();
	if (depthInMilliMeters >= aMin && depthInMilliMeters < aMax && aArea.contains(srcIter.getPos())) {
	dstIter.v() = 255;
	}
	else {
	dstIter.v() = 0;
	}
	}
	}

	return resultChannel;
	}

	ci::Channel8uRef binarizeBodyIndex(const ci::Channel8uRef& aBodyIndexChannel)
	{
	// make the result channel the same size as the input and make iterators for walking the source and dest pixels
	auto resultChannel = Channel8u::create(aBodyIndexChannel->getWidth(), aBodyIndexChannel->getHeight());
	Channel8u::Iter srcIter = aBodyIndexChannel->getIter();
	Channel8u::Iter dstIter = resultChannel->getIter();

	// if a user exists in the pixel position make it 255 otherwise it should be 0
	while (srcIter.line() && dstIter.line()) {
	while (srcIter.pixel() && dstIter.pixel()) {
	if (srcIter.v() >= 0 && srcIter.v() < 6) {
	dstIter.v() = 255;
	}
	else {
	dstIter.v() = 0;
	}
	}
	}

	return resultChannel;
	}
	#pragma once

	#include "cinder/Area.h"
	#include "cinder/Channel.h"

	#include "Kinect2.h"

	// kinect 2 depth frame's dimensions
	const int DEPTH_FRAME_WIDTH = 512;
	const int DEPTH_FRAME_HEIGHT = 424;

	// filters and returns the depth frame by aMin and aMax distance from the kinect (in millimeters) and optionally imposes a ROI filter via aArea
	ci::Channel16uRef filterDepthByRange(const ci::Channel16uRef& aDepthChannel, float aMin, float aMax, const ci::Area& aArea = ci::Area(0, 0, DEPTH_FRAME_WIDTH, DEPTH_FRAME_HEIGHT)) const;

	// filters and returns the depth frame by aMin and aMax distance from the kinect (in millimeters) and optionally imposes a ROI filter via aArea
	// good for blob detection using OpenCV, returns the filtered channel with binary values ( passed is 255, cut is 0)
	ci::Channel8uRef binarizeDepthByRange(const ci::Channel16uRef& aDepthChannel, float aMin, float aMax, const ci::Area& aArea = ci::Area(0, 0, DEPTH_FRAME_WIDTH, DEPTH_FRAME_HEIGHT)) const;

	// normalizes body Index channel so that all users have the pixel value of 255 , perfect for blob detection using OpenCV
	ci::Channel8uRef binarizeBodyIndex(const ci::Channel8uRef& aBodyIndexChannel);
	#include "Body.h"
	#include "KinectUtils.h"
	#include "OcvUtils.h"

	std::vector<BodyRef> bodies;
	ci::Channel8uRef filteredDepthChannel;

	if (shouldFilterByDepth) {
	filteredDepthChannel = binarizeDepthByRange(mDepthChannel, 0, 8000);
	}
	else { // should use body index and binarize
	filteredDepthChannel = binarizeBodyIndex(mBodyIndexChannel);
	}

	//process depth source channel
	if (filteredDepthChannel) {
	//////// preprocess
	cv::Mat processedDepthMat = toOcv(*mFilteredDepthChannel);
	// blur and denoise the input kinect image
	cv::blur(processedDepthMat, processedDepthMat, cv::Size2f(13, 13));
	// threshold
	cv::threshold(processedDepthMat, processedDepthMat, 0, 255, cv::THRESH_OTSU);
	// this is optional, I liked the result more with one step of erosion
	cv::erode(processedDepthMat, processedDepthMat, cv::Mat(), cv::Point(-1, -1), 1);

	//contour and body creation
	cv::Mat contourMat, labels, stats, centroids;
	// the processed mat is copied because the following cv processes modify the original image, if you don't need it this could be skipped
	processedDepthMat.copyTo(contourMat);

	// https://stackoverflow.com/questions/39770382/opencv-how-to-find-the-pixels-inside-a-contour-in-c
	int nLabels = connectedComponentsWithStats(contourMat, labels, stats, centroids);
	for (int label = 1; label < nLabels; label++) { //0 is background
	//filter by area here
	int area = stats.at<int>(label, cv::CC_STAT_AREA);

	// extracting data
	int x1 = stats.at<int>(label, cv::CC_STAT_LEFT);
	int y1 = stats.at<int>(label, cv::CC_STAT_TOP);
	int width = stats.at<int>(label, cv::CC_STAT_WIDTH);
	int height = stats.at<int>(label, cv::CC_STAT_HEIGHT);
	int x2 = x1 + width;
	int y2 = y1 + height;
	// find screenPositions
	vector<ivec2> screenPositions;
	for (int i = x1; i < x2; i++)
	{
	for (int j = y1; j < y2; j++)
	{
	cv::Point p(i, j);
	if (label == labels.at<int>(p))
	{
	screenPositions.push_back(fromOcv(p));
	}
	}

	}
	// find contour
	// Get the mask for the i-th contour
	cv::Mat1b mask = labels == label;

	// Compute the contour
	vector<vector<cv::Point>> contours;
	findContours(mask, contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_NONE);
	mContours.push_back(contours[0]);

	auto body = Body::create();
	body->mBoundingRect = Rectf(x1, y1, x2, y2);
	body->mScreenPositions = std::move(screenPositions);
	auto moments = cv::moments(contours[0]);
	body->mCentroid = fromOcv(cv::Point(int(moments.m10 / moments.m00), int(moments.m01 / moments.m00)));
	body->mMoments = std::move(moments);
	body->mPolyLine = makePolylineFromContour(contours[0]);
	body->mContour = std::move(contours[0]);
	body->calc3dData(mDevice, mDepthChannel);

	bodies.push_back(body);
	}
	}