depthimage_to_laserscan代码解读

上代码：

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();//
        float center_y = cam_model.cy();

        // Combine unit conversion (if necessary) with scaling by focal length for computing (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){
            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); // 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
                    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;
                }
            }
        }
    }

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

　　结论：

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