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.
