SLAM問題的后端有主要有濾波和優化兩種方案。目前，普遍認為優化的方法在精度上要超過濾波方法，因為它可以進行多次的線性化。近年來出現的SLAM算法也大都是基于優化的算法（如ORB-SLAM、DSO等）。優化問題的核心便是Bundle Adjustment。本文對V-SLAM中純視覺的Bundle Adjustment問題進行了介紹，給出了簡單的實現，并利用仿真數據進行了測試。

1. 基本原理

關于Bundle Adjustment是什么，大家應該都比較熟悉，這里就不多做介紹了（可以參考文獻[1][2]）。本文主要給出一些實現細節。在基于特征點的視覺SLAM中，我們一般通過最小化重投影誤差，來優化相機的位姿和地圖點的位置。下面針對這個問題展開。

1.1 地圖點的參數化方式

地圖點的參數化方式主要有兩種：一種是用三維向量

表達；另一種采用逆深度表達。為了簡單和直觀，我們這里還是使用比較傳統的三維向量表達。

1.2 相機模型

相機模型也有很多啦，這里我們同樣是使用最簡單的一種：針孔相機模型。一個3D地圖點

投影到圖像上形成2D像素點 （3D-2D投影）可以表示為

其中

為內參矩陣， 為相機的位姿， 為3D地圖點的齊次坐標。我們設定攝像機內參數是已知的，將上述方程簡寫為

1.3 誤差、最小二乘問題

我們需要求解的最小二乘優化問題為

需要優化的量為相機的位姿

和地圖點的位置 ， 包含了所有的3D-2D投影。 為cost function， 為魯棒核函數，我們這里也是用最常用的Huber函數：

為了便于操作，這里我們將其轉換為一個權重

（這是g2o的做法），使得

那么，權重

為

至此，我們需要求解的最小二乘優化問題，變為

1.4 雅克比Jacobian

求解最小二乘問題，需要提供cost function：

相對于優化量 的雅克比。（可參考高博十四講）

1）cost function關于相機位姿

的雅克比，我們采用李代數擾動模型計算，為

其中，

2）cost function關于地圖點

的雅克比為

1.5 增量方程的求解

無論使用Gauss-Netwon還是Levenburg-Marquadt方法求解最小二乘問題（7）,都會求解一個關于增量的線性方程：

Bundle Adjustment中的

矩陣，具有特殊的稀疏結構，如下圖所示。利用這種稀疏結構可以加速求解過程。我們這里的實現使用了舒爾補的方法求解。（TODO，其他求解方法）

H矩陣的稀疏結構

其中只與相機位姿相關的矩陣

，以及只與地圖點相關的矩陣塊 均為對角塊矩陣。將(11)式寫成分塊形式。

利用舒爾補消元，我們首先求解

：

然后，再去求解

:

由于

為對角塊矩陣，求逆比較快。上述過程就是利用了這一特性，實現了求解的加速。

1.6 固定部分狀態

對于一個Bundle Adjustment問題，我們必須固定一部分狀態，或者給一部分狀態一個先驗。不然，就會有無窮多解。可以想象一下，一個網絡的誤差已經達到了最小后，可以整體在空間內任意移動，而不會對誤差造成任何影響。我們只要固定這個網絡中的任意1個以上的節點，其他的節點都會有確定的狀態。（我們通常會把第一個位姿固定）

怎么固定呢？一種可以加一個先驗約束，就是加一個先驗cost function。另外一種就是直接把狀態fix住，不讓它參與優化就好了。 我們采用后一種方法。

2. 實現

我們的實現比較簡單：
1. 僅針對上述優化問題；
2. 不考慮擴展性；
3. 不考慮內存問題（不考慮稀疏矩陣存儲，十分占內存，只能用來解小規模問題。TODO）；
4. 不考慮稀疏矩陣的加速；
5. 只實現了LM優化算法；
6. 不支持先驗約束，有motion only BA， struct only BA 和 Full BA三種形式。
7. 沒有考慮魯棒性的問題，錯誤的輸入可能直接就崩掉了。
8. 速度慢（TODO）
實現的過程我們參考了g2o的做法。全部工程代碼請見：ydsf16/vslam?github.com

我們首先定義幾個類，分別是地圖點，相機位姿，CostFunction。地圖點和相機位姿類用來存儲需要優化的狀態，通過id進行標識。CostFunction用來存儲代價函數、觀測，計算雅克比、殘差等。

2.1 地圖點類

存儲待優化的地圖點，同時定義了位置的加法。如果設置了fixed_標志，則不加入狀態向量。

class MapPoint{ public:MapPoint(const Eigen::Vector3d& position, int id, bool fixed = false);const Eigen::Vector3d& getPosition();void setPosition(const Eigen::Vector3d& position);int getId();void setId(int id);void setFixed();bool isFixed();void addDeltaPosition(const Eigen::Vector3d& delta_position);int state_index_; private:Eigen::Vector3d position_; int id_;bool fixed_; }; //class MapPoint

2.2 相機類

存儲待優化的相機，同時定義了位姿的加法，其實就是左乘啦。如果設置了fixed_標志，則不加入狀態向量。

class Camera{ public:Camera(const Sophus::SE3& pose, int id, bool fixed = false);const Sophus::SE3& getPose();void setPose(const Sophus::SE3& pose);int getId();void setId(int id);void setFixed();bool isFixed();void addDeltaPose(const Eigen::Matrix<double, 6, 1>& delta_pose);void addDeltaPose(const Sophus::SE3& delta_pose);int state_index_; private:Sophus::SE3 pose_;int id_;bool fixed_; }; // class CameraPose

2.3 CostFunction類

用來存儲cost function，計算cost， 計算雅克比，計算Huber weight等作用。總之，跟cost function相關的操作都放在了這里。

class CostFunction{ public:CostFunction(MapPoint* map_point, Camera* camera, double fx, double fy, double cx, double cy, const Eigen::Vector2d& ob_z);void setHuberParameter(double b = 1.0);void setCovariance(const Eigen::Matrix2d& cov);void computeInterVars(Eigen::Vector2d& e, Eigen::Matrix2d& weighted_info, double& weighted_e2);void computeJT(Eigen::Matrix<double, 2, 6>& JT);void computeJX(Eigen::Matrix<double, 2, 3>& JX);MapPoint* map_point_;Camera* camera_; private:double fx_, fy_, cx_, cy_;Eigen::Vector2d ob_z_;Eigen::Matrix2d info_matrix_;double huber_b_; }; // class CostFunction

2.4 BundleAdjustment類

整個BA問題的類。利用map結構存儲地圖點和相機，主要是為了方便快速查找。利用set結構存儲cost function。在這個類里面，將所有cost function的雅克比、信息矩陣整合成大的雅克比、信息矩陣、Hassian矩陣等。在這里面，我們實現了LM優化算法。

class BundleAdjustment{ public:BundleAdjustment();~BundleAdjustment(); // free memory.void addMapPoint(MapPoint* mpt);void addCamera(Camera* cam);void addCostFunction(CostFunction* cost_func);MapPoint* getMapPoint(int id);Camera* getCamera(int id);void setConvergenceCondition(int max_iters, double min_delta, double min_error);void setVerbose(bool flag);void optimize();private:void optimizationInit(); // init for the optimization.void computeStateIndexes(); // compute index for every state.void computeHAndbAndError();void solveNormalEquation();void inverseM(const Eigen::MatrixXd& M, Eigen::MatrixXd& M_inv);void updateStates();void recoverStates();std::map<int, MapPoint*> mappoints_; // all mappointsstd::map<int, Camera*> cameras_; // all cameras.std::set<CostFunction*> cost_functions_; // all cost functions.Eigen::MatrixXd J_; // Jocabian matrix.Eigen::MatrixXd JTinfo_;Eigen::MatrixXd H_; // Hassian matrix.Eigen::MatrixXd r_; // residual vector.Eigen::MatrixXd b_;Eigen::MatrixXd info_matrix_; // information matrix.Eigen::MatrixXd Delta_X_; // Delta_X_Eigen::MatrixXd I_;int n_cam_state_; // number of cameras in the state vector.int n_mpt_state_; // number of map points in the state vector./* Convergence condition */int max_iters_;double min_delta_;double min_error_;double sum_error2_;double last_sum_error2_;bool verbose_; }; // BundleAdjustment

2.5 BA問題的構建

這個過程跟g2o幾乎一樣。

step 1. 構造地圖點、相機，加入到BundleAdjustment類。
step 2. 構造cost function，加入到BundleAdjustment類。
step 3. 開始迭代優化
step 4. 根據id將結果取出 BundleAdjustment ba_mba;ba_mba.setConvergenceCondition(20, 1e-5, 1e-10);ba_mba.setVerbose(true);// add mappointsfor(size_t i = 0; i < noise_mappoints.size(); i ++){const Eigen::Vector3d& npt = noise_mappoints.at(i);MapPoint* mpt = new MapPoint(npt, i);mpt->setFixed(); // fixed all mappoints.ba_mba.addMapPoint(mpt);} // add mappoints// add cameras.for(size_t i = 0; i < noise_cameras.size(); i ++){const Sophus::SE3& ncam = noise_cameras.at(i);Camera* cam = new Camera(ncam, i);ba_mba.addCamera(cam);} // add cameras.// add observationsfor(size_t i = 0; i < noise_observations.size(); i ++){const Observation& ob = noise_observations.at(i);MapPoint* mpt = ba_mba.getMapPoint(ob.mpt_id_);Camera* cam = ba_mba.getCamera(ob.cam_id_);CostFunction* cost_func = new CostFunction(mpt, cam, fx, fy, cx, cy, ob.ob_);ba_mba.addCostFunction(cost_func);} // add observations.// Optimizeba_mba.optimize();// Compute pose Errordouble sum_rot_error = 0.0;double sum_trans_error = 0.0;for(size_t i = 0; i < cameras.size(); i ++){Camera* cam = ba_mba.getCamera(i);const Sophus::SE3& opt_pose = cam->getPose();const Sophus::SE3& org_pose = cameras.at(i);Sophus::SE3 pose_err = opt_pose * org_pose.inverse();sum_rot_error += pose_err.so3().log().norm();sum_trans_error += pose_err.translation().norm();}std::cout << "Mean rot error: " << sum_rot_error / (double)(cameras.size())<< "tMean trans error: " << sum_trans_error / (double)(cameras.size()) << std::endl;

3. 實驗

為了能夠與真值對比，我采用了仿真實驗。就是人為的仿真出m個相機，n個地圖點和一些觀測。然后給數據加一些噪聲，測試算法的有效性。仿真實驗有明確的ground truth，調試程序非常好用。下面給出一組數據的結果。

相機數量：6地圖點數量：275觀測數量：1650

3.1 motion only BA實驗（固定地圖點，只優化位姿）

首先，我們可以不添加任何噪聲進行實驗，可以發現優化后的結果與真值完全一樣。

然后，添加如下所示的噪聲，進行測試

mpt_noise = 0.01; // m
cam_trans_noise = 0.1; // m
cam_rot_noise = 0.1; // rad
ob_noise = 1.0; // pixel

3.2 struct only BA實驗（固定相機，只優化地圖點）

添加如下所示的噪聲，進行測試

mpt_noise = 0.1;
cam_trans_noise = 0.0;
cam_rot_noise = 0.00;
ob_noise = 1.0;

3.3 Full BA實驗（只固定第一個相機，優化其余相機和地圖點）

添加如下所示的噪聲，進行測試

mpt_noise = 0.05;
cam_trans_noise = 0.1;
cam_rot_noise = 0.1;
ob_noise = 1.0;

實測發現，算法可以很好的收斂。

相機位姿的估計精度會比地圖點的精度更高，因為一個相機可以看到幾百個地圖點，而一個地圖點只能被幾個相機看到；相機的約束方程數量遠大于地圖點的約束方程數量。

還發現，計算速度很慢，主要的原因在于

1. 內存頻繁拷貝
2. 增量方程求解速度很慢
3. 沒有利用稀疏矩陣的性質
4. Eigen的特性不熟悉
......

總之，還有一大堆問題需要解決。這只是一個很基礎的版本。以后有時間再搞吧。

參考資料

[1] Triggs B . Bundle Adjustment - A Modern Synthesis[M]// Vision Algorithms: Theory and Practice. Springer Berlin Heidelberg, 1999.

[2] 視覺SLAM十四講

[3] Methods for Non-Linear Least Squares Problems

總結

以上是生活随笔為你收集整理的矩阵求逆c语言实现_[V-SLAM] Bundle Adjustment 实现的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。