深度相机的障碍物检测

深度相机的障碍物检测

这里简单备份一下，有关深度相机障碍物检测的学习内容

1. 准备相机的驱动

Ubuntu18.04+ROS+ros_astra_camera-master

采用的是astra深度相机，安装Linux驱动：ros_astra_camera-master

2. 图像转LaserScan

2.1 depthimage_to_laserscan

​ 下面这个包可以将深度图像数据转换为二维激光雷达常用的LaserScan消息格式。

git clone -b melodic-devel https://github.com/ros-perception/depthimage_to_laserscan.git

​ 修改一下launch文件中深度图像的话题，以及output_frame

<remap from="image"       to="/camera/depth/image_raw"/> <!-- change here for your camera depth topic name. Default: "/camera/depth/image_raw" -->
<param name="output_frame_id" type="str"    value="camera_link"/>  <!--default: camera_depth_frame. Frame id of the laser scan. -->

​ 运行以下的指令可以进行转换，最后的订阅是深度相机的消息：/camera/depth/image_raw。转换的原理可以参考这个博客：https://www.cnblogs.com/cv-pr/p/5725831.html

roslaunch depthimage_to_laserscan launchfile_sample.launch

2.2 仿真

采用中科院软件所的gazebo仿真环境，仅依靠2D激光雷达(红色)的costmap：

添加深度图像转换的LaserScan(白色)，并添加到costmap中的障碍物层，需要修改costmap_common_params.yaml：

  observation_sources:  depth_scan #user
  depth_scan:
    data_type: LaserScan
    topic:  depth_scan #修改了depthimage_to_laserscan包发布的topic名称
    marking:  true
    clearing: true
    inf_is_valid:  true
    observation_keep_time:  0.0
    expected_update_rate: 0

3. costmap_depth_camera

参考：https://blog.csdn.net/robinvista/article/details/115732239

3.1 具体操作

​ 通过图像转LaserScan消息有个缺点，它只是截取了水平面的一段数据，其他高度上的数据都被忽略了。对于直上直下、上下一样粗细的简单障碍物是没有太大的问题，但是如果我们的障碍物分布是三维的，那样显然就丢失了大部分的数据。所以更安全的方法是将所有的三维点云全部投影到二维平面上，这样就能考虑三维分布。

​ 实现该功能的ROS包是costmap_depth_camera:https://github.com/tsengapola/costmap_depth_camera

​ 原作者直接对原始点云进行处理，而深度相机返回的点云数据量是及其庞大的，参考上面链接中作者的方法，采用PCL中的下采样函数对原始点云进行压缩

pcl::VoxelGrid<pcl::PointXYZI> downsample;          //创建滤波对象
downsample.setInputCloud(combined_observations);    //设置需要过滤的点云给滤波对象
downsample.setLeafSize(0.1f, 0.1f, 0.1f);        //设置滤波时创建的体素体积为1cm的立方体
downsample.filter(*combined_observations);          //执行滤波处理，存储输出

​ 可以通过设置max_obstacle_height：1.5把高出的障碍物部分排除掉，其他的障碍物正常投影到水平面。

3.2 实验

costmap_depth_camera需要订阅imu，而手头相机中没有imu，仅为了测试一下，就随意播放了一个imu数据包

rosbag play -l camera_imu_2022-01-25-15-35-11.bag --topic /imu

修改costmap_depth_camera.yaml中的sensor_frame以及订阅points的topic：

depth_cam: {sensor_frame: camera_link, topic: /camera/depth/points}

修改costmap_depth_camera.launch中的tf2_ros

<node pkg="tf2_ros" type="static_transform_publisher" name="baselink2camera" args="0.0 0.0 1 0 0 0 base_link camera_link" output="screen" /> 

启动launch

roslaunch costmap_depth_camera costmap_depth_camera.launch 

直接使用了原作者的map，深度相机前的障碍物信息能够实时检测到，以后再测试一下精度。

3.3 costmap_depth_camera代码剖析(https://blog.csdn.net/robinvista/article/details/115732239)

​ costmap_depth_camera包是costmap_2d的插件，从下向上看，底层的observation.h开始，observation顾名思义就是传感器的观测数据，具体来说就是深度相机输出的点云数据。简单理解，一个observation就对应一帧点云。该文件定义了一个Observation类，其成员变量主要是以下三个。既然要处理点云，首先要存储起来，存储在cloud和frustum中。一般来说，点云还有一个信息要存储，那就是传感器自己的原点位置坐标origin_，也就是点云坐标相对于哪个坐标系表示的。因为机器人移动时传感器的位置也会改变，所以要存下来。

geometry_msgs::Point origin_;
pcl::PointCloud<pcl::PointXYZI>* cloud_;
pcl::PointCloud<pcl::PointXYZ>* frustum_;

​ 此外，还有与深度相机有关的参数，如下：

double min_detect_distance_, max_detect_distance_, FOV_W_, FOV_V_;

​ Observation类主要的成员函数如下。它们都是用来对深度相机的视锥做计算的。这几个函数为什么放在Observation类中？因为每个Observation对应一帧点云，这样可以支持多个深度相机，它们在机器人上的安装位置不同，看到的视野不同，这就对应不同的视锥。

void findFrustumVertex(); //顶点
void findFrustumNormal();
void findFrustumPlane();

​ 然后是observation_buffer文件，它定义了ObservationBuffer类，这个类是基于Observation类的。它的成员变量中最重要的是个List链表observation_list_。这里为什么使用list而不用vector？因为vector适用于对象数量变化少，简单对象，随机访问频繁；list适用于对象数量变化大，对象复杂，插入和删除频繁。Observation类用来存储点云，显然数据量非常大，还要经常更新，所以list更合适。

std::list<Observation> observation_list_;

​ ObservationBuffer顾名思义，就是传感器点云数据的缓存，每接收到一帧点云数据就存到observation_list_中。至于存储多少点云取决于时间，如果点云帧的时间超过一定值就会被剔除掉，否则会一直存储造成内存溢出。这个时间范围是observation_keep_time变量决定的，如果它的值是0，则新的点云来到之后旧的点云就会被扔掉。

​ 既然ObservationBuffer类的目的是缓存，那最重要的函数就是bufferCloud(const sensor_msgs::PointCloud2& cloud)了。它的输入是一帧点云数据cloud，通过for循环把cloud里的点云存储到observation_list_里。其他几个函数都比较简单。

​ 下面是最顶层的depth_camera_obstacle_layer.cpp，它定义了DepthCameraObstacleLayer类，这个类继承自costmap_2d::CostmapLayer。既然它继承自ROS的costmap_2d包，那我们不得不研究一下costmap_2d包了。costmap_2d包是navigation导航功能包集里面的一个功能包。实际上，我们完全可以把costmap_depth_camera包下载到navigation里，然后一起编译调试。

​ depth_camera_obstacle_layer.cpp位于plugins文件夹下，它的风格与costmap_2d/plugins文件夹中的文件类似。打开obstacle_layer.cpp就能看到相同的成员，例如：

std::vector<boost::shared_ptr<costmap_depth_camera::ObservationBuffer>> observation_buffers_;  
//Used to store observations from various sensors
std::vector<boost::shared_ptr<costmap_depth_camera::ObservationBuffer>> marking_buffers_;
//Used to store observation buffers used for marking obstacles
std::vector<boost::shared_ptr<costmap_depth_camera::ObservationBuffer>> clearing_buffers_;
//Used to store observation buffers used for clearing obstacles

​ onInitialize()函数用来配置参数，给后面几个函数用。传感器的数据也在这里被回调，但是不是普通的回调函数，而是使用了带滤波功能的MessageFilter。接收到点云数据后触发回调函数pointCloud2Callback，它再调用ObservationBuffer里面的bufferCloud函数把点云数据存储起来。所以传感器数据的入口就是，pointCloud2Callback函数。它订阅的话题是：/camera/depth/color/points，这个话题名定义在costmap_depth_camera.yaml中。

​ ClearMarkingbyKdtree函数的功能是清除栅格的标记值。具体是哪个栅格？就是pc_3d_map_这个有点复杂的变量。它表示一个三维的栅格，因为深度相机扫描的区域——视锥就是三维的。变量pc_3d_map 维护的是机器人附近一定范围内的三维地图。pc-3d-map如何存储三维地图？

std::map<unsigned int, std::map<int, float>> pc_3d_map_; //unsigned int存储水平面的xy坐标，第二个std::map存储垂直方向的z轴坐标，最后一个float存储每个体素的强度信息。

​ 在ClearMarkingbyKdtree函数中，通过嵌套的两个for循环遍历pc-3d-map，三维体素地图的每个体素。pc-3d-map不是把长方体所有离散体素都存起来了，而是只保存那些被障碍物占据的体素。在遍历时，只是遍历被占据的栅格，后面的ProcessCluster函数的作用是把聚类后的点存到pc-3d-map里。然后ClearMarkingbyKdtree函数只检验上一帧被占用的那些体素（用的是上一帧点云生成的、在ProcessCluster中添加的），如果发现最新的点云数据已经不占用了，那就把这个体素从pc_3d_map中删除。

​ 其中使用了frustum_utils.isInsideFRUSTUMs函数判断这个体素中心点是不是在深度相机的视锥里面，只处理在里面的点。如果满足三个条件就清除对应体素的标记。标记的意思是，如果一个体素被标记那么所在的空间就被障碍物占据，没有标记就没有障碍物。这三个条件是：如果必须强制清空这一整帧clear-all-marking-in-this-frame = true，点的距离小于一定的清除距离number-clearing-threshold <= forced-clearing-distance ，这个点的check_radius-半径范围内的邻居点个数小于number_clearing_threshold-。

​ ClearMarkingbyKdtree与ProcessCluster这两个函数是这个包的核心。前面提到作者写这个包的初衷，就是想解决障碍物标记如何清除的问题：

Considering ray casting method can not satisfy sparse 3D space problem of clearing. 
This plugin is based on kd-tree search to clear the markings, it comprises two parts:
    - Marking pointcloud and store it using std::map.
    - Clearing marked pointcloud using kd-tree search.

​ updateBounds函数是整个包里计算量最大一个，因为点云最近邻搜索、下采样和聚类分割操作都在这里完成。下采样前面说了，用的是PCL里的pcl::VoxelGrid，近邻搜索用的是PCL里的pcl::search::KdTree，聚类用的是PCL里原始的pcl::EuclideanClusterExtraction函数。

​ http://wiki.ros.org/costmap_2d

​ 最后的updateCosts函数就是在制作二维的代价地图，存储在master_grid中。你可能会问，前面的地图数据pc_3d_map_是三维的，代价地图却是二维的，那三维到底是怎么投影到二维的呢？这个具体的操作就是：既然pc_3d_map_第一项存储的是占据体素的xy坐标索引，如果某个xy坐标对应的索引在pc_3d_map_里面，那么不管垂直方向的占据体素有几个，都认为整个xy坐标索引被占用了，相应的master_array被赋值，这样就实现了投影，很简单吧。

​ 还有一个不起眼的frustum_utils.cpp，顾名思义，这个函数是提供与视锥有关的功能函数的，例如判断一个点是不是在一个视锥里的函数isInsideFRUSTUMs。在Observation类中也有与视锥有关的函数，那为什么要分开放呢？因为Observation类中的函数依然是用来计算视锥的一些属性值的，例如视锥的8个顶点，6个平面和法向量。这些值只在实例化时被执行一次，后面都不再执行。而frustum_utils.cpp里的函数是利用视锥的属性值进行高级的计算，它是要重复运行的，所以单独存放。

​ 总结一下，这个包处理的数据都是点云数据，具体使用的消息是sensor_msgs::PointCloud2类型，大量使用PCL库，所以理论上也能用在普通的多线激光雷达上面。