kinect2.0数据采集及点云生成代码C++

#include "prepare.h" #include <iomanip> UINT16 Image::iDepthMax; UINT16 Image::iDepthMin; int Image::iDepthWidth; int Image::iDepthHeight; int Image::iColorWidth; int Image::iColorHeight; UINT Image::iDepthFrameSize; UINT Image::iColorFrameSize; Kinect::Kinect() { pKinectSensor = nullptr; pDepthFrameSource = nullptr; pDepthFrameReader = nullptr; pDepthFrameDescription = nullptr; pColorFrameSource = nullptr; pColorFrameReader = nullptr; pColorFrameDescription = nullptr; pDepthFrame = nullptr; pColorFrame = nullptr; pCoordinateMapper = nullptr; } Kinect::~Kinect() { cout << endl; if (pCoordinateMapper != nullptr) { cout << "release coordinate mapper" << endl; pCoordinateMapper->Release(); pCoordinateMapper = nullptr; } if (pDepthFrame != nullptr) { cout << "release depth frame" << endl; pDepthFrame->Release(); pDepthFrame = nullptr; } if (pColorFrame != nullptr) { cout << "release color frame" << endl; pColorFrame->Release(); pColorFrame = nullptr; } if (pDepthFrameReader != nullptr) { cout << "release depth frame reader" << endl; pDepthFrameReader->Release(); pDepthFrameReader = nullptr; } if (pColorFrameReader != nullptr) { cout << "release color frame reader" << endl; pColorFrameReader->Release(); pColorFrameReader = nullptr; } if (pDepthFrameDescription != nullptr) { cout << "release depth frame description" << endl; pDepthFrameDescription->Release(); pDepthFrameDescription = nullptr; } if (pColorFrameDescription != nullptr) { cout << "release color frame description" << endl; pColorFrameDescription->Release(); pColorFrameDescription = nullptr; } if (pDepthFrameSource != nullptr) { cout << "release depth frame source" << endl; pDepthFrameSource->Release(); pDepthFrameSource = nullptr; } if (pColorFrameSource != nullptr) { cout << "release color frame source" << endl; pColorFrameSource->Release(); pColorFrameSource = nullptr; } cout << "close sensor" << endl; pKinectSensor->Close(); cout << "release sensor" << endl; pKinectSensor->Release(); pKinectSensor = nullptr; } void Kinect::releaseDepthFrame() { if (pDepthFrame != nullptr) { pDepthFrame->Release(); pDepthFrame = nullptr; } } void Kinect::releaseColorFrame() { if (pColorFrame != nullptr) { pColorFrame->Release(); pColorFrame = nullptr; } } bool Kinect::kinectStart() { //get default sensor cout << "try to get sensor" << endl; if (GetDefaultKinectSensor(&pKinectSensor) != S_OK) { cerr << "get sensor failed" << endl << endl; return 1; } //open sensor cout << "try to open sensor" << endl; if (pKinectSensor->Open() != S_OK) { cerr << "open sensor failed" << endl << endl; return 1; } /* PREPARE FOR DEPTH FRAME */ //get depth frame source cout << "try to get depth frame source" << endl; if (pKinectSensor->get_DepthFrameSource(&pDepthFrameSource) != S_OK) { cerr << "get depth frame source failed" << endl << endl; return 1; } //save depth range pDepthFrameSource->get_DepthMaxReliableDistance(&Image::iDepthMax); pDepthFrameSource->get_DepthMinReliableDistance(&Image::iDepthMin); //get depth frame description cout << "try to get depth frame description" << endl; if (pDepthFrameSource->get_FrameDescription(&pDepthFrameDescription) != S_OK) { cerr << "get depth frame description failed" << endl << endl; return 1; } //save depth description pDepthFrameDescription->get_Width(&Image::iDepthWidth); pDepthFrameDescription->get_Height(&Image::iDepthHeight); Image::iDepthFrameSize = Image::iDepthWidth * Image::iDepthHeight; //get depth frame reader cout << "try to get depth frame reader" << endl; if (pDepthFrameSource->OpenReader(&pDepthFrameReader) != S_OK) { cerr << "get depth frame reader failed" << endl << endl; return 1; } /* PREPARE FOR COLOR FRAME */ //get color frame source cout << "try to get color frame source" << endl; if (pKinectSensor->get_ColorFrameSource(&pColorFrameSource) != S_OK) { cerr << "get color frame source failed" << endl << endl; return 1; } //get color frame description cout << "try to get color frame description" << endl; if (pColorFrameSource->get_FrameDescription(&pColorFrameDescription) != S_OK) { cerr << "get color frame description failed" << endl << endl; return 1; } //save color description pColorFrameDescription->get_Width(&Image::iColorWidth); pColorFrameDescription->get_Height(&Image::iColorHeight); Image::iColorFrameSize = Image::iColorWidth * Image::iColorHeight; //get color frame reader cout << "try to get color frame reader" << endl; if (pColorFrameSource->OpenReader(&pColorFrameReader) != S_OK) { cerr << "get color frame reader failed" << endl << endl; return 1; } /* PREPARE COORDINATE MAPPER */ cout << "try to get coordinate mapper" << endl; if (pKinectSensor->get_CoordinateMapper(&pCoordinateMapper) != S_OK) { cerr << "get coordinate mapper failed" << endl << endl; return 1; } /* RELEASE DESCRIPTION AND SOURCE */ if (pDepthFrameDescription != nullptr) { cout << "release depth frame description" << endl; pDepthFrameDescription->Release(); pDepthFrameDescription = nullptr; } if (pColorFrameDescription != nullptr) { cout << "release color frame description" << endl; pColorFrameDescription->Release(); pColorFrameDescription = nullptr; } if (pDepthFrameSource != nullptr) { cout << "release depth frame source" << endl; pDepthFrameSource->Release(); pDepthFrameSource = nullptr; } if (pColorFrameSource != nullptr) { cout << "release color frame source" << endl; pColorFrameSource->Release(); pColorFrameSource = nullptr; } cout << endl; //print info cout << "depth frame description: " << Image::iDepthWidth << setw(5) << " * " << Image::iDepthHeight << endl; cout << "color frame description: " << Image::iColorWidth << setw(5) << " * " << Image::iColorHeight << endl; cout << "depth reliable distance: " << Image::iDepthMin << "mm - " << Image::iDepthMax << "mm" << endl << endl; return 0; } void Kinect::coordinateMap(UINT16* pDepthBuffer, DepthSpacePoint* pDepthSpacePoint, CameraSpacePoint* pCameraSpacePoint, Mat* MappedDepth, uchar* pDepthBufferT, Mat* ColorBuffer, PointCloud<PointXYZRGB>::Ptr cloud_color) { while (pCoordinateMapper->MapColorFrameToDepthSpace(Image::iDepthFrameSize, pDepthBuffer, Image::iColorFrameSize, pDepthSpacePoint) != S_OK); while (pCoordinateMapper->MapColorFrameToCameraSpace(Image::iDepthFrameSize, pDepthBuffer, Image::iColorFrameSize, pCameraSpacePoint) != S_OK); MappedDepth->setTo(0); for (int k = 0; k < Image::iColorHeight; k++) { for (int j = 0; j < Image::iColorWidth; j++) { DepthSpacePoint depthpoint = pDepthSpacePoint[k*Image::iColorWidth + j]; CameraSpacePoint camerapoint = pCameraSpacePoint[k*Image::iColorWidth + j]; if ((depthpoint.X >= 0) && (depthpoint.X < (Image::iDepthWidth)) && (depthpoint.Y >= 0) && (depthpoint.Y < (Image::iDepthHeight))) { MappedDepth->at<uchar>(k, j) = pDepthBufferT[(int)(depthpoint.Y * Image::iDepthWidth + depthpoint.X)]; //Transparency //ColorBuffer->at<uchar>(k, 4 * j + 3) = MappedDepth->at<uchar>(k, j); //ColorBuffer->at<uchar>(k, 4 * j + 3) = 0; cloud_color->points[k*Image::iColorWidth + j].x = camerapoint.X; cloud_color->points[k*Image::iColorWidth + j].y = camerapoint.Y; cloud_color->points[k*Image::iColorWidth + j].z = camerapoint.Z; Vec4b color = ColorBuffer->at<Vec4b>(k, j); cloud_color->points[k*Image::iColorWidth + j].r = color[2]; cloud_color->points[k*Image::iColorWidth + j].g = color[1]; cloud_color->points[k*Image::iColorWidth + j].b = color[0]; } } } } void Kinect::coordinateMap(UINT