一卢鹦、什么是后端優(yōu)化
上一篇文章介紹了視覺里程計的設(shè)計與實(shí)現(xiàn)鼎文，也就是所謂的“前端”凡简。既然有前端就一定有后端，本文就來介紹一下SLAM的后端優(yōu)化呀闻。
前端的視覺里程計可以給出一個增量式的地圖化借，但由于不可避免的誤差累計，這個地圖在長時間內(nèi)是不準(zhǔn)確的捡多。而SLAM致力于構(gòu)建一個長生命周期的可靠的解決方案蓖康，因此只有前端是遠(yuǎn)遠(yuǎn)不夠的。當(dāng)?shù)貓D增長到一定程度后垒手，累計誤差會使后來的數(shù)據(jù)越來越不準(zhǔn)確蒜焊。這時我們需要把所有地圖數(shù)據(jù)放到一起做一次完整的優(yōu)化，從而降低各部分的誤差科贬。
后端優(yōu)化有很多種方案泳梆，過去采用以擴(kuò)展卡爾曼濾波（Extended Kalman Filter，EKF）為主的濾波器方案榜掌，現(xiàn)在大多都采用非線性優(yōu)化方案鸭丛。EKF由于假設(shè)了馬爾可夫性質(zhì)，只利用前一狀態(tài)來估計當(dāng)前狀態(tài)的值唐责，這有點(diǎn)像視覺里程計中只考慮相鄰兩幀的關(guān)系一樣鳞溉，很難做到全局的優(yōu)化。而現(xiàn)在常用的非線性優(yōu)化方法鼠哥，則是把所有數(shù)據(jù)都考慮進(jìn)來熟菲，放在一起優(yōu)化，雖然會增大計算量朴恳，但效果好得多抄罕。
二、Bundle Adjustment（BA）
本文主要來介紹一下采用Bundle Adjustment的非線性優(yōu)化方法于颖。
其實(shí)在之前的文章《3D-2D相機(jī)位姿估計》和《3D-3D相機(jī)位姿估計》中呆贿，我們都用了BA來做非線性優(yōu)化，但只是優(yōu)化相鄰兩張圖片間的位姿和路標(biāo)點(diǎn)。而現(xiàn)在做入，對于后端優(yōu)化來說冒晰，我們需要優(yōu)化整個地圖的全部位姿和全部路標(biāo)點(diǎn)，數(shù)據(jù)量比之前大了不知多少倍竟块。
雖然理論上來說壶运，數(shù)據(jù)量大并不影響B(tài)A方法。但唯一的障礙是數(shù)據(jù)量大會導(dǎo)致計算時間急劇增大浪秘。因為在用梯度下降法求解時蒋情，每一輪迭代至少要解一個線性方程組，這就等同于求一個矩陣的逆耸携。矩陣求逆的時間復(fù)雜度是O(n3)棵癣，于是巨大的數(shù)據(jù)量導(dǎo)致這個矩陣維度極高，從而使求解用時大的離譜夺衍。這也就解釋了為什么EKF曾經(jīng)是后端優(yōu)化的主流狈谊，因為它計算量小呀。
那為什么非線性優(yōu)化又后來居上了呢刷后？
21世紀(jì)以來的畴，人們逐漸意識到如果矩陣具有一定形式的稀疏性，可以加速求逆的過程尝胆。而SLAM后端的非線性優(yōu)化恰恰可以利用這一性質(zhì)丧裁！
SLAM的相機(jī)位姿和路標(biāo)點(diǎn)其實(shí)具有非常特殊的結(jié)構(gòu)耐朴，并非隨機(jī)產(chǎn)生轧坎。相機(jī)位姿和路標(biāo)點(diǎn)之間是多對多的關(guān)系，一個相機(jī)位姿可以觀測到多個路標(biāo)點(diǎn)丹鸿，一個路標(biāo)點(diǎn)也可以被多個相機(jī)位姿觀測到贪染。由于相機(jī)的大范圍運(yùn)動缓呛，局部區(qū)域的路標(biāo)點(diǎn)只會被局部的幾個相機(jī)位姿觀測到，而其它大部分相機(jī)位姿都觀測不到這些點(diǎn)杭隙，這是產(chǎn)生稀疏性的根源哟绊。當(dāng)我們構(gòu)建了非線性優(yōu)化的代價函數(shù)后，需要求代價函數(shù)對所有優(yōu)化變量的偏導(dǎo)數(shù)痰憎，稀疏性意味著這些偏導(dǎo)數(shù)大部分為0票髓，只有小部分不為0，這些不為0的項對應(yīng)著相機(jī)位姿與其能夠觀測到的路標(biāo)點(diǎn)的組合铣耘。Schur消元法利用矩陣的稀疏性求逆洽沟，是BA中求解增量方程的常用手段。
原理大概是這么個原理蜗细，具體到實(shí)際操作裆操，又會遇到很多問題。我們以g2o圖優(yōu)化方法構(gòu)建BA為例看一看如何實(shí)現(xiàn)一個后端優(yōu)化。
三踪区、g2o求解BA
與之前的做法一樣昆烁，用頂點(diǎn)表示相機(jī)位姿和路標(biāo)點(diǎn)，用邊表示它們之間的觀測朽缴。自定義的頂點(diǎn)和邊如下善玫。
// 相機(jī)位姿頂點(diǎn)水援，維度為9
class VertexCameraBAL : public g2o::BaseVertex<9,Eigen::VectorXd>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
VertexCameraBAL() {}
virtual bool read ( std::istream& /*is*/ )
{
return false;
}
virtual bool write ( std::ostream& /*os*/ ) const
{
return false;
}
virtual void setToOriginImpl() {}
virtual void oplusImpl ( const double* update )
{
Eigen::VectorXd::ConstMapType v ( update, VertexCameraBAL::Dimension );
_estimate += v;
}
};
// 路標(biāo)點(diǎn)頂點(diǎn)密强，維度為3
class VertexPointBAL : public g2o::BaseVertex<3, Eigen::Vector3d>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
VertexPointBAL() {}
virtual bool read ( std::istream& /*is*/ )
{
return false;
}
virtual bool write ( std::ostream& /*os*/ ) const
{
return false;
}
virtual void setToOriginImpl() {}
virtual void oplusImpl ( const double* update )
{
Eigen::Vector3d::ConstMapType v ( update );
_estimate += v;
}
};
// 代表觀測的邊，是二元邊蜗元，兩端分別連接相機(jī)位姿頂點(diǎn)和路標(biāo)點(diǎn)頂點(diǎn)
class EdgeObservationBAL : public g2o::BaseBinaryEdge<2, Eigen::Vector2d,VertexCameraBAL, VertexPointBAL>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
EdgeObservationBAL() {}
virtual bool read ( std::istream& /*is*/ )
{
return false;
}
virtual bool write ( std::ostream& /*os*/ ) const
{
return false;
}
virtual void computeError() override // The virtual function comes from the Edge base class. Must define if you use edge.
{
const VertexCameraBAL* cam = static_cast<const VertexCameraBAL*> ( vertex ( 0 ) );
const VertexPointBAL* point = static_cast<const VertexPointBAL*> ( vertex ( 1 ) );
( *this ) ( cam->estimate().data(), point->estimate().data(), _error.data() );
}
template<typename T>
bool operator() ( const T* camera, const T* point, T* residuals ) const
{
T predictions[2];
CamProjectionWithDistortion ( camera, point, predictions );
residuals[0] = predictions[0] - T ( measurement() ( 0 ) );
residuals[1] = predictions[1] - T ( measurement() ( 1 ) );
return true;
}
virtual void linearizeOplus() override
{
// use numeric Jacobians
// BaseBinaryEdge<2, Vector2d, VertexCameraBAL, VertexPointBAL>::linearizeOplus();
// return;
// using autodiff from ceres. Otherwise, the system will use g2o numerical diff for Jacobians
const VertexCameraBAL* cam = static_cast<const VertexCameraBAL*> ( vertex ( 0 ) );
const VertexPointBAL* point = static_cast<const VertexPointBAL*> ( vertex ( 1 ) );
typedef ceres::internal::AutoDiff<EdgeObservationBAL, double, VertexCameraBAL::Dimension, VertexPointBAL::Dimension> BalAutoDiff;
Eigen::Matrix<double, Dimension, VertexCameraBAL::Dimension, Eigen::RowMajor> dError_dCamera;
Eigen::Matrix<double, Dimension, VertexPointBAL::Dimension, Eigen::RowMajor> dError_dPoint;
double *parameters[] = { const_cast<double*> ( cam->estimate().data() ), const_cast<double*> ( point->estimate().data() ) };
double *jacobians[] = { dError_dCamera.data(), dError_dPoint.data() };
double value[Dimension];
// 注意或渤，這里使用了Ceres的自動求導(dǎo)，第一個參數(shù)對象必須包含一個重載的函數(shù)調(diào)用運(yùn)算符奕扣，也就是前面定義的operator()
bool diffState = BalAutoDiff::Differentiate ( *this, parameters, Dimension, value, jacobians );
// copy over the Jacobians (convert row-major -> column-major)
if ( diffState )
{
_jacobianOplusXi = dError_dCamera;
_jacobianOplusXj = dError_dPoint;
}
else
{
assert ( 0 && "Error while differentiating" );
_jacobianOplusXi.setZero();
_jacobianOplusXi.setZero();
}
}
};
由于g2o只有數(shù)值求導(dǎo)薪鹦，所以這里用了Ceres自動求導(dǎo)以提高效率。
使用上面定義頂點(diǎn)和邊構(gòu)建圖優(yōu)化問題的代碼如下惯豆。
// set up the vertexs and edges for the bundle adjustment.
void BuildProblem(const BALProblem* bal_problem, g2o::SparseOptimizer* optimizer, const BundleParams& params)
{
const int num_points = bal_problem->num_points();
const int num_cameras = bal_problem->num_cameras();
const int camera_block_size = bal_problem->camera_block_size();
const int point_block_size = bal_problem->point_block_size();
// Set camera vertex with initial value in the dataset.
const double* raw_cameras = bal_problem->cameras();
for(int i = 0; i < num_cameras; ++i)
{
ConstVectorRef temVecCamera(raw_cameras + camera_block_size * i,camera_block_size);
VertexCameraBAL* pCamera = new VertexCameraBAL();
pCamera->setEstimate(temVecCamera); // initial value for the camera i..
pCamera->setId(i); // set id for each camera vertex
// remeber to add vertex into optimizer..
optimizer->addVertex(pCamera);
}
// Set point vertex with initial value in the dataset.
const double* raw_points = bal_problem->points();
// const int point_block_size = bal_problem->point_block_size();
for(int j = 0; j < num_points; ++j)
{
ConstVectorRef temVecPoint(raw_points + point_block_size * j, point_block_size);
VertexPointBAL* pPoint = new VertexPointBAL();
pPoint->setEstimate(temVecPoint); // initial value for the point i..
pPoint->setId(j + num_cameras); // each vertex should have an unique id, no matter it is a camera vertex, or a point vertex
// remeber to add vertex into optimizer..
pPoint->setMarginalized(true);
optimizer->addVertex(pPoint);
}
// Set edges for graph..
const int num_observations = bal_problem->num_observations();
const double* observations = bal_problem->observations(); // pointer for the first observation..
for(int i = 0; i < num_observations; ++i)
{
EdgeObservationBAL* bal_edge = new EdgeObservationBAL();
const int camera_id = bal_problem->camera_index()[i]; // get id for the camera;
const int point_id = bal_problem->point_index()[i] + num_cameras; // get id for the point
if(params.robustify)
{
g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
rk->setDelta(1.0);
bal_edge->setRobustKernel(rk);
}
// set the vertex by the ids for an edge observation
bal_edge->setVertex(0,dynamic_cast<VertexCameraBAL*>(optimizer->vertex(camera_id)));
bal_edge->setVertex(1,dynamic_cast<VertexPointBAL*>(optimizer->vertex(point_id)));
bal_edge->setInformation(Eigen::Matrix2d::Identity());
bal_edge->setMeasurement(Eigen::Vector2d(observations[2*i+0],observations[2*i + 1]));
optimizer->addEdge(bal_edge) ;
}
}
這里池磁，每個路標(biāo)點(diǎn)都調(diào)用了setMarginalized方法以利用矩陣的稀疏性。每條邊都調(diào)用了setRobustKernel方法添加了Huber核函數(shù)楷兽，以避免異常值對結(jié)果產(chǎn)生過大影響地熄。
啟動優(yōu)化的代碼這里就不貼出來了。我們運(yùn)行程序芯杀，采用一個未優(yōu)化的點(diǎn)云數(shù)據(jù)作為輸入端考，并將優(yōu)化后的結(jié)果輸出，將它們同時顯示在Meshlab上揭厚，效果如下却特。
左邊是優(yōu)化前的點(diǎn)云，右邊是優(yōu)化后的點(diǎn)云筛圆。明顯優(yōu)化后的點(diǎn)云看起來整齊干凈了許多裂明，而優(yōu)化前的點(diǎn)云則比較雜亂。
本文使用了高翔博士的示例代碼太援，地址是：https://github.com/gaoxiang12/slambook/tree/master/ch10/g2o_custombundle
大家可以下載測試闽晦。
四、參考資料
《視覺SLAM十四講》第10講 后端1 高翔
