lio-sam中雅克比推导

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;