kinect深度图与彩图匹配

理论部分

参数说明

p_rgb，p_ir分别为彩图、深度图的像素坐标

P_rgb，P_ir分别为彩色摄像头、深度摄像头对应的相机坐标系下的三维点坐标矩阵P=[X,Y,Z]’

X_rgb，Y_rgb，Z_rgb表示彩色摄像头下三维点坐标值，以此类推深度摄像头

K_rgb，K_ir分别为彩图、深度图的内参矩阵

R_rgb，T_rgb为彩图的外参矩阵，同理可以得到深度图的外参矩阵R_ir，T_ir

公式推导

整个过程可以理解为，已知深度图对应的像素点–>（小孔成像原理）相对于深度图的三维点坐标–>（两相机之间的外参）相对于彩图的三维点坐标–>（小孔成像原理）彩图像素点

由小孔成像原理可得

p = K * P

即相对于深度相机三维点坐标

P_ir = K_ir ^(-1)* p_ir

深度相机与彩图相机之间的外参

R_ir2rgb = R_rgb * R_ir^(-1)

T_ir2rgb = T_rgb - R_rgb * R_ir^(-1) * T_ir = T_rgb - R * T_ir

相对于彩图相机三维点坐标

P_rgb = R_ir2rgb * P_ir + T_ir2rgb

对应彩图像素点

p_rgb = K_rgb * P_rgb

最后得到关系式

Z_rgb∗p_rgb=K_rgb∗R_ir2rgb∗K_ir^(-1)∗Z_ir∗p_ir+K_rgb∗T_ir2rgb

总结一下，整个匹配过程就是将彩图像素点附在深度图图之上，从而实现匹配，而匹配的过程涉及三个步骤，第一步是将深度图像素点坐标转换为深度图相机的相机坐标，第二步是将深度图相机的相机坐标转换为彩图相机的相机坐标，第三步把彩图的相机坐标转换为对应的彩图像素坐标，从而找到每一个深度图中像素点对应彩图的像素值，进而做到匹配。

代码部分

#include 
   
    #include 
    
     #include 
     
      #include 
      
       #include 
       
        #include 
        
         #include 
         
          #include 
          
           using namespace cv;using namespace std;int main(){ Mat bgr(1080, 1920, CV_8UC4); bgr = imread("color.jpg"); Mat depth(424, 512, CV_16UC1); depth = imread("depth00.png", IMREAD_ANYDEPTH); // jpg图片读入后的格式不一定和定义时候的一样，比如这里读入后的格式就是8UC3，所以注意之前在保存图片时要存为PNG // 3. 显示 thread th = std::thread([&]{ while (true) { imshow("原始彩色图", bgr); waitKey(1); imshow("原始深度图", depth*20); waitKey(1); } }); Eigen::Matrix3f K_ir; // ir内参矩阵 K_ir << 368.8057, 0, 255.5000, 0, 369.5268, 211.5000, 0, 0, 1; Eigen::Matrix3f K_rgb; // rgb内参矩阵 K_rgb << 1044.7786, 0, 985.9435, 0, 1047.2506, 522.7765, 0, 0, 1; Eigen::Matrix3f R_ir2rgb; Eigen::Matrix3f R; Eigen::Vector3f T_temp; Eigen::Vector3f T; R_ir2rgb << 0.9996, 0.0023, -0.0269, -0.0018, 0.9998, 0.0162, 0.0269, -0.0162, 0.9995; T_temp << 65.9080, -4.1045, -13.9045; R = K_rgb*R_ir2rgb*K_ir.inverse(); T = K_rgb*T_temp; //投影计算部分 Mat result(424, 512, CV_8UC3); int i = 0; for (int row = 0; row < 424; row++) { for (int col = 0; col < 512; col++) { unsigned short* p = (unsigned short*)depth.data; unsigned short depthValue = p[row * 512 + col]; //cout << "depthValue " << depthValue << endl; if (depthValue != -std::numeric_limits
           
            ::infinity() && depthValue != -std::numeric_limits
            
             ::infinity() && depthValue != 0 && depthValue != 65535) { // 投影到彩色图上的坐标 Eigen::Vector3f uv_depth(col, row, 1.0f); Eigen::Vector3f uv_color = depthValue / 1000.f*R*uv_depth + T / 1000; //用于计算映射，核心式子 int X = static_cast
             
              (uv_color[0] / uv_color[2]); //计算X，即对应的X值 int Y = static_cast
              
               (uv_color[1] / uv_color[2]); //计算Y，即对应的Y值 if ((X >= 0 && X < 1920) && (Y >= 0 && Y < 1080)) { result.data[i * 3] = bgr.data[3 * (Y * 1920 + X)]; result.data[i * 3 + 1] = bgr.data[3 * (Y * 1920 + X) + 1]; result.data[i * 3 + 2] = bgr.data[3 * (Y * 1920 + X) + 2]; } } i++; } } thread th2 = std::thread([&]{ while (true) { imshow("结果图", result); waitKey(1); } }); th.join(); th2.join(); return 0;}

效果图

原始深度图

效果图

你可能感兴趣的文章