用RGBD模拟激光雷达数据：depthimage_to_laserscan

Posted 2020-10-26 hiram-zhang

参考：
http://wiki.ros.org/depthimage_to_laserscan
https://github.com/ros-perception/depthimage_to_laserscan
http://blog.csdn.net/Jasmine_shine/article/details/46530143

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)
（2）在DepthImageToLaserScanROS类的reconfigureCb()函数中添加调用：
dtl_.set_y_thresh(config.ythresh_min, config.ythresh_max);
（3）在DepthImageToLaserScan类中添加成员函数和成员变量:   
void set_y_thresh(const float ythresh_min, const float ythresh_max);
float ythresh_min_;
float ythresh_max_;
同时对set_y_thresh()进行具体实现。
（4）在DepthImageToLaserScan类的convert()方法实现中添加 :
double y = (v - center_y) * depth * constant_y;
if(y<ythresh_min_||y>ythresh_max_)
{
      r = std::numeric_limits<float>::quiet_NaN();
      continue;
}

3.启动文件
[dtl.launch]:
<launch>
    <include file="$(find openni2_launch)/launch/openni2.launch"/>

    <node pkg="depthimage_to_laserscan"  type="depthimage_to_laserscan" name="depthimage_to_laserscan" args="standalone depthimage_to_lasersacn/DepthImageToLaserScanNodelet  /camera/rgb/image_viewer">

     <remap from="image" to="/camera/depth_registered/image_raw"/> 
     <remap from="camera_info" to="/camera/depth_registered/camera_info"/>
     <remap from="scan" to="/xtion_scan"/>
     
     <param name="scan_height" value="400"/>
     <param name="output_frame_id" value="xtion_scan_frame"/>
     <param name="range_min" type="double" value="0.45"/>
     <param name="range_max" type="double" value="4"/>
     <param name="ythresh_min" type="double" value="-0.60"/>
     <param name="ythresh_max" type="double" value="0.40"/>
     
    </node>

    <node pkg="tf" type="static_transform_publisher" name="xtion_to_laser" args="0 0 0 0 0 0 1 laser xtion_scan_frame 100" />    

</launch>