深度图坐标转化彩色图坐标

1. 理论

• 像素坐标系坐标原点在图像的左上角，X轴沿着水平方向向右，Y轴竖直向下
• 图像坐标系坐标原点在图像的中心，X轴沿着水平方向向右，Y轴竖直向下
• 相机坐标系X轴沿着水平方向向右，Y轴竖直向下，Z轴向前
• 像素(u,v)和三维点(x,y,z)的对应关系是
• u=fx*x/z+cx (1)
• v=fy*y/z+cy (2)
• x=(u-cx)*z/fx (3)
• y=(v-cy)*z/fy (4)
• fx,cx,fy,cy是相机的内参
• 具体分析可以看这里

2.流程

• 从上面两个公式我们知道转化需要距离，所以只能从深度图往彩色图转化
• 问题可描述为已知深度图的点(ud,vd,z)，求对应彩色图的(u,v)
  • 根据(3)(4)使用求出(ud,vd,z)深度相机坐标系下的坐标
  • 将上述坐标转化到彩色相机坐标系，转化关系和两个相机的安装位置有关
  • 根据(1)(2)使用彩色相机坐标系下的坐标求出彩色图的(u,v)
  • 如果将彩色图的像素值赋值给深度图，会看到一个缩小版的图片
  • 可以声明和彩色图像大小相同的数组，将深度存到对应的位置，就能直接从彩色图获取深度

3.代码

• 这里使用的是ROS的数据格式

typedef union USHORT_UNION {
   
   
  ushort num;
  char c[2];
} ushort_union;

ushort Little(char c0, char c1) {
   
   
  ushort_union tmp;
  tmp.c[0] =</