理论部分
参数说明
- 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
需要注意的是，p_ir和p_rgb使用的都是齐次坐标(将一个原本是n维的向量用一个n+1维向量来表示)，因此在构造p_ir时，应将原始的像素坐标（x，y）乘以深度值，而最终的RGB像素坐标必须将p_rgb除以z分量，即（x/z，y/z），且z分量的值即为该点到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);
}
});
#pragma endregion
// 非齐次
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;
// 4.2 计算投影
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值
//cout << “X: ” << X << ” Y: ” << Y << endl;
if ((X >= 0 && X < 1920) && (Y >= 0 && Y < 1080))
{
//cout << “X: ” << X << ” Y: ” << Y << endl;
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;
}
效果图
