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 } }