深度图转激光原理以及代码解析

预备知识

参考 https://www.cnblogs.com/gemstone/articles/2293806.html

相机模型

计算机中图像的表示

世界坐标到图像的映射关系

实现步骤

深度图转激光的主要思想是将一定高度范围内的数据进行投影，与点云图转激光类似。
具体步骤如下：

1. 深度图坐标点到世界坐标的转换

通过深度图可以获取到
（1）图像坐标p(x,y)
深度信息depth得知
（2）相机原点Oc到物体所在平面的距离Zc
相机内参得知
（3）焦距f

由(1) (2) (3)，根据针孔相机模型，可求出世界坐标P(Xw,Yw,Zw)，P点的相机坐标P(Xc,Yc,Zc)

2. 计算坐标点的投影角度

如图示，计算直线𝐴𝑂c和B𝑂c的夹角𝐴𝑂cB，计算公式如下：
𝜃=𝑎𝑡𝑎𝑛(𝑥/𝑧)

3. 映射到激光数据

将角𝐴𝑂cB影射到相应的激光数据槽中。

已知激光的最小最大范围[𝛼,𝛽],激光束共细分为𝑁分，那么可用数组𝑙𝑎𝑠𝑒𝑟[𝑁]表示激光数据。那么点P投影到数组laser中的索引值𝑛

可如下计算：

𝑛=(𝜃−𝛼)/((𝛽−𝛼)/𝑁)=𝑁(𝜃−𝛼)/(𝛽−𝛼)

𝑙𝑎𝑠𝑒𝑟[𝑛]的值为P在Xc𝑂cZc面上投影的点B到相机光心𝑂c的距离𝑟，即

𝑙𝑎𝑠𝑒𝑟[𝑛]=𝑟=𝑂B=sqrt(𝑧^2 + 𝑥^2)

4. 对深度图像扫描高度

比较一定高度范围内，如果有距离最小的点，则替换原来距离𝑟。

代码解析

深度图转激光源码参考在ROS包depthimage_to_laserscan中的实现。

核心思路：先行扫描将数据存到ranges[]中，然后再高度扫面，比较下一行与原来数据ranges[]，小数据替换原来ranges[]中的大数据，这样就将高度给压缩了。

/**
    * Converts the depth image to a laserscan using the DepthTraits to assist.
    ...
    * @param depth_msg The UInt16 or Float32 encoded depth message.
    * @param cam_model The image_geometry camera model for this image.
    * @param scan_msg The output LaserScan.
    * @param scan_height The number of vertical pixels to feed into each angular_measurement.
    * 
    */
    template<typename T>
    void convert(const sensor_msgs::ImageConstPtr& depth_msg, const image_geometry::PinholeCameraModel& cam_model, 
         const sensor_msgs::LaserScanPtr& scan_msg, const int& scan_height) const{
      // Use correct principal point from calibration 使用校正的正确的主要点
      float center_x = cam_model.cx();//图像中心位置x
      float center_y = cam_model.cy();//图像中心位置y

      // Combine unit conversion (if necessary) with scaling by focal length for computing (X,Y)
      //在计算(X，Y)的时候，结合单位转换(如果必要的话)
      double unit_scaling = depthimage_to_laserscan::DepthTraits<T>::toMeters( T(1) );
      float constant_x = unit_scaling / cam_model.fx();
      float constant_y = unit_scaling / cam_model.fy();
      
      const T* depth_row = reinterpret_cast<const T*>(&depth_msg->data[0]);
      int row_step = depth_msg->step / sizeof(T);

      int offset = (int)(cam_model.cy()-scan_height/2);
      depth_row += offset*row_step; // Offset to center of image

      // 高度遍历
      for(int v = offset; v < offset+scan_height_; v++, depth_row += row_step){ 
        // 行遍历计算距离r,保存到一维雷达数组中ranges[]中
        for (int u = 0; u < (int)depth_msg->width; u++) // Loop over each pixel in row
        {    
          T depth = depth_row[u];
          
          double r = depth; // Assign to pass through NaNs and Infs
          double th = -atan2((double)(u - center_x) * constant_x, unit_scaling); // 计算夹角AOC，Atan2(x, z),实际上这里省略了深度值，因为分子分母都有，所以就略去了 but depth divides out
          int index = (th - scan_msg->angle_min) / scan_msg->angle_increment;//计算对应激光数据的索引
          
          if (depthimage_to_laserscan::DepthTraits<T>::valid(depth)){ // Not NaN or Inf
            // Calculate in XYZ,计算XYZ
            double x = (u - center_x) * depth * constant_x;
            double z = depthimage_to_laserscan::DepthTraits<T>::toMeters(depth);
            
            // Calculate actual distance，计算激光的真实距离
            r = sqrt(pow(x, 2.0) + pow(z, 2.0));
          }
      
      // Determine if this point should be used.判断激光距离是否超出预设的有效范围
      if(use_point(r, scan_msg->ranges[index], scan_msg->range_min, scan_msg->range_max)){
        scan_msg->ranges[index] = r;
      }
    }
      }
    }
    
    image_geometry::PinholeCameraModel cam_model_; ///< image_geometry helper class for managing sensor_msgs/CameraInfo messages.
    
    float scan_time_; ///< Stores the time between scans.
    float range_min_; ///< Stores the current minimum range to use.
    float range_max_; ///< Stores the current maximum range to use.
    int scan_height_; ///< Number of pixel rows to use when producing a laserscan from an area.
    std::string output_frame_id_; ///< Output frame_id for each laserscan.  This is likely NOT the camera's frame_id.
  };