激光雷达 LOAM 程序解析1：点云注册

最新推荐文章于 2023-05-11 19:36:23 发布

他人是一面镜子，保持谦虚的态度

最新推荐文章于 2023-05-11 19:36:23 发布

阅读量1.8k

点赞数 4

分类专栏：激光SLAM

本文链接：https://blog.csdn.net/hltt3838/article/details/109328707

版权

激光SLAM 专栏收录该内容

23 篇文章 54 订阅

订阅专栏

5、根据水平角度 α 可以得到获取每个点时相对与开始点的时间

一、系统框架分析

整个LOAM的流程：点云注册，激光里程计（高频低精度），激光建图（低频高精度），坐标发布；系统的结构图如下所示：

程序中对应的结构如下图所示，这样我们便可以很好的分析整个系统：

想说的话：本人已经看过LOAM的论文，论文本身并不难理解，但当看程序的时候却是开始有点混乱，如果我们按照上面的结构图，一部分一部分看，思路会清晰不少。总而言之，应该去分模块学习，然后整体关联。所以本人程序分析的第一部分是：点云注册

二、程序分析

2.1 程序入口

点云注册的入口程序在 multi_scan_registration_node.cpp 中，如下：

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

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)) //点云数据在这 被 订阅的
  {
    ros::spin();
  }
  return 0;
}

再看一下 multiScan.setup(node, privateNode) 这个程序，包括了节点的选择和参数的读取，如下：

bool MultiScanRegistration::setup(ros::NodeHandle& node, ros::NodeHandle& privateNode)
{
  RegistrationParams config;
  if (!setupROS(node, privateNode, config))
    return false;
  configure(config);
  return true;
}

看一下 setupROS(node, privateNode, config) 这个程序，

/*
获取激光雷达配置参数，订阅点云 topic 
*/
bool MultiScanRegistration::setupROS(ros::NodeHandle& node, ros::NodeHandle& privateNode, RegistrationParams& config_out)
{
  if (!ScanRegistration::setupROS(node, privateNode, config_out))
    return false;

  //获取扫描映射参数
  std::string lidarName;

  if (privateNode.getParam("lidar", lidarName)) {
    if (lidarName == "VLP-16") {
      _scanMapper = MultiScanMapper::Velodyne_VLP_16();
    } else if (lidarName == "HDL-32") {
      _scanMapper = MultiScanMapper::Velodyne_HDL_32();
    } else if (lidarName == "HDL-64E") {
      _scanMapper = MultiScanMapper::Velodyne_HDL_64E();
    } else {
      ROS_ERROR("Invalid lidar parameter: %s (only \"VLP-16\", \"HDL-32\" and \"HDL-64E\" are supported)", lidarName.c_str());
      return false;
    }

    ROS_INFO("Set  %s  scan mapper.", lidarName.c_str());
    if (!privateNode.hasParam("scanPeriod")) {
      config_out.scanPeriod = 0.1;
      ROS_INFO("Set scanPeriod: %f", config_out.scanPeriod);
    }
  } else {
    float vAngleMin, vAngleMax;
    int nScanRings;

    if (privateNode.getParam("minVerticalAngle", vAngleMin) &&
        privateNode.getParam("maxVerticalAngle", vAngleMax) &&
        privateNode.getParam("nScanRings", nScanRings)) {
      if (vAngleMin >= vAngleMax) {
        ROS_ERROR("Invalid vertical range (min >= max)");
        return false;
      } else if (nScanRings < 2) {
        ROS_ERROR("Invalid number of scan rings (n < 2)");
        return false;
      }

      _scanMapper.set(vAngleMin, vAngleMax, nScanRings);
      ROS_INFO("Set linear scan mapper from %g to %g degrees with %d scan rings.", vAngleMin, vAngleMax, nScanRings);
    }
  }

  // 订阅输入云主题
  _subLaserCloud = node.subscribe<sensor_msgs::PointCloud2>
      ("/multi_scan_points", 2, &MultiScanRegistration::handleCloudMessage, this);

  return true;
}

最主要的是订阅点云的主题，当然前提是有这个主题被发布，一般都是 rosplay *bag
_subLaserCloud = node.subscribe<sensor_msgs::PointCloud2> ("/multi_scan_points", 2, &MultiScanRegistration::handleCloudMessage, this);

MultiScanRegistration::handleCloudMessage 相当于一个回调函数，订阅点云数据后，进行相关的处理，也是整个 “点云注册”注册模块的开始！

下面看一下这个程序：

//处理新的点云数据
void MultiScanRegistration::handleCloudMessage(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg)
{
  if (_systemDelay > 0) 
  {
    --_systemDelay;
    return;
  }
  //获取新的输入云
  pcl::PointCloud<pcl::PointXYZ> laserCloudIn;
  pcl::fromROSMsg(*laserCloudMsg, laserCloudIn);
  process(laserCloudIn, fromROSTime(laserCloudMsg->header.stamp));
}

参数 _systemDelay 意思是系统延迟，这个是可以设置的参数，为什么要有这个系统的延迟，换句话说系统上电开始并不是就采集雷达的数据，而是延迟了一会，我的理解是：上电开始激光雷达数据效果不理想，“预热一下传感器”哈哈，也可以纠正我的说法，谢谢！这个方法并不是程序的要求，更像是实际应用的一个技巧！

然后程序开始执行 process(laserCloudIn, fromROSTime(laserCloudMsg->header.stamp)); 输入的是最新点云数据和时间，下面我们就要对程序具体的分析！

2.2 具体分析

这部分的主要内容是：具体分析程序，并和LOAM论文对应起来，进行理论和实践的结合，增加自己的理解能力；因本人的水平有限，理解不到之处，还请提出，必虚心学习！

process（...）太大了，这一模块的结果就是实现了点云的注册；我把 process 分成了9个部分，后面会对应每个部分进行讲解！

void MultiScanRegistration::process(const pcl::PointCloud<pcl::PointXYZ>& laserCloudIn, const Time& scanTime)
{
  //1、计算开始和结束的垂直角度
  size_t cloudSize = laserCloudIn.size();
  float startOri = -std::atan2(laserCloudIn[0].y, laserCloudIn[0].x);
  float endOri   = -std::atan2(laserCloudIn[cloudSize - 1].y,
                             laserCloudIn[cloudSize - 1].x) + 2 * float(M_PI);
                            
  //必须满足终止角度 > 初始角度并且，近似一个周期2pi
  if (endOri - startOri > 3 * M_PI) {
    endOri -= 2 * M_PI;
  } else if (endOri - startOri < M_PI) {
    endOri += 2 * M_PI;
  }
  //2、 
  bool halfPassed = false;
  pcl::PointXYZI point;
  _laserCloudScans.resize(_scanMapper.getNumberOfScanRings());
  // clear all scanline points
  std::for_each(_laserCloudScans.begin(), _laserCloudScans.end(), [](auto&&v) {v.clear(); }); 

  //3、需要转换坐标系：前左上转换为左上前坐标系，不处理无效点数据
  for (int i = 0; i < cloudSize; i++) {
    point.x = laserCloudIn[i].y;
    point.y = laserCloudIn[i].z;
    point.z = laserCloudIn[i].x;

    // 去除无效点数据
    if (!pcl_isfinite(point.x) ||
        !pcl_isfinite(point.y) ||
        !pcl_isfinite(point.z)) {
      continue;
    }

    //去除零值的点数据
    if (point.x * point.x + point.y * point.y + point.z * point.z < 0.0001) {
      continue;
    }

    //4、根据垂直角度找到对应的激光束ID，这个时候坐标系已经发生了改变，注意使用的坐标轴数据
    float angle = std::atan(point.y / std::sqrt(point.x * point.x + point.z * point.z));
    int scanID = _scanMapper.getRingForAngle(angle);
    if (scanID >= _scanMapper.getNumberOfScanRings() || scanID < 0 ){
      continue;
    }

    /*5、求解每个点的水平角度，根据水平角度可以得到获取每个点时相对与开始点的时间，因为激光雷达是先垂直扫描再水平扫描的，但是角度函数atan2的值域是 − π -\pi −π到 π \pi π，因此必须要进行变换。这里将整个水平方向(360度)扫描的点分为两部分，第一部分与开始点起始角度startOri相差角度在 π \pi π以内，第二部分与结束点终止角度endOri相差角度在 π \pi π以内。这里默认使用的角度是逆时针增加。*/ 
    float ori = -std::atan2(point.x, point.z);
    if (!halfPassed) {
      if (ori < startOri - M_PI / 2) {
        ori += 2 * M_PI;
      } else if (ori > startOri + M_PI * 3 / 2) {
        ori -= 2 * M_PI;
      }

      if (ori - startOri > M_PI) {
        halfPassed = true;
      }
    } else {
      ori += 2 * M_PI;

      if (ori < endOri - M_PI * 3 / 2) {
        ori += 2 * M_PI;
      } else if (ori > endOri + M_PI / 2) {
        ori -= 2 * M_PI;
      }
    }

    // 根据水平角度可以得到获取每个点时相对与开始点的时间，匀速，一看就是根据占的比例分配的
    float relTime = config().scanPeriod * (ori - startOri) / (endOri - startOri);
    
    //对点云数据格式信息的利用达到最大化：把pcl::PointXYZI中的intensity设置为相对开始点的时间＋对应线束id，一个数据包含了两个信息！！
    point.intensity = scanID + relTime;
    //那这个相对时间relTime有什么用呢？马上就会用到：和IMU数据一起近似去除激光的非匀速运动，构建匀速运动模型
    
    //6、
    projectPointToStartOfSweep(point, relTime);
    //7、
    _laserCloudScans[scanID].push_back(point);
  }
    //8、
  processScanlines(scanTime, _laserCloudScans);
    //9、发布主题
  publishResult();
}

}

1、计算水平起始方位和终止方位

/--------------------------代码---------------------------------------/

size_t cloudSize = laserCloudIn.size();                                                                   //每帧点云的个数
float startOri       = - std::atan2(laserCloudIn[0].y, laserCloudIn[0].x);   //第一个激光束的起始方位
float endOri         = - std::atan2(laserCloudIn[cloudSize - 1].y, laserCloudIn[cloudSize - 1].x) + 2 * float(M_PI);   //最后一个激光束的终止方位

/--------------------------代码---------------------------------------/

分析：16线激光雷达一次可以获得16个距离，也就是16个数据为一组；每一帧点云数据的意思是激光雷达 扫描周围一圈 获得的数据；包括很多组16线数据；如果把每一组数据按照行的形似存在数组内，也就是：n*16 的矩阵，数据的形式如下图：

相关知识：atan2（y,x）

atan2(double y,double x) 返回的是原点至点(x,y)的方位角，即与 x 轴的夹角。返回值的单位为弧度，取值范围为（-π, π]。结果为正表示从 X 轴逆时针旋转的角度，结果为负表示从 X 轴顺时针旋转的角度。若要用度表示反正切值，请将结果再乘以 180/π。另外要注意的是，函数atan2(y,x)中参数的顺序是倒置的，atan2(y,x)计算的值相当于点(x,y)的角度值。

那么，上面程序中 startOri 和 endOri 所代表的角度就是第一个和最后一个激光线对应的水平角 α，这个应该很好理解吧！

疑问：为什么角度前面有负号（-）？

答：感觉这里是一个坑！这里的递减，是指通过atan2函数解算出来的角度值递减，如果逆时针旋转，那么atan2函数解算出来的角度值增加

/--------------------------代码---------------------------------------/
if (endOri - startOri > 3 * M_PI) {
endOri -= 2 * M_PI;
} else if (endOri - startOri < M_PI) {
endOri += 2 * M_PI;
}

/--------------------------代码---------------------------------------/

疑问：为什么要近似一个周期2pi，为啥不是 2个,1.5个？

答：2pi 代表激光雷达扫描了一圈，周围的环境感知了一下，因为是水平角嘛，算是完成了一帧的数据，才会对其进行后续的处理！

2、计算水平起始方位和终止方位

/--------------------------代码---------------------------------------/

bool halfPassed = false;
pcl::PointXYZI point;
_laserCloudScans.resize(_scanMapper.getNumberOfScanRings());
// clear all scanline points
std::for_each(_laserCloudScans.begin(), _laserCloudScans.end(), [](auto&&v) {v.clear(); });

/--------------------------代码---------------------------------------/

理解：这一段代码是为了获取获得激光雷达的线数和清空变量，为后面数据做准备！

3、转换坐标系，不处理无效点数据

/--------------------------代码---------------------------------------/

for (int i = 0; i < cloudSize; i++) {
    point.x = laserCloudIn[i].y;
    point.y = laserCloudIn[i].z;
    point.z = laserCloudIn[i].x;

    // 去除无效点数据
    if (!pcl_isfinite(point.x) ||
        !pcl_isfinite(point.y) ||
        !pcl_isfinite(point.z)) {
      continue;
    }

    //去除零值的点数据
    if (point.x * point.x + point.y * point.y + point.z * point.z < 0.0001) {
      continue;
    }

/--------------------------代码---------------------------------------/

疑问：为什么要进行坐标的转换？

答：因为LOAM坐标系为左上前， 原始的激光坐标系是 右前上， 可以通过安装将激光坐标系变换为 前左上，

故需要前左上到左上前的转换，代码对应！

4、根据垂直角度找到对应的激光束ID

/--------------------------代码---------------------------------------/

    float angle = std::atan(point.y / std::sqrt(point.x * point.x + point.z * point.z));
    int scanID = _scanMapper.getRingForAngle(angle);
    if (scanID >= _scanMapper.getNumberOfScanRings() || scanID < 0 ){
      continue;
    }

/--------------------------代码---------------------------------------/

注意：又是一个坑！这个时候坐标系已经发生了改变，注意使用的坐标轴数据

根据上面清晰的图片，应该不难理解吧，求的角度 angle 就是垂直角度 w，而16线激光雷达的角度是固定的，根据角度就可以知道是哪个线（ID）获取的数据！

5、根据水平角度 α 可以得到获取每个点时相对与开始点的时间

/--------------------------代码---------------------------------------/
    float ori = -std::atan2(point.x, point.z);
    if (!halfPassed) {
      if (ori < startOri - M_PI / 2) {
        ori += 2 * M_PI;
      } else if (ori > startOri + M_PI * 3 / 2) {
        ori -= 2 * M_PI;
      }

      if (ori - startOri > M_PI) {
        halfPassed = true;
      }
    } else {
      ori += 2 * M_PI;

      if (ori < endOri - M_PI * 3 / 2) {
        ori += 2 * M_PI;
      } else if (ori > endOri + M_PI / 2) {
        ori -= 2 * M_PI;
      }
    }

    // 根据水平角度可以得到获取每个点时相对与开始点的时间，匀速，一看就是根据占的比例分配的
    float relTime = config().scanPeriod * (ori - startOri) / (endOri - startOri);
    //对点云数据格式信息的利用达到最大化：把pcl::PointXYZI中的intensity设置为相对开始点的时间＋对应线束id，一个数据包含了两个信息！！
    point.intensity = scanID + relTime;

/--------------------------代码---------------------------------------/

理解：

前面我们已经知道水平的起始方位和终止方位，现在我们可以根据当前的点的水平角度求相对开始的时间，并且注意坐标系换了哈，但是本质求法没有变，程序有点像 插值法！

疑问：那这个相对时间relTime有什么用呢？

答：马上就会用到，和IMU数据一起近似去除激光的非匀速运动，构建匀速运动模型。针对这个问题，我的思考是，理想情况下激光雷达固定不变，这样数据就没有畸变，但是实际情况下激光雷达放在载体上是运动的，当然要运动了，不然为啥叫扫面环境呢！运动就会导致一个问题，就是一帧点云数据中，每一组数据（16线为一组）数据接受的位置不一样，那坐标系就不一样呀，所以我们要把每一组数据投影到统一的位置（起始位置），这样问题解决了吧！统一到起始位置要知道相对起始位置的距离呀，距离怎么求，现在有时间了，用IMU积分求嘛，再正常不过了，很常规的思想，而且IMU短时间精度高的特性也被很好的利用了呀！

6、数据投影到起始位置

上一步怎么说的来着，应验了吧，哈哈，都是常规操作，有没有感觉现在思路很清晰了！就应该这样做嘛！

/------------------------------------代码------------------------------------------/

projectPointToStartOfSweep(point, relTime);

void BasicScanRegistration::projectPointToStartOfSweep(pcl::PointXYZI& point, float relTime)
{
  // project point to the start of the sweep using corresponding IMU data
  if (hasIMUData())
  {
    setIMUTransformFor(relTime);
    transformToStartIMU(point);
  }
}

程序中包含的两个子程序：

1、setIMUTransformFor(relTime);

void BasicScanRegistration::setIMUTransformFor(const float& relTime)
{
  interpolateIMUStateFor(relTime, _imuCur);

  float relSweepTime = toSec(_scanTime - _sweepStart) + relTime;
  _imuPositionShift = _imuCur.position - _imuStart.position - _imuStart.velocity * relSweepTime;
}

子子程序哈哈

interpolateIMUStateFor(relTime, _imuCur);

/* 
近似匀速运动模型
relTime：相对于扫描时间的时间
outputState：输出状态实例 
-----------------------------------------------------------------
当IMU数据存在时，使用IMU数据积分得到速度位置对点云数据进行补偿，得到激光匀速运动模型下的坐标。
首先是获取IMU数据，使用插值得到精确时间_scanTime + relTime的IMU数据(包含速度，位置，三个姿态角)
根据relTime进行IMU数据插值，得到速度，位置信息
*/
void BasicScanRegistration::interpolateIMUStateFor(const float &relTime, IMUState &outputState)
{
    // IMU数据和点云点的时间差：点云点时间 - IMU时间
  double timeDiff = toSec(_scanTime - _imuHistory[_imuIdx].stamp) + relTime;
  while (_imuIdx < _imuHistory.size() - 1 && timeDiff > 0) {
    _imuIdx++;
    timeDiff = toSec(_scanTime - _imuHistory[_imuIdx].stamp) + relTime;
  }

  // 不能插值，1._imuIdx == 0：使用最旧的数据  2. timeDiff > 0：使用最新的数据
  if (_imuIdx == 0 || timeDiff > 0) {
    outputState = _imuHistory[_imuIdx];
  } 
  else  // 如果IMU缓冲区中可以插值，就直接插值
  {
    float ratio = -timeDiff / toSec(_imuHistory[_imuIdx].stamp - _imuHistory[_imuIdx - 1].stamp);
    IMUState::interpolate(_imuHistory[_imuIdx], _imuHistory[_imuIdx - 1], ratio, outputState);
  }
/*经过上面处理，因此我们拥有了当前点时间(_scanTime + relTime)对应的激光位置和速度，进一步和开始点(_scanTime + 0)的激光位置、速度进行处理有Startposition + PositionShift + 匀速模型(开始点对应的速度) = Curposition。可以考虑激光加速运动下，PositionShift表示了开始点时刻到当前时刻激光由于加速移动的距离。*/
}

2、 transformToStartIMU(point);

/*
对于位置偏移，需要在世界坐标系下对点云数据进行处理，这里使用了之前提到的IMU局部坐标系和世界坐标系的转换，并且由于点云测量坐标和激光运动趋势相反，因此这里需要加上位置偏移量，特别注意不是减！然后进一步得到IMU局部坐标系下的点云坐标。 
*/
void BasicScanRegistration::transformToStartIMU(pcl::PointXYZI& point)
{
  // IMU局部坐标系到世界坐标系转换
  rotateZXY(point, _imuCur.roll, _imuCur.pitch, _imuCur.yaw);

  // 加上位置偏移量，这里 加 是因为激光运动和点云运动是相反的。
  // 可以考虑加速运动下，PositionShift为正，并且点云测量坐标系变小，因此需要让点云坐标变大
  point.x += _imuPositionShift.x();
  point.y += _imuPositionShift.y();
  point.z += _imuPositionShift.z();

  // 世界坐标系到IMU局部坐标系转换
  rotateYXZ(point, -_imuStart.yaw, -_imuStart.pitch, -_imuStart.roll);
}

子子程序 rotateZXY(point, _imuCur.roll, _imuCur.pitch, _imuCur.yaw);

inline void rotateZXY(Vector3& v,
                      const Angle& angZ,
                      const Angle& angX,
                      const Angle& angY)
{
  rotZ(v, angZ);
  rotX(v, angX);
  rotY(v, angY);
}


/** \brief Rotate the given vector by the specified angle around the x-axis.
 *
 * @param v the vector to rotate
 * @param ang the rotation angle
 */
inline void rotX(Vector3& v, const Angle& ang)
{
  float y = v.y();
  v.y() = ang.cos() * y - ang.sin() * v.z();
  v.z() = ang.sin() * y + ang.cos() * v.z();
}

/** \brief Rotate the given point by the specified angle around the x-axis.
 *
 * @param p the point to rotate
 * @param ang the rotation angle
 */
template <typename PointT>
inline void rotX(PointT& p, const Angle& ang)
{
  float y = p.y;
  p.y = ang.cos() * y - ang.sin() * p.z;
  p.z = ang.sin() * y + ang.cos() * p.z;
}

可以举一反三理解，其他两个也是一个意思！

/----------------------------------------代码--------------------------------------------/

7、处理好的点进行保存

~~/----------------------------------------代码--------------------------------------------/~~

_laserCloudScans[scanID].push_back(point);

保存数据到 _laserCloudScans

~~/----------------------------------------代码--------------------------------------------/~~

8、特征提取

~~/----------------------------------------代码--------------------------------------------/~~

processScanlines(scanTime, _laserCloudScans);

/*
特征提取
对所有点云数据进行运动补偿之后就是进行特征点提取了。首先变换点云数据存储格式，得到一维状态下的点云数据。使用_scanIndices来确定每束数据的开始位置，结束位置
*/
void BasicScanRegistration::processScanlines(const Time& scanTime, std::vector<pcl::PointCloud<pcl::PointXYZI>> const& laserCloudScans)
{
  // 为下一次处理做准备，同时清空当前特征点容器
  reset(scanTime);  

   // 将不同激光束点云数据合并为一维形式，使用_scanIndices来确定每束数据的开始位置，结束位置。
  size_t cloudSize = 0;
  for (int i = 0; i < laserCloudScans.size(); i++) {
    _laserCloud += laserCloudScans[i];

    IndexRange range(cloudSize, 0);
    cloudSize += laserCloudScans[i].size();
    range.second = cloudSize > 0 ? cloudSize - 1 : 0;
    _scanIndices.push_back(range);
  }

  extractFeatures();
  updateIMUTransform();
}

~~/----------------------------------------代码--------------------------------------------/~~

9、主题发布，供其他模块使用

~~/----------------------------------------代码--------------------------------------------/~~

publishResult();

/*
布数据
最后将得到点云数据以及IMU数据进行发布。_pubLaserCloud是近似匀速模型下的所有点云数据。其他的前面已经涉及到了，就不再赘述。 
*/
void ScanRegistration::publishResult()
{
  auto sweepStartTime = toROSTime(sweepStart());
   // 发布点云数据，特征点云数据
  publishCloudMsg(_pubLaserCloud, laserCloud(), sweepStartTime, "/camera");
  publishCloudMsg(_pubCornerPointsSharp, cornerPointsSharp(), sweepStartTime, "/camera");
  publishCloudMsg(_pubCornerPointsLessSharp, cornerPointsLessSharp(), sweepStartTime, "/camera");
  publishCloudMsg(_pubSurfPointsFlat, surfacePointsFlat(), sweepStartTime, "/camera");
  publishCloudMsg(_pubSurfPointsLessFlat, surfacePointsLessFlat(), sweepStartTime, "/camera");

   // 发布IMU测量的激光位姿变换数据
  publishCloudMsg(_pubImuTrans, imuTransform(), sweepStartTime, "/camera");
}

~~/----------------------------------------代码--------------------------------------------/~~