前言

上一篇博客中地图栅格化处理与提取完成了从整个地图中提取出要和当前帧做匹配的局部地图

下面要做的就是要构建角点约束和面点约束

后端的构建约束问题和前端不一样。原因就是前端从上一帧上去找，而后端是在局部地图上找，点要多很多，并且没有了线束信息，所以原理上不一样了。

线特征的提取
通过kdtree在局部地图中找到5个最近的线特征，为了判断他们是否符合线特征的特性，需要对5个点构成的协方差矩阵进行特征值分解，当上述5个点在一条直线上时，他们只有一个主方向，也就是特征值是一个大特征值，以及两个小特征值，大特征值对应的特征向量就是对应直线的方向向量。

面特征的提取
通过kdtree在地图中找到最近的面特征也是5个， 理论上也可以通过特种值分解的方式，最小的特征值对应的特征向量就是平面的法向量， 不过代码里选用的是平面拟合的方式。

平面方程为 Ax+By+Cz+D=0，D可以当成1（相当于等号两边分别乘1/D），也就是3个未知数，5个方程。写成矩阵的形式，可以构成5*3的矩阵，线性方程组，可以求出解。得到结果后，验证的方法为分别求出5个点到平面的距离，如果太远，则说明平面拟合不成功。

代码分析

代码在lasermapping.cpp中
上一篇博客中地图栅格化处理与提取，得到了局部地图，继续代码的分析

			if (laserCloudCornerFromMapNum > 10 && laserCloudSurfFromMapNum > 50)//角点和面点个数满足要求
			{
1
2

局部地图中的有效点云数目进行判断
局部地图中的角点和面点不能太少，要求

• 角点不少于10个
• 面点不少于50个

				kdtreeCornerFromMap->setInputCloud(laserCloudCornerFromMap);
				kdtreeSurfFromMap->setInputCloud(laserCloudSurfFromMap);
				printf("build tree time %f ms \n", t_tree.toc());//打印建立kd-tree的时间
1
2
3

把局部地图的角点和面点 送入KD tree 之后进行最近邻搜索
通过pcl建立kd-tree 还是比较耗时的

之后进入优化的迭代

				for (int iterCount = 0; iterCount < 2; iterCount++)//迭代两次
				{
1
2

建立对应关系的迭代次数不超过2次

					ceres::LossFunction *loss_function = new ceres::HuberLoss(0.1);//生成损失函数
					ceres::LocalParameterization *q_parameterization =
						new ceres::EigenQuaternionParameterization();// 添加四元数得参数模块
					ceres::Problem::Options problem_options;//声明ceres的 优化问题

					ceres::Problem problem(problem_options);// 构建问题
					problem.AddParameterBlock(parameters, 4, q_parameterization);//添加四元数得参数模块
					problem.AddParameterBlock(parameters + 4, 3);//平移的参数块，parameters+4就是上面四元数后的指针便宜，3就是平移的参数个数
1
2
3
4
5
6
7
8

建立ceres问题
建立参数块
这边和帧间里程计类似，就是参数块的建立有些区别，帧间是用了四元数和平移两个数组，这边用的parameters一个数组。
前4个是四元数，后三个是平移

下面开始构建角点约束

构建角点约束

					for (int i = 0; i < laserCloudCornerStackNum; i++)
					{
1
2

循环上一帧（滤波后）的每个角点

						pointOri = laserCloudCornerStack->points[i];//取出该点
						// 把当前点根据初值投影到地图坐标系下去
						pointAssociateToMap(&pointOri, &pointSel);
1
2
3

取出该点
把当前点根据初值投影到地图坐标系下去
投影的方法就是用当前的初始位姿估计，做位姿变换（乘四元数加平移）

						// 地图中寻找和该点最近的5个点
						kdtreeCornerFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis); 
1
2

通过kd-tree在局部地图中寻找和该点最近的5个点
点的索引在 pointSearchInd 数组中
点的距离在 pointSearchSqDis 数组中

						// 判断最远的点的距离不能超过1m，否则就是无效约束
						if (pointSearchSqDis[4] < 1.0)
						{ 
1
2
3

判断最远的点的距离不能超过1m，否则就是无效约束

							std::vector<Eigen::Vector3d> nearCorners;//5个最近点构成的数组
							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);//构建5个点的数组
							}
							//计算这5个点的均值
							center = center / 5.0;
1
2
3
4
5
6
7
8
9
10
11
12

由这5个点构成一个数组，并计算中心点

							//构建协方差矩阵
							for (int j = 0; j < 5; j++)
							{
								Eigen::Matrix<double, 3, 1> tmpZeroMean = nearCorners[j] - center;//每个点减去均值
								covMat = covMat + tmpZeroMean * tmpZeroMean.transpose();
							}
1
2
3
4
5
6

构建协方差矩阵
方法就是：

• 每个点减去均值
• 各自点向量乘上各自点向量的转置，然后加在一起

							//进行特征值分解
							Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> saes(covMat);
1
2

通过Eigen的特征值分解器进行特征值分解

          Eigen::Vector3d unit_direction = saes.eigenvectors().col(2);
1

分解后特征向量是根据特征值大小，由大到小排列，这里取出最后一列的特征向量，即线的方向

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

最大特征值大于次特征值的3倍，认为该5个点构成的是一条线，满足线特征

								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 约束一致
1
2
3
4
5
6

根据拟合出来的线特征方向，以平均点为中心，建立两个虚拟点

								// 构建角点约束，和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++;	
1
2
3
4

构建角点约束，和lidar odom 约束一致

构建面点约束

下面是构建面点约束

					for (int i = 0; i < laserCloudSurfStackNum; i++)
					{
1
2

循环上一帧（滤波后）的每个面点

						pointOri = laserCloudSurfStack->points[i];//取出该点
						//double sqrtDis = pointOri.x * pointOri.x + pointOri.y * pointOri.y + pointOri.z * pointOri.z;
						pointAssociateToMap(&pointOri, &pointSel);//投影到局部地图中
						kdtreeSurfFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);//从kd-tree中找到5个最近邻的面点
1
2
3
4

把该投影到局部地图中，从kd-tree中找到5个最近邻的面点

下面开始构建方程，方程如下：

						Eigen::Matrix<double, 5, 3> matA0;//等号左边5*3的矩阵
						Eigen::Matrix<double, 5, 1> matB0 = -1 * Eigen::Matrix<double, 5, 1>::Ones();//等号右边5*1的矩阵
1
2

声明等号左边和右边的矩阵

						//构建 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::Vector3d norm = matA0.colPivHouseholderQr().solve(matB0);
							// 法向量模的倒数
							double negative_OA_dot_norm = 1 / norm.norm();
							//法向量归一化
							norm.normalize();
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18

最远的面点满足条件后，开始构建 Ax+By+Cz+1=0 的平面方程
然后通过构建超定方程来求解这个平面方程

用的Eigen 的colPivHouseholderQr 求解方法，结果 norm 就是[A B C ]
也就是平面的法向量

下面通过点到平面的距离来看是否符合平面约束

							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;
								}
							}
1
2
3
4
5
6
7
8
9
10
11
12
13

代码里好像和公式不太一样，其实是一样的
首先法向量已经被归一化了，所以分母部分已经是1了
D就成了上面求得法向量模的倒数
然后距离和0.2m做判断，大于0.2m则认为平面拟合不成功

							Eigen::Vector3d curr_point(pointOri.x, pointOri.y, pointOri.z);//取出该帧的点
							//如果平面有效，则添加面点的平面约束
							if (planeValid)
							{
								//利用平面方程构建约束，和前端有所不同
								ceres::CostFunction *cost_function = LidarPlaneNormFactor::Create(curr_point, norm, negative_OA_dot_norm);
								problem.AddResidualBlock(cost_function, loss_function, parameters, parameters + 4);
								surf_num++;
							}
1
2
3
4
5
6
7
8
9

如果平面有效，则添加面点的平面约束
利用平面方程构建约束，和前端有所不同

总结

• 对局部地图的角度和面点个数进行判断（是满足优化条件）

• 局部地图构建KD-tree

• 构建ceres问题与参数块

• 构建角点约束
  a通过kd-tree找到5个最近邻角点
  b通过特征值分解求线的方向
  c通过最大特征值与次特征值的大小判断是否满足线特征
  d以中心点与线的方向构建2个虚拟点
  e构建角点约束（当前点和2个虚拟点）

• 构建面点约束
  a通过kd-tree找到5个最近邻面点
  b通过求解平面方程得到法向量
  c通过5个点到平面的距离判断是否满足面特征
  d构建面点约束