loam——laserOdometry

最新推荐文章于 2022-04-14 20:50:50 发布

豆皮卷香菜

最新推荐文章于 2022-04-14 20:50:50 发布

阅读量188

点赞数

分类专栏： slam 文章标签： slam

本文链接：https://blog.youkuaiyun.com/l898985121/article/details/118656379

版权

slam 专栏收录该内容

20 篇文章

订阅专栏

本文详细介绍了基于激光雷达点云的配准优化算法，包括同步检查、初始化、匀速模型预测、Levenberg-Marquardt（LM）迭代以及特征点的寻找和匹配。在LM迭代过程中，通过TransformToStart函数进行坐标变换，并在每次迭代中寻找最近点，以实现点云的精确配准。此外，还探讨了在点云配准中的关键步骤和策略，如每隔5次迭代重新查找最近点，确保配准的精度和稳定性。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

直接看主函数中的while循环
首先确保所有数据都收到了

if (newCornerPointsSharp && newCornerPointsLessSharp && newSurfPointsFlat && 
        newSurfPointsLessFlat && newLaserCloudFullRes && newImuTrans &&
        fabs(timeCornerPointsSharp - timeSurfPointsLessFlat) < 0.005 &&
        fabs(timeCornerPointsLessSharp - timeSurfPointsLessFlat) < 0.005 &&
        fabs(timeSurfPointsFlat - timeSurfPointsLessFlat) < 0.005 &&
        fabs(timeLaserCloudFullRes - timeSurfPointsLessFlat) < 0.005 &&
        fabs(timeImuTrans - timeSurfPointsLessFlat) < 0.005) {  //同步作用，确保同时收到同一个点云的特征点以及IMU信息才进入
      newCornerPointsSharp = false;
      newCornerPointsLessSharp = false;
      newSurfPointsFlat = false;
      newSurfPointsLessFlat = false;
      newLaserCloudFullRes = false;
      newImuTrans = false;

	.....
}

然后确保在第一帧的时候系统要被初始化

if (!systemInited) {
	........
}

这里像是在循环之前预测一个初始值，按照匀速模型

//T平移量的初值赋值为加减速的位移量，为其梯度下降的方向（沿用上次转换的T（一个sweep匀速模型），同时在其基础上减去匀速运动位移，即只考虑加减速的位移量）
      transform[3] -= imuVeloFromStartX * scanPeriod;
      transform[4] -= imuVeloFromStartY * scanPeriod;
      transform[5] -= imuVeloFromStartZ * scanPeriod;

只要有足够的特征点就可以进入循环

if (laserCloudCornerLastNum > 10 && laserCloudSurfLastNum > 100) {
	....
}

下面看一下循环内部，

先初始化一些数据结构：

		std::vector<int> indices;
        pcl::removeNaNFromPointCloud(*cornerPointsSharp,*cornerPointsSharp, indices);
        int cornerPointsSharpNum = cornerPointsSharp->points.size();
        int surfPointsFlatNum = surfPointsFlat->points.size();

确定LM迭代次数为25次

		//Levenberg-Marquardt算法(L-M method)，非线性最小二乘算法，最优化算法的一种
        //最多迭代25次
        for (int iterCount = 0; iterCount < 25; iterCount++) {
          laserCloudOri->clear();
          coeffSel->clear();
          
          ......
          
      	}

在LM迭代当中：
首先TransformToStart()函数，
看不懂这个函数在干什么
这个函数在25次LM迭代内部，所以每迭代一次，都要执行一下。

for (int i = 0; i < cornerPointsSharpNum; i++) {
            TransformToStart(&cornerPointsSharp->points[i], &pointSel);
......

}

感觉这个代码注释的有点问题，
首先transform每个迭代步骤内都会被更新。这个transform是只上一帧和这一帧之间的坐标变换。
包含transform去做坐标变换，显然应该是将当前帧往上一帧的坐标系内变换，如果transfom收敛到正确值，那么这个变换之后会导致点到直线距离或者点到面的距离非常小，所以这个更新应该是这个意思。展示的就是下一帧的点逐渐向上一帧的对应点移动的过程。
这里可以写个小代码验证迭代过程

/*****************************************************************************
    将当前帧点云TransformToStart和将上一帧点云TransformToEnd的作用：
         去除畸变，并将两帧点云数据统一到同一个坐标系下计算
*****************************************************************************/

//当前点云中的点相对第一个点去除因匀速运动产生的畸变，效果相当于得到在点云扫描开始位置静止扫描得到的点云
void TransformToStart(PointType const * const pi, PointType * const po)
{
  //插值系数计算，云中每个点的相对时间/点云周期10
  float s = 10 * (pi->intensity - int(pi->intensity));

  //线性插值：根据每个点在点云中的相对位置关系，乘以相应的旋转平移系数
  float rx = s * transform[0];
  float ry = s * transform[1];
  float rz = s * transform[2];
  float tx = s * transform[3];
  float ty = s * transform[4];
  float tz = s * transform[5];

  //平移后绕z轴旋转（-rz）
  float x1 = cos(rz) * (pi->x - tx) + sin(rz) * (pi->y - ty);
  float y1 = -sin(rz) * (pi->x - tx) + cos(rz) * (pi->y - ty);
  float z1 = (pi->z - tz);

  //绕x轴旋转（-rx）
  float x2 = x1;
  float y2 = cos(rx) * y1 + sin(rx) * z1;
  float z2 = -sin(rx) * y1 + cos(rx) * z1;

  //绕y轴旋转（-ry）
  po->x = cos(ry) * x2 - sin(ry) * z2;
  po->y = y2;
  po->z = sin(ry) * x2 + cos(ry) * z2;
  po->intensity = pi->intensity;
}

每迭代5次就要重新找一下最近点

//每迭代五次，重新查找最近点
            if (iterCount % 5 == 0) {
              std::vector<int> indices;
              pcl::removeNaNFromPointCloud(*laserCloudCornerLast,*laserCloudCornerLast, indices);
              //kd-tree查找一个最近距离点，边沿点未经过体素栅格滤波，一般边沿点本来就比较少，不做滤波
              kdtreeCornerLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);
              int closestPointInd = -1, minPointInd2 = -1;
					....
			}

如果确实很近的话，

				//寻找相邻线距离目标点距离最小的点
              //再次提醒：velodyne是2度一线，scanID相邻并不代表线号相邻，相邻线度数相差2度，也即线号scanID相差2
              if (pointSearchSqDis[0] < 25) {//找到的最近点距离的确很近的话
                closestPointInd = pointSearchInd[0];
                //提取最近点线号
                int closestPointScan = int(laserCloudCornerLast->points[closestPointInd].intensity);

                float pointSqDis, minPointSqDis2 = 25;//初始门槛值5米，可大致过滤掉scanID相邻，但实际线不相邻的值

现象前找，再向后找，总之再找一个最近点

//寻找距离目标点最近距离的平方和最小的点
                for (int j = closestPointInd + 1; j < cornerPointsSharpNum; j++) {//向scanID增大的方向查找
                  if (int(laserCloudCornerLast->points[j].intensity) > closestPointScan + 2.5) {//非相邻线
                    break;
                  }

                  pointSqDis = (laserCloudCornerLast->points[j].x - pointSel.x) * 
                               (laserCloudCornerLast->points[j].x - pointSel.x) + 
                               (laserCloudCornerLast->points[j].y - pointSel.y) * 
                               (laserCloudCornerLast->points[j].y - pointSel.y) + 
                               (laserCloudCornerLast->points[j].z - pointSel.z) * 
                               (laserCloudCornerLast->points[j].z - pointSel.z);

                  if (int(laserCloudCornerLast->points[j].intensity) > closestPointScan) {//确保两个点不在同一条scan上（相邻线查找应该可以用scanID == closestPointScan +/- 1 来做）
                    if (pointSqDis < minPointSqDis2) {//距离更近，要小于初始值5米
                        //更新最小距离与点序
                      minPointSqDis2 = pointSqDis;
                      minPointInd2 = j;
                    }
                  }
                }
                                //同理
                for (int j = closestPointInd - 1; j >= 0; j--) {//向scanID减小的方向查找
                  if (int(laserCloudCornerLast->points[j].intensity) < closestPointScan - 2.5) {
                    break;
                  }

                  pointSqDis = (laserCloudCornerLast->points[j].x - pointSel.x) * 
                               (laserCloudCornerLast->points[j].x - pointSel.x) + 
                               (laserCloudCornerLast->points[j].y - pointSel.y) * 
                               (laserCloudCornerLast->points[j].y - pointSel.y) + 
                               (laserCloudCornerLast->points[j].z - pointSel.z) * 
                               (laserCloudCornerLast->points[j].z - pointSel.z);

                  if (int(laserCloudCornerLast->points[j].intensity) < closestPointScan) {
                    if (pointSqDis < minPointSqDis2) {
                      minPointSqDis2 = pointSqDis;
                      minPointInd2 = j;
                    }
                  }
                }
              }

把两个最近点记下来

//记住组成线的点序
              pointSearchCornerInd1[i] = closestPointInd;//kd-tree最近距离点，-1表示未找到满足的点
              pointSearchCornerInd2[i] = minPointInd2;//另一个最近的，-1表示未找到满足的点

紧接着下面是LM法的构造和求解
这里有几个参考文献：
https://blog.youkuaiyun.com/weixin_44684139/article/details/118581383
https://www.cnblogs.com/wellp/p/8877990.html
https://zhuanlan.zhihu.com/p/113946848