雖然現(xiàn)在的輪子很多，但我們在使用過程中會碰到很多問題，而我們經(jīng)常不知道從哪里下手，說明輪子不是你造的你不熟悉。因此我們不僅要重復造輪子，還要好好造，深入造，才能用好輪子，把輪子轉(zhuǎn)化成自身的力量。同樣的道理適用于這篇文章。雖然網(wǎng)上BA的資料無窮無盡，但我們還是要好好深入理解其原理，并且一定要通過實踐才能懂得其中原理。在“第一屆SLAM論壇”中沈劭劼老師的發(fā)言中，他提到團隊的成員都要手寫B(tài)A，既然大佬都這么做，我們就照做吧。這篇文章是我手寫B(tài)A的筆記，主要從原理推導入手，把公式都寫一遍，然后通過g2o、ceres和eigen三種方式來編程實現(xiàn)，以便加深對BA的理解。本文所有例子代碼地址：在公眾號「3D視覺工坊」，后臺回復「Bundle Adjustment」，即可直接下載。一、前言Bundle Adjustment中文譯作光束平差法、捆集調(diào)整等，是指從視覺重建中提煉出最優(yōu)的3D模型和相機參數(shù)（內(nèi)參和外參）。從每個特征點反射出來的幾束光線（bundles of light rays），在我們把相機姿態(tài)和特征點的位置做出最優(yōu)的調(diào)整（adjustment）之后，最后收束到光心的這個過程，簡稱BA。

二、原理

三、必備知識

1.SO(3)的對數(shù)映射 Exponential Map

2.向量外積

3.SO(3)的Jacobian

根據(jù)公式(12)，SO3的對數(shù)映射的導數(shù)是：

4.SE(3)的對數(shù)映射 Exponential Map

5.SE(3)的Jacobian

7.Pose-Pose的Jacobian

那么，在位姿求導的公式(23)中，位姿用4*4矩陣來表達。但當它用向量展開之后，最后一行是被忽略的。位姿變成12個向量。雖然這表明一個6自由度的實體過度參數(shù)化了，但通過這樣的表達，很多重要的步驟變成線性的了，使得我們可以更加有效地獲取精確的導數(shù)。我們用T代替p，則公式(23)表示成：

其結(jié)果可表示成：

把f設為F，根據(jù)上式則有：

記住公式(26)，待會要用來計算左乘模型的Jacobian矩陣。

9.Pose-Point的Jacobian

該問題則轉(zhuǎn)換成重投影誤差函數(shù)對位姿的求導，利用公式(33)和(31)，結(jié)果為：

四、推導

1.針孔相機的投影函數(shù)

由于排版問題，剩下部分請讀者跳轉(zhuǎn)至微信公眾號「3D視覺工坊」閱讀原文：https://mp.weixin.qq.com/s/hIxM3dNCmL6kb3_VW_66-g