LeGO-LOAM的残差公式及雅克比推导
最近在深入学习loam系列的源码，以前只是把它跑起来，并没有深入阅读源码。这几天在看源码的过程中，发现LeGO-LOAM的优化需要的东西（比如残差、雅克比、高斯牛顿），全是手推的公式，并不像A-LOAM那样调的ceres库，而且涉及大量的欧拉角的外旋（这部分下篇文章去总结），这就看起来相当复杂，今天总结一下残差和雅克比的推导过程。
大家都知道LOAM系列的特征提取有两类：角点和面点。对应的残差也是有两类，点线残差、点面残差，后面我们将以点线残差的推导为例。
LeGO-LOAM残差构建的特点：
在代码层面，相较于A-LOAM和F-LOAM，它进行了残差加权处理；
手写高斯牛顿去迭代，考虑了系数矩阵退化的处理；（论文说是LM，实际为GN）；
对于旋转部分的雅克比，采用的是欧拉角的形式推导；（可能存在万向锁的问题，车辆应该不存在）；
详细的推导过程如下：
首先，点线的雅克比主要求残差d对位姿T的导数，使得残差最小；在这里就是，d对T的导数，转换一下就是，d先对点p的导数，再求p对T的导数，如下：
第一部分：
：
以上求解出来的就是代码里对应的la，lb，lc，以及残差d，代码里还进行了加权；（当然要单位化）。。
更改原代码格式，使用Eigen库矩阵写法：
#if 1
Eigen::Vector3f v1, v2, v3;
Eigen::Vector3f l_abc_;
v1 << x0-x1, y0-y1, z0-z1;
v2 << x0-x2, y0-y2, z0-z2;
v3 << x1-x2, y1-y2, z1-z2;
Eigen::Vector3f temp = v1.cross(v2);
float a012_ = temp.norm(); // 叉乘之后, 平方根
float l12_ = v3.norm();
float ld2_ = a012_ / l12_;
// 残差对当前点坐标的偏导数
l_abc_ << temp(1)*(z2-z1) + temp(2)*(y1-y2),
temp(0)*(z1-z2) + temp(2)*(x2-x1),
temp(0)*(y2-y1) + temp(1)*(x1-x2);
l_abc_ /= ld2_;
#endif 第二部分：
：
以上求解出来，即可构建增量方程HX=g，进行非线性最小二乘迭代优化。
更改原代码格式，使用Eigen库矩阵写法：
// !残差对位姿的偏导
// 残差对位姿的偏导 = 残差对点的偏导关系 * 点对外参的偏导关系, 残差对点的偏导关系前面求得了
// d(d)/Twl = d(d)/dPw * d(Pw)/d(Twl)
#if 1
Eigen::Vector3f point;
Eigen::Vector3f dRp_x,dRp_y,dRp_z;
Eigen::Matrix3f dRp_xyz;
Eigen::Vector3f ddist_p;
Eigen::Vector3f ar_xyz, trans_xyx;
point << pointOri.x, pointOri.y, pointOri.z; // 点
ddist_p << coeff.x, coeff.y, coeff.z; // 残差对点的偏导关系(残差d对p的导数)
// 点转到世界系下对外参的偏导关系(Rp对欧拉角的导数)
dRp_x << (crx * sry * srz*point(0)+crx * crz * sry*point(1)-srx * sry*point(2)),
(-srx * srz*point(0)- crz * srx*point(1)- crx*point(2)),
(crx * cry * srz*point(0)+ crx * cry * crz*point(1)-cry * srx*point(2));
dRp_y << ((cry * srx * srz - crz * sry)*point(0)+(sry * srz + cry * crz * srx)*point(1)+(crx * cry)*point(2)),
0,
(((-cry * crz - srx * sry * srz)*point(0))+(cry * srz - crz * srx * sry)*point(1)- crx * sry * point(2));
dRp_z << ((crz * srx * sry - cry * srz)*point(0)+(-cry * crz - srx * sry * srz)*point(1)),
((crx * crz)*point(0)- crx * srz*point(1)),
((sry * srz + cry * crz * srx)*point(0)+(crz * sry - cry * srx * srz)*point(1));
dRp_xyz.block<1,3>(0,0) = dRp_x; // 按行填充
dRp_xyz.block<1,3>(1,0) = dRp_y;
dRp_xyz.block<1,3>(2,0) = dRp_z;
// 两部分相乘 得到残差对外参的偏导
ar_xyz = dRp_xyz * ddist_p; // 旋转, Rp+t对t求导是dRp_xyz
trans_xyx = Eigen::Matrix3f::Identity() * ddist_p; // 平移, Rp+t对t求导是单位阵
// 构造AB矩阵
matA(i, 0) = ar_xyz(0); // 旋转
matA(i, 1) = ar_xyz(1);
matA(i, 2) = ar_xyz(2);
matA(i, 3) = ddist_p(0); // 平移
matA(i, 4) = ddist_p(1);
matA(i, 5) = ddist_p(2);
matB(i, 0) = -coeff.intensity; // 残差 高斯牛顿的g=-Jf, f为残差(误差函数)
#endif以上即是，LeGO_LOAM对于雅可比矩阵的构建的过程，比较繁琐，耐心推到两遍就理解了。（文章采用手推雅可比的方式，大概也是为了文章的轻量化这一特征。）