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(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(y, x) = color.at(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 = (v - cy) * z / fy;

    vector<double> xyz;
    xyz.push_back(x);
    xyz.push_back(y);
    xyz.push_back(z);

    return xyz;

}

3. 交互功能如下

操作	功能
“s”	保存当前帧彩色图
鼠标	框选区域（左上角按鼠标左键，拖到右下角松开）

4. 完整代码

#include "stdafx.h"
#include 
#include 
#include "opencv2/core.hpp"
#include "opencv2/imgproc.hpp"
#include "opencv2/highgui.hpp"
#include "opencv2/videoio.hpp"

#include   
#pragma comment ( lib, "kinect20.lib" )  

using namespace cv;
using namespace std;

//全局变量
Rect select1 = Rect(0,0,0,0);
bool tracking = false;
vector<double> params(5);//存放缩放系数和kinect2的内参

Mat depth_16bit(424, 512, CV_16UC1);
Mat depth(424, 512, CV_8UC1);


vector<double> getxyz(int u, int v, Mat depth_16bit, vector<double> params)
{
    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 = (v - cy) * z / fy;

    vector<double> xyz;
    xyz.push_back(x);
    xyz.push_back(y);
    xyz.push_back(z);

    return xyz;

}

void onMouse(int event,int x,int y,int,void*)//鼠标从左上角到右下角进行框选
{
    if(event==CV_EVENT_LBUTTONDOWN)
    {
        select1.x=x;
        select1.y=y;
        tracking=false;
    }
    else if(event==CV_EVENT_LBUTTONUP)
    {
        select1.width=x-select1.x;
        select1.height=y-select1.y;
        tracking=true;//左键完后，开始跟踪

        int u = select1.x + 0.5 * select1.width;
        int v = select1.y + 0.5 * select1.height;
        vector<double> xyz = getxyz(v, u, depth_16bit, params);
        cout << "3d position of the selected pixel: [" << xyz[0] << " " << xyz[1] << " " << xyz[2] << "]" << endl;

    }
}

int main()
{
    //kinect2的缩放系数和内参
    params[0] = 1000;
    params[1] = 0.9677*1e3;
    params[2] = 0.5423*1e3;
    params[3] = 1.0634*1e3;
    params[4] = 1.0634*1e3;

    HRESULT hResult = S_OK;     //用于检测操作是否成功
    IKinectSensor *kinect;           //创建一个感应器
    GetDefaultKinectSensor(&kinect);
    kinect->Open();     //打开感应器

    IColorFrameSource *colorsource;
    IColorFrameReader *colorreader;
    IFrameDescription *colorde;
    kinect->get_ColorFrameSource(&colorsource);
    colorsource->OpenReader(&colorreader);
    colorsource->get_FrameDescription(&colorde);

    IDepthFrameSource *depthrsource;
    IDepthFrameReader *depthreader;
    IFrameDescription *depthde;
    kinect->get_DepthFrameSource(&depthrsource);
    depthrsource->OpenReader(&depthreader);
    depthrsource->get_FrameDescription(&depthde);

    int width = 0;      //长和宽
    int height = 0;
    colorde->get_Height(&height);
    colorde->get_Width(&width);
    Mat color(height, width, CV_8UC4);//注意：这里必须为4通道的图，Kinect的数据只能以Bgra格式传出
    namedWindow("color");

    int width_d = 0;
    int height_d = 0;
    depthde->get_Height(&height_d);
    depthde->get_Width(&width_d);

    //namedWindow("depth");
    UINT16 *data = new UINT16[width_d*height_d];

    cout << "color picture size:" << width << "x" << height << endl;
    cout << "depth picture size:" << width_d << "x" << height_d << endl;

    //深度图与彩色图对齐 
    vector  MappingMatrix(width_d * height_d);
    Mat coordinateMapperMat(height_d, width_d, CV_8UC4);
    namedWindow("Map color");
    setMouseCallback("Map color", onMouse, 0);

    int num = 1;

    while (1)
    {
        //彩色图显示
        IColorFrame*frame;
        hResult = colorreader->AcquireLatestFrame(&frame);
        if (SUCCEEDED(hResult))
        {
            frame->CopyConvertedFrameDataToArray(height*width * 4, 
                reinterpret_cast(color.data), ColorImageFormat::ColorImageFormat_Bgra);   //传出数据
        }

        if (frame != NULL)   //释放
        {
            frame->Release();
            frame = NULL;
        }
        if (waitKey(30) == VK_ESCAPE)
            break;  

        cv::imshow("color", color);

        stringstream ss;
        ss << num << ".jpg";
        if (waitKey(100)==115)//s的ASCII码为115
        {
            cv::imwrite(ss.str(), color);
            cout<" saved."<//深度图显示
        IDepthFrame*frame1;
        hResult = depthreader->AcquireLatestFrame(&frame1);

        if (SUCCEEDED(hResult))
        {
            frame1->CopyFrameDataToArray(height_d*width_d, data);
            for (int i = 0; i < width_d*height_d; i++)
            {               
                byte intensity = (byte)(data[i] >> 5); //类型的转换,data内的值即为距离，单位mm             
                reinterpret_cast(depth.data)[i] = intensity;
                depth_16bit.at<short>( i / width_d, i%width_d) = data[i];       

            }

        }

        if (frame1 != NULL)
        {
            frame1->Release();
            frame1 = NULL;
        }
        if (waitKey(30) == VK_ESCAPE)
            break;
        cv::imshow("depth", depth);     

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

            hResult = pCoordinateMapper->MapDepthFrameToColorSpace(width_d * height_d, reinterpret_cast(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(y, x) = color.at(colorY, colorX);
                    }
                }
            }
        }

        if (waitKey(30) == VK_ESCAPE)
            break;
        rectangle(coordinateMapperMat, select1, Scalar(255,0,0), 3, 8, 0);      

        cv::imshow("Map color", coordinateMapperMat);
        waitKey(30);
    }

    //colorful
    if (colorsource != NULL)    //全部释放掉
    {
        colorsource->Release();
        colorsource = NULL;
    }
    if (colorreader != NULL)
    {
        colorreader->Release();
        colorreader = NULL;
    }
    if (colorde != NULL)
    {
        colorde->Release();
        colorde = NULL;
    }
    //depth
    if (depthrsource != NULL)
    {
        depthrsource->Release();
        depthrsource = NULL;
    }
    if (depthreader != NULL)
    {
        depthreader->Release();
        depthreader = NULL;
    }
    if (depthde != NULL)
    {
        depthde->Release();
        depthde = NULL;
    }
    //kinect2
    if (kinect)
    {
        kinect->Close();
    }
    if (kinect != NULL)
    {
        kinect->Release();
        kinect = NULL;
    }


    destroyAllWindows();

}

5.参考

• Opencv+Kinect2.0 的环境配置和获取彩色图
• Opencv+Kinect2.0获取景深图
• 一起做RGB-D SLAM (2) 第二讲 从图像到点云