kinect2深度图对齐以及三维坐标的计算

1. kinect2 深度图与彩色图对齐

IDepthFrame*frame1;
hResult = depthreader->AcquireLatestFrame(&frame1);
Mat depth_16bit(424, 512, CV_16UC1);//深度数据，需要赋值，这里假设已赋值
Mat color(height, width, CV_8UC4);//彩色图，需要赋值，这里假设已赋值
Mat coordinateMapperMat(height_d, width_d, CV_8UC4);//对齐后彩色图的存储

if (SUCCEEDED(hResult))
{
    ICoordinateMapper* pCoordinateMapper;
    kinect->get_CoordinateMapper(&pCoordinateMapper);

    hResult = pCoordinateMapper->MapDepthFrameToColorSpace(width_d * height_d, reinterpret_cast<UINT16*>(depth_16bit.data), width_d * height_d, &MappingMatrix[0]);
}
if (SUCCEEDED(hResult))
{
    coordinateMapperMat = cv::Scalar(0, 0, 0, 0);
    for (int y = 0; y < height_d; y++)
    {
        for (int x = 0; x < width_d; x++)
        {
            unsigned int index = y * width_d + x;
            ColorSpacePoint point = MappingMatrix[index];
            int colorX = static_cast<int>(std::floor(point.X + 0.5));
            int colorY = static_cast<int>(std::floor(point.Y + 0.5));                   
            if ((colorX >= 0) && (colorX < width) && (colorY >= 0) && (colorY < height))
            {
                coordinateMapperMat.at<cv::Vec4b>(y, x) = color.at<cv::Vec4b>(colorY, colorX);
            }
        }
    }
}

2. 根据深度图计算框选区域三维坐标

vector<double> getxyz(int u, int v, Mat depth_16bit, vector<double> params)//获取指定像素的3维坐标，可以经过两个循环计算出点云
{
    int camera_factor = params[0];
    double cx = params[1];//以下内参需要标定
    double cy = params[2];
    double fx = params[3];
    double fy = params[4];

    double z = depth_16bit.at<short>(u, v)/camera_factor;
    double x = (u - cx) * z / fx;
    double y =