A-LOAM(后端2)地图中的线面特征提取及优化问题构建-算法流程+代码注释

后端约束构建

在前端⾥程记部分，我们通过当前帧的线特征和⾯特征分别和上⼀帧的线特征和⾯特征进⾏匹配，构建约束，然后进⾏优化求解。由于机械式激光雷达的性质，我们在寻找匹配的过程中需要注意线束相关的约束，以免构建的约束不符合实际。在后端的当前帧和地图匹配的时候，我们就需要从地图⾥寻找线特征和⾯特征的约束对，此时，由于没有了线束信息，我们就需要采取额外的操作来判断其是否符合线特征和⾯特征的给定约束。

11. cere基本参数设置

//ceres::LossFunction *loss_function = NULL;
// 建立ceres问题
ceres::LossFunction *loss_function = new ceres::HuberLoss(0.1);
ceres::LocalParameterization *q_parameterization = //这么定义是因为四元数不符合广义的加法
						new ceres::EigenQuaternionParameterization();
ceres::Problem::Options problem_options;

ceres::Problem problem(problem_options);
problem.AddParameterBlock(parameters, 4, q_parameterization); //四元数，parameters的前4位
problem.AddParameterBlock(parameters + 4, 3); //从 第parameters + 4 位置开始数3个为平移

线特征提取

通过kdtree在地图中找到5个最近的线特征，为了判断他们是否符合线特征的特性，我们需要对其进⾏特征值分解，通常来说，当上述5个点都在⼀条直线上时，他们只有⼀个主⽅向，也就是特征值是⼀个⼤特征值，以及两个⼩特征值，最⼤特征值对应的特征向量就对应着直线的⽅向向量。

12. 提取所有边缘点 ，这个边缘点是当前帧的边缘点，投影到地图坐标系下面，并对每个边缘点在已构建地图中找出5个与其最近的点，注意：这里与前端的区别就在于，前端是与上一帧找点，这里是与整个地图找点

pointOri = laserCloudCornerStack->points[i];//取出所有边缘点
//double sqrtDis = pointOri.x * pointOri.x + pointOri.y * pointOri.y + pointOri.z * pointOri.z;
// 把当前点根据初值投到地图坐标系下去，这里是用初始值进行投影
pointAssociateToMap(&pointOri, &pointSel);
// 地图中寻找和该点最近的5个点
kdtreeCornerFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);

13. 开始提取
    1. 计算5个点的均值
    2. 构建协方差矩阵，（原点-均值）∈3×1矩阵，乘以它自己的转置构成协方差∈3×3

// 构建协方差矩阵
for (int j = 0; j < 5; j++)
{
	Eigen::Matrix<double, 3, 1> tmpZeroMean = nearCorners[j] - center;
	covMat = covMat + tmpZeroMean * tmpZeroMean.transpose();
}

3. 对协方差进行特征值分解
4. 判断最大特征值大于次大特征值的3倍，符合的话则认为是线特征，特征向量就是线特征的方向，也是直线方向向量

if (saes.eigenvalues()[2] > 3 * saes.eigenvalues()[1])

5.符合的话则用这个向量构建两个虚拟的点，构建方式如下

point_a = 0.1 * unit_direction + point_on_line;
point_b = -0.1 * unit_direction + point_on_line;

点线约束构建

与前端的约束构建方式一模一样，可以参考A-LOAM(前端-4)的帧间lidar里程计-算法流程+代码注释 里面的角点约束构建部分

代码

for (int i = 0; i < laserCloudCornerStackNum; i++)
{
	pointOri = laserCloudCornerStack->points[i];//取出所有边缘点
	//double sqrtDis = pointOri.x * pointOri.x + pointOri.y * pointOri.y + pointOri.z * pointOri.z;
	// 把当前点根据初值投到地图坐标系下去，这里是用初始值进行投影
	pointAssociateToMap(&pointOri, &pointSel);
	// 地图中寻找和该点最近的5个点
	kdtreeCornerFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis); 
	// 判断最远的点距离不能超过1m，否则就是无效约束
	if (pointSearchSqDis[4] < 1.0)
	{ 
		std::vector<Eigen::Vector3d> nearCorners;
		Eigen::Vector3d center(0, 0, 0);
		for (int j = 0; j < 5; j++)
		{
			Eigen::Vector3d tmp(laserCloudCornerFromMap->points[pointSearchInd[j]].x,
								laserCloudCornerFromMap->points[pointSearchInd[j]].y,
								laserCloudCornerFromMap->points[pointSearchInd[j]].z);
			center = center + tmp;
			nearCorners.push_back(tmp);
		}
		// 计算这五个点的均值
		center = center / 5.0;

		Eigen::Matrix3d covMat = Eigen::Matrix3d::Zero();
		// 构建协方差矩阵
		for (int j = 0; j < 5; j++)
		{
			Eigen::Matrix<double, 3, 1> tmpZeroMean = nearCorners[j] - center;
			covMat = covMat + tmpZeroMean * tmpZeroMean.transpose();
		}
		// 进行特征值分解
		Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> saes(covMat);

		// if is indeed line feature
		// note Eigen library sort eigenvalues in increasing order
		// 根据特征值分解情况看看是不是真正的线特征
		// 特征向量就是线特征的方向 符合的话就是直线方向向量
		Eigen::Vector3d unit_direction = saes.eigenvectors().col(2);
		Eigen::Vector3d curr_point(pointOri.x, pointOri.y, pointOri.z);
		// 最大特征值大于次大特征值的3倍认为是线特征
		if (saes.eigenvalues()[2] > 3 * saes.eigenvalues()[1])
		{ 
			Eigen::Vector3d point_on_line = center;
			Eigen::Vector3d point_a, point_b;
			// 根据拟合出来的线特征方向，以平均点为中心构建两个虚拟点
			point_a = 0.1 * unit_direction + point_on_line;
			point_b = -0.1 * unit_direction + point_on_line;
			// 构建约束，和lidar odom约束一致 放入约束仍然是两个点
			ceres::CostFunction *cost_function = LidarEdgeFactor::Create(curr_point, point_a, point_b, 1.0);
			problem.AddResidualBlock(cost_function, loss_function, parameters, parameters + 4);
			corner_num++;	
		}							
	}
}

面特征提取

同样⾸先通过kdtree在地图中找到最近的⾯特征，原则上⾯特征也可以使⽤特征值分解的⽅式，选出最
⼩特征值对应的特征向量及平⾯的法向量，不过代码⾥选⽤的是平⾯拟合的⽅式：
我们知道平⾯⽅程为 $$Ax+By+Cz+D=0$$
考虑到等式的形式，可以进⼀步写成$$Ax+By+Cz+1=0$$
注意这里的A、B、C不是之前的ABC，A=A/D···，下式就是上式整体除以D，下式也就是三个未知数，五个⽅程（也是找5个点），写成矩阵的形式就是⼀个5×3⼤⼩的矩阵，求出结果之后，我们还需要对结果进⾏校验，来观察其是否符合平⾯约束，具体就是分别求出5个点到求出平⾯的距离，如果太远，则说明该平⾯拟合不成功。

14. 在地图中提取离当前帧的每个面点最近的5个点，构建一个超定方程来求解这个平面方程，然后用eigen求解

kdtreeSurfFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);

Eigen::Matrix<double, 5, 3> matA0;
Eigen::Matrix<double, 5, 1> matB0 = -1 * Eigen::Matrix<double, 5, 1>::Ones();
// 构建平面方程Ax + By +Cz + 1 = 0
// 通过构建一个超定方程来求解这个平面方程
if (pointSearchSqDis[4] < 1.0)
{
	
	for (int j = 0; j < 5; j++)
	{
		matA0(j, 0) = laserCloudSurfFromMap->points[pointSearchInd[j]].x;
		matA0(j, 1) = laserCloudSurfFromMap->points[pointSearchInd[j]].y;
		matA0(j, 2) = laserCloudSurfFromMap->points[pointSearchInd[j]].z;
		//printf(" pts %f %f %f ", matA0(j, 0), matA0(j, 1), matA0(j, 2));
	}
	// find the norm of plane
	// 调用eigen接口求解该方程，解就是这个平面的法向量
	Eigen::Vector3d norm = matA0.colPivHouseholderQr().solve(matB0); //norm为A、B、C

15. 求得的normA、B、C，根据ABC求D$$D=\sqrt{(}{A}^2+B^2+C^2)$$

double negative_OA_dot_norm = 1 / norm.norm();

Eigen中norm、normalize、normalized的区别
16. 求得法向量，并代入公式中求解个点到平面的距离，判断是否符合平面的约束

// 法向量归一化
norm.normalize();

// Here n(pa, pb, pc) is unit norm of plane
bool planeValid = true;
// 根据求出来的平面方程进行校验，看看是不是符合平面约束
for (int j = 0; j < 5; j++)
{
	// if OX * n > 0.2, then plane is not fit well
	// 这里相当于求解点到平面的距离
	if (fabs(norm(0) * laserCloudSurfFromMap->points[pointSearchInd[j]].x +
				norm(1) * laserCloudSurfFromMap->points[pointSearchInd[j]].y +
				norm(2) * laserCloudSurfFromMap->points[pointSearchInd[j]].z + negative_OA_dot_norm) > 0.2)
	{
		planeValid = false;	// 点如果距离平面太远，就认为这是一个拟合的不好的平面
		break;
	}
}

点面约束构建

这里的点面约束构建与前端的方式不同，这里利用平面方程构建约束

残差构建重点语句如下：
这里point_w为当前帧的点转到世界坐标系下的点，norm为平面的法向量，对于平面的方程来讲，平面的法向量就是ABC，所以前面要normalize()来归一化，一个点表示一个向量，向量为该点与原点连接形成的，一个向量点乘一个法向量就可以得到在法向量上面的投影距离，平面方程中的D，我暂且理解为外点所构成的与平面相平行的平面之间的距离，因为当D=0时，Ax+By+Cz = 0，D可以反映平面的位置(偏移量)

residual[0] = norm.dot(point_w) + T(negative_OA_dot_norm);	// 求解点到平面的距离

代码

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;
	}

	static ceres::CostFunction *Create(const Eigen::Vector3d curr_point_, const Eigen::Vector3d plane_unit_norm_,
									   const double negative_OA_dot_norm_)
	{
		return (new ceres::AutoDiffCostFunction<
				LidarPlaneNormFactor, 1, 4, 3>(
			new LidarPlaneNormFactor(curr_point_, plane_unit_norm_, negative_OA_dot_norm_)));
	}

	Eigen::Vector3d curr_point;
	Eigen::Vector3d plane_unit_norm;
	double negative_OA_dot_norm;
};

A-LOAM系列讲解
A-LOAM(前端-1)的特征提取及均匀化-算法流程+代码注释
A-LOAM(前端-2)异常点的剔除-算法流程+代码注释
A-LOAM(前端-3)的雷达畸变及运动补偿-算法流程+代码注释
A-LOAM(前端-4)的帧间lidar里程计-算法流程+代码注释
A-LOAM(后端1)基于栅格点云地图构建-算法流程+代码注释
A-LOAM(后端2)地图中的线面特征提取及优化问题构建-算法流程+代码注释
A-LOAM总结-(前端+后端)算法流程分析
关于ROS中map、odom、base_link三个坐标系的理解