3次元ポイントクラウドのための座標変換

retrievePointCloudMap.cpp

//3次元ポイントクラウドのための座標変換
void retrievePointCloudMap(Mat &depth,Mat &pointCloud_XYZ){
    unsigned short* dp = (unsigned short*)depth.data;
	Point3f *point = (Point3f *)pointCloud_XYZ.data;
	for(int y = 0;y < depth.rows;y++){
		for(int x = 0;x < depth.cols;x++, dp++,point++){
#if KINECT_DEPTH_WIDTH == 320
			//奥行き画像の解像度が320x240の場合
			Vector4 RealPoints = NuiTransformDepthImageToSkeleton(x,y,*dp);
#else if KINECT_DEPTH_WIDTH == 640
			//奥行き画像の解像度が640x480の場合
			Vector4 RealPoints = NuiTransformDepthImageToSkeleton(x,y,*dp, NUI_IMAGE_RESOLUTION_640x480);
#endif
			point->x = RealPoints.x;
			point->y = RealPoints.y;
			point->z = RealPoints.z;
		}
	}
}