责任编辑受上面三篇昌明的启迪颇深：
LOAM，LeGO-LOAM，和LIO-SAM在上面三个难题上采用了基本上完全一致的思路：
从扫描器点云抽取特点点云。都是依照扫描器线的截面抽取Corner和surface三种特点点云；特点点云到Map的相匹配强化。都是采用完全相同的经济损失表达式算式，因此都采用柯西开普勒法展开插值强化。责任编辑对第三个难题的基本原理展开详尽的预测。责任编辑从开发人员的视角对解法展开了精简，以期于认知。
一、位姿切换G()
在LOAM中，在展开扫描器点云与Map的相匹配以后，早已对扫描器点云展开了帧内标定和特点抽取，抽取到Corner和Surface三种特点点云，接着就须要将特点点云与Map展开相匹配，找出扫描器点云的最优化体位切换，将扫描器点云切换到世界极座标（即Map极座标），使扫描器点云正式成为Map的一小部份。
他们假设表达式G(T)是那个最优化体位切换，能将声纳极座标中的点X变为世界极座标的点X^w，与Map中特点边沿和正方形的距为0，则有：
X^w=G(T)=R*X+t \qquad (1)
当中T是体位，包涵转动R和移位t两部份，当中R用环的则表示，rx，ry，rz依次则表示环的的roll,pitch,yaw
T=(R,t\space)=(rx, ry, rz, tx, ty, tz)^T \qquad (2)
当中X则表示声纳极座标下某一特点点（corner或surface）的座标
X=(px,py,pz)^T \qquad (3)
特别注意：这儿T才是待解的未明表达式，而X则是二个特点点的座标，是未知物理量
当中转动行列式R表述如下表所示，采用的是环的则表示形式，s则表示sin，c则表示cos，比如cry则表示cos(ry)。
R=R(rx,ry,rz)= \begin{bmatrix} cry⋅crz+sry⋅srx⋅srz & crz⋅sry⋅srx-cry⋅srz & crx⋅sry \\ crx⋅srz & crx⋅crz & -srx \\ cry⋅srx⋅srz-crz⋅sry & cry⋅crz⋅srx+sry⋅srz & cry⋅crx \\ \end{bmatrix} \qquad (4)
当中移位t的表述如下表所示：
t = (tx, ty, tz)^T \qquad (5)
二、经济损失表达式D()
为了对体位切换G(.)展开强化，他们须要表述经济损失表达式（即误差）：未知Map极座标中的点，求该点与Map的距d，将那个距表述为表达式D()
loss=d=D(X^w,Map) \qquad (6)
将公式1和公式6结合起来：
loss=d=D(X^w,Map)=D(G(T),Map)=D(R*X+t,Map) \qquad (7)
特别注意，对于经济损失表达式D()，切换T(R,t)才是通过经济损失表达式解的表达式，X是未知特点点云中每个点的座标
代码中corner特点点云对应的经济损失表达式D()是求特点点云p点到Map直线ab的距，即po的长度，如下表所示图所示：
面积法求空间点p到直线ab的距解采用的是面积法，即首先用pa和pb的叉积求得平行四边形paqb的面积，接着除以线段ab的长度，就得到了po的长度。
三、经济损失表达式对体位分量求偏导
为了使用柯西开普勒法对代价表达式D()展开插值强化，须要求代价表达式D()对T(R,t)中每个体位分量的偏导，总共有6个体位分量：rx，ry，rz，tx, ty, tz。
误差对转动求偏导的过程，以rx为例：
\frac{∂loss}{∂rx} =\frac{∂D(G(T),Map)}{∂rx} \\ =\frac{∂D}{∂G}*\frac{∂G}{∂rx} \\ =\frac{∂D}{∂G}*\frac{∂(R*X+t)}{∂rx} \\ =\frac{∂D}{∂G}*\frac{∂(R*X)}{∂rx}+\frac{∂D}{∂G}*\frac{∂(t)}{∂rx} \\ =\frac{∂D}{∂G}*\frac{∂(R*X)}{∂rx} \qquad (8)
误差对移位求偏导的过程，以tx为例： \frac{∂loss}{∂tx} =\frac{∂D(G(T),Map)}{∂tx} =\frac{∂D}{∂G}*\frac{∂G}{∂tx} \\ =\frac{∂D}{∂G}*\frac{∂(R*X+t\space)}{∂tx}\\ =\frac{∂D}{∂G}*\frac{∂(R*X\space)}{∂tx}+\frac{∂D}{∂G}*\frac{∂(t\space)}{∂tx}\\ =\frac{∂D}{∂G} \qquad (9)
公式8和公式9中都出现了∂D/∂G，结合公式1和公式6，可知：
\frac{∂D}{∂G}=\frac{∂d}{∂X^w} =\frac{∂D(X^w,Map)}{∂X^w} \qquad (10)
d是点到Map的距，X^w是一个点，距对点求导，能认知为求一个点的移动方向，在那个方向上移动d减小的最快，显然是垂线方向。
故这部份，针对corner点，他们能直接用点到与之相匹配的直线之间的垂线方向对应的向量则表示，针对surface点，他们能直接用点到与之相匹配的正方形之间的垂线方向对应的向量(也是正方形的法向量)则表示。假设垂线对应的向量为（la, lb, lc），则有：
\frac{∂D}{∂G}=\frac{∂d}{∂X^w} =(\frac{∂d}{∂x},\frac{∂d}{∂y},\frac{∂d}{∂z}) =(la,lb,lc) \qquad (11)
结合公式3，公式8和公式11
\frac{∂loss}{∂rx}=(la,lb,lc)*\frac{∂(R*X)}{∂rx}\\ =(la,lb,lc)*\frac{∂(R)}{∂rx}*X\\ =(la,lb,lc)*\frac{∂(R)}{∂rx}*(px,py,pz)^T\\ =(la,lb,lc)* \begin{bmatrix} sry*crx*srz & crz*sry*crx & -srx*sry \\ -srx*srz & -srx*crz & -crx \\ cry*crx*srz & cry*crz*crx & -cry*srx \\ \end{bmatrix} *(px,py,pz)^T \qquad (12)
∂loss/∂rx是代码中的arx，同理，能依次计算出经济损失表达式对ry、rz的偏导数，对应代码中的ary和arz。
结合公式9和公式11可知，经济损失表达式对移位tx，ty，tz的偏导数是la，lb，lc。
至此，代价表达式D()对每个体位分量的偏导数都有了：
\frac{∂loss}{∂T}=(arx, ary, arz, la, lb, lc) \qquad (13)
四、正规方程
柯西开普勒法的核心是通过正规方程解插值增量Δx：
J^TJ\space\Delta x=-J^Tf(x) \qquad (14)
正规方程中的x是他们要强化的体位T，而Δx则是须要通过柯西开普勒法解的体位增量。
正规方程中的J是经济损失表达式loss关于体位T的各个分量的偏导数，即雅可比行列式，对应于代码中的matA，是一个m×n的行列式，当中m是特点点云中点的总数（即观测值的总数），n则表示待强化表达式（即体位分量）的总数，这儿是6。将特点点云中每一个点的座标带入经济损失表达式、公式11和公式12，得到一组偏导数的数值，编组在一起，就得到如下表所示的雅克比行列式：
J = \begin{bmatrix} arx_0 & ary_0 & arz_0 & la_0 & lb_0 & lc_0 \\ arx_1 & ary_1 & arz_1 & la_1 & lb_1 & lc_1 \\ arx_2 & ary_2 & arz_2 & la_2 & lb_2 & lc_2 \\ \qquad…… \\ arx_m & ary_m & arz_m & la_m & lb_m & lc_m \\ \end{bmatrix} \qquad (15)
正规方程中的f(x)是经济损失表达式的结果，即点到Map的距，对应于代码中的matB，是一个m×1的行列式。将特点点云中的每一个点的座标带入经济损失表达式，得到一个结果，编组在一起，就得到f(x)：
f(x)=\begin{bmatrix} loss_0 \\ loss_1 \\ loss_2 \\ …… \\ loss_m \\ \end{bmatrix} \qquad (16)
五、插值强化
解的过程如下表所示：
基于前一状态的体位，和IMU积分的结果，对体位T展开估计，获得初始值；基于体位T的前一状态值，带入特点点云的每一个点的座标，求得雅克比行列式J和f(x)；计算出雅可比行列式的转置J^T，计算出正规方程AX=B中的A和B；解线性方程AX=B中的X，即Δx；更新体位T：T=T+Δx；重复2~5，直至Δx中的转动和移位增量小于某一阈值。最后，想了解更多体系的相关课程学习，推荐深蓝学院SLAM的专门课程：
