前言
LOAM点云匹配部分极为经典,可以说是LOAM整个框架的核心,其运算速度快,精度高,自14年发布,并在后续拿下kitti冠军后,直到现在仍然被广泛使用,但在后续的推广中仍然有一些问题,这里做一些解析并记录下自己应用中的问题。
算法解析
算法流程
算法具体解析的文章实在太多了,这里不做赘述,只简单介绍下相关流程和原理,大致流程如下:
线/面特征与线/面地图的残差与对应优化向量计算
读过LOAM的都知道点云匹配部分是一个点到线ICP加一个点到面ICP,优化时可以理解为都是计算点到点的残差,只不过一个是在对应线上找到目标点,一个是在对应面上找到目标点,然后分别计算源点到目标点的残差,这样两种ICP的残差计算方程就统一了,后续计算雅克比矩阵时需要用到源点到目标点的距离(即残差)以及源点到目标点的单向量,具体为什么需要它们会在雅克比推导中给出。
- 当前帧线特征与地图线特征残差计算
对当帧线特征中的某个点A,依次从地图线特征中找到其最近的5个点,用这个5个点拟合一条直线L,计算A与L的距离D以及A垂直于L的向量V,该距离即为残差量,V为优化方向,保存下来待后续使用,对当帧线特征的所有点执行以上过程。 - 当前帧面特征与地图面特征残差计算
对当帧面特征中的某个点A,依次从地图线特征中找到其最近的5个点,用这个5个点拟合一个平面P,计算A与P的距离D以及A垂直于P的向量V,该距离即为残差量,V为优化方向,保存下来待后续使用,对当帧线特征的所有点执行以上过程。
后面的过程要了解下LM算法才能继续。
LM算法推导
LM算法的推导参考LOAM-LM方法
雅克比推导参考了LeGO-LOAM中的数学公式推导,这里我重新捋了一遍,并且推导了基于Tait-Bryan xyz extrinsic rotations的欧拉角的雅克比矩阵,原版LOAM中雅克比为基于Tait-Bryan zyx extrinsic rotations的欧拉角。
主体部分推导
重点在于每个点的残差计算方程即
f
i
(
X
)
f_i(X)
fi(X)以及它对系统状态
X
X
X的偏导数
f
i
′
(
X
)
f'_i(X)
fi′(X),其中
f
i
(
X
)
f_i(X)
fi(X)指每个点到线/面对应点的残差计算方程,
X
X
X指当前系统状态,即当前求解的位姿,由旋转R和平移T构成,表示为
x
,
y
,
z
,
r
o
l
l
,
p
i
t
c
h
,
y
a
w
x,y,z,roll,pitch,yaw
x,y,z,roll,pitch,yaw,ROS中,欧拉角为Tait-Bryan xyz extrinsic rotations类型,即绕固定轴,先绕x轴,再绕y轴,最后绕z轴旋转roll,pitch,yaw角度的方式,首先定义
f
i
(
X
)
f_i(X)
fi(X)的形式,我们先去掉i,就以单一一个点来推导:
p
w
=
G
(
p
l
,
X
)
=
R
p
l
+
T
f
(
X
)
=
D
(
p
w
,
p
t
)
p^w=G(p^l,X)=Rp^l+T\\[3mm] f(X)=D(p^w,p^t)
pw=G(pl,X)=Rpl+Tf(X)=D(pw,pt)
其中
D
(
p
w
,
p
t
)
D(p^w,p^t)
D(pw,pt)为两点间距离,
p
l
p^l
pl为当前帧的点在局部坐标系下坐标,
p
w
p^w
pw为当前帧的点在世界坐标系下坐标,
p
t
p^t
pt为对应线/面上目标点在世界坐标系的坐标。开始计算偏导:
∂
f
∂
X
=
∂
f
∂
G
(
p
l
,
X
)
∗
∂
G
(
p
l
,
X
)
∂
X
\frac{\partial f}{\partial X}=\frac {\partial f}{\partial G(p^l,X)}*\frac {\partial G(p^l,X)} {\partial X}
∂X∂f=∂G(pl,X)∂f∗∂X∂G(pl,X)
其中:
D
(
p
w
,
p
t
)
=
[
(
p
w
−
p
t
)
T
(
p
w
−
p
t
)
]
1
2
=
(
p
w
T
p
w
−
p
w
T
p
t
−
p
t
T
p
w
−
p
t
T
p
t
)
]
1
2
∂
f
∂
G
(
p
l
,
X
)
=
∂
D
(
p
w
,
p
t
)
∂
p
w
=
1
2
∗
2
(
p
w
−
p
t
)
D
(
p
w
,
p
t
)
=
(
p
w
−
p
t
)
.
n
o
r
m
l
i
z
e
(
)
=
V
D(p^w,p^t)=[(p^w-p^t)^T(p^w-p^t)]^{\frac {1}{2}}=(p^{wT}p^w-p^{wT}p^t-p^{tT}p^w-p^{tT}p^t)]^{\frac {1}{2}}\\[3mm] \frac {\partial f}{\partial G(p^l,X)}=\frac{\partial D(p^w,p^t)}{\partial p^w}=\frac {1}{2}*\frac {2(p^w-p^t)} {D(p^w,p^t)}=(p^w-p^t).normlize()=V
D(pw,pt)=[(pw−pt)T(pw−pt)]21=(pwTpw−pwTpt−ptTpw−ptTpt)]21∂G(pl,X)∂f=∂pw∂D(pw,pt)=21∗D(pw,pt)2(pw−pt)=(pw−pt).normlize()=V
可以看到,这个V就是两点间方向的单位向量,求残差时已经得到了。偏导剩余部分要分为对平移量T的求导和对欧拉角的求导两部分,先看平移部分:
∂
G
(
p
l
,
X
)
∂
T
=
∂
(
R
p
l
+
T
)
∂
T
=
E
\frac {\partial G(p^l,X)} {\partial T}=\frac {\partial (Rp^l+T)} {\partial T}=E
∂T∂G(pl,X)=∂T∂(Rpl+T)=E
旋转部分比较麻烦,以对roll的求导来看,我们用ex代替roll、ey代替pitch、ez代替yaw简化表示:
∂
G
(
p
l
,
X
)
∂
e
x
=
∂
(
R
p
l
+
T
)
∂
e
x
=
∂
R
∂
e
x
∗
p
l
\frac {\partial G(p^l,X)} {\partial ex}=\frac {\partial (Rp^l+T)} {\partial ex}=\frac {\partial R} {\partial ex}*p^l
∂ex∂G(pl,X)=∂ex∂(Rpl+T)=∂ex∂R∗pl
实际上就是就求旋转矩阵分别对roll、pitch、yaw的偏导。
∂ G ( p l , X ) ∂ e x = ∂ ( R p l + T ) ∂ e x = ∂ R ∂ e x ∗ p l \begin{aligned} \frac {\partial G(p^l,X)} {\partial ex}&=\frac {\partial (Rp^l+T)} {\partial ex}\\[3mm] &=\frac {\partial R} {\partial ex}*p^l \end{aligned} ∂ex∂G(pl,X)=∂ex∂(Rpl+T)=∂ex∂R∗pl
基于Tait-Bryan xyz extrinsic rotations的雅克比推导
不同类型的欧拉角表达方式其对应的旋转矩阵是不同的,可以参考Euler angles,中给出的结,这里直接贴出Tait-Bryan xyz extrinsic rotations的欧拉角对应的旋转矩阵
123对应ZYX,c为cos,s为sin,那么偏导就容易了:
∂
R
∂
e
x
=
[
0
c
z
s
y
c
x
+
s
x
s
z
s
z
c
x
−
c
z
s
x
s
y
)
0
−
c
z
s
x
+
s
z
s
y
c
x
−
s
x
s
z
s
y
−
c
z
c
x
0
c
y
c
x
−
c
y
s
x
)
]
∂
R
∂
e
y
=
[
−
c
z
s
y
c
z
c
y
s
x
c
z
c
x
c
y
)
−
s
y
s
z
s
z
c
y
s
x
c
x
s
z
c
y
−
c
y
−
s
y
s
x
−
s
y
c
x
)
]
∂
R
∂
e
z
=
[
−
s
z
c
y
−
s
z
s
y
s
x
−
c
x
c
z
c
z
c
x
c
y
)
c
y
c
z
−
s
z
c
x
+
c
z
s
y
s
x
c
x
c
z
s
y
+
s
z
s
x
0
0
0
)
]
\frac {\partial R} {\partial ex}=\begin{bmatrix}\\ 0 & c_zs_yc_x+s_xs_z & s_zc_x-c_zs_xs_y) \\ 0 & -c_zs_x+s_zs_yc_x & -s_xs_zs_y-c_zc_x \\ 0 & c_yc_x & -c_ys_x) \\ \end{bmatrix}\\[3mm] \frac {\partial R} {\partial ey}=\begin{bmatrix}\\ -c_zs_y & c_zc_ys_x & c_zc_xc_y) \\ -s_ys_z & s_zc_ys_x & c_xs_zc_y \\ -c_y & -s_ys_x & -s_yc_x) \\ \end{bmatrix}\\[3mm] \frac {\partial R} {\partial ez}=\begin{bmatrix}\\ -s_zc_y & -s_zs_ys_x-c_xc_z & c_zc_xc_y) \\ c_yc_z & -s_zc_x+c_zs_ys_x & c_xc_zs_y+s_zs_x \\ 0 & 0 & 0) \\ \end{bmatrix}\\[3mm]
∂ex∂R=⎣⎡000czsycx+sxsz−czsx+szsycxcycxszcx−czsxsy)−sxszsy−czcx−cysx)⎦⎤∂ey∂R=⎣⎡−czsy−sysz−cyczcysxszcysx−sysxczcxcy)cxszcy−sycx)⎦⎤∂ez∂R=⎣⎡−szcycycz0−szsysx−cxcz−szcx+czsysx0czcxcy)cxczsy+szsx0)⎦⎤
于是:
∂
f
∂
X
=
∂
f
∂
G
(
p
l
,
X
)
∗
∂
G
(
p
l
,
X
)
∂
X
=
V
∗
[
∂
R
∂
e
x
∗
p
l
,
∂
R
∂
y
∗
p
l
,
∂
R
∂
z
∗
p
l
,
E
]
J
=
[
∂
f
0
∂
X
T
,
∂
f
1
∂
X
T
,
∂
f
2
∂
X
T
,
.
.
.
,
∂
f
n
∂
X
T
]
\begin{aligned} \frac{\partial f}{\partial X}&=\frac {\partial f}{\partial G(p^l,X)}*\frac {\partial G(p^l,X)} {\partial X}\\[3mm] &=V*[\frac {\partial R} {\partial ex}*p^l, \frac {\partial R} {\partial y}*p^l, \frac {\partial R} {\partial z}*p^l, E] \end{aligned}\\[3mm] J=[{\frac{\partial f_0}{\partial X}}^T,{\frac{\partial f_1}{\partial X}}^T,{\frac{\partial f_2}{\partial X}}^T,...,{\frac{\partial f_n}{\partial X}}^T]
∂X∂f=∂G(pl,X)∂f∗∂X∂G(pl,X)=V∗[∂ex∂R∗pl,∂y∂R∗pl,∂z∂R∗pl,E]J=[∂X∂f0T,∂X∂f1T,∂X∂f2T,...,∂X∂fnT]
至此,雅克比就被求解了,剩下的套LM公式就好。
代码剖析
代码太多了,而且很多文字都对应剖析了,这里就提下雅克比部分就好:
//求roll、pitch、yaw对应的sin和cos,用以求上面提到R对它们的求导
float srx = _transformTobeMapped.rot_x.sin();
float crx = _transformTobeMapped.rot_x.cos();
float sry = _transformTobeMapped.rot_y.sin();
float cry = _transformTobeMapped.rot_y.cos();
float srz = _transformTobeMapped.rot_z.sin();
float crz = _transformTobeMapped.rot_z.cos();
size_t laserCloudSelNum = _laserCloudOri.size();
if (laserCloudSelNum < 50)
continue;
Eigen::Matrix<float, Eigen::Dynamic, 6> matA(laserCloudSelNum, 6);
Eigen::Matrix<float, 6, Eigen::Dynamic> matAt(6, laserCloudSelNum);
Eigen::Matrix<float, 6, 6> matAtA;
Eigen::VectorXf matB(laserCloudSelNum);
Eigen::VectorXf matAtB;
Eigen::VectorXf matX;
//对每个点依次求解
for (int i = 0; i < laserCloudSelNum; i++)
{
//文中提到p^l
pointOri = _laserCloudOri.points[i];
//源点到目标点的方向单位向量,即V
coeff = _coeffSel.points[i];
//雅克比中对roll求导部分
float arx = (crx*sry*srz*pointOri.x + crx * crz*sry*pointOri.y - srx * sry*pointOri.z) * coeff.x
+ (-srx * srz*pointOri.x - crz * srx*pointOri.y - crx * pointOri.z) * coeff.y
+ (crx*cry*srz*pointOri.x + crx * cry*crz*pointOri.y - cry * srx*pointOri.z) * coeff.z;
//雅克比中对pitch求导部分
float ary = ((cry*srx*srz - crz * sry)*pointOri.x
+ (sry*srz + cry * crz*srx)*pointOri.y + crx * cry*pointOri.z) * coeff.x
+ ((-cry * crz - srx * sry*srz)*pointOri.x
+ (cry*srz - crz * srx*sry)*pointOri.y - crx * sry*pointOri.z) * coeff.z;
//雅克比中对yaw求导部分
float arz = ((crz*srx*sry - cry * srz)*pointOri.x + (-cry * crz - srx * sry*srz)*pointOri.y)*coeff.x
+ (crx*crz*pointOri.x - crx * srz*pointOri.y) * coeff.y
+ ((sry*srz + cry * crz*srx)*pointOri.x + (crz*sry - cry * srx*srz)*pointOri.y)*coeff.z;
matA(i, 0) = arx;
matA(i, 1) = ary;
matA(i, 2) = arz;
雅克比中对x,y,z求导部分直,V*E等于V
matA(i, 3) = coeff.x;
matA(i, 4) = coeff.y;
matA(i, 5) = coeff.z;
//残差
matB(i, 0) = -coeff.intensity;
}
matAt = matA.transpose();
matAtA = matAt * matA;
matAtB = matAt * matB;
//求解LM的更新公式JT*J*x=-JT*f
matX = matAtA.colPivHouseholderQr().solve(matAtB);
//作者实际实现的是高斯-牛顿法,可能出现matATA奇异的问题,这里做了一些防止退化的操作,有专门论文讲解,原理是根据特征值和特征向量判断出更新量的向量空间,认为特征值很小的基上的更新量是不可靠的,因此会干掉该分量,有空再写文章记录。
if (iterCount == 0)
{
Eigen::Matrix<float, 1, 6> matE;
Eigen::Matrix<float, 6, 6> matV;
Eigen::Matrix<float, 6, 6> matV2;
Eigen::SelfAdjointEigenSolver< Eigen::Matrix<float, 6, 6> > esolver(matAtA);
matE = esolver.eigenvalues().real();
matV = esolver.eigenvectors().real();
//这儿有个bug,eigen特征向量是列向量,处理是按照行向量处理的
matV2 = matV;
isDegenerate = false;
float eignThre[6] = { 100, 100, 100, 100, 100, 100 };
for (int i = 0; i < 6; i++)
{
if (matE(0, i) < eignThre[i])
{
for (int j = 0; j < 6; j++)
{
matV2(i, j) = 0;
}
isDegenerate = true;
}
else
{
break;
}
}
matP = matV.inverse() * matV2;
}
if (isDegenerate)
{
Eigen::Matrix<float, 6, 1> matX2(matX);
matX = matP * matX2;
}
//更新状态量
_transformTobeMapped.rot_x += matX(0, 0);
_transformTobeMapped.rot_y += matX(1, 0);
_transformTobeMapped.rot_z += matX(2, 0);
_transformTobeMapped.pos.x() += matX(3, 0);
_transformTobeMapped.pos.y() += matX(4, 0);
_transformTobeMapped.pos.z() += matX(5, 0);
//单词更新小于阈值,则判断收敛
float deltaR = sqrt(pow(rad2deg(matX(0, 0)), 2) +
pow(rad2deg(matX(1, 0)), 2) +
pow(rad2deg(matX(2, 0)), 2));
float deltaT = sqrt(pow(matX(3, 0) * 100, 2) +
pow(matX(4, 0) * 100, 2) +
pow(matX(5, 0) * 100, 2));
if (deltaR < _deltaRAbort && deltaT < _deltaTAbort)
break;
}