kinect深度图与彩图匹配

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 <opencv2\core\core.hpp>
#include <opencv2\highgui\highgui.hpp>
#include <fstream>
#include <iostream>
#include <sstream>
#include <Eigen/Core>
#include <Eigen/LU>
#include <thread>

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<unsigned short>::infinity() && depthValue != -std::numeric_limits<unsigned short>::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<int>(uv_color[0] / uv_color[2]);         //计算X，即对应的X值
int Y = static_cast<int>(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;
}

效果图