A simple script for ray hitting the kinect pointcloud

3- Ray hit Kinect Pointcloud

A simple script for ray hitting the kinect pointcloud

main.cpp

#include "rayhit.h"

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

// spineMidPos will be the position from which we'll cast the ray
auto& spineMid = body.getJointMap().find(JointType_SpineMid)->second;
auto& spineMidPos = spineMid.getPosition();
// getting shoulder positions to calculate a ROI (Region Of Interest) for our ray hitting, this can help boost the performance
auto& shoulderRight = body.getJointMap().find(JointType_ShoulderRight)->second;
auto& shoulderRightPos = shoulderRight.getPosition();
auto& shoulderLeft = body.getJointMap().find(JointType_ShoulderLeft)->second;
auto& shoulderLeftPos = shoulderLeft.getPosition();

// the orienation the ray will be casted towards
quat rayOrient = spineMid.getOrientation();
// intialize pos with the position of spineMide in case we couldn't find a ray hit location
vec3 pos = spineMidPos;

// mDepthChannel is the 16-bit channel supplied by the KCB2 block, the connection is made once in setup something like mDevice->connectDepthEventHandler([&](const Kinect2::DepthFrame& frame) { mDepthChannel = frame.getChannel(); });
if (mDepthChannel) {
	// ====== Ray
	// make a ray, transform it based on the orienation 
 	Ray ray = Ray(spineMidPos, vec3(0, 0, 1));
	ray.transform(toMat4(rayOrient));
	
	// ====== making the ROI (a circle) and obtaining the points inside it
	// trying to limit the ROI based on the shoulder positions in the depth channel, 
	vec2 spineMidPosDepth = mDevice->mapCameraToDepth(spineMidPos);
	vec2 shoulderRightPosDepth = mDevice->mapCameraToDepth(shoulderRightPos);
	vec2 shoulderLeftPosDepth = mDevice->mapCameraToDepth(shoulderLeftPos);
	// based on tests a minimim of 60 is considered for the ROI radius if the user is rotated too much and the shoulders distance is less
	float maxDist = max(distance(shoulderLeftPosDepth, shoulderRightPosDepth), 60.0f);
	// make a vector of positions on the screen and fill it with the positions inside the ROI
	vector<ivec2> depthRange;
	for (int i = 0; i < mDepthChannel->getWidth(); i++) {
	  	for (int j = 0; j < mDepthChannel->getHeight(); j++) {
	    		vec2 pos = ivec2(i, j);
			if (length(pos - spineMidPosDepth) < maxDist) {
			  depthRange.push_back(pos);
			}
    		}
	}

	// ======= obtaining a pointcloud based on the 2d positions from the previous step and ray hitting the ray with it
	vector<vec3> posVector = mDevice->mapDepthToCamera(depthRange, mDepthChannel);
	vec3 hitPoint = rayhit(ray, posVector);
  	// if a nearest point was found set pos to it
	if (hitPoint != vec3()) {
	  pos = hitPoint;
  	}
}

// if hit, pos will be the result position, else it will remain as the position of SpineMid

rayhit.cpp

#include "rayhit.h"

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

vec3 rayhit(const ci::Ray& aRay, const std::vector<ci::vec3>& aPointcloud)
{
  	// the formula:
	/*distance = | (p2 - p1) x(p1 - p0) |
	------------------ -
	| (p2 - p1) |*/
	float dist;
	vec3 nearest;
  
  	auto& rayOrigin = aRay.getOrigin();
  	auto& rayDir = normalize(aRay.getDirection());
  	vec3 endPos = rayOrigin + rayDir * 1000.0f;
  
  	if (aPointcloud.size() > 0 ) {
    		auto& pos = aPointcloud[0]; 
    		nearest = pos;
    		dist = length(cross(endPos - rayOrigin, rayOrigin - pos)) / length(endPos - rayOrigin);
  	}
  	else {
    		CI_LOG_W("Pointcloud was empty!");
    		return vec3();
  	}
	// rays don't have ends but here we'll need one to be able to calculate the distance based on the formula above, so we calculate an arbitary endPos far away
	
	for (auto& pos : aPointcloud) {
		float newDist = length(cross(endPos - rayOrigin, rayOrigin - pos)) / length(endPos - rayOrigin);
		if (newDist < dist) {
			dist = newDist;
			nearest = pos;
		}
	}
  
  	return nearest;
}

rayhit.h

#include "cinder/Ray.h"
#include "cinder/Vector.h"

//this chooses the closest point in the aPointcloud to aRay
ci::vec3          rayHit(const ci::Ray& aRay, const std::vector<ci::vec3>& aPointcloud);