由于相机位置的不同,彩色图和深度图并不是完全对应的,将他们对齐就需要上一章提到的相机内参了。
效果
主要函数
其实在rsutil.h文件中已经给了现有的函数,可以用来转化不同相机之间的坐标,看清楚参数直接调用即可。
//将深度图的像素点根据内参转换到深度摄像头坐标系下的三维点
rs2_deproject_pixel_to_point(Pdc3, &intrinDepth, pd_uv, depth_m);
//将深度摄像头坐标系的三维点转化到彩色摄像头坐标系下
rs2_transform_point_to_point(Pcc3, &extrinDepth2Color, Pdc3);
//将彩色摄像头坐标系下的深度三维点映射到二维平面上
rs2_project_point_to_pixel(pc_uv, &intrinColor, Pcc3);
代码
Mat align_Depth2Color(Mat depth, Mat color, rs2::pipeline_profile profile)
{
//声明数据流
auto depth_stream = profile.get_stream(RS2_STREAM_DEPTH).as<rs2::video_stream_profile>();
auto color_stream = profile.get_stream(RS2_STREAM_COLOR).as<rs2::video_stream_profile>();
//获取内参
const auto intrinDepth = depth_stream.get_intrinsics();
const auto intrinColor = color_stream.get_intrinsics();
//直接获取从深度摄像头坐标系到彩色摄像头坐标系的欧式变换矩阵
//auto extrinDepth2Color=depth_stream.get_extrinsics_to(color_stream);
rs2_extrinsics extrinDepth2Color;
rs2_error *error;
rs2_get_extrinsics(depth_stream, color_stream, &extrinDepth2Color, &error);
//平面点定义
float pd_uv[2], pc_uv[2];
//空间点定义
float Pdc3[3], Pcc3[3];
//获取深度像素与现实单位比例(D435默认1毫米,SR300为0.125)
float depth_scale = get_depth_scale(profile.get_device());
int y = 0, x = 0;
//初始化结果
//Mat result=Mat(color.rows,color.cols,CV_8UC3,Scalar(0,0,0));
Mat result = Mat(color.rows, color.cols, CV_16U, Scalar(0));
//对深度图像遍历
for (int row = 0; row < depth.rows; row++)
{
for (int col = 0; col < depth.cols; col++)
{
//将当前的(x,y)放入数组pd_uv,表示当前深度图的点
pd_uv[0] = col;
pd_uv[1] = row;
//取当前点对应的深度值
uint16_t depth_value = depth.at<uint16_t>(row, col);
//换算到米
float depth_m = depth_value * depth_scale;
//将深度图的像素点根据内参转换到深度摄像头坐标系下的三维点
rs2_deproject_pixel_to_point(Pdc3, &intrinDepth, pd_uv, depth_m);
//将深度摄像头坐标系的三维点转化到彩色摄像头坐标系下
rs2_transform_point_to_point(Pcc3, &extrinDepth2Color, Pdc3);
//将彩色摄像头坐标系下的深度三维点映射到二维平面上
rs2_project_point_to_pixel(pc_uv, &intrinColor, Pcc3);
//取得映射后的(u,v)
x = (int)pc_uv[0];
y = (int)pc_uv[1];
if(x<0||x>color.cols)
continue;
if(y<0||y>color.rows)
continue;
//最值限定
x = x < 0 ? 0 : x;
x = x > depth.cols - 1 ? depth.cols - 1 : x;
y = y < 0 ? 0 : y;
y = y > depth.rows - 1 ? depth.rows - 1 : y;
result.at<uint16_t>(y, x) = depth_value;
}
}
//返回一个与彩色图对齐了的深度信息图像
return result;
}
//-------------------------------------------------------------------------------------------------------------//
完整代码下载地址:
(后续上传,github好慢。。。)