用RGBD投影激光雷达数据：depthimage_to

转载自知乎上：https://zhuanlan.zhihu.com/p/56559798
参考：
http://wiki.ros.org/depthimage_to_laserscan
https://github.com/ros-perception/depthimage_to_laserscan
http://blog.csdn.net/Jasmine_shine/article/details/46530143

原创博主的github 下载的话记得给颗五角星(kinetic版本) https://github.com/hiramzhang/gridmap_laser_rgbd_fusion

1.功能描述

将asus_xtion_pro获取到的深度图像depthimage进行外点、无效点剔除和图像有效区域筛选后得到待处理的深度图像{u,v};
通过深度图像{u,v}和相机模型参数cam_modle，可以求出深度图像每个像素点对应的空间点云{x,y,z};
将空间点云{x,y,z}投影到xz平面，同时用ythresh_min<y<ythresh_max进行条件约束。

2.代码修改

在https://github.com/ros-perception/depthimage_to_laserscan代码基础上，做如下修改：
（1）修改：在Depth.cfg中添加：
gen.add("ythresh_min", double_t, 0, "Minimum y thresh (in meters).", -0.30, -1.0, 1.0)
gen.add("ythresh_max", double_t, 0, "Maximum y thresh (in meters).", 0.30, -1.0, 1.0)
位置：art_ws/src/depthimage_to_laserscan/cfg/Depth.cfg
目的：保存参数，为（3）（4）做准备

(2)修改：在DepthImageToLaserScanROS类的reconfigureCb()函数中添加调用：
dtl_.set_y_thresh(config.ythresh_min, config.ythresh_max);
位置：art_ws/src/depthimage_to_laserscan/src/DepthImageToLaserScanROS.cpp

（3）修改：在DepthImageToLaserScan类中添加成员函数和成员变量:
void set_y_thresh(const float ythresh_min, const float ythresh_max);
float ythresh_min_;
float ythresh_max_;
同时对set_y_thresh()进行具体实现。

实现添加内容：
void DepthImageToLaserScan::set_y_thresh(const float ythresh_min, const float ythresh_max){
ythresh_min_ = ythresh_min;
ythresh_max_ = ythresh_max;
}
位置：art_ws/src/depthimage_to_laserscan/src/DepthImageToLaserScan.cpp
目的：为（4）调用做准备
（4）修改：在DepthImageToLaserScan类的convert()方法实现中添加 :
void set_y_thresh(const float ythresh_min, const float ythresh_max);

double y = (v - center_y) * depth * constant_y;
if(y<ythresh_min_||y>ythresh_max_)
{
r = std::numeric_limits<float>::quiet_NaN();
continue;
}

float ythresh_min_;
float ythresh_max_;
目的：设置y的筛选条件
位置：/home/w/art_ws/src/depthimage_to_laserscan/include/depthimage_to_laserscan/DepthImageToLaserScan.h

由于摄像头有区别，去掉dtl.launch 中的<include file="$(find astra_launch)/launch/astra.launch"/>
catkin_make 编译一遍
3.启动文件

roslaunch astra_launch astra.launch
//rosrun depthimage_to_laserscan depthimage_to_laserscan image:=/camera/depth/image_raw 这条命令不行
roslaunch depthimage_to_laserscan dtl.launch
rosrun tf static_transform_publisher 0 0 0 0 0 0 1 map camera_depth_frame 10
rosrun tf static_transform_publisher 0 0 0 0 0 0 1 map xtion_scan_frame 10
rosrun rviz rviz

rosparam get /depthimage_to_laserscan/ythresh_max

注：dtl.launch 中改参数，改后编译

4 主要内容

主要改的内容：(4)部

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);
          int index = (th - scan_msg->angle_min) / scan_msg->angle_increment;
          if (depthimage_to_laserscan::DepthTraits<T>::valid(depth)){ /
            double x = (u - center_x) * depth * constant_x;
            double z = depthimage_to_laserscan::DepthTraits<T>::toMeters(depth);
            double y = (v - center_y) * depth * constant_y;//定义y数值
                  if(y<-0.3||y>0.3)//如果y不符合条件就把他忽略
                 {
                     r = std::numeric_limits<float>::quiet_NaN();//精髓在这一步，没看懂
                     continue;
                 }
            // 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;
      }
   }
      }
    }

5 各个文件之间的调用关系