【原文核心对照代码】【一文足以系列】A-LOAM里程计部分简短精解

前言

本文将通过论文对照代码的方式阐述A-LOAM这一神奇算法。全文保持各个章节短小精悍的风格。本文会省去一些细节，但是了解大部分的论文和代码实现已经足够了。

点曲率计算与边缘点面点区分

论文中通过对点云点的曲率进行如下求曲率的计算。将计算的结果跟阈值进行比较。大于阈值认为是边缘点角点，小于另一个阈值认为是面点。

如下是计算曲率：

for (int i = 5; i < cloudSize - 5; i++)
{ 
    float diffX = laserCloud->points[i - 5].x + laserCloud->points[i - 4].x + laserCloud->points[i - 3].x + laserCloud->points[i - 2].x + laserCloud->points[i - 1].x - 10 * laserCloud->points[i].x + laserCloud->points[i + 1].x + laserCloud->points[i + 2].x + laserCloud->points[i + 3].x + laserCloud->points[i + 4].x + laserCloud->points[i + 5].x;
    float diffY = laserCloud->points[i - 5].y + laserCloud->points[i - 4].y + laserCloud->points[i - 3].y + laserCloud->points[i - 2].y + laserCloud->points[i - 1].y - 10 * laserCloud->points[i].y + laserCloud->points[i + 1].y + laserCloud->points[i + 2].y + laserCloud->points[i + 3].y + laserCloud->points[i + 4].y + laserCloud->points[i + 5].y;
    float diffZ = laserCloud->points[i - 5].z + laserCloud->points[i - 4].z + laserCloud->points[i - 3].z + laserCloud->points[i - 2].z + laserCloud->points[i - 1].z - 10 * laserCloud->points[i].z + laserCloud->points[i + 1].z + laserCloud->points[i + 2].z + laserCloud->points[i + 3].z + laserCloud->points[i + 4].z + laserCloud->points[i + 5].z;

    cloudCurvature[i] = diffX * diffX + diffY * diffY + diffZ * diffZ;
    cloudSortInd[i] = i;
    cloudNeighborPicked[i] = 0;
    cloudLabel[i] = 0;
}

根据论文中这部分

论文中为了在选取面点和边缘点时均匀选取，所以采取了将每个scan的点云进行分组，论文中分成了4组，a-loam里分成了6组。
且每组选取两个边缘点和四个面点。
代码会进行部分截取

这里就是分组：

int sp = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * j / 6; 
int ep = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * (j + 1) / 6 - 1;

选取曲率最大的点和曲率稍微大一点的点：

if (cloudNeighborPicked[ind] == 0 &&
    cloudCurvature[ind] > 0.1)
{
    largestPickedNum++;

    if (largestPickedNum <= 2)
    {                        
        cloudLabel[ind] = 2;

        cornerPointsSharp.push_back(laserCloud->points[ind]);
        cornerPointsLessSharp.push_back(laserCloud->points[ind]);
    }

    else if (largestPickedNum <= 20)
    {                        
        cloudLabel[ind] = 1; 
        cornerPointsLessSharp.push_back(laserCloud->points[ind]);
    }

选取面点：

for (int k = sp; k <= ep; k++)
{
    int ind = cloudSortInd[k];

    if (cloudNeighborPicked[ind] == 0 &&
        cloudCurvature[ind] < 0.1)
    {
        cloudLabel[ind] = -1; 
        surfPointsFlat.push_back(laserCloud->points[ind]);

剩下的点认为是平坦程度一般的点：

for (int k = sp; k <= ep; k++)
{
    if (cloudLabel[k] <= 0)
    {
        surfPointsLessFlatScan->push_back(laserCloud->points[k]);
    }
}

平坦程度一般的点有很多 所以这里做一个体素滤波 或者说是下采样：

pcl::PointCloud<PointType> surfPointsLessFlatScanDS;
pcl::VoxelGrid<PointType> downSizeFilter;

downSizeFilter.setInputCloud(surfPointsLessFlatScan);
downSizeFilter.setLeafSize(0.2, 0.2, 0.2);
downSizeFilter.filter(surfPointsLessFlatScanDS);

激光雷达运动补偿

A-LOAM中由于没有集成轮速计或者imu，所以只能使用帧间匀速运动模型。这在车辆运动过程中使用这个模型是可以的，如果是手持激光雷达使用匀速运动模型，通常会有很大的误差。

对应论文中这个图片：

重投影到每一帧开始时刻的代码实现：
kitti数据已经对点云数据做了运动补偿处理，所以DISTORTION为0 不做具体的补偿

void TransformToStart(PointType const *const pi, PointType *const po)
{
    double s;
    if (DISTORTION)
        s = (pi->intensity - int(pi->intensity)) / SCAN_PERIOD;
    else
        s = 1.0;

    Eigen::Quaterniond q_point_last = Eigen::Quaterniond::Identity().slerp(s, q_last_curr);
    Eigen::Vector3d t_point_last = s * t_last_curr;
    Eigen::Vector3d point(pi->x, pi->y, pi->z);
    Eigen::Vector3d un_point = q_point_last * point + t_point_last;

    po->x = un_point.x();
    po->y = un_point.y();
    po->z = un_point.z();
    po->intensity = pi->intensity;
}

我们已知上一时刻到当前时刻的位姿变换，又因为是匀速运动模型，所以当前时刻到下一时刻，或者说这一帧从第0ms到第100ms的位姿变换，等同于上一时刻到当前时刻的位姿变换。这里求一下当前这个点在0ms到100ms的一个比例，然后利用上一时刻到当前时刻的位姿进行差值，就能求出当前时刻的点的位姿。

下面的代码是将全部点转换到末尾时刻的：
可以看到若是将全部点转换到末尾时刻，首先要将点转换到初始时刻

void TransformToEnd(PointType const *const pi, PointType *const po)
{
    // undistort point first
    pcl::PointXYZI un_point_tmp;
    TransformToStart(pi, &un_point_tmp);

    Eigen::Vector3d un_point(un_point_tmp.x, un_point_tmp.y, un_point_tmp.z);
    Eigen::Vector3d point_end = q_last_curr.inverse() * (un_point - t_last_curr);

    po->x = point_end.x();
    po->y = point_end.y();
    po->z = point_end.z();

    //Remove distortion time info
    po->intensity = int(pi->intensity);
}

我们已知的是q_start_end 这个是从初始时刻到末尾时刻的位姿变换，也是可以将点从末尾时刻转换到初始时刻的位姿变换。
所以我们要求的是q_end_start 即将全部初始时刻的点全部转换到末尾时刻。

帧间运动估计

论文中使用的是点到线的距离和点到面的距离，通过优化帧间位姿q t，来减小两个距离。

图中我们已知一个当前的特征点，但这个特征点未在图中表示出来。

左图 j 点是一个和当前时刻特征点上一时刻最近的一个点，且属于同一个scanID，l 点也是距离当前时刻特征点最近的一个点，只是要求不是同一个scanID，并且如果找到了，scanID不能距离当前特征点的scanID太远。

右图是寻找两个和当前时刻特征点距离最近的点，且属于同一个scanID。另一个点也是距离最近，同样有不是同一个scanID和scanID不能距离当前特征点scanID太远两个要求。

其中论文中点到线的距离求法：

对应代码如下：

struct LidarEdgeFactor
{
	LidarEdgeFactor(Eigen::Vector3d curr_point_, Eigen::Vector3d last_point_a_,
					Eigen::Vector3d last_point_b_, double s_)
		: curr_point(curr_point_), last_point_a(last_point_a_), last_point_b(last_point_b_), s(s_) {}

	template <typename T>
	bool operator()(const T *q, const T *t, T *residual) const
	{
		Eigen::Matrix<T, 3, 1> cp{T(curr_point.x()), T(curr_point.y()), T(curr_point.z())};
		Eigen::Matrix<T, 3, 1> lpa{T(last_point_a.x()), T(last_point_a.y()), T(last_point_a.z())};
		Eigen::Matrix<T, 3, 1> lpb{T(last_point_b.x()), T(last_point_b.y()), T(last_point_b.z())};

		Eigen::Quaternion<T> q_last_curr{q[3], q[0], q[1], q[2]};
		Eigen::Quaternion<T> q_identity{T(1), T(0), T(0), T(0)};

		q_last_curr = q_identity.slerp(T(s), q_last_curr);
		Eigen::Matrix<T, 3, 1> t_last_curr{T(s) * t[0], T(s) * t[1], T(s) * t[2]};

		Eigen::Matrix<T, 3, 1> lp;

		lp = q_last_curr * cp + t_last_curr;

		Eigen::Matrix<T, 3, 1> nu = (lp - lpa).cross(lp - lpb);	// 模是三角形的面积
		Eigen::Matrix<T, 3, 1> de = lpa - lpb;

		residual[0] = nu.x() / de.norm();
		residual[1] = nu.y() / de.norm();
		residual[2] = nu.z() / de.norm();

		return true;
	}

对应代码：

struct LidarPlaneNormFactor
{

	LidarPlaneNormFactor(Eigen::Vector3d curr_point_, Eigen::Vector3d plane_unit_norm_,
						 double negative_OA_dot_norm_) : curr_point(curr_point_), plane_unit_norm(plane_unit_norm_),
														 negative_OA_dot_norm(negative_OA_dot_norm_) {}

	template <typename T>
	bool operator()(const T *q, const T *t, T *residual) const
	{
		Eigen::Quaternion<T> q_w_curr{q[3], q[0], q[1], q[2]};
		Eigen::Matrix<T, 3, 1> t_w_curr{t[0], t[1], t[2]};
		Eigen::Matrix<T, 3, 1> cp{T(curr_point.x()), T(curr_point.y()), T(curr_point.z())};
		Eigen::Matrix<T, 3, 1> point_w;
		point_w = q_w_curr * cp + t_w_curr;

		Eigen::Matrix<T, 3, 1> norm(T(plane_unit_norm.x()), T(plane_unit_norm.y()), T(plane_unit_norm.z()));
		residual[0] = norm.dot(point_w) + T(negative_OA_dot_norm);
		return true;
	}

残差细节可以细读源码。
大概意思是：基于匀速运动模型，根据当前点的比例，计算当前点基于上一时刻的位姿。通过这个位姿将这个点转换到上一时刻，在和上一时刻的两个点或者三个点求点到线的距离或者点到面的距离。

寻找最近点的函数：

KD树构造：

pcl::KdTreeFLANN<pcl::PointXYZI>::Ptr kdtreeCornerLast(new pcl::KdTreeFLANN<pcl::PointXYZI>());

将上一帧点云数据存入KD树：kdtreeCornerLast->setInputCloud(laserCloudCornerLast);

使用KD树寻找最近的点：

kdtreeCornerLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);