lio-sam做的是scan-map，点变到世界系下，优化本帧在是世界系下的位姿，和loam有所不同。
符号：
本帧特征点云（相对机体系）
P
s
c
a
n
l
i
d
a
r
P_{scan}^{lidar}
Pscanlidar ， 本帧点云变换至世界系
P
s
c
a
n
P_{scan}
Pscan，局部地图中匹配的点云
P
m
a
p
P_{map}
Pmap（世界系），位姿
X
=
{
R
,
T
}
X=\{R,T\}
X={R,T}，点到对应线、面的距离残差
f
(
X
)
f(X)
f(X)，围绕机体
x
x
x轴旋转的角度为
r
x
rx
rx，
s
i
n
(
r
x
)
、
c
o
s
(
r
x
)
sin(rx)、cos(rx)
sin(rx)、cos(rx)记作
s
x
、
c
x
s_x、c_x
sx、cx，其他轴同理。
坐标变换关系：
待优化的误差函数：
其实没有
P
m
a
p
P_{map}
Pmap这个点，误差函数是点到线、面的距离，这里认为
P
m
a
p
P_{map}
Pmap是线、面上垂直
P
s
c
a
n
P_{scan}
Pscan的点（后面和
P
m
a
p
P_{map}
Pmap没有关系）。
（打错了，是
f
=
d
i
s
t
(
P
s
c
a
n
,
P
m
a
p
)
f=dist(P_{scan},P_{map})
f=dist(Pscan,Pmap) ）
误差函数的雅可比
J
J
J
1、分步求导第一项：
即为特征点到对应线面的距离的单位向量
[
l
a
,
l
b
,
l
c
]
[la,lb,lc]
[la,lb,lc]（行向量），在求误差
f
f
f时，便已经得到，存放在coeff.xyz中。
2、分步求导第二项：
（应该是
[
r
x
,
r
y
,
r
z
,
x
,
y
,
z
]
[rx,ry,rz,x,y,z]
[rx,ry,rz,x,y,z]，下面写法括号里的
T
T
T不该带转置的）
R
R
R是机体在世界系下的位姿，先围绕
z
z
z轴旋转的角度为
r
z
rz
rz，后围绕
x
x
x轴旋转的角度为
r
x
rx
rx，最后围绕
y
y
y轴旋转的角度为
r
y
ry
ry，
R
=
R
y
R
x
R
z
R=RyRxRz
R=RyRxRz， 表示为（推导过程可以见附录中wiki截图）：
2.1 我们先只看对
r
x
rx
rx的求导部分：
其中，
P
s
c
a
n
P_{scan}
Pscan即点的坐标，存储在pointOri.xyz，这一块的雅可比为：
对应代码中的：
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;
2.2
r
y
,
r
z
ry,rz
ry,rz略过，对
T
T
T的求导部分为：
这一块的雅可比为：
对应代码中的：
matA(i, 3) = coeff.x;
matA(i, 4) = coeff.y;
matA(i, 5) = coeff.z;
3、最后把3个旋转，和T（x，y，z）的雅可比拼凑起来，就得到该特征点对应到1X6的雅可比矩阵：
附录：
参考1 2 3；
和参考123有些差异的地方，其中的
R
R
R如下，为啥差着负号，还没搞懂，没看过loam源码，估计是优化的R和计算误差时用的R是逆的关系，优化用
R
t
−
1
t
R_{t-1}^t
Rt−1t，误差函数是当前帧点变到上一帧
R
t
t
−
1
R_{t}^{t-1}
Rtt−1，但最后优化结果直接加在
R
t
−
1
w
o
r
l
d
R_{t-1}^{world}
Rt−1world上了（也就是
R
t
t
−
1
R_{t}^{t-1}
Rtt−1）？不知道不知道…
这里附上wiki中的公式:
标量对列向量求导（参考1中有误）：
代码：
// 求roll、pitch、yaw对应的sin和cos
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();
Eigen::Matrix<float, Eigen::Dynamic, 6> matA(laserCloudSelNum, 6); // J
Eigen::Matrix<float, 6, Eigen::Dynamic> matAt(6, laserCloudSelNum); // JT
Eigen::Matrix<float, 6, 6> matAtA; // JT*J
Eigen::VectorXf matB(laserCloudSelNum); // f
Eigen::VectorXf matAtB; // JT*f
Eigen::VectorXf matX; // delta
// 对每个点依次构建雅克比
for (int i = 0; i < laserCloudSelNum; i++)
{
// scan中每个特征点的坐标（当前雷达系下）
pointOri = _laserCloudOri.points[i];
// scan中特征点到map中对应点的距离的方向单位向量（即误差在xyz方向上的单位分量）
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;
// 雅克比中对roll，pitch，yaw求导部分值
matA(i, 0) = arx;
matA(i, 1) = ary;
matA(i, 2) = arz;
//雅克比中对x，y，z求导部分值
matA(i, 3) = coeff.x;
matA(i, 4) = coeff.y;
matA(i, 5) = coeff.z;
// 残差f
matB(i, 0) = -coeff.intensity;
}
matAt = matA.transpose();
matAtA = matAt * matA;
matAtB = matAt * matB;
// LM求解：JT*J*x=-JT*f （solve： A * X = B， 求解X）
matX = matAtA.colPivHouseholderQr().solve(matAtB);
// 更新状态量： X = X + δX
_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;