RealSense SR300 坑5米 彩色图深度图的对齐

由于相机位置的不同，彩色图和深度图并不是完全对应的，将他们对齐就需要上一章提到的相机内参了。

效果

主要函数

其实在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();
 auto color_stream = profile.get_stream(RS2_STREAM_COLOR).as();
 //获取内参
 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(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(y, x) = depth_value;
  }
 }
 //返回一个与彩色图对齐了的深度信息图像
 return result;
}

//-------------------------------------------------------------------------------------------------------------//
完整代码下载地址：
(后续上传，github好慢。。。）