LOAM代码笔记整理一——multiScanRegistration

LOAM主要由四个节点组成：multiScanRegistration、laserOdometry、laserMapping、transformMaintenance。
在这里插入图片描述

其中，核心算法封装在BasicScanRegistration、BasicLaserOdomotry、BasicLaserMapping和BasicTransformMaintenance中，这几个类的实现不涉及ROS的函数，为loam移植到其他平台提供了方便。

按照数据流的顺序一步步分析loam算法。首先是multiScanRegistration。

激光扫描模型

首先介绍一下VLP16的激光扫描模型：
论文中称单个线束为一个Scan, 对全部16线组成的一帧点云称为一个Sweep,

虽然是用的多线激光雷达,但是LOAM是针对单个Scan提取特征点的,这里主要考虑到线束间角分辨率(竖直分辨率)与单个线内点间角分辨率(水平分辨率)存在的差异.

角分辨率越大, 代表越远的物体, 反射的两点距离越大, 中间丢失的信息越多.因此, LOAM没有针对Scan和Scan之间的点的关联性提取和描述特征, 而是直接针对单个Scan提取特征.

要读懂代码中特征提取中的一些处理, 需要弄清楚VLP16扫描时的运动模型,简单的总结为:

一帧内所有的点, 都是按顺序穿行扫描的, 同一个时间点,只会有一次发送,紧接着一次接收。先从水平第一个角度,一般在0°左右,扫描这个水平角度上竖直方向所有16个点(对应16个SCAN)的深度,当然这16个点也是串行按顺序的,然后转到下一个水平角度, 比如0.3°开始, 水平分辨率0.4°,那么下个角度就是0.7°,然后1.1°.一直顺时针扫完一圈, 完成一个Sweep数据的采集.当然， Velodyne的User Manual里面讲的更清楚。

由于从驱动得到的一个Sweep是以点云的形式输出（也就是一堆点，每个点有XYZI的信息，点和点之间无其他关系信息）, 因此我们并不知道每个点属于哪个Scan, 对应哪个水平角度,因此, 我们需要根据上面的扫描模型去计算每个点的竖直角度和水平角度.

multiScanRegistration

根据VLP16的激光扫描模型, 对单帧点云（paper中称为一个Sweep）进行分线束（分为16束）, 每束称为一个Scan，并记录每个点所属线束和每个点在此帧电云内的相对扫描时间（相对于本帧第一个点）。

针对单个Scan提取特征点, 而相对时间会在laserOdometry中用于运动补偿.所有Scan的特征点,拼到两个点云中(因为是corner和surface两种特征点,所以是两个点云).至此,每帧点云,输出两种对应的特征点云, 给下一个节点laserOdometry。

相关的文件有src目录下的multi_scan_registration_node.cpp和src/lib下的BasicScanRegistration.cpp、ScanRegistration.cpp、multiScanRegistration.cpp文件。

multi_scan_registration_node.cpp：
是设置节点的主程序，其它三个节点的主程序也类似。

#include <ros/ros.h>
#include "loam_velodyne/MultiScanRegistration.h"


/** Main node entry point. */
int main(int argc, char **argv)
{
  ros::init(argc, argv, "scanRegistration");
  ros::NodeHandle node;
  ros::NodeHandle privateNode("~");

  loam::MultiScanRegistration multiScan;

  if (multiScan.setup(node, privateNode)) {
    // initialization successful
    ros::spin();
  }

  return 0;
}

点云注册的算法被封装在了MultiScanRegistration类中，他的继承关系为：
BasicLaserRegistration ———> ScanRegistraition ——>MultiScanRegistraion

BasicLaserRegistration通过计算点的曲率将特征点分为了两类——角点（cornerPoints）和平面点（surfacePoints）。得到的特征点存在cornerPointsSharp和surfacePointsFlat变量里，根据曲率阈值surfaceCurvatureThreshold把完整点云中大于阈值的点存入cornerPointsLessSharp中，小于阈值的存入surfacePointsLessFlat中，发送给laserOdomotry节点。

ScanRegistration在父类的基础上增加了读取节点参数、发布/订阅消息的函数。MultiScanRegistraion在父类的基础上增加了对点云数据的预处理，雷达点云输入接口在此。

_subLaserCloud = node.subscribe<sensor_msgs::PointCloud2>
      ("/multi_scan_points", 2, &MultiScanRegistration::handleCloudMessage, this);

LOAM代码笔记整理一——multiScanRegistration

猜你喜欢