inline ofxPCL::PointXYZRGBCloud getPointCloudFromKinect(ofxKinect &kinect, int step = 2) {
ofxPCL::PointXYZRGBCloud cloud(new ofxPCL::PointXYZRGBCloud::element_type);
int count = 0;
int w = kinect.getWidth();
int h = kinect.getHeight();
cloud->width = ceil((float)w / (float)step) * ceil((float)h / (float)step);
cloud->height = 1;
cloud->is_dense = true;
cloud->points.resize(cloud->width * cloud->height);
for(int y = 0; y < h; y += step) {
for(int x = 0; x < w; x += step) {
if(kinect.getDistanceAt(x, y) > 0) {
ofVec3f v = kinect.getWorldCoordinateAt(x, y);
cloud->points[count].x = v.x;
cloud->points[count].y = v.y;
cloud->points[count].z = v.z;
ofColor col = kinect.getColorAt(x, y);
uint32_t rgb = ((uint32_t)col.r << 16 | (uint32_t)col.g << 8 | (uint32_t)col.b);
cloud->points[count].rgb = *reinterpret_cast<float*>(&rgb);
count++;
}
}
}
cloud->points.resize(count);
cloud->width = count;
return cloud;
}
float get_depth_in_m(ofxKinect &kinect,const int &x,const int &y)
{
//cout<<"get_depth_in_m\n";
float sum=0,count=0,val;
for(int i = y-5; i<=y+5;i++)
{
if(i>=0 && i<kinect.height)
{
val = kinect.getDistanceAt(x,i);
if(val>0)
{
sum += val;
count++;
}
}
}
if(count==0) return 0.0;
else return sum/count;
}
void KinectController::updateAnalisys(ofxKinect & kinect, float bbX, float bbY, float bbZ, float bbW, float bbH, float bbD){
if(kinect.isFrameNew()){
static int counter = 0;
int w = 640;
int h = 480;
#if ANGLE_FROM_ACCEL
//ofVec3f worldOffset(worldX,worldY,worldZ);
ofMatrix4x4 m = camTransform.getModelViewMatrix();
int i = 0;
for(int y = 0; y < h; y +=step) {
for(int x = 0; x < w; x +=step) {
const short & distance = kinect.getRawDepthPixelsRef()[x+y*w];
//if(distance > 0) {
const ofVec3f & v = kinect.getWorldCoordinateAt(x, y, distance);
/*if(correctAngle){
v = v*m;
}
if(!applyBB || insideBB3D(v,ofVec3f(bbX,bbY,bbZ), ofVec3f(bbW, bbH, bbD))){*/
mesh.getVertices()[i] = v;
/*}else{
mesh.getVertices()[i].set(0,0,0);
}*/
//}
i++;
}
}
#elif ANGLE_FROM_PCL_GROUND_PLANE
pcl::PointCloud<pcl::PointXYZ>::Ptr pcPtr = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
for(int y = 0; y < h; y += step) {
for(int x = 0; x < w; x += step) {
//if(kinect.getDistanceAt(x, y) > 0) {
float z = kinect.getDistanceAt(x, y);
pcPtr->push_back(pcl::PointXYZ(x,y,z));
//}
}
}
pcPtr->width=640;
pcPtr->height=480;
pcl::ModelCoefficients::Ptr planeCoeffs = fitPlane<pcl::PointXYZ>(pcPtr,10,5);
plane.set(planeCoeffs->values[0],planeCoeffs->values[1],planeCoeffs->values[2],planeCoeffs->values[3]);
for(int i=0;i<pcPtr->points.size();i++){
ofVec3f v(pcPtr->points[i].x,pcPtr->points[i].y,pcPtr->points[i].z);
if(plane.distanceToPoint(v)>60)
mesh.addVertex( kinect.getWorldCoordinateAt(v.x, v.y, v.z) );
}
//pcPtr = findAndSubtractPlane<pcl::PointXYZ>(pcPtr,60,5);
//ofxPCL::toOf(pcPtr,mesh,1,1,1);
#elif CV_ANALISYS
cv::Mat backgroundMat = toCv(background);
if(captureBg>0){
backgroundMat += toCv(kinect.getDistancePixelsRef());
captureBg--;
}else if(captureBg==0){
backgroundMat /= 10;
cv::GaussianBlur(backgroundMat,backgroundMat,Size(11,11),10);
captureBg=-1;
}else{
// difference threshold
cv::Mat diffMat = toCv(diff);
cv::Mat currentMat = toCv(kinect.getDistancePixelsRef());
cv::Mat gaussCurrentMat = toCv(gaussCurrent);
cv::GaussianBlur(currentMat,gaussCurrentMat,Size(11,11),10);
cv::absdiff(backgroundMat, gaussCurrentMat, diffMat);
//diffMat = toCv(background) - toCv(kinect.getDistancePixelsRef());
threshold(diff,thresPix,.001);
thres8Bit = thresPix;
cv::Mat kernel;
cv::Mat thresMat = toCv(thres8Bit);
cv::Point anchor(-1,-1);
erode(toCv(thres8Bit),thresMat,kernel,anchor,10);
dilate(toCv(thres8Bit),thresMat,kernel,anchor,5);
cv::Mat fgMat = toCv(fg);
bgSubstractor(toCv(thres8Bit),fgMat);
contourFinder.findContours(fg);
for(int i=0;i<oscContours.size();i++){
oscContours[i]->newFrame(frame);
}
polylines = contourFinder.getPolylines();
for(int i=0;i<(int)polylines.size();i++){
ofPoint centroid = polylines[i].getCentroid2D();
polylines[i] = polylines[i].getResampledByCount(16);
float z = kinect.getDistanceAt(centroid);
for(int j=0;j<oscContours.size();j++){
oscContours[j]->sendBlob(polylines[i],z);
}
}
frame++;
if(recording){
//convertColor(kinect.getDepthPixelsRef(),rgbDepth,CV_GRAY2RGB);
recorder.addFrame(kinect.getRawDepthPixelsRef());
}
}
#endif
}
}
