這篇文章針對(duì)有一定SLAM基礎(chǔ)的同學(xué)或者對(duì)李群李代數(shù)的應(yīng)用感興趣的數(shù)學(xué)專業(yè)同學(xué),已經(jīng)很小眾了坠敷，但對(duì)于真正干這行并想要更深理解的同學(xué)可能會(huì)很有幫助，因此花了些時(shí)間整理發(fā)出來(lái)匠襟。要從理論推導(dǎo)到后面代碼應(yīng)用都讀懂難度還是比較大，盡量講解。
李群李代數(shù)其實(shí)已經(jīng)算SLAM里最深的數(shù)學(xué)知識(shí)了(至少我目前接觸到的)浩姥，而事實(shí)上的SLAM應(yīng)用也只是用了它們中的很小一部分疚察。這篇文章不會(huì)詳細(xì)地介紹李群李代數(shù)，畢竟google或者百度一下lie group SLAM之類的已經(jīng)有不少人寫過(guò)相關(guān)博客或者論文了氯庆。這篇文章的重點(diǎn)是針對(duì)開源的SLAM系統(tǒng)蹭秋，推導(dǎo)出和代碼中應(yīng)用一致的公式，以幫助相關(guān)專業(yè)的同學(xué)知道李群李代數(shù)的現(xiàn)實(shí)應(yīng)用堤撵。下面這個(gè)鏈接是<視覺SLAM十四講>的作者所寫的關(guān)于李群李代數(shù)的入門
視覺SLAM中的數(shù)學(xué)基礎(chǔ) 第三篇 李群與李代數(shù) - 半閑居士 - 博客園
關(guān)于定義之類的我就不寫了仁讨，提取出幾個(gè)比較關(guān)鍵的部分。
李群關(guān)鍵點(diǎn): 旋轉(zhuǎn)矩陣和轉(zhuǎn)換矩陣分別屬于SO(3)和SE(3)李群实昨，我們重點(diǎn)針對(duì)SO(3)講解陪竿。李群有幾個(gè)(僅僅是這篇文章要使用的)重要性質(zhì). 性質(zhì)1: SO(3)和SE(3)李群矩陣和它的轉(zhuǎn)置互為逆矩陣，行列式為1(有為-1的情況但不會(huì)在SLAM中出現(xiàn))。 性質(zhì)2: 加法不封閉性和乘法封閉性族跛，即兩個(gè)李群矩陣相加后一般就不再是李群了闰挡，也就是李群當(dāng)中不存在加法。兩個(gè)李群想乘之后仍然是李群〗负澹現(xiàn)在如果有一個(gè)物體旋轉(zhuǎn)了角度长酗，再此基礎(chǔ)上我們?nèi)绻傩D(zhuǎn)一定角度，數(shù)學(xué)操作是對(duì)這個(gè)再乘上另一個(gè)旋轉(zhuǎn)矩陣桐绒，最終得到的仍然是一個(gè)旋轉(zhuǎn)矩陣夺脾。但是你如果把這兩個(gè)旋轉(zhuǎn)矩陣加起來(lái)則沒有任何實(shí)際的物理意義。性質(zhì)3:李群中的元素并不是獨(dú)立的茉继，一個(gè)旋轉(zhuǎn)矩陣有9個(gè)元素咧叭，而實(shí)際旋轉(zhuǎn)的自由度是3.
李代數(shù)關(guān)鍵點(diǎn):每一個(gè)李群都有一個(gè)對(duì)應(yīng)的李代數(shù)，我們記為小寫的so(3)和se(3)烁竭。李代數(shù)的元素是線性獨(dú)立的菲茬。一個(gè)旋轉(zhuǎn)矩陣對(duì)應(yīng)的李代數(shù)有3個(gè)元素。記一個(gè)李代數(shù)為派撕，定義如下操作

一個(gè)李群和它的李代數(shù)的對(duì)應(yīng)關(guān)系為矩陣指數(shù)關(guān)系

李代數(shù)是有加法封閉性的婉弹，即兩個(gè)李代數(shù)相加仍可以得到一個(gè)李代數(shù)，對(duì)這個(gè)新的so(3)李代數(shù)進(jìn)行指數(shù)操作可以得到一個(gè)新的旋轉(zhuǎn)矩陣终吼。
李群李代數(shù)之間的關(guān)系對(duì)旋轉(zhuǎn)的求導(dǎo)有很大的幫助镀赌。第一個(gè)鏈接前言部分里的公式(2)所提到的非線性最小二乘就是SLAM問(wèn)題的關(guān)鍵。我把那個(gè)公式再寫一下

已知兩對(duì)匹配的3d點(diǎn)(實(shí)際應(yīng)用中可能是圖像的2d點(diǎn)和它現(xiàn)實(shí)中的3d點(diǎn)的匹配际跪，或者2d圖像點(diǎn)和它在另一圖像中的對(duì)應(yīng)2d點(diǎn)匹配)商佛，找到一個(gè)轉(zhuǎn)換矩陣使他們匹配后點(diǎn)與點(diǎn)之間的距離最小。最小二乘都會(huì)涉及到對(duì)要求解的量的求導(dǎo)姆打。即

是已知常量威彰，所以關(guān)鍵在于

這個(gè)式子乍一看還不簡(jiǎn)單？上下的\mathbf{T}約掉穴肘，結(jié)果自然就是，但是實(shí)際上它是一個(gè)轉(zhuǎn)換矩陣舔痕，對(duì)一個(gè)轉(zhuǎn)換矩陣评抚，這么做是錯(cuò)誤的〔矗回想導(dǎo)數(shù)的定義

應(yīng)用在式子3中就應(yīng)該是

現(xiàn)在你應(yīng)該看到問(wèn)題來(lái)了慨代，由于李群的加法不封閉性，根本就沒有意義啸如，不再是李群侍匙。導(dǎo)數(shù)的定義對(duì)李群不再有意義，我們上面假設(shè)能分子分母直接劃掉相同的變量得到結(jié)果是根據(jù)導(dǎo)數(shù)的定義得來(lái)的，即假設(shè)加法成立想暗，我們的式5就可以得到

但很遺憾妇汗，不行。
那么我們?nèi)绾螌?duì)轉(zhuǎn)換矩陣求導(dǎo)呢说莫？我們下面要講解的應(yīng)用中杨箭，把對(duì)轉(zhuǎn)換矩陣的求導(dǎo)分開為對(duì)位移的求導(dǎo)和對(duì)旋轉(zhuǎn)的求導(dǎo)(實(shí)際上有直接的公式可以應(yīng)用不用分開，但是分開求會(huì)很直觀)储狭。我們前面寫到互婿，寫為齊次點(diǎn)即，代入式(3)辽狈，我們得到

1作為常數(shù)部分我們不管慈参，前半部分可以分別對(duì)旋轉(zhuǎn)求導(dǎo)和對(duì)平移求偏導(dǎo)。

對(duì)位移的偏導(dǎo)非常好解決刮萌，位移是有加法的驮配，求偏導(dǎo)時(shí)視為常亮，所以上式的位移部分可以直接得到答案那就是單位矩陣尊勿。一切的問(wèn)題都來(lái)源于對(duì)旋轉(zhuǎn)矩陣的偏導(dǎo)

旋轉(zhuǎn)矩陣也是不存在加法的僧凤。那么我們?cè)撛趺崔k呢？接下來(lái)就是李代數(shù)的應(yīng)用了元扔。我們把上式等價(jià)于對(duì)旋轉(zhuǎn)的李代數(shù)求導(dǎo)躯保！為什么可以這么等價(jià)？我給出一個(gè)直觀的解釋澎语。我們知道途事，平移之所以能很簡(jiǎn)單的求導(dǎo)是因?yàn)闈M足加法封閉性，即如果我們把對(duì)平移的求導(dǎo)應(yīng)用到式(4)中擅羞，會(huì)有一個(gè)尸变，這個(gè)趨于0表示針對(duì)當(dāng)前的趨于不移動(dòng)。當(dāng)對(duì)旋轉(zhuǎn)求導(dǎo)時(shí)减俏，我們應(yīng)該找到一個(gè)小量召烂，"加"到旋轉(zhuǎn)上，使物體對(duì)于當(dāng)前旋轉(zhuǎn)娃承，不再旋轉(zhuǎn)奏夫，只要我們?cè)O(shè)計(jì)出一個(gè)小量，滿足這個(gè)條件历筝，我們就可以針對(duì)這個(gè)小量求導(dǎo)酗昼。我們不能直接加上一個(gè)所有元素都趨于0的旋轉(zhuǎn)小量，但是旋轉(zhuǎn)對(duì)應(yīng)的李代數(shù)可以梳猪。假設(shè)針對(duì)當(dāng)前的旋轉(zhuǎn)的李代數(shù)麻削，加上一個(gè)小量，，當(dāng)趨于0時(shí)呛哟，旋轉(zhuǎn)不變叠荠，于是乎，我們可以針對(duì)這個(gè)小量求導(dǎo)竖共。有點(diǎn)晦澀蝙叛，多思考一下，這已經(jīng)是我能想到的最直觀的解釋了公给，理論證明可無(wú)窮盡也借帘。那么對(duì)旋轉(zhuǎn)的求導(dǎo)可以變換為

這個(gè)式子是可以求解的。這個(gè)式子在很多有關(guān)SLAM的書籍中寫到過(guò)淌铐。這里還是用大家很可能有的中文教材<視覺SLAM十四講>來(lái)講解的肺然。上面的式子對(duì)應(yīng)的是第一版十四講4.3.3的推導(dǎo)結(jié)果。這是李代數(shù)求導(dǎo)的第一種方法腿准。這種方法其實(shí)相對(duì)接下來(lái)要講的第二種方法直觀际起，因?yàn)橹辽傥覀兪歉鶕?jù)導(dǎo)數(shù)的定義，實(shí)實(shí)在在的有李代數(shù)的加法來(lái)求解的吐葱。第二種求導(dǎo)方法則不再拘泥于單純意義上的加法了街望。
旋轉(zhuǎn)矩陣真的沒有加法嗎？其實(shí)對(duì)于旋轉(zhuǎn)來(lái)說(shuō)弟跑，乘法就是加法灾前，我們要對(duì)一個(gè)旋轉(zhuǎn)"加上"另一個(gè)旋轉(zhuǎn)，實(shí)際上是對(duì)旋轉(zhuǎn)矩陣做乘法孟辑。所以我們?nèi)绻麑?duì)當(dāng)前的旋轉(zhuǎn)量乘一個(gè)近乎于單位矩陣的旋轉(zhuǎn)哎甲，其實(shí)就是對(duì)它"加"了一個(gè)小量缔杉，當(dāng)這個(gè)旋轉(zhuǎn)小量趨于單位矩陣時(shí)碳却，就沒有旋轉(zhuǎn)發(fā)生。旋轉(zhuǎn)矩陣趨于單位陣對(duì)應(yīng)的李代數(shù)就趨于0向量庆聘。那么第二個(gè)求導(dǎo)方法就是針對(duì)當(dāng)前的旋轉(zhuǎn)乘上一個(gè)小的(近乎于單位矩陣)的旋轉(zhuǎn)小量貌虾，并對(duì)它的李代數(shù)趨于0時(shí)求極限吞加。

這就是十四講第一版書上4.3.4擾動(dòng)模型了。不過(guò)正如書上所說(shuō)尽狠，"擾動(dòng)"可以是左擾動(dòng)也可以是右擾動(dòng)衔憨。書上給出的求導(dǎo)結(jié)果是左擾動(dòng)結(jié)果，下面我給出右擾動(dòng)的推導(dǎo)過(guò)程以及結(jié)果晚唇。

這里"李群性質(zhì)1"是當(dāng)李代數(shù)很小時(shí)(所以我們稱之為擾動(dòng))，盗似，這里是3x3單位矩陣哩陕。"李代數(shù)性質(zhì)2"是。讀者可以自己證明。

下面是激動(dòng)人心的實(shí)際應(yīng)用

有了李群的導(dǎo)數(shù)悍及，接下來(lái)就是實(shí)際應(yīng)用部分了闽瓢。 我這里的實(shí)際應(yīng)用不是說(shuō)讓你自己寫幾行代碼去實(shí)現(xiàn)，而是去看看比較成熟的SLAM系統(tǒng)是在哪里用到上面的推導(dǎo)的心赶。這需要你打開相關(guān)的論文和代碼對(duì)比來(lái)看扣讼，需要些精力。我經(jīng)常操作的是視覺慣性slam缨叫，好的開源庫(kù)有VINS-FUSION,VINS-Mono(都來(lái)自于港科大椭符，前者更新有更強(qiáng)的應(yīng)用不過(guò)后者被更多人熟知),okvis等。okvis是多年前的作品耻姥，非常經(jīng)典销钝，但是代碼難度比前面的大不少。綜合了一下情況我決定使用VINS-Mono的代碼琐簇，把李代數(shù)的求導(dǎo)結(jié)果和他們的代碼里一一對(duì)應(yīng)起來(lái)蒸健。代碼在他們的github中 HKUST-Aerial-Robotics/VINS-Mono: A Robust and ... - GitHub
我們以VINS-Mono想要求解的reprojection error為例。
vins的原論文 VINS-Mono: A Robust and Versatile Monocular Visual ... - arXiv 式25把處于不同圖像的對(duì)應(yīng)的2d點(diǎn)投影到同一單位球上然后求殘差婉商。
在vins-mono里projection_factor.cpp里Evaluate函數(shù)中包含了誤差函數(shù)以及倒數(shù)的結(jié)果似忧。
首先來(lái)看前面幾行幾個(gè)參數(shù)

    Eigen::Vector3d Pi(parameters[0][0], parameters[0][1], parameters[0][2]);
    Eigen::Quaterniond Qi(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]);

    Eigen::Vector3d Pj(parameters[1][0], parameters[1][1], parameters[1][2]);
    Eigen::Quaterniond Qj(parameters[1][6], parameters[1][3], parameters[1][4], parameters[1][5]);

    Eigen::Vector3d tic(parameters[2][0], parameters[2][1], parameters[2][2]);
    Eigen::Quaterniond qic(parameters[2][6], parameters[2][3], parameters[2][4], parameters[2][5]);

    double inv_dep_i = parameters[3][0];

這幾行的parameter來(lái)源于傳入的要優(yōu)化的參數(shù)，具體來(lái)源于在estimator.cpp中的這兩行

ProjectionFactor *f = new ProjectionFactor(pts_i, pts_j);
problem.AddResidualBlock(f, loss_function, para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0], para_Feature[feature_index]);

其中para_Pose[imu_i]對(duì)應(yīng)于parameter[0]丈秩，是IMUi時(shí)刻的位姿盯捌，para_Pose[imu_j]對(duì)應(yīng)于parameter[1]，是IMUj時(shí)刻位姿癣籽，para_Ex_Pose[0]是相機(jī)和IMU的外參挽唉，對(duì)應(yīng)parameter[2]。最后一個(gè)就對(duì)應(yīng)inv_dep_i.即該像素點(diǎn)深度的倒數(shù)筷狼。這幾個(gè)是想要被優(yōu)化的量瓶籽。隨后幾行

    Eigen::Vector3d pts_camera_i = pts_i / inv_dep_i;
    Eigen::Vector3d pts_imu_i = qic * pts_camera_i + tic;
    Eigen::Vector3d pts_w = Qi * pts_imu_i + Pi;
    Eigen::Vector3d pts_imu_j = Qj.inverse() * (pts_w - Pj);
    Eigen::Vector3d pts_camera_j = qic.inverse() * (pts_imu_j - tic);

對(duì)應(yīng)vins的原論文式25第三個(gè)的求法。注意式子中的back projection函數(shù)在原文中它的作用是turns a pixel location into a unit vector using camera intrinsic parameters埂材。具體參見論文圖6. 代碼中的pts_i對(duì)應(yīng)的是公式中的

這是論文中所說(shuō)的指向球面的unit vector塑顺。用它除以深度的倒數(shù)(乘上深度)，就對(duì)應(yīng)代碼中的pts_i / inv_dep_i俏险，獲得一個(gè)在相機(jī)坐標(biāo)系下的3d點(diǎn)

這個(gè)3d點(diǎn)乘以相機(jī)到body(或者說(shuō)IMU)坐標(biāo)系下的轉(zhuǎn)換矩陣得到在IMU坐標(biāo)系下的坐標(biāo)严拒。

對(duì)應(yīng)的代碼中的qic * pts_camera_i + tic。剩下的式25的第3個(gè)式子的位姿變換和代碼的每一行一一對(duì)應(yīng)竖独。上式用轉(zhuǎn)到世界坐標(biāo)系裤唠，再轉(zhuǎn)到 圖像所在的坐標(biāo)系最后轉(zhuǎn)到相機(jī)下的unit vector即pts_camera_j.

隨后
代碼中

#ifdef UNIT_SPHERE_ERROR 
    residual =  tangent_base * (pts_camera_j.normalized() - pts_j.normalized());
#else
    double dep_j = pts_camera_j.z();
    residual = (pts_camera_j / dep_j).head<2>() - pts_j.head<2>();
#endif

用UNIT_SPHERE_ERROR才和式25的第一行對(duì)應(yīng)，如果不用的話就和普通的pinhole相機(jī)模型的點(diǎn)對(duì)應(yīng)莹痢。為了方便起見种蘸，我還是推導(dǎo)的不用UNIT_SPHERE_ERROR所得到的雅各比矩陣墓赴。因?yàn)槲野l(fā)現(xiàn)VINS, parameter.h中的#define UNIT_SPHERE_ERROR這一行被注釋掉了。
代碼下一行
residual = sqrt_info * residual;
okvis也有這一行航瞭，這是因?yàn)閏eres能接受的最小二乘優(yōu)化只能是最簡(jiǎn)單的形式诫硕，而帶有協(xié)方差的優(yōu)化的形式為，所以代碼中會(huì)對(duì)做LLT分解刊侯，有章办，那個(gè)sqrt_info自然就是了。
綜上針對(duì)某一個(gè)像素點(diǎn)在frame下的像素坐標(biāo)我們的residual就是

式子中來(lái)源于測(cè)量(比如我們通過(guò)特征點(diǎn)匹配的方法知道了i,j是對(duì)應(yīng)的滨彻，那時(shí)得到的j的坐標(biāo)就是通過(guò)測(cè)量得到的), 表示取向量第1位到第n位藕届。把上式具體寫出來(lái)就是

residual需要求導(dǎo)的對(duì)象是body在frame時(shí)的pose，即疮绷。
首先我們計(jì)算簡(jiǎn)單的對(duì)位置求導(dǎo)翰舌。

其中為2維，為3d點(diǎn)冬骚，很容易得到

然后(8)式對(duì)求導(dǎo)椅贱，得到

對(duì)應(yīng)代碼中的

jaco_i.leftCols<3>() = ric.transpose() * Rj.transpose();

即

這里用到了SO(3)群的基本性質(zhì)，SO(3)下的矩陣的逆等于它的轉(zhuǎn)置只冻。
之后我們對(duì)旋轉(zhuǎn)求導(dǎo)數(shù)

導(dǎo)數(shù)的前一部分和式10一樣庇麦，后一部分涉及到李群的求導(dǎo)，為書寫方便我們先令
喜德，因?yàn)樽笫奖緛?lái)就是frame像素點(diǎn)投影到相機(jī)平面的3d點(diǎn)山橄。后一部分

現(xiàn)在只對(duì)求導(dǎo)數(shù)，那其他量視為是常量舍悯，我們令航棱，另外3d點(diǎn)得到的是body 坐標(biāo)系下的3d點(diǎn)，我們記為萌衬。那么上式就變成了

現(xiàn)在的問(wèn)題就是如何得到了,簡(jiǎn)單記為饮醇。那這兒很明顯了，應(yīng)用我們右擾動(dòng)的推導(dǎo)結(jié)果秕豫，可以直接得到

代入朴艰，我們就有了

這和代碼中的

jaco_i.rightCols<3>() = ric.transpose() * Rj.transpose() * Ri * -Utility::skewSymmetric(pts_imu_i);

對(duì)應(yīng)混移。
總的來(lái)說(shuō)代碼第一個(gè)求導(dǎo)部分得到了殘差對(duì)frame處位姿的求導(dǎo)祠墅。

        if (jacobians[0])
        {
            Eigen::Map<Eigen::Matrix<double, 2, 7, Eigen::RowMajor>> jacobian_pose_i(jacobians[0]);

            Eigen::Matrix<double, 3, 6> jaco_i;
            jaco_i.leftCols<3>() = ric.transpose() * Rj.transpose();\\對(duì)位移求導(dǎo)
            jaco_i.rightCols<3>() = ric.transpose() * Rj.transpose() * Ri * -Utility::skewSymmetric(pts_imu_i);\\對(duì)旋轉(zhuǎn)求導(dǎo)

            jacobian_pose_i.leftCols<6>() = reduce * jaco_i;\\reduce為式10
            jacobian_pose_i.rightCols<1>().setZero(); \\這里有一個(gè)0列，回顧前面可以發(fā)現(xiàn)我們?cè)谇笃珜?dǎo)時(shí)忽略了一個(gè)1的
        }

其他部分的推導(dǎo)我就不講了歌径，以后有時(shí)間的話我會(huì)寫上IMU部分的推導(dǎo)毁嗦，有預(yù)積分會(huì)麻煩很多。
你自己如果有興趣的話回铛，你可以推導(dǎo)okvis的reprojection error部分狗准，它也是用ceres優(yōu)化芯急，優(yōu)化部分的結(jié)構(gòu)和VINS-MONO相似。雖然他們的代碼更難讀懂驶俊，但是我已經(jīng)推導(dǎo)過(guò)了，和前面的講解一致免姿，所以你也可以試一下饼酿。