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

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] = c0;
  tmp.c[1] = c1;
  return tmp.num;
}

float aligns[640][480];
void dep2color(const sensor_msgs::ImageConstPtr& imag, const sensor_msgs::Image::ConstPtr& depth) {
  for(int i = 0; i < 640; ++i){
    for(int j = 0; j < 480; ++j) {
      aligns[i][j] = 0;
    }
  }
  
  for(int u = 0; u < 640; ++u) {
    for(int v = 0; v < 480; ++v) {
      int index = v * depth->width + u;
      float z = Little(depth->data[index * 2], depth->data[index * 2 + 1])/1000.0; 
      if(z == 0) continue;
      float x = (u-322.5)*z/386.3+0.044;
      float y = (v-238.4)*z/386.3;
      int uu = 615.5*x/z+313.8;
      if(uu < 0 || uu >= 640) continue;
      int vv = 614.3*y/z+233.2;
      if(vv < 0 || vv >= 480) continue;
      aligns[uu][vv] = z;
    }
  }
}