实验室有一个镭神C16的激光雷达，最近在我这，想拿来玩一玩。本意是做一个实时的检测，通过ROS获取激光雷达的激光点云，用pointpillars模型来进行实时的三维目标检测任务。但是镭神c16这一个激光雷达，不太好处理，目前只能用自带的驱动，进行一个实时的显示。所以下边的思路就是，先存储镭神c16的点云数据，再用ros进行三维目标检测。

文章目录

  • 1. 思路：
  • 2. 实验环境：
  • 3. 步骤：
    • 1. 前提条件
    • 2. 编译环境
      • 创建一个工作空间并进入
      • 将ROS包复制或者克隆到当前文件夹下
      • 编译
      • 迁移OpenPCDet中的一些文件
    • 3. 修改代码
      • **ros.py代码**
        • 1. 简单修改
        • 2. 修改旋转参数
        • 3.添加类似与NMS的功能，去除不靠谱的检测结果
        • 4. 新增发布跟检测结果匹配的每一帧点云
      • **launch/pointpillars.launch代码**
      • **launch/pointpillars.rviz代码**
    • 4. 运行
  • 4. 图片效果：
    • 1.未设定阈值的检测效果：
    • 2.设定阈值之后的效果
    • 3. 最终效果
  • 5. 主要参考资料:

1. 思路：

  通过ros来实现点云消息的订阅和检测框bbox消息的发布，来实现一个检测效果的可视化功能。

2. 实验环境：

  • Ubuntu16.04
  • cuda 10.1
  • ros-kinetic
    检测环境主要依托OpenPCDet，环境搭建可参考我的这篇博客

3. 步骤：

1. 前提条件

已经搭建过了OpenPCDet。
已经搭建了ROS环境，我用的是kinetic版本，用其他版本应该也可以。

2. 编译环境

创建一个工作空间并进入

mkdir -p ~/pointpillars_ros/srccd pointpillars_ros/src

将ROS包复制或者克隆到当前文件夹下

git clone https://github.com/BIT-DYN/pointpillars_roscd ..

编译

# 进入到搭建好的openpcdet环境conda activate openpcdetpip install --user rospkg catkin_pkgpip install pyquaternion# 因为我用的是kinetic，所以需要安装下边的库，如果是其他版本的ros，下边修改kinetic安装对应版本即可sudo apt-get install ros-kinetic-pcl-rossudo apt-get install ros-kinetic-jsk-recognition-msgsudo apt-get install ros-kinetic-jsk-rviz-plugins## 注意，执行catkin_make时，在最外层的文件夹下，也就是我例子中的~/pointpillars_roscatkin_make

迁移OpenPCDet中的一些文件

为保险起见，我将下边这些文件全部放入到src/pointpillars/tools文件夹下了。

可以先修改一下demo.py文件的配置文件和预训练模型的位置，测试是否能成功调用pcdet，如果可以的话，说明环境迁移过来后没问题，再尝试修改ros.py的代码。

3. 修改代码

ros.py代码

1. 简单修改

# 下边的路径改成自己的sys.path.append("/home/ubuntu/pointpillars_ros/src/pointpillars_ros") # Line 22# 54行后边""" Initialize ros parameters """config_path = rospy.get_param("/config_path", "/home/ubuntu/pointpillars_ros/src/pointpillars_ros/tools/cfgs/kitti_models/pointpillar.yaml")ckpt_path = rospy.get_param("/ckpt_path", "/home/ubuntu/pointpillars_ros/src/pointpillars_ros/tools/models/pointpillar.pth")# 订阅的激光点云名字，改成自己的，我用的是kitti的.bag文件self.sub_velo = rospy.Subscriber("/kitti/velo/pointcloud", PointCloud2, self.lidar_callback, queue_size=1,buff_size=2**12)

2. 修改旋转参数

修改旋转参数，大概在ros.py的86行前后。我问了开源作者，他们安装的雷达有偏角，所以这里置0就行。

# 旋转轴#rand_axis = [0,1,0]#旋转角度#yaw = 0.1047#yaw = 0.0#返回旋转矩阵#rot_matrix = self.rotate_mat(rand_axis, yaw)#np_p_rot = np.dot(rot_matrix, np_p[:,:3].T).T# convert to xyzi point cloudx = np_p[:, 0].reshape(-1)y = np_p[:, 1].reshape(-1)z = np_p[:, 2].reshape(-1)if np_p.shape[1] == 4: # if intensity field existsi = np_p[:, 3].reshape(-1)else:i = np.zeros((np_p.shape[0], 1)).reshape(-1)points = np.stack((x, y, z, i)).T

3.添加类似与NMS的功能，去除不靠谱的检测结果

另外ros.py代码中是没有去除掉scores低的检测框，我打印看了一下，如下图所示，置信度为0.107的也没有去除掉：
修改ros.py代码的大概110行前后（去除掉不合适的检测框，我设置的阈值为0.5），如下所示：

# 组装数组字典input_dict = {'points': points,'frame_id': msg.header.frame_id,}data_dict = self.demo_dataset.prepare_data(data_dict=input_dict) # 数据预处理data_dict = self.demo_dataset.collate_batch([data_dict])load_data_to_gpu(data_dict) # 将数据放到GPU上pred_dicts, _ = self.model.forward(data_dict) # 模型前向传播scores = pred_dicts[0]['pred_scores'].detach().cpu().numpy()mask = scores > 0.5scores = scores[mask]boxes_lidar = pred_dicts[0]['pred_boxes'][mask].detach().cpu().numpy()label = pred_dicts[0]['pred_labels'][mask].detach().cpu().numpy()num_detections = boxes_lidar.shape[0]#rospy.loginfo("The num is: %d ", num_detections)# print(boxes_lidar)# print(scores)# print(label)

4. 新增发布跟检测结果匹配的每一帧点云

分析：
 以上的代码，订阅了激光点云信息（并在rviz界面展示），发布检测框（并在rviz显示），检测是需要时间的，所以检测框与当前显示帧的点云并不匹配。
思路：
 得到检测框结果之后，重新发布当前帧点云信息，名字修改一下。然后在rviz可视化界面选择重新发布的点云就可以了。
在ros.py中添加新发布的点云，在rviz文件中新增新的点云话题。
修改后的代码可查看:

国内gitee地址：https://gitee.com/ximing689/pointpillars_ros

launch/pointpillars.launch代码

<launch># 主要修改下边第一行<node pkg="rosbag" type="play" name="player" output="log" args="-l /media/ubuntu/ximing/dataset/ros_kitti/bag/2011_10_03/kitti_2011_10_03_drive_0027_synced.bag" /><node name="pointpillars_ros" type="ros.py" pkg="pointpillars_ros" output="screen"/><node type="rviz" name="rviz" pkg="rviz" args="-d $(find pointpillars_ros)/launch/pointpillars.rviz" /></launch>

launch/pointpillars.rviz代码

主要修改你所用的话题的名字，我改了点云和图像的名字

4. 运行

执行下边的命令

conda activate openpcdetsource ~/pointpillars_ros/devel/setup.bashroslaunch pointpillars_ros pointpillars.launch

4. 图片效果：

我使用kitti原数据的某一段时间，转成.bag格式，用pointpillars的模型进行测试的，效果也不行，猜测原因可能是1.旋转矩阵参数没有修改;2.激光点云显示的跟检测框有延时，不能很好匹配;3.第三点可能就是模型不行(可能性不大)。

1.未设定阈值的检测效果：

2.设定阈值之后的效果

设定阈值之后，明显减少了很多检测框，不过依然存在检测延时（检测框不准）。

3. 最终效果

初次部署这个代码的时候，检测框跟实际点云完全不匹配，目标框是杂乱的。经过分析，将点云和检测框进行匹配对齐之后，显示的检测效果还是不错的（目前还没做图像的对齐，所以图像和点云显示不一致）。
哔哩哔哩

下图是用雷神c16雷达在实验室采集的30秒的数据，不到1分钟，696MB。

目前用rosbag record存储了一些雷神C16激光雷达的数据，还不太会使用雷达实时读取，实时检测。后期再想办法吧。

5. 主要参考资料:

  非常感谢这些网友分享的开源资料，让一个小白的我，也能快速demo出一个效果。

  • ROS点云的Pointpillars实时目标检测
  • Github资料

遇到的问题，解决办法：

  • error while loading shared libraries: libopencv_core3.so.3.3
  • ROS运行python报错：/usr/bin/env: ‘python\r’: No such file or directory

视频效果：https://www.bilibili.com/video/BV1ce4y1D76o/
代码地址：https://gitee.com/ximing689/pointpillars_ros