depthimage_to_laserscan代码解读

上代码：

 1 template<typename T>
 2     void convert(const sensor_msgs::ImageConstPtr& depth_msg, const image_geometry::PinholeCameraModel& cam_model,
 3                  const sensor_msgs::LaserScanPtr& scan_msg, const int& scan_height) const{
 4         // Use correct principal point from calibration
 5         float center_x = cam_model.cx();//
 6         float center_y = cam_model.cy();
 7 
 8         // Combine unit conversion (if necessary) with scaling by focal length for computing (X,Y)
 9         double unit_scaling = depthimage_to_laserscan::DepthTraits<T>::toMeters( T(1) );
10         float constant_x = unit_scaling / cam_model.fx();//
11         float constant_y = unit_scaling / cam_model.fy();
12 
13         const T* depth_row = reinterpret_cast<const T*>(&depth_msg->data[0]);
14         int row_step = depth_msg->step / sizeof(T);
15 
16         int offset = (int)(cam_model.cy()-scan_height/2);
17         depth_row += offset*row_step; // Offset to center of image
18 
19         for(int v = offset; v < offset+scan_height_; v++, depth_row += row_step){
20             for (int u = 0; u < (int)depth_msg->width; u++) // Loop over each pixel in row
21             {
22                 T depth = depth_row[u];
23 
24                 double r = depth; // Assign to pass through NaNs and Infs
25                 double th = -atan2((double)(u - center_x) * constant_x, unit_scaling); // Atan2(x, z), but depth divides out
26                 int index = (th - scan_msg->angle_min) / scan_msg->angle_increment;
27 
28                 if (depthimage_to_laserscan::DepthTraits<T>::valid(depth)){ // Not NaN or Inf
29                     // Calculate in XYZ
30                     double x = (u - center_x) * depth * constant_x;
31                     double z = depthimage_to_laserscan::DepthTraits<T>::toMeters(depth);
32 
33                     // Calculate actual distance
34                     r = sqrt(pow(x, 2.0) + pow(z, 2.0));
35                 }
36 
37                 // Determine if this point should be used.
38                 if(use_point(r, scan_msg->ranges[index], scan_msg->range_min, scan_msg->range_max)){
39                     scan_msg->ranges[index] = r;
40                 }
41             }
42         }
43     }

　　可见，convert函数选取深度图的中心数行，然后计算每一个像素基于相机中心的距离，并选取同一列的距离最小值（由函数use_point实现）填充给laserscan的data[index]。

　　结论：

depthimage_to_laserscan不能有效的对复杂的3D环境有很好的投影成2D laserscan的效果
如果将列的范围扩大到最大，对2D SLAM是否有更好的避障效果？实时性能否保证？
对于深度相机，可以考虑pointcloud_to_laserscan.