主要內容
1. 概述
1）考慮k時刻的狀態估計，用過去0到k中的數據來估計現在的狀態分布：
2）按照貝葉斯法則：
注：從左到右，一次為后驗概率，似然概率 和 先驗概率；其中，似然有量測信息給定，先驗概率基於過去所有的狀態估計和量測得來的
3）對先驗概率進行進一步展開，按照
為條件概率展開：
注：如果考慮更久之前的狀態，可以對上式進行進一步的展開，現在只關注k時刻和k-1時刻。
4） 繼續處理先驗概率
一種方法是假設馬爾科夫性，即認為k時刻的狀態只與k-1時刻的狀態有關，而與再之前的無關，以擴展卡爾曼濾波（EKF）為代表的濾波器方法；
二是依然考慮k時刻的狀態與之前所有狀態的關系，以非線性優化為主體的 優化框架。（SLAM的主流）
2. EKF
1） 基於馬爾科夫性，先驗概率公式展開可以表示為：
從這個公式中可以看出，k時刻的先驗概率可以表示描述為：基於k-1時刻的后驗概率
，結合輸入信息以及k-1時刻到k時刻的轉移概率，可以求解出k時刻的先驗概率。
在線性系統和高斯噪聲下，卡爾曼濾波器構成了該系統中的最大后驗概率估計，即無偏最優估計；而在SLAM這種非線性的情況下，EKF給出了單次線性近似下的最大后驗估計（MAP）。
2） 討論
2.1）優點：形式簡潔，應用在計算資源受限，或待估計量比較簡單的場合
2.2）典型方法及文獻：
IF(信息濾波)，IKF(Iterated KF)，UKF(Unscented KF)，和粒子濾波器，SWF(Sliding Window Filter)，等等，或者用分治法等思路改進EKF的效率。相關文獻如下：
[1] Sujan V A, Dubowsky S. Efficient information-based visual robotic mapping in unstructured environments[J]. The International Journal of Robotics Research, 2005, 24(4): 275-293.
[2] A kalman-filter-based method for pose estimation in visual servoing
F Janabi-Sharifi, M Marey - IEEE transactions on Robotics, 2010 - ieeexplore.ieee.org
[3] Li S, Ni P. Square-root unscented Kalman filter based simultaneous localization and mapping[C]//The 2010 IEEE International Conference on Information and Automation. IEEE, 2010: 2384-2388.
[4] Gil A, Reinoso Ó, Ballesta M, et al. Multi-robot visual SLAM using a Rao-Blackwellized particle filter[J]. Robotics and Autonomous Systems, 2010, 58(1): 68-80.
[5] Sliding window filter with application to planetary landing
G Sibley, L Matthies, G Sukhatme - Journal of Field Robotics, 2010 - Wiley Online Library
[6] Chen S Y. Kalman filter for robot vision: a survey[J]. IEEE Transactions on Industrial Electronics, 2011, 59(11): 4409-4420.
3) EKF局限性
3.1） 濾波器方法在一定程度上假設了馬爾科夫性；
3.2） EKF僅在某一點處做了一次線性化，即認為該點處的線性化近似在后驗概率處仍然是有效的。——存在非線性誤差
3.3） EKF需要存儲狀態量的均值和方差，由於SLAM中路標數量很大，因此存儲的量比較大，因此，EKF SLAM被普遍認為不適用於大型場景。
3. BA與圖優化
3.1) 投影模型和BA代價函數
3.2) BA求解
增量線性方程的求解
3.3）稀疏性和邊緣化
矩陣H的稀疏結構；
H矩陣當中的非對角部分的非零矩陣塊可以理解為其對應的兩個變量之間存在聯系，或者可以稱之為約束——圖優化結構與增量方程的稀疏性存在着明顯的聯系
通常有路標的數量遠遠大於相機位姿的數量，因此H矩陣又稱為箭頭形矩陣或鎬形矩陣。
3.4） 利用H矩陣的稀疏性加速計算的方法——Schur消元（在SLAM亦稱為Marginalization, 邊緣化）
對線性方程組進行高斯消元，目標是消去右上角的非對角部分E.
求解過程相當於做了條件概率展開，結果是求出了關於xc的邊緣分布，所以又稱為邊緣化。
3.5） S的稀疏性是不規則的，取決於實際觀測的結果。
S矩陣非對角線上的非零矩陣塊，表示了該處對應的兩個相機變量之間存在着共同觀測的路標點,有時候稱為共視（Co-visibility）；
ORB-SLAM 選擇具有共同觀測的幀作為關鍵幀，因此Schur消元后得到的S是稠密的
3.6） 魯棒核函數
核函數保證每條邊的誤差不會大的沒邊而掩蓋掉其他的邊
常用的核函數：Huber核 Cauch核 Tukey核
Schur消元也並不意味着將所有路標消元，將相機變量消元也是SLAM當中采用的手段。
4. BA實現：G2O
4.1） 參數的配置和解析：（command_args.cpp BundleParams.h ）
可以初始設置參數的名字，默認值，以及各種不同的類型參數
可以解析可執行傳入的參數，解析並保存到對應的變量中，進行使用
可通過-h選項，輸出當前已有的參數名以及相關默認的值
eg: -input ~qifarui/code/slambook-master/ch10/g2o_custombundle/data/problem-16-22106-pre.txt -h (先設置輸入的文件名，然后通過 -h進行輸出)
4.2）BALProblem.cpp
PLY點雲文件生成（包括相機中心點的寫入，以及 空間特征點的寫入）
相機，路邊以及量測信息讀入（特征點位置以及相機位姿信息的讀入，以及相機和特征點量測信息）
4.3）利用軸角進行坐標變換
羅德里格斯公式實現坐標變換
利用相機位姿信息求解相機原點在世界坐標系下的坐標（ c = -R't）。
軸角和四元數的轉化
利用相機中心在世界坐標系下的坐標生成相機的位姿信息
（Eigen中的Map類——Eigen如何與原生raw C/C++ 數組混合編程）
4.4） Normalize 函數實現：（作用？？？？）
特征點的歸一化實現（相對於中間點的比例縮放）
相機中心點按照特征點， 在幾何體中心點等比例進行縮放。
4.5） Perturb（）擾動誤差添加
根據設定的標准差，對輸入的特征點坐標，相機位姿（平移加旋轉）添加白噪聲
4.6） g2o 優化設置
4.6.1） SetSolverOptionsFromFlags 求解設置：
使用何種方法（列文伯格， DogLeg等）來定義非線性優化的下降策略
使用哪類線性求解器（稠密 or稀疏（額外需要對求解器進行設置-矩陣排序））
4.6.2） BuildProblem 構造最優化問題，增加邊和頂點：
a 相機頂點類VertexCameraBAL定義，及頂點初始添加（包括相機位姿（旋轉+平移），相機參數）
b 路邊定點VertexCameraBAL定義，及頂點初始添加
注意：
each vertex should have an unique id, no matter it is a camera vertex, or a point vertex ；
pPoint→setMarginalized(true); // 設置路標點的schur消元
4.6.3) 邊EdgeObservationBAL定義, 以及添加
a 誤差計算函數實現
b （）運算付重載，用於投影誤差計算
c CamProjectionWithDistortion:根據坐標點以及相機參數（內參 外參以及畸變系數）得到像素坐標點
d ceres庫的自動求導功能使用：AutoDiff，獲得雅克比矩陣;
注：自動求導功能需要類型將需要求導的公式實現在括號運算符里。
e 量測信息獲取，並添加（根據相機和路標點ID設置邊連接的頂點，對應邊的量測信息設置）
f 量測信息協方差矩陣設置
g 根據配置信息，設置魯棒核函數huber
4.6.4) 優化器初始化，根據設置的迭代次數，開啟優化
4.7） 將優化器優化的結果寫入的本地優化變量中，包括相機的參數信息以及路標點的位置信息，通過memcpy的形式
4.8） 根據配置的文件名，將最終的結果寫入到點雲ply文件中。
注： g2o庫說明：
1） 由於g2o庫本身公開API說明較少，只能通過網上的開源項目及其本身的源代碼來了解它
2） g2o在做BA的優化時，必須將其所有點雲全部Schur掉，否則會報錯。其原因在於使用了g2o::LinearSolver<BalBlockSolver::PoseMatrixType>* linearSolver的線性求解器，其中模板參數當中的 PoseMatrixType在程序中為相機姿態參數的維度，於是BA當中Schur消元后解的線性方程組必須是只含有相機的姿態變量。
BA實現g2o結果如下：
bal problem have 16 cameras and 22106 points. Forming 83718 observatoins. beginning problem... Normalization complete... begin optimizaiton .. iteration= 0 chi2= 1593976.900329 time= 0.430721 cumTime= 0.430721 edges= 83718 schur= 1 lambda= 9370.114177 levenbergIter= 1 iteration= 1 chi2= 1503859.189085 time= 0.327325 cumTime= 0.758047 edges= 83718 schur= 1 lambda= 3123.371392 levenbergIter= 1 iteration= 2 chi2= 1340948.148116 time= 0.322999 cumTime= 1.08105 edges= 83718 schur= 1 lambda= 1041.123797 levenbergIter= 1 iteration= 3 chi2= 1104714.717556 time= 0.335624 cumTime= 1.41667 edges= 83718 schur= 1 lambda= 347.041266 levenbergIter= 1 iteration= 4 chi2= 879105.141579 time= 0.323237 cumTime= 1.73991 edges= 83718 schur= 1 lambda= 115.680422 levenbergIter= 1 iteration= 5 chi2= 723703.972964 time= 0.318097 cumTime= 2.058 edges= 83718 schur= 1 lambda= 38.560141 levenbergIter= 1 iteration= 6 chi2= 615709.929828 time= 0.317493 cumTime= 2.3755 edges= 83718 schur= 1 lambda= 12.853380 levenbergIter= 1 iteration= 7 chi2= 503775.361750 time= 0.316782 cumTime= 2.69228 edges= 83718 schur= 1 lambda= 4.284460 levenbergIter= 1 iteration= 8 chi2= 381980.060879 time= 0.320198 cumTime= 3.01248 edges= 83718 schur= 1 lambda= 1.428153 levenbergIter= 1 iteration= 9 chi2= 267385.644652 time= 0.34434 cumTime= 3.35682 edges= 83718 schur= 1 lambda= 0.476051 levenbergIter= 1 iteration= 10 chi2= 170314.069365 time= 0.342958 cumTime= 3.69977 edges= 83718 schur= 1 lambda= 0.158684 levenbergIter= 1 iteration= 11 chi2= 108750.143596 time= 0.325639 cumTime= 4.02541 edges= 83718 schur= 1 lambda= 0.052895 levenbergIter= 1 iteration= 12 chi2= 79228.387608 time= 0.33411 cumTime= 4.35952 edges= 83718 schur= 1 lambda= 0.035263 levenbergIter= 1 iteration= 13 chi2= 62903.760510 time= 0.359433 cumTime= 4.71896 edges= 83718 schur= 1 lambda= 0.023509 levenbergIter= 1 iteration= 14 chi2= 51637.899996 time= 0.332307 cumTime= 5.05126 edges= 83718 schur= 1 lambda= 0.015672 levenbergIter= 1 iteration= 15 chi2= 42491.525780 time= 0.352168 cumTime= 5.40343 edges= 83718 schur= 1 lambda= 0.010448 levenbergIter= 1 iteration= 16 chi2= 37296.044380 time= 0.32017 cumTime= 5.7236 edges= 83718 schur= 1 lambda= 0.005843 levenbergIter= 1 iteration= 17 chi2= 36145.312948 time= 0.375634 cumTime= 6.09924 edges= 83718 schur= 1 lambda= 0.001948 levenbergIter= 1 iteration= 18 chi2= 36069.306555 time= 0.334505 cumTime= 6.43374 edges= 83718 schur= 1 lambda= 0.000649 levenbergIter= 1 iteration= 19 chi2= 36067.831214 time= 0.32886 cumTime= 6.7626 edges= 83718 schur= 1 lambda= 0.000216 levenbergIter= 1 optimization complete..
生成的點雲結果通過 meshlab 打開以后，結果如下：（上為initial， 下為final）
5. BA實現：Ceres
該部分將介紹Ceres實現，其中和g2o相同部分的輸入，配置解析等代碼講不會介紹。
5.1）最小二乘優化問題構建：Problem
void BuildProblem(BALProblem* bal_problem, Problem* problem, const BundleParams& params) { const int point_block_size = bal_problem->point_block_size(); const int camera_block_size = bal_problem->camera_block_size(); double* points = bal_problem->mutable_points(); double* cameras = bal_problem->mutable_cameras(); // Observations is 2 * num_observations long array observations // [u_1, u_2, ... u_n], where each u_i is two dimensional, the x // and y position of the observation. const double* observations = bal_problem->observations(); for(int i = 0; i < bal_problem->num_observations(); ++i){ CostFunction* cost_function; // Each Residual block takes a point and a camera as input // and outputs a 2 dimensional Residual cost_function = SnavelyReprojectionError::Create(observations[2*i + 0], observations[2*i + 1]); // If enabled use Huber's loss function. LossFunction* loss_function = params.robustify ? new HuberLoss(1.0) : NULL; // Each observatoin corresponds to a pair of a camera and a point // which are identified by camera_index()[i] and point_index()[i] // respectively. double* camera = cameras + camera_block_size * bal_problem->camera_index()[i]; double* point = points + point_block_size * bal_problem->point_index()[i]; problem->AddResidualBlock(cost_function, loss_function, camera, point); } }
5.1.1） 向問題中添加殘差，各個觀測的殘差信息：
problem->AddResidualBlock(cost_function, loss_function, camera, point);
其中參數包括代價函數（殘差計算函數） 損失函數（魯棒核函數——根據配置項進行設定），
待估計的參數位姿和路標點
5.1.2） 代價函數：ceres::AutoDiffCostFunction<SnavelyReprojectionError,2,9,3>
自動求導；
模板參數：殘差計算定義類SnavelyReprojectionError， 殘差維度，待估計相機參數維度， 待估計路標點維度；
new的構造參數： SnavelyReprojectionError變量
class SnavelyReprojectionError { public: SnavelyReprojectionError(double observation_x, double observation_y):observed_x(observation_x),observed_y(observation_y){} template<typename T> bool operator()(const T* const camera, const T* const point, T* residuals)const{ // camera[0,1,2] are the angle-axis rotation T predictions[2]; CamProjectionWithDistortion(camera, point, predictions); residuals[0] = predictions[0] - T(observed_x); residuals[1] = predictions[1] - T(observed_y); return true; } static ceres::CostFunction* Create(const double observed_x, const double observed_y){ return (new ceres::AutoDiffCostFunction<SnavelyReprojectionError,2,9,3>( new SnavelyReprojectionError(observed_x,observed_y))); } private: double observed_x; double observed_y; };
5.1.3） 根據相機和路標點的連接索引ID，將對應的量測信息和待估計參數添加到Problem中
（其中待估計參數直接復用原有變量的地址，不會額外進行申請變量，而在g2o中將這些信息以邊，頂點這種額外的變量類型添加到優化問題中，同時優化完成以后，又需要將這些信息返回到之前的變量中）
5.2） 求解器設置：Solver::Options
5.2.1） 最大迭代次數，最小化進度輸出，線程個數設置等
options->max_num_iterations = params.num_iterations; options->minimizer_progress_to_stdout = true; options->num_threads = params.num_threads;
5.2.2）優化方法增量方程求解中：下降策略 or 信賴區域策略
StringToTrustRegionStrategyType(params.trust_region_strategy, &options->trust_region_strategy_type);
5.2.3）線性求解器設置
ceres::StringToLinearSolverType(params.linear_solver, &options->linear_solver_type); ceres::StringToSparseLinearAlgebraLibraryType(params.sparse_linear_algebra_library, &options->sparse_linear_algebra_library_type); ceres::StringToDenseLinearAlgebraLibraryType(params.dense_linear_algebra_library, &options->dense_linear_algebra_library_type); options->num_threads = params.num_threads;
5.2.4） Schur消元順序管理類型ceres::ParameterBlockOrdering設置：
通過編號大小設置消元的優先級，優先消元編號最小的變量。
void SetOrdering(BALProblem* bal_problem, ceres::Solver::Options* options, const BundleParams& params) { const int num_points = bal_problem->num_points(); const int point_block_size = bal_problem->point_block_size(); double* points = bal_problem->mutable_points(); const int num_cameras = bal_problem->num_cameras(); const int camera_block_size = bal_problem->camera_block_size(); double* cameras = bal_problem->mutable_cameras(); if (params.ordering == "automatic") return; ceres::ParameterBlockOrdering* ordering = new ceres::ParameterBlockOrdering; // The points come before the cameras for(int i = 0; i < num_points; ++i) ordering->AddElementToGroup(points + point_block_size * i, 0); for(int i = 0; i < num_cameras; ++i) ordering->AddElementToGroup(cameras + camera_block_size * i, 1); options->linear_solver_ordering.reset(ordering); }
5.2.5) 梯度閾值，相鄰兩次迭代之間目標函數差值的閾值設置
options.gradient_tolerance = 1e-16; options.function_tolerance = 1e-16;
5.3 求解函數：Solve
參數輸入：配置信息，優化問題，輸出優化信息
Solver::Summary summary; Solve(options, &problem, &summary); std::cout << summary.FullReport() << "\n";
Ceres優勢：
1）能夠在原數組上進行操作
2）優化設置非常便捷（Solver::Options類型成員變量賦值， schur消元順序等）
3）API說明詳細，可讀性高，多讀讀源碼，加深理解和應用
輸出結果：
bal problem have 16 cameras and 22106 points. Forming 83718 observatoins. beginning problem... Normalization complete... the problem is successfully build.. iter cost cost_change |gradient| |step| tr_ratio tr_radius ls_iter iter_time total_time 0 4.185660e+06 0.00e+00 2.16e+07 0.00e+00 0.00e+00 1.00e+04 0 1.27e-01 3.83e-01 1 1.980525e+05 3.99e+06 5.34e+06 2.40e+03 9.60e-01 3.00e+04 1 2.56e-01 6.39e-01 2 5.086543e+04 1.47e+05 2.11e+06 1.01e+03 8.22e-01 4.09e+04 1 2.41e-01 8.80e-01 3 1.859667e+04 3.23e+04 2.87e+05 2.64e+02 9.85e-01 1.23e+05 1 2.43e-01 1.12e+00 4 1.803857e+04 5.58e+02 2.69e+04 8.66e+01 9.93e-01 3.69e+05 1 2.39e-01 1.36e+00 5 1.803391e+04 4.66e+00 3.11e+02 1.02e+01 1.00e+00 1.11e+06 1 2.41e-01 1.60e+00 6 1.803390e+04 1.85e-03 2.12e+00 4.60e-01 1.01e+00 3.32e+06 1 2.41e-01 1.84e+00 7 1.803390e+04 1.01e-06 9.61e-02 8.99e-03 1.03e+00 9.95e+06 1 2.39e-01 2.08e+00 8 1.803390e+04 1.62e-09 4.97e-03 1.98e-04 1.05e+00 2.98e+07 1 2.40e-01 2.32e+00
結果如下：
參考鏈接
http://www.ceres-solver.org/index.html
[ceres-solver] AutoDiff
Ceres（一）自動求導（AutomaticDiff）
SLAM十四講 解決第10講 num_linear_solver_threads找不到的問題