1.前言
以BA為主的圖優化，很多路標點，大量特征點，因為有這大量的特征點，所以實時不好，要在更大的場景上用，必須進行優化。這裏後端有兩種方法，一種位姿圖，一種因子圖。
2.位姿圖
BA帶有很多的相機位姿和空間點，時間一長，BA計算效率會下降。但是實際情況中，經過若幹次觀測之後，收斂的特征點和空間位置肯定會收斂到一個點，而那些發散的外點，也通常看不到了，所以優化幾次就可以把它們給固定住，沒有必要再優化了。
位姿圖其實就是BA中，不管路標了，原來是路標和頂點之間構成邊，而這裏是頂點和頂點之間構成邊，求得是兩個相機位姿之間的相對運動。路標點這時候只是對姿態節點的約束。
實時BA，只能處理幾萬個點，而一個關鍵幀就幾百個點。所以要實時，必須要做一些優化。比如滑動窗口法，丟棄一些歷史數據。比如位姿圖，舍棄對路標點的優化。
2.1位姿圖的優化
先了解一下李代數SO3,SE3.SO3是由三維向量fai組成的，fai.hat是一個3*3的矩陣，exp(fai.hat)就是旋轉矩陣。
SE3是由6維向量ep組成的，ep是由p和fai組成，p就是一個三維向量。ep.hat形式為fai.hat,p,0.trans,0組成的一個4*4矩陣，exp(ep.hat)就是變換矩陣。
李代數就更簡單，就是SO3就是由旋轉矩陣R組成的，R*R.trans=I,det(R)=1.
SE3就是變換矩陣組成的，R屬於SO3.變換矩陣是4*4的矩陣。
先進行的ln,再進行的矩陣到向量的 變換。所以誤差eij是一個6維的向量，屬於SE3.
這裏誤差的計算公式是，v1=_vertices[0]->estimate,v2=_vertices[1]->estimate,v1,v2都是SE3形式。測量值也是SE3的格式。
_error=(_measurement.inverse()*v1.inverse()*v2).log()
不明白這裏的誤差為什麽還要取個log.
這裏求導數，先求J.造了一個JRInv的函數，裏面放入的變量是SE3::exp(_error)=e.
這裏A11就為SO3::hat(e.so3().log());
A12=SO3::hat(e.translation());A21=0,A22=SO3::hat(e.so3().log());
J=A*0.5+I.
而誤差對epi的導數就為-J*(v2.inverse().Adj())
誤差對epj的導數就為J*(v2.inverse().Adj())
2.2實踐
數據是g2o文件，就是sphere.g2o.轉圈上升，t-1時刻與t時刻的邊，裏程計，層與層之間，回環。
sphere.g2o文件前半部分由節點組成，VERTEX_SE3，默認用四元數和平移向量，就是Id,平移，旋轉。qw在最後。前面都是VERTEX_SE3.
後半部分就都是邊了，EDGE_SE3:兩個id,tx,ty,tz,qx,qy,qz,qw,信息矩陣的右上角（因為信息矩陣為對稱陣，只需保存一半就可以）
SE3實際上是四元數而非李代數。
2.2.1簡單優化
如果是簡單優化的話，就比較好做了，因為有sphere.g2o文件，文件提供了各個節點和邊，所以直接讀取做優化就可以了。頂點和邊可以用g2o原有的形式。
先設fin，ifstream fin(argv[1]),還要做兩個判斷，argc!=2,!fin,!fin裏的輸出通常就是文件不存在。
定義頂點數和邊數vertexCnt,edgeCnt,初值為0。
用!fin.eof()做一個while循環，在循環中
string name,fin>>name,如果name為節點，那麽讀取頂點值。這裏v用的是g2o::VertexSE3()形式，fin先賦值給index,然後v->setId(index),v->read(fin);v是指針，指針訪問從來都是->.圖模型添加頂點，頂點數加1.當index為0的時候，v->setFixed(true),因為是初值啊。
如果name為邊，那麽讀邊，邊形式g2o::EdgeSE3(),定義idx1,idx2,fin>>idx1>>idx2,fin分別賦值給idx1,idx2,這兩個id是節點id,邊的id設的是edgeCnt++,然後邊設置頂點0為optimizer.vertices()[idx1],頂點1為optimizer.vertices()[idx2],然後e從fin裏讀值，e->read(fin),圖模型添加邊。

其他都差不多，只不過這裏optimizer.optimize(30)之後，optimizer.save("result.g2o"),把優化結果保存到result.g2o中。
2.2.2位姿圖優化的實踐
這裏定義了自己的頂點和邊。
先定義求J_R^{-1}的近似函數JRInv,變量是SE3 e,返回值是6*6的矩陣。
這裏A11=SE3::hat(e.so3().log()),A12=SE3::hat(e.translation()),
A21=0,A22=A11.
定義了自己的頂點VertexSE3LieAlgebra,頂點為6維向量，類型為SE3.
頂點對read函數進行了操作，用來讀tx,ty,tz,qx,qy,qz,qw的，is賦值給data,估計值就可以設為
setEstimate(SE3(Eigen::Quaterniond(data[6],data[3],data[4],data[5]),Eigen::Vector3d(data[0],data[1],data[2])));到時候v直接read fin就可以了。
頂點對write函數也進行了操作，變量os,os輸出的有id(), _estimate.translation().transpose()，定義完q=_estimate.unit_quaternion(),輸出q.coeffs()[0]到3.
更新值up由Sophus::SO3(update[3],update[4],update[5])和Eigen::vector3d組成，而且更新的方式是乘即_estimate=up*_estimate.
定義了自己的邊EdgeSE3LieAlgebra,誤差值是6為的SE3,兩個頂點就是我們之前定義的頂點。
在read函數，跟頂點一樣，把前7個值賦給data,後四個數組成四元數q,q要規範化一下，用q.normalize,這7個數組成了測量值，即setMeasurement(q,Eigen::Vector3d(data[0],data[1],data[2],data[3]).對於0<=i<information().rows(),i<=j<information().cols(),is把剩下的數賦給信息矩陣的(i,j),對於i和j不等的情況，(j,i)=(i,j).
write函數，變量os,把_vertices[0],_vertices[1]賦值給指針v1,v2,輸出v1,v2的id.同頂點，輸出信息矩陣，輸出測量值的四元數的協方差。
計算誤差和導數，跟之前一樣。
接下來的主函數和2.2.1一樣，只是頂點和邊的類型不同。
位姿圖有帶狀的（機器人直線前進的時候），球樣的稠密的位姿圖。
2.3小結
後端和前端可不同時，有時候稱為跟蹤和建圖，這裏的建圖其實是後端來著。前端實時，後端則可以慢悠悠地優化，只要在優化完成時把結果返回成前端就可以了。

。

後端 位姿圖 第11章