文章目录
1. cornerOptimization
void cornerOptimization()
{
// 更新点的位姿变换参数，使点从当前坐标系映射到全局地图坐标系
updatePointAssociateToMap();
// 使用多线程并行优化处理每个角点
#pragma omp parallel for num_threads(numberOfCores)
for (int i = 0; i < laserCloudCornerLastDSNum; i++)
{
PointType pointOri, pointSel, coeff; // 定义原始点、选中的点和优化系数
std::vector<int> pointSearchInd; // 存储最近邻点的索引
std::vector<float> pointSearchSqDis; // 存储最近邻点的距离平方
pointOri = laserCloudCornerLastDS->points[i]; // 获取当前点
pointAssociateToMap(&pointOri, &pointSel); // 将当前点映射到全局坐标系
kdtreeCornerFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis); // 查找最近的5个邻点
// 初始化协方差矩阵相关变量
cv::Mat matA1(3, 3, CV_32F, cv::Scalar::all(0)); // 协方差矩阵
cv::Mat matD1(1, 3, CV_32F, cv::Scalar::all(0)); // 特征值矩阵
cv::Mat matV1(3, 3, CV_32F, cv::Scalar::all(0)); // 特征向量矩阵
// 如果第五个最近邻点的距离小于1.0，则认为该点具有一定的局部平面结构
if (pointSearchSqDis[4] < 1.0) {
float cx = 0, cy = 0, cz = 0; // 初始化质心坐标
for (int j = 0; j < 5; j++) {
// 计算5个最近邻点的质心
cx += laserCloudCornerFromMapDS->points[pointSearchInd[j]].x;
cy += laserCloudCornerFromMapDS->points[pointSearchInd[j]].y;
cz += laserCloudCornerFromMapDS->points[pointSearchInd[j]].z;
}
cx /= 5; cy /= 5; cz /= 5; // 计算质心坐标
// 初始化协方差矩阵元素
float a11 = 0, a12 = 0, a13 = 0, a22 = 0, a23 = 0, a33 = 0;
for (int j = 0; j < 5; j++) {
// 计算每个邻点相对于质心的偏移量
float ax = laserCloudCornerFromMapDS->points[pointSearchInd[j]].x - cx;
float ay = laserCloudCornerFromMapDS->points[pointSearchInd[j]].y - cy;
float az = laserCloudCornerFromMapDS->points[pointSearchInd[j]].z - cz;
// 计算协方差矩阵的各个元素
a11 += ax * ax; a12 += ax * ay; a13 += ax * az;
a22 += ay * ay; a23 += ay * az;
a33 += az * az;
}
// 取平均值
a11 /= 5; a12 /= 5; a13 /= 5; a22 /= 5; a23 /= 5; a33 /= 5;
// 填充协方差矩阵
matA1.at<float>(0, 0) = a11; matA1.at<float>(0, 1) = a12; matA1.at<float>(0, 2) = a13;
matA1.at<float>(1, 0) = a12; matA1.at<float>(1, 1) = a22; matA1.at<float>(1, 2) = a23;
matA1.at<float>(2, 0) = a13; matA1.at<float>(2, 1) = a23; matA1.at<float>(2, 2) = a33;
// 对协方差矩阵进行特征值分解，提取特征值和特征向量
cv::eigen(matA1, matD1, matV1);
// 判断主方向特征值是否显著大于次方向特征值
if (matD1.at<float>(0, 0) > 3 * matD1.at<float>(0, 1)) {
// 定义点到直线的两端点坐标
float x0 = pointSel.x;
float y0 = pointSel.y;
float z0 = pointSel.z;
float x1 = cx + 0.1 * matV1.at<float>(0, 0);
float y1 = cy + 0.1 * matV1.at<float>(0, 1);
float z1 = cz + 0.1 * matV1.at<float>(0, 2);
float x2 = cx - 0.1 * matV1.at<float>(0, 0);
float y2 = cy - 0.1 * matV1.at<float>(0, 1);
float z2 = cz - 0.1 * matV1.at<float>(0, 2);
// 计算点到直线的垂直距离
float a012 = sqrt(((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) * ((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1))
+ ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1)) * ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))
+ ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1)) * ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1)));
// 计算直线的长度
float l12 = sqrt((x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2) + (z1 - z2)*(z1 - z2));
// 计算法向量的分量
float la = ((y1 - y2)*((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1))
+ (z1 - z2)*((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))) / a012 / l12;
float lb = -((x1 - x2)*((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1))
- (z1 - z2)*((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))) / a012 / l12;
float lc = -((x1 - x2)*((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))
+ (y1 - y2)*((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))) / a012 / l12;
// 点到直线的距离平方
float ld2 = a012 / l12;
// 计算权重因子
float s = 1 - 0.9 * fabs(ld2);
// 设置优化系数
coeff.x = s * la;
coeff.y = s * lb;
coeff.z = s * lc;
coeff.intensity = s * ld2;
// 如果权重因子足够大，保存该点用于优化
if (s > 0.1) {
laserCloudOriCornerVec[i] = pointOri;
coeffSelCornerVec[i] = coeff;
laserCloudOriCornerFlag[i] = true;
}
}
}
}
}
1. 点集中心计算
从给定点的 5 个最近邻点中，计算点的中心位置：
c
x
=
1
5
∑
j
=
1
5
x
j
,
c
y
=
1
5
∑
j
=
1
5
y
j
,
c
z
=
1
5
∑
j
=
1
5
z
j
c_x = \frac{1}{5} \sum_{j=1}^{5} x_j, \quad c_y = \frac{1}{5} \sum_{j=1}^{5} y_j, \quad c_z = \frac{1}{5} \sum_{j=1}^{5} z_j
cx=51j=1∑5xj,cy=51j=1∑5yj,cz=51j=1∑5zj
float cx = 0, cy = 0, cz = 0; // 初始化质心坐标
for (int j = 0; j < 5; j++) {
// 计算5个最近邻点的质心
cx += laserCloudCornerFromMapDS->points[pointSearchInd[j]].x;
cy += laserCloudCornerFromMapDS->points[pointSearchInd[j]].y;
cz += laserCloudCornerFromMapDS->points[pointSearchInd[j]].z;
}
cx /= 5; cy /= 5; cz /= 5; // 计算质心坐标
2. 协方差矩阵计算
对最近邻点的分布计算协方差矩阵
A
\mathbf{A}
A：
a
11
=
1
5
∑
j
=
1
5
(
x
j
−
c
x
)
2
,
a
12
=
1
5
∑
j
=
1
5
(
x
j
−
c
x
)
(
y
j
−
c
y
)
,
a
13
=
1
5
∑
j
=
1
5
(
x
j
−
c
x
)
(
z
j
−
c
z
)
a_{11} = \frac{1}{5} \sum_{j=1}^{5} (x_j - c_x)^2, \quad a_{12} = \frac{1}{5} \sum_{j=1}^{5} (x_j - c_x)(y_j - c_y), \quad a_{13} = \frac{1}{5} \sum_{j=1}^{5} (x_j - c_x)(z_j - c_z)
a11=51j=1∑5(xj−cx)2,a12=51j=1∑5(xj−cx)(yj−cy),a13=51j=1∑5(xj−cx)(zj−cz)
a
22
=
1
5
∑
j
=
1
5
(
y
j
−
c
y
)
2
,
a
23
=
1
5
∑
j
=
1
5
(
y
j
−
c
y
)
(
z
j
−
c
z
)
,
a
33
=
1
5
∑
j
=
1
5
(
z
j
−
c
z
)
2
a_{22} = \frac{1}{5} \sum_{j=1}^{5} (y_j - c_y)^2, \quad a_{23} = \frac{1}{5} \sum_{j=1}^{5} (y_j - c_y)(z_j - c_z), \quad a_{33} = \frac{1}{5} \sum_{j=1}^{5} (z_j - c_z)^2
a22=51j=1∑5(yj−cy)2,a23=51j=1∑5(yj−cy)(zj−cz),a33=51j=1∑5(zj−cz)2
协方差矩阵表示为：
A
=
[
a
11
a
12
a
13
a
12
a
22
a
23
a
13
a
23
a
33
]
\mathbf{A} = \begin{bmatrix} a_{11} & a_{12} & a_{13} \\ a_{12} & a_{22} & a_{23} \\ a_{13} & a_{23} & a_{33} \end{bmatrix}
A=
a11a12a13a12a22a23a13a23a33
// 初始化协方差矩阵元素
float a11 = 0, a12 = 0, a13 = 0, a22 = 0, a23 = 0, a33 = 0;
for (int j = 0; j < 5; j++) {
// 计算每个邻点相对于质心的偏移量
float ax = laserCloudCornerFromMapDS->points[pointSearchInd[j]].x - cx;
float ay = laserCloudCornerFromMapDS->points[pointSearchInd[j]].y - cy;
float az = laserCloudCornerFromMapDS->points[pointSearchInd[j]].z - cz;
// 计算协方差矩阵的各个元素
a11 += ax * ax; a12 += ax * ay; a13 += ax * az;
a22 += ay * ay; a23 += ay * az;
a33 += az * az;
}
// 取平均值
a11 /= 5; a12 /= 5; a13 /= 5; a22 /= 5; a23 /= 5; a33 /= 5;
// 填充协方差矩阵
matA1.at<float>(0, 0) = a11; matA1.at<float>(0, 1) = a12; matA1.at<float>(0, 2) = a13;
matA1.at<float>(1, 0) = a12; matA1.at<float>(1, 1) = a22; matA1.at<float>(1, 2) = a23;
matA1.at<float>(2, 0) = a13; matA1.at<float>(2, 1) = a23; matA1.at<float>(2, 2) = a33;
3. 特征值分解
对协方差矩阵
A
\mathbf{A}
A 进行特征值分解：
A
=
V
D
V
⊤
\mathbf{A} = \mathbf{V} \mathbf{D} \mathbf{V}^\top
A=VDV⊤
其中，
D
\mathbf{D}
D 是对角矩阵，其对角线上的值是特征值，
V
\mathbf{V}
V 是对应的特征向量。
补充： （1） 特征值与特征向量的定义
对于一个对称矩阵
A
∈
R
n
×
n
\mathbf{A} \in \mathbb{R}^{n \times n}
A∈Rn×n，特征值
λ
\lambda
λ 和对应特征向量
v
\mathbf{v}
v 满足以下关系：
A
v
=
λ
v
\mathbf{A} \mathbf{v} = \lambda \mathbf{v}
Av=λv
其中：
-
A
\mathbf{A}
A 是输入矩阵（如
matA1）。 - λ \lambda λ 是特征值。
- v \mathbf{v} v 是与 λ \lambda λ 对应的特征向量。
（2）特征值分解的公式
特征值分解可以表示为：
A
=
V
Λ
V
⊤
\mathbf{A} = \mathbf{V} \mathbf{\Lambda} \mathbf{V}^\top
A=VΛV⊤
其中：
-
Λ
\mathbf{\Lambda}
Λ 是对角矩阵，对角线上元素为特征值：
Λ = [ λ 1 0 ⋯ 0 0 λ 2 ⋯ 0 ⋮ ⋮ ⋱ ⋮ 0 0 ⋯ λ n ] \mathbf{\Lambda} = \begin{bmatrix} \lambda_1 & 0 & \cdots & 0 \\ 0 & \lambda_2 & \cdots & 0 \\ \vdots & \vdots & \ddots & \vdots \\ 0 & 0 & \cdots & \lambda_n \end{bmatrix} Λ= λ10⋮00λ2⋮0⋯⋯⋱⋯00⋮λn -
V
\mathbf{V}
V 是正交矩阵，其列为特征向量：
V = [ v 1 v 2 ⋯ v n ] \mathbf{V} = \begin{bmatrix} \mathbf{v}_1 & \mathbf{v}_2 & \cdots & \mathbf{v}_n \end{bmatrix} V=[v1v2⋯vn]
满足 V ⊤ V = I \mathbf{V}^\top \mathbf{V} = \mathbf{I} V⊤V=I，其中 I \mathbf{I} I 是单位矩阵。
（3） 数学推导
特征值分解的步骤：
-
特征值求解：
求解特征值 λ \lambda λ，需要解以下特征方程：
det ( A − λ I ) = 0 \det(\mathbf{A} - \lambda \mathbf{I}) = 0 det(A−λI)=0
其中， det ( ⋅ ) \det(\cdot) det(⋅) 表示行列式。 -
特征向量求解：
对于每个特征值 λ i \lambda_i λi，通过解以下方程得到对应的特征向量 v i \mathbf{v}_i vi：
( A − λ i I ) v i = 0 (\mathbf{A} - \lambda_i \mathbf{I}) \mathbf{v}_i = 0 (A−λiI)vi=0
（4）代码中分解结果的含义
在代码 cv::eigen(matA1, matD1, matV1) 中：
matA1: 输入矩阵 A \mathbf{A} A。matD1: 输出的特征值矩阵 Λ \mathbf{\Lambda} Λ，表示为：
Λ = [ λ 1 , λ 2 , λ 3 ] \mathbf{\Lambda} = [\lambda_1, \lambda_2, \lambda_3] Λ=[λ1,λ2,λ3]matV1: 输出的特征向量矩阵 V \mathbf{V} V，每列是一个特征向量：
V = [ v 11 v 12 v 13 v 21 v 22 v 23 v 31 v 32 v 33 ] \mathbf{V} = \begin{bmatrix} v_{11} & v_{12} & v_{13} \\ v_{21} & v_{22} & v_{23} \\ v_{31} & v_{32} & v_{33} \end{bmatrix} V= v11v21v31v12v22v32v13v23v33
其中， v 1 , v 2 , v 3 \mathbf{v}_1, \mathbf{v}_2, \mathbf{v}_3 v1,v2,v3 是 λ 1 , λ 2 , λ 3 \lambda_1, \lambda_2, \lambda_3 λ1,λ2,λ3 的特征向量。
对应代码：
// 对协方差矩阵进行特征值分解，提取特征值和特征向量
cv::eigen(matA1, matD1, matV1);
4. 主方向选择
主方向对应的特征值满足以下条件：
λ
1
>
3
λ
2
\lambda_1 > 3 \lambda_2
λ1>3λ2
原因分析：矩阵 A \mathbf{A} A 的特征值 λ 1 , λ 2 , λ 3 \lambda_1, \lambda_2, \lambda_3 λ1,λ2,λ3 表示局部点云分布在三个主方向上的方差大小。其中：
- λ 1 \lambda_1 λ1 表示点云在主方向上的扩展程度。
- λ 2 , λ 3 \lambda_2, \lambda_3 λ2,λ3 表示点云在次方向上的扩展程度。
条件解释： 如果 λ 1 \lambda_1 λ1 显著大于 λ 2 \lambda_2 λ2 和 λ 3 \lambda_3 λ3，例如满足 λ 1 > 3 λ 2 \lambda_1 > 3 \lambda_2 λ1>3λ2：
- 说明点云在 v 1 \mathbf{v}_1 v1 主方向上延展显著，而在其他方向上分布紧凑。
- 这种特征说明局部点云的形状更接近一条直线。在满足 λ 1 > 3 λ 2 \lambda_1 > 3 \lambda_2 λ1>3λ2 的条件下，局部点云形状可以近似为一条直线：
- 直线的方向：由主方向的特征向量 v 1 = ( v x 1 , v y 1 , v z 1 ) \mathbf{v}_1 = (v_{x1}, v_{y1}, v_{z1}) v1=(vx1,vy1,vz1) 确定。
- 直线的位置：由局部点云的质心 ( c x , c y , c z ) (c_x, c_y, c_z) (cx,cy,cz) 确定。
直线的表达形式为：
P
(
t
)
=
(
c
x
,
c
y
,
c
z
)
+
t
⋅
(
v
x
1
,
v
y
1
,
v
z
1
)
\mathbf{P}(t) = (c_x, c_y, c_z) + t \cdot (v_{x1}, v_{y1}, v_{z1})
P(t)=(cx,cy,cz)+t⋅(vx1,vy1,vz1)
其中
t
t
t 是直线上点的参数，表示点沿着方向向量
v
1
\mathbf{v}_1
v1 偏移的程度。
端点的计算逻辑： 在实际计算中，为了定义有限长度的直线段：
-
选择点云质心作为直线的中心点。
-
通过在质心两侧沿 v 1 \mathbf{v}_1 v1 方向各偏移固定距离 0.1 0.1 0.1 来定义两个端点 P 1 P_1 P1 和 P 2 P_2 P2。
-
端点 P 1 P_1 P1：在质心处沿 v 1 \mathbf{v}_1 v1 方向偏移正方向距离 0.1 0.1 0.1：
P 1 = ( c x + 0.1 v x 1 , c y + 0.1 v y 1 , c z + 0.1 v z 1 ) P_1 = (c_x + 0.1 v_{x1}, c_y + 0.1 v_{y1}, c_z + 0.1 v_{z1}) P1=(cx+0.1vx1,cy+0.1vy1,cz+0.1vz1) -
端点 P 2 P_2 P2：在质心处沿 v 1 \mathbf{v}_1 v1 方向偏移负方向距离 0.1 0.1 0.1：
P 2 = ( c x − 0.1 v x 1 , c y − 0.1 v y 1 , c z − 0.1 v z 1 ) P_2 = (c_x - 0.1 v_{x1}, c_y - 0.1 v_{y1}, c_z - 0.1 v_{z1}) P2=(cx−0.1vx1,cy−0.1vy1,cz−0.1vz1)
几何意义： -
v 1 \mathbf{v}_1 v1 表示直线的方向向量。
-
通过质心加减一定的偏移量（ 0.1 ⋅ v 1 0.1 \cdot \mathbf{v}_1 0.1⋅v1）即可定义直线的两个端点，从而表示局部点云拟合的直线。
-
λ 1 > 3 λ 2 \lambda_1 > 3 \lambda_2 λ1>3λ2 表明点云的形状趋向于一维分布。
-
使用主方向的特征向量 v 1 \mathbf{v}_1 v1 和质心 ( c x , c y , c z ) (c_x, c_y, c_z) (cx,cy,cz)，可以定义直线的方向和位置。
float x0 = pointSel.x; //中心点
float y0 = pointSel.y; //中心点
float z0 = pointSel.z; //中心点
float x1 = cx + 0.1 * matV1.at<float>(0, 0);//端点P1
float y1 = cy + 0.1 * matV1.at<float>(0, 1);//端点P1
float z1 = cz + 0.1 * matV1.at<float>(0, 2);//端点P1
float x2 = cx - 0.1 * matV1.at<float>(0, 0);//端点P2
float y2 = cy - 0.1 * matV1.at<float>(0, 1);//端点P2
float z2 = cz - 0.1 * matV1.at<float>(0, 2);//端点P2
5. 距离与权重计算
这里将点线的距离转化为点到面的距离 ：参考博客
平面由三个特定的点确定：
- 中心点 p 0 \mathbf{p}_0 p0：这是当前处理的角点，经过关联变换后的点，坐标为 ( x 0 , y 0 , z 0 ) (x_0, y_0, z_0) (x0,y0,z0)。
- 端点1 p 1 \mathbf{p}_1 p1：这是基于协方差矩阵的主特征向量方向延伸一定距离得到的点，坐标为 ( x 1 , y 1 , z 1 ) (x_1, y_1, z_1) (x1,y1,z1)。
- 端点2 p 2 \mathbf{p}_2 p2：这是与端点1 相反方向延伸得到的点，坐标为 ( x 2 , y 2 , z 2 ) (x_2, y_2, z_2) (x2,y2,z2)。
这三个点共同确定了一个几何平面，该平面用于后续的优化过程。具体来说：
- p 0 \mathbf{p}_0 p0 是当前处理的角点。
- p 1 \mathbf{p}_1 p1 和 p 2 \mathbf{p}_2 p2 是沿着主方向（由协方差矩阵的主特征向量确定）的两个端点，用于定义该平面的方向和位置。
通过这三个点，可以唯一确定一个平面，该平面用于评估和优化角点在地图中的位置。代码计算了由三个点 p 0 \mathbf{p}_0 p0, p 1 \mathbf{p}_1 p1, p 2 \mathbf{p}_2 p2 构成的三角形的面积和端点之间的距离。
-
计算三角形的面积 a 012 a_{012} a012：
计算公式：
a 012 = [ ( x 0 − x 1 ) ( y 0 − y 2 ) − ( x 0 − x 2 ) ( y 0 − y 1 ) ] 2 + [ ( x 0 − x 1 ) ( z 0 − z 2 ) − ( x 0 − x 2 ) ( z 0 − z 1 ) ] 2 + [ ( y 0 − y 1 ) ( z 0 − z 2 ) − ( y 0 − y 2 ) ( z 0 − z 1 ) ] 2 a_{012} = \sqrt{ \left[(x_0 - x_1)(y_0 - y_2) - (x_0 - x_2)(y_0 - y_1)\right]^2 + \left[(x_0 - x_1)(z_0 - z_2) - (x_0 - x_2)(z_0 - z_1)\right]^2 + \left[(y_0 - y_1)(z_0 - z_2) - (y_0 - y_2)(z_0 - z_1)\right]^2 } a012=[(x0−x1)(y0−y2)−(x0−x2)(y0−y1)]2+[(x0−x1)(z0−z2)−(x0−x2)(z0−z1)]2+[(y0−y1)(z0−z2)−(y0−y2)(z0−z1)]2
原因与用途：
这个公式实际上计算的是三角形 p 0 \mathbf{p}_0 p0, p 1 \mathbf{p}_1 p1, p 2 \mathbf{p}_2 p2 的两倍面积。通过向量的叉积公式，可以得到三角形面积的两倍。这个面积用于后续计算平面参数时的归一化处理，确保平面参数的稳定性和准确性。
float a012 = sqrt(((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) * ((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1))
+ ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1)) * ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))
+ ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1)) * ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1)));
-
计算两端点之间的距离 l 12 l_{12} l12：
计算公式：
l 12 = ( x 1 − x 2 ) 2 + ( y 1 − y 2 ) 2 + ( z 1 − z 2 ) 2 l_{12} = \sqrt{(x_1 - x_2)^2 + (y_1 - y_2)^2 + (z_1 - z_2)^2} l12=(x1−x2)2+(y1−y2)2+(z1−z2)2
原因与用途：
这个距离表示端点1和端点2之间的欧几里得距离。它用于归一化平面参数，确保平面方程的标准化，使得优化过程中的权重计算更加合理。平面的法向量 ( a , b , c ) (a, b, c) (a,b,c) 可以通过两个向量的叉积得到，这两个向量分别是 p 1 − p 0 \mathbf{p}_1 - \mathbf{p}_0 p1−p0 和 p 2 − p 0 \mathbf{p}_2 - \mathbf{p}_0 p2−p0。通过叉积，可以得到垂直于这两个向量的法向量。
向量 u = p 1 − p 0 = ( x 1 − x 0 , y 1 − y 0 , z 1 − z 0 ) \mathbf{u} = \mathbf{p}_1 - \mathbf{p}_0 = (x_1 - x_0, y_1 - y_0, z_1 - z_0) u=p1−p0=(x1−x0,y1−y0,z1−z0)
向量 v = p 2 − p 0 = ( x 2 − x 0 , y 2 − y 0 , z 2 − z 0 ) \mathbf{v} = \mathbf{p}_2 - \mathbf{p}_0 = (x_2 - x_0, y_2 - y_0, z_2 - z_0) v=p2−p0=(x2−x0,y2−y0,z2−z0)
则法向量为：
n = u × v = ∣ i j k x 1 − x 0 y 1 − y 0 z 1 − z 0 x 2 − x 0 y 2 − y 0 z 2 − z 0 ∣ \mathbf{n} = \mathbf{u} \times \mathbf{v} = \begin{vmatrix} \mathbf{i} & \mathbf{j} & \mathbf{k} \\ x_1 - x_0 & y_1 - y_0 & z_1 - z_0 \\ x_2 - x_0 & y_2 - y_0 & z_2 - z_0 \\ \end{vmatrix} n=u×v= ix1−x0x2−x0jy1−y0y2−y0kz1−z0z2−z0
展开后得到：
n = [ ( y 1 − y 0 ) ( z 2 − z 0 ) − ( z 1 − z 0 ) ( y 2 − y 0 ) ] i − [ ( x 1 − x 0 ) ( z 2 − z 0 ) − ( z 1 − z 0 ) ( x 2 − x 0 ) ] j + [ ( x 1 − x 0 ) ( y 2 − y 0 ) − ( y 1 − y 0 ) ( x 2 − x 0 ) ] k \mathbf{n} = \left[(y_1 - y_0)(z_2 - z_0) - (z_1 - z_0)(y_2 - y_0)\right] \mathbf{i} - \left[(x_1 - x_0)(z_2 - z_0) - (z_1 - z_0)(x_2 - x_0)\right] \mathbf{j} + \left[(x_1 - x_0)(y_2 - y_0) - (y_1 - y_0)(x_2 - x_0)\right] \mathbf{k} n=[(y1−y0)(z2−z0)−(z1−z0)(y2−y0)]i−[(x1−x0)(z2−z0)−(z1−z0)(x2−x0)]j+[(x1−x0)(y2−y0)−(y1−y0)(x2−x0)]k
由此可得：
a = ( y 1 − y 0 ) ( z 2 − z 0 ) − ( z 1 − z 0 ) ( y 2 − y 0 ) b = − [ ( x 1 − x 0 ) ( z 2 − z 0 ) − ( z 1 − z 0 ) ( x 2 − x 0 ) ] c = ( x 1 − x 0 ) ( y 2 − y 0 ) − ( y 1 − y 0 ) ( x 2 − x 0 ) a = (y_1 - y_0)(z_2 - z_0) - (z_1 - z_0)(y_2 - y_0) \\ b = -[(x_1 - x_0)(z_2 - z_0) - (z_1 - z_0)(x_2 - x_0)] \\ c = (x_1 - x_0)(y_2 - y_0) - (y_1 - y_0)(x_2 - x_0) \\ a=(y1−y0)(z2−z0)−(z1−z0)(y2−y0)b=−[(x1−x0)(z2−z0)−(z1−z0)(x2−x0)]c=(x1−x0)(y2−y0)−(y1−y0)(x2−x0)
标准化平面参数： 为了确保平面方程的标准化，使得优化过程中不同参数的影响力一致，需要将平面参数进行归一化。这里使用了之前计算的面积 a 012 a_{012} a012 和距离 l 12 l_{12} l12 来进行归一化处理。
标准化后的平面参数为：
a ′ = a s , b ′ = b s , c ′ = c s , d ′ = d s a' = \frac{a}{s}, \quad b' = \frac{b}{s}, \quad c' = \frac{c}{s}, \quad d' = \frac{d}{s} a′=sa,b′=sb,c′=sc,d′=sd
其中， s s s 是归一化因子，用于确保法向量的单位长度或其他标准化需求。
float la = ((y1 - y2)*((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1))
+ (z1 - z2)*((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))) / a012 / l12;
float lb = -((x1 - x2)*((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1))
- (z1 - z2)*((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))) / a012 / l12;
float lc = -((x1 - x2)*((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))
+ (y1 - y2)*((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))) / a012 / l12;
4 . 计算权重因子
计算权重因子 s s s，用于调整平面参数在优化过程中的影响力：
s = 1 − 0.9 ⋅ ∣ d ∣ x ori 2 + y ori 2 + z ori 2 4 s = 1 - 0.9 \cdot \frac{|d|}{\sqrt[4]{x_{\text{ori}}^2 + y_{\text{ori}}^2 + z_{\text{ori}}^2}} s=1−0.9⋅4xori2+yori2+zori2 ∣d∣
6. 优化目标
如果
s
>
0.1
s > 0.1
s>0.1，则更新点的优化目标系数：
coeff
=
(
s
⋅
l
a
,
s
⋅
l
b
,
s
⋅
l
c
,
s
⋅
l
d
2
)
\text{coeff} = (s \cdot l_a, s \cdot l_b, s \cdot l_c, s \cdot l_d^2)
coeff=(s⋅la,s⋅lb,s⋅lc,s⋅ld2)
2. surfOptimization
void surfOptimization()
{
// 更新点的位姿变换参数，将点从当前坐标系映射到全局地图坐标系
updatePointAssociateToMap();
// 使用多线程并行优化每个平面点
#pragma omp parallel for num_threads(numberOfCores)
for (int i = 0; i < laserCloudSurfLastDSNum; i++)
{
PointType pointOri, pointSel, coeff; // 定义原始点、映射点和优化系数
std::vector<int> pointSearchInd; // 最近邻点的索引
std::vector<float> pointSearchSqDis; // 最近邻点的平方距离
// 获取当前点并映射到全局坐标系
pointOri = laserCloudSurfLastDS->points[i];
pointAssociateToMap(&pointOri, &pointSel);
// 查找当前点的 5 个最近邻点
kdtreeSurfFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);
// 初始化平面拟合矩阵
Eigen::Matrix<float, 5, 3> matA0; // 最近邻点的坐标矩阵
Eigen::Matrix<float, 5, 1> matB0; // 方程右侧常数项
Eigen::Vector3f matX0; // 解向量，表示平面法向量
matA0.setZero();
matB0.fill(-1); // 初始化常数项为 -1
matX0.setZero();
// 如果第五个邻点的距离小于阈值，进行平面拟合
if (pointSearchSqDis[4] < 1.0)
{
// 构造邻点的坐标矩阵 matA0
for (int j = 0; j < 5; j++) {
matA0(j, 0) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].x;
matA0(j, 1) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].y;
matA0(j, 2) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].z;
}
// 求解平面方程的系数 pa, pb, pc
matX0 = matA0.colPivHouseholderQr().solve(matB0);
float pa = matX0(0, 0); // 平面方程法向量的 x 分量
float pb = matX0(1, 0); // 平面方程法向量的 y 分量
float pc = matX0(2, 0); // 平面方程法向量的 z 分量
float pd = 1; // 平面方程的常数项
// 对平面方程系数进行归一化
float ps = sqrt(pa * pa + pb * pb + pc * pc);
pa /= ps; pb /= ps; pc /= ps; pd /= ps;
// 验证邻点是否满足平面方程的误差阈值
bool planeValid = true;
for (int j = 0; j < 5; j++) {
if (fabs(pa * laserCloudSurfFromMapDS->points[pointSearchInd[j]].x +
pb * laserCloudSurfFromMapDS->points[pointSearchInd[j]].y +
pc * laserCloudSurfFromMapDS->points[pointSearchInd[j]].z + pd) > 0.2) {
planeValid = false;
break;
}
}
// 如果平面有效，计算当前点到平面的距离
if (planeValid) {
float pd2 = pa * pointSel.x + pb * pointSel.y + pc * pointSel.z + pd;
// 计算点的权重因子
float s = 1 - 0.9 * fabs(pd2) / sqrt(sqrt(pointOri.x * pointOri.x
+ pointOri.y * pointOri.y + pointOri.z * pointOri.z));
// 计算优化系数
coeff.x = s * pa;
coeff.y = s * pb;
coeff.z = s * pc;
coeff.intensity = s * pd2;
// 如果权重因子大于阈值，记录该点的优化信息
if (s > 0.1) {
laserCloudOriSurfVec[i] = pointOri;
coeffSelSurfVec[i] = coeff;
laserCloudOriSurfFlag[i] = true;
}
}
}
}
}
1. 平面方程拟合
通过邻点构造线性方程组：
[
x
1
y
1
z
1
x
2
y
2
z
2
x
3
y
3
z
3
x
4
y
4
z
4
x
5
y
5
z
5
]
[
a
b
c
]
=
[
−
1
−
1
−
1
−
1
−
1
]
\begin{bmatrix} x_1 & y_1 & z_1 \\ x_2 & y_2 & z_2 \\ x_3 & y_3 & z_3 \\ x_4 & y_4 & z_4 \\ x_5 & y_5 & z_5 \end{bmatrix} \begin{bmatrix} a \\ b \\ c \end{bmatrix} =\begin{bmatrix} -1 \\ -1 \\ -1 \\ -1 \\ -1 \end{bmatrix}
x1x2x3x4x5y1y2y3y4y5z1z2z3z4z5
abc
=
−1−1−1−1−1
记为
A
X
=
B
\mathbf{A} \mathbf{X} = \mathbf{B}
AX=B，使用最小二乘法求解：
X
=
(
A
⊤
A
)
−
1
A
⊤
B
\mathbf{X} = (\mathbf{A}^\top \mathbf{A})^{-1} \mathbf{A}^\top \mathbf{B}
X=(A⊤A)−1A⊤B
2. 平面方程归一化
求得的平面法向量
X
=
[
a
,
b
,
c
]
⊤
\mathbf{X} = [a, b, c]^\top
X=[a,b,c]⊤，需进行归一化：
p
a
=
a
a
2
+
b
2
+
c
2
,
p
b
=
b
a
2
+
b
2
+
c
2
,
p
c
=
c
a
2
+
b
2
+
c
2
,
p
d
=
1
a
2
+
b
2
+
c
2
p_a = \frac{a}{\sqrt{a^2 + b^2 + c^2}}, \quad p_b = \frac{b}{\sqrt{a^2 + b^2 + c^2}}, \quad p_c = \frac{c}{\sqrt{a^2 + b^2 + c^2}}, \quad p_d = \frac{1}{\sqrt{a^2 + b^2 + c^2}}
pa=a2+b2+c2
a,pb=a2+b2+c2
b,pc=a2+b2+c2
c,pd=a2+b2+c2
1
3. 点到平面的距离
点
P
(
x
,
y
,
z
)
P(x, y, z)
P(x,y,z) 到平面
p
a
x
+
p
b
y
+
p
c
z
+
p
d
=
0
p_a x + p_b y + p_c z + p_d = 0
pax+pby+pcz+pd=0 的距离：
d
=
p
a
x
+
p
b
y
+
p
c
z
+
p
d
d = p_a x + p_b y + p_c z + p_d
d=pax+pby+pcz+pd
4. 权重因子计算
定义权重因子
s
s
s，根据点到平面的距离和点的深度调整权重：
s
=
1
−
0.9
⋅
∣
d
∣
x
2
+
y
2
+
z
2
s = 1 - 0.9 \cdot \frac{|d|}{\sqrt{\sqrt{x^2 + y^2 + z^2}}}
s=1−0.9⋅x2+y2+z2
∣d∣
5. 优化系数
优化系数
(
coeff.x
,
coeff.y
,
coeff.z
,
coeff.intensity
)
(\text{coeff.x}, \text{coeff.y}, \text{coeff.z}, \text{coeff.intensity})
(coeff.x,coeff.y,coeff.z,coeff.intensity)：
coeff.x
=
s
⋅
p
a
,
coeff.y
=
s
⋅
p
b
,
coeff.z
=
s
⋅
p
c
,
coeff.intensity
=
s
⋅
d
\text{coeff.x} = s \cdot p_a, \quad \text{coeff.y} = s \cdot p_b, \quad \text{coeff.z} = s \cdot p_c, \quad \text{coeff.intensity} = s \cdot d
coeff.x=s⋅pa,coeff.y=s⋅pb,coeff.z=s⋅pc,coeff.intensity=s⋅d