LEGO-LOAM（LOAM）部分公式推導---未完待續

一、featureAssociation相關推導

1）幀間比對雅可比矩陣推導

首先明确LEGO-LOAM中，運動坐标系（符合右手系）的設定為：

是以對于平面運動來說，影響最大的三個分量為 t x , t z , r y t_x,t_z,r_y tx​,tz​,ry​，是以僅考慮這三個分量對雅可比矩陣，也就是對非線性優化問題的貢獻。首先讓我們考慮整個非線性優化過程，首先将本幀點 p p p轉換至上一幀坐标系中，得到 p ′ p' p′，即

p ′ = T p p' = Tp p′=Tp

通過計算 p ′ p' p′點和其近鄰線（以corner為例，存在于上一幀點雲系中）的點線間距離，得到殘差 f f f，即

f = d i s t ( p ′ , l i n e ) = d i s t ( T p , l i n e ) f = dist(p', line) = dist(Tp, line) f=dist(p′,line)=dist(Tp,line)

于是雅可比矩陣可以寫為

J = ∂ f ∂ T = ∂ ( d i s t ( T p , l i n e ) ) ∂ T J =\frac{{\partial f}}{{\partial T}} = \frac{{\partial (dist(Tp,line))}}{{\partial T}} J=∂T∂f​=∂T∂(dist(Tp,line))​

由于我們隻關心三個分量，且每一幀包含多個點線配對，可以計算許多個殘差，則雅可比矩陣寫開以後是這個類型: J = J= J=

f f f對這三個位姿量的求導，使用鍊式法則來計算，首先将殘差對使用這個位姿量轉換過去的點 p ′ = ( x , y , z ) p'=(x,y,z) p′=(x,y,z)進行求導（将本幀點轉換至上一幀坐标系中的點），然後再使用被轉換過去的點 p ′ = ( x , y , z ) p'=(x,y,z) p′=(x,y,z)對位姿量求導。也就是：

∂ f k ∂ t x = ∂ f k ∂ x ∂ x ∂ t x + ∂ f k ∂ y ∂ y ∂ t x + ∂ f k ∂ z ∂ z ∂ t x \frac{{\partial {f_k}}}{{\partial {t_x}}} = \frac{{\partial {f_k}}}{{\partial x}}\frac{{\partial x}}{{\partial {t_x}}} + \frac{{\partial {f_k}}}{{\partial y}}\frac{{\partial y}}{{\partial {t_x}}} + \frac{{\partial {f_k}}}{{\partial z}}\frac{{\partial z}}{{\partial {t_x}}} ∂tx​∂fk​​=∂x∂fk​​∂tx​∂x​+∂y∂fk​​∂tx​∂y​+∂z∂fk​​∂tx​∂z​

每一項鍊式法則的求導分為兩部分，在程式中分别在findCorrespondingCornerFeatures函數和calculateTransformationCorner函數中完成。之後我們再細說，在這裡先推導 ∂ f k ∂ x \frac{{\partial {f_k}}}{{\partial x}} ∂x∂fk​​和 ∂ x ∂ t x \frac{{\partial x}}{{\partial {t_x}}} ∂tx​∂x​的具體形式，首先推導前半部分 ∂ f k ∂ x \frac{{\partial {f_k}}}{{\partial x}} ∂x∂fk​​，根據公式（這裡的公式複制自知乎某回答），點線距離為：

即

其中，

令分母為：

接下來求 ∂ f k ∂ x \frac{{\partial {f_k}}}{{\partial x}} ∂x∂fk​​，也就是這裡的 ∂ d ε ∂ x \frac{{\partial {d_\varepsilon }}}{{\partial x}} ∂x∂dε​​。 x x x即為公式裡面的 x 0 x_0 x0​。

∂ d ε ∂ x 0 = 1 2 ( m 11 2 + m 22 2 + m 33 2 ) ( 2 m 11 ∂ m 11 ∂ x 0 + 2 m 22 ∂ m 22 ∂ x 0 ) l 12 = m 11 ∂ m 11 ∂ x 0 + m 22 ∂ m 22 ∂ x 0 l 12 m 11 2 + m 22 2 + m 33 2 \frac{{\partial {d_\varepsilon }}}{{\partial {x_0}}}{\rm{ = }}\frac{{\frac{1}{2}(\sqrt {m_{11}^2 + m_{22}^2 + m_{33}^2} )(2{m_{11}}\frac{{\partial {m_{11}}}}{{\partial {x_0}}} + 2{m_{22}}\frac{{\partial {m_{22}}}}{{\partial {x_0}}})}}{{{l_{12}}}}{\rm{ = }}\frac{{{m_{11}}\frac{{\partial {m_{11}}}}{{\partial {x_0}}} + {m_{22}}\frac{{\partial {m_{22}}}}{{\partial {x_0}}}}}{{{l_{12}}\sqrt {m_{11}^2 + m_{22}^2 + m_{33}^2} }} ∂x0​∂dε​​=l12​21​(m112​+m222​+m332​

​)(2m11​∂x0​∂m11​​+2m22​∂x0​∂m22​​)​=l12​m112​+m222​+m332​

​m11​∂x0​∂m11​​+m22​∂x0​∂m22​​​

對另兩個位姿分量的就省略了，是同理的，這一塊對應源碼中的：

tripod1 = laserCloudCornerLast->points[pointSearchCornerInd1[i]];
                tripod2 = laserCloudCornerLast->points[pointSearchCornerInd2[i]];

                float x0 = pointSel.x;
                float y0 = pointSel.y;
                float z0 = pointSel.z;
                float x1 = tripod1.x;
                float y1 = tripod1.y;
                float z1 = tripod1.z;
                float x2 = tripod2.x;
                float y2 = tripod2.y;
                float z2 = tripod2.z;

                float m11 = ((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1));
                float m22 = ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1));
                float m33 = ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1));

                float a012 = sqrt(m11 * m11  + m22 * m22 + m33 * m33);

                float l12 = sqrt((x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2) + (z1 - z2)*(z1 - z2));

                float la =  ((y1 - y2)*m11 + (z1 - z2)*m22) / a012 / l12;

                float lb = -((x1 - x2)*m11 - (z1 - z2)*m33) / a012 / l12;

                float lc = -((x1 - x2)*m22 + (y1 - y2)*m33) / a012 / l12;

                float ld2 = a012 / l12;
           

接下來求鍊式法則的後面那部分，也就是 ∂ x ∂ t x \frac{{\partial x}}{{\partial {t_x}}} ∂tx​∂x​。 x x x指代的是經過變換矩陣變換，将本幀點轉換至上一幀坐标系中的點的坐标。是以，首先需要明确旋轉變換關系。LEGO-LOAM(LOAM)中的幀間坐标變換關系如下：

其中， p ′ = ( x , y , z ) p'=(x,y,z) p′=(x,y,z)表示被轉換到上一幀坐标系中的點， p = ( x c , y c , z c ) p=(x_c,y_c,z_c) p=(xc​,yc​,zc​)表示本幀這個點。 R R R和 t x , t y , t z t_x,t_y,t_z tx​,ty​,tz​分别代表旋轉和平移變換關系，角度為本幀坐标系轉了多少歐拉角能轉到上一幀坐标系，象征着所有歐拉角加符号帶入公式才能獲得本幀坐标系點轉換到上一幀坐标系中。至于LOAM為什麼這樣設定不清楚（有點擰巴）。

要求 ∂ x ∂ t x \frac{{\partial x}}{{\partial {t_x}}} ∂tx​∂x​，必須要明确R是什麼形式。根據wiki上給出的公式（上圖右邊紅色框），歐拉角（-rx,-ry,-rz）轉換為旋轉矩陣的公式為：

這裡之是以加負号，與歐拉角設定相關？

是以易求 ∂ x ∂ t x \frac{{\partial x}}{{\partial {t_x}}} ∂tx​∂x​，僅看第一行第一列乘出來的東西就行，即：

化簡可以得到

2）高頻率裡程計位姿累積函數：integrateTransformation

void integrateTransformation(){
        float rx, ry, rz, tx, ty, tz;
        AccumulateRotation(transformSum[0], transformSum[1], transformSum[2], 
                           -transformCur[0], -transformCur[1], -transformCur[2], rx, ry, rz);

        float x1 = cos(rz) * (transformCur[3] - imuShiftFromStartX) 
                 - sin(rz) * (transformCur[4] - imuShiftFromStartY);
        float y1 = sin(rz) * (transformCur[3] - imuShiftFromStartX) 
                 + cos(rz) * (transformCur[4] - imuShiftFromStartY);
        float z1 = transformCur[5] - imuShiftFromStartZ;

        float x2 = x1;
        float y2 = cos(rx) * y1 - sin(rx) * z1;
        float z2 = sin(rx) * y1 + cos(rx) * z1;

        tx = transformSum[3] - (cos(ry) * x2 + sin(ry) * z2);
        ty = transformSum[4] - y2;
        tz = transformSum[5] - (-sin(ry) * x2 + cos(ry) * z2);

        PluginIMURotation(rx, ry, rz, imuPitchStart, imuYawStart, imuRollStart, 
                          imuPitchLast, imuYawLast, imuRollLast, rx, ry, rz);

        transformSum[0] = rx;
        transformSum[1] = ry;
        transformSum[2] = rz;
        transformSum[3] = tx;
        transformSum[4] = ty;
        transformSum[5] = tz;
    }
           

（1）AccumulateRotation函數：

輸入：

• 上一幀粗配準的雷射裡程計的累積位姿（全局），僅取旋轉部分歐拉角：transformSum[0], transformSum[1], transformSum[2]
• 上一幀和本幀間的位姿變化量（增量），僅取旋轉部分歐拉角： -transformCur[0], -transformCur[1], -transformCur[2]。由于transformCur處于本幀坐标系，取負号表示從上一幀變換到本幀的歐拉角變化量。
• 本幀粗配準的雷射裡程計的累積位姿，僅取旋轉部分歐拉角，由以上兩組量求得：rx, ry, rz

首先從位姿變換基礎公式入手，根據wiki上給出的公式（上圖畫黃色框處），這裡歐拉角（x,y,z）轉換為旋轉矩陣的公式為：

設上一幀全局位姿的旋轉部分為 R 1 = ( x 1 , y 1 , z 1 ) R_1=(x_1, y_1, z_1) R1​=(x1​,y1​,z1​)，本幀全局位姿的旋轉部分為 R 2 = ( x 2 , y 2 , z 2 ) R_2=(x_2, y_2, z_2) R2​=(x2​,y2​,z2​)，上一幀到本幀的旋轉變化量 δ R = ( δ x , δ y , δ z ) \delta R=(\delta x, \delta y, \delta z) δR=(δx,δy,δz)，于是根據姿态變換公式：

R 2 = R 1 ⋅ δ R R_2=R_1·\delta R R2​=R1​⋅δR

根據上面歐拉角轉旋轉矩陣的通式，與待求歐拉角相關的 R 2 R_2 R2​為：

觀察到，第二行第三列直接與 x 2 x_2 x2​相關，是以考慮先求 x 2 x_2 x2​，即先求 R 2 ( 2 , 3 ) R_2^{(2,3)} R2(2,3)​，也就是 ( R 1 R_1 R1​和 δ R \delta R δR也按照上面通式轉為旋轉矩陣) R 1 R_1 R1​的第二行乘以 δ R \delta R δR的第三列，即：

展開就能用來求 − s i n x 2 -sinx_2 −sinx2​，于是就有了AccumulateRotation函數内部的：

float srx = cos(lx)*cos(cx)*sin(ly)*sin(cz) - cos(cx)*cos(cz)*sin(lx) - cos(lx)*cos(ly)*sin(cx);
ox = -asin(srx);
           

接下來求 y 2 y_2 y2​，可以看到：

a r c t a n ( R 2 ( 1 , 3 ) / R 2 ( 2 , 3 ) ) = y 2 arctan(R_2^{(1,3)} / R_2^{(2,3)})=y_2 arctan(R2(1,3)​/R2(2,3)​)=y2​，同理，按照上述幾行幾列的思路寫出公式，可得函數内部的：

float srycrx = sin(lx)*(cos(cy)*sin(cz) - cos(cz)*sin(cx)*sin(cy)) + cos(lx)*sin(ly)*(cos(cy)*cos(cz) 
              + sin(cx)*sin(cy)*sin(cz)) + cos(lx)*cos(ly)*cos(cx)*sin(cy);
 float crycrx = cos(lx)*cos(ly)*cos(cx)*cos(cy) - cos(lx)*sin(ly)*(cos(cz)*sin(cy) 
              - cos(cy)*sin(cx)*sin(cz)) - sin(lx)*(sin(cy)*sin(cz) + cos(cy)*cos(cz)*sin(cx));
 oy = atan2(srycrx / cos(ox), crycrx / cos(ox));
           

接下來求 z 2 z_2 z2​，可以看到：

a r c t a n ( R 2 ( 2 , 1 ) / R 2 ( 2 , 2 ) ) = z 2 arctan(R_2^{(2,1)} / R_2^{(2,2)})=z_2 arctan(R2(2,1)​/R2(2,2)​)=z2​，同理，按照上述幾行幾列的思路寫出公式，可得函數内部的：

float srzcrx = sin(cx)*(cos(lz)*sin(ly) - cos(ly)*sin(lx)*sin(lz)) + cos(cx)*sin(cz)*(cos(ly)*cos(lz) 
             + sin(lx)*sin(ly)*sin(lz)) + cos(lx)*cos(cx)*cos(cz)*sin(lz);
float crzcrx = cos(lx)*cos(lz)*cos(cx)*cos(cz) - cos(cx)*sin(cz)*(cos(ly)*sin(lz) 
             - cos(lz)*sin(lx)*sin(ly)) - sin(cx)*(sin(ly)*sin(lz) + cos(ly)*cos(lz)*sin(lx));
oz = atan2(srzcrx / cos(ox), crzcrx / cos(ox));
           

接下來觀察旋轉部分，這裡我們忽略IMU的使用，即imuShiftFromStartZ = imuShiftFromStartY = 0 （LEGO-LOAM的IMU聽說也很雞肋）。就有如下代碼：

float x1 = cos(rz) * (transformCur[3]) 
                 - sin(rz) * (transformCur[4]);
        float y1 = sin(rz) * (transformCur[3]) 
                 + cos(rz) * (transformCur[4]);
        float z1 = transformCur[5];

        float x2 = x1;
        float y2 = cos(rx) * y1 - sin(rx) * z1;
        float z2 = sin(rx) * y1 + cos(rx) * z1;

        tx = transformSum[3] - (cos(ry) * x2 + sin(ry) * z2);
        ty = transformSum[4] - y2;
        tz = transformSum[5] - (-sin(ry) * x2 + cos(ry) * z2);
           

可以這麼了解，先使用rx，ry，rz相關的轉換矩陣将參考系與世界坐标系同軸（平行），然後transformSum是位于世界坐标系的，是以可以直接做加法。這個原因主要是因為transformCur是存在于本幀坐标系的，推導公式也能推導出來上面式子。