基于loam_velodyne的64线激光数据可视化

0.背景

因为课题组任务以及自己课题的需要，需要对velodyne64线激光数据进行可视化处理，使用LOAM算法，参考了网上已有的教程和方法，复现还是有些吃力，并且发现这些教程讲述的并不是很清晰明，因此整理一篇较为详细的博文供大家参考，特别是像我一样刚入这个领域的小白。

最近发现很多网友在复现的过程中出现了些问题，最近有空也复现了一下，发现当时写的依旧不是很详细，特别补充了些内容和描述，希望可以帮助到需要可视化激光雷达的你。

使用环境：Ubuntu16.04+ROS-kinetic

1.安装velodyne驱动

$ sudo apt-get install ros-kinetic-velodyne
$ cd ~/catkin_ws/src/
$ git clone https://github.com/ros-drivers/velodyne.git
$ cd ~/catkin_ws
$ catkin_make

2.安装loam_velodyne

$ cd ~/catkin_ws/src/
$ git clone https://github.com/laboshinl/loam_velodyne.git
$ cd ~/catkin_ws
$ catkin_make -DCMAKE_BUILD_TYPE=Release

这一步可以先不用编译，等修改完文件后再编译也可以。

3.修改工程

修改位于 /loam_velodyne/src/内 的scanRegistration.cpp文件(此文件我在2018/05/30二次复现是源代码里没有了，百度网盘下载，这是我修改好的)

需要添加两部分内容

3.1添加头文件

#include <velodyne_pointcloud/point_types.h> 
#include <velodyne_pointcloud/rawdata.h>

添加的头文件是激光雷达的驱动中的文件，位于  /velodyne/velodyne_pointcloud/include/velodyne_pointcloud  中

3.2添加以下代码

在

namespace loam {

后添加如下代码即可（主要缩进，一定要按规范）

float cloudCurvature[2500000];//由于64线雷达秒生成的点云数据量为2,200,000个点，因此设置为2500000
int cloudSortInd[2500000];
int cloudNeighborPicked[2500000];
int cloudLabel[2500000];
std::vector<int> scanStartInd(N_SCANS, 64);//这里有修改scanStartInd(N_SCANS, **) 如果为64线，则为64，32线则为32，依次类推。
  std::vector<int> scanEndInd(N_SCANS, 64);
  double timeScanCur = laserCloudMsg->header.stamp.toSec();
  pcl::PointCloud<pcl::PointXYZI> laserCloudIn;
  pcl::PointCloud<velodyne_pointcloud::PointXYZIR> laserCloudIn_vel;

  pcl::fromROSMsg(*laserCloudMsg, laserCloudIn);
  pcl::fromROSMsg(*laserCloudMsg, laserCloudIn_vel);

  std::vector<int> indices;
  pcl::removeNaNFromPointCloud(laserCloudIn, laserCloudIn, indices);

  int cloudSize = laserCloudIn.points.size();
  float startOri = -atan2(laserCloudIn.points[0].y, laserCloudIn.points[0].x);
  float endOri = -atan2(laserCloudIn.points[cloudSize - 1].y,
                        laserCloudIn.points[cloudSize - 1].x) + 2 * M_PI;

  if (endOri - startOri > 3 * M_PI) {
    endOri -= 2 * M_PI;
  } else if (endOri - startOri < M_PI) {
    endOri += 2 * M_PI;
  }
  bool halfPassed = false;
  int count = cloudSize;
  PointType point;
  std::vector<pcl::PointCloud<PointType> > laserCloudScans(N_SCANS);
  for (int i = 0; i < cloudSize; i++) {
    point.x = laserCloudIn.points[i].y;
    point.y = laserCloudIn.points[i].z;
    point.z = laserCloudIn.points[i].x;
  }
    int scanID;
   /* float angle = atan2(point.y,sqrt(point.x * point.x + point.z * point.z)) * 180 / M_PI;
    int roundedAngle = int(angle + (angle<0.0?-0.5:+0.5)); 
    std::cout << angle << std::endl;
    if (roundedAngle > 0){
      scanID = roundedAngle;
    }
    else {
      scanID = roundedAngle + (N_SCANS - 1);
    }*/
    scanID = laserCloudIn_vel.points[indices[i]].ring;
    if (scanID > (N_SCANS - 1) || scanID < 0 ){
      count--;
      continue;
    }

3.3修改CMakeLists.txt以及package.xml

两个修改文件均位于/loam_velodyne 目录下

3.3.1修改CMakeLists.txt

添加 velodyne_pointcloud 即可（5行-11行都行），如下；

find_package(
...
velodyne_pointcloud
)

3.3.2修改package.xml

添加<build_depend>velodyne_pointcloud</build_depend>

添加<run_depend>velodyne_pointcloud</run_depend>

如下（19行以后，按类别添加进去就好了）；

<build_depend>velodyne_pointcloud</build_depend> 
...
...
<run_depend>velodyne_pointcloud</run_depend> 

3.4 修改launch文件

修改位于loam_velodyne/launch内的loam_velodyne.launch文件

修改第7行的代码为：（最好修改了，没有修改也能运行）

<param name="lidar" value="HDL-64E" /> <!-- options: VLP-16  HDL-32  HDL-64E -->

4.重新编译

$ cd ~/catkin_ws
$ catkin_make -DCMAKE_BUILD_TYPE=Release
$ source ~/catkin_ws/devel/setup.bash

一定要source一下，不然那运行不了，之前教程里漏了，万分抱歉

5.使用（先运行launch文件，后播放bag文件）

terminal 1:

$ roslaunch loam_velodyne loam_velodyne.launch

terminal 2: (需要在绝对地址中进行播放bag文件)

rosbag play #文件名

说明：这里我是在无人车上录制的bag文件进行可视化操作的

6.运行效果图

7.参考资料

【1】运行loam_velodyne时的步骤和存在的问题

【2】修改loam_velodyne使其能兼容HDL-32E

【3】loam_velodyne

【4】issue#10

8.其他

如果遇到无法找到相应文件时请参阅参考资料1