一、什么是后端優(yōu)化
上一篇文章介紹了視覺里程計的設(shè)計與實現(xiàn)，也就是所謂的“前端”。既然有前端就一定有后端，本文就來介紹一下SLAM的后端優(yōu)化。
前端的視覺里程計可以給出一個增量式的地圖，但由于不可避免的誤差累計，這個地圖在長時間內(nèi)是不準(zhǔn)確的。而SLAM致力于構(gòu)建一個長生命周期的可靠的解決方案，因此只有前端是遠遠不夠的。當(dāng)?shù)貓D增長到一定程度后，累計誤差會使后來的數(shù)據(jù)越來越不準(zhǔn)確。這時我們需要把所有地圖數(shù)據(jù)放到一起做一次完整的優(yōu)化，從而降低各部分的誤差。
后端優(yōu)化有很多種方案，過去采用以擴展卡爾曼濾波（Extended Kalman Filter，EKF）為主的濾波器方案，現(xiàn)在大多都采用非線性優(yōu)化方案。EKF由于假設(shè)了馬爾可夫性質(zhì)，只利用前一狀態(tài)來估計當(dāng)前狀態(tài)的值，這有點像視覺里程計中只考慮相鄰兩幀的關(guān)系一樣，很難做到全局的優(yōu)化。而現(xiàn)在常用的非線性優(yōu)化方法，則是把所有數(shù)據(jù)都考慮進來，放在一起優(yōu)化，雖然會增大計算量，但效果好得多。
二、Bundle Adjustment（BA）
本文主要來介紹一下采用Bundle Adjustment的非線性優(yōu)化方法。
其實在之前的文章《3D-2D相機位姿估計》和《3D-3D相機位姿估計》中，我們都用了BA來做非線性優(yōu)化，但只是優(yōu)化相鄰兩張圖片間的位姿和路標(biāo)點。而現(xiàn)在，對于后端優(yōu)化來說，我們需要優(yōu)化整個地圖的全部位姿和全部路標(biāo)點，數(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的相機位姿和路標(biāo)點其實具有非常特殊的結(jié)構(gòu)，并非隨機產(chǎn)生。相機位姿和路標(biāo)點之間是多對多的關(guān)系，一個相機位姿可以觀測到多個路標(biāo)點，一個路標(biāo)點也可以被多個相機位姿觀測到。由于相機的大范圍運動，局部區(qū)域的路標(biāo)點只會被局部的幾個相機位姿觀測到，而其它大部分相機位姿都觀測不到這些點，這是產(chǎn)生稀疏性的根源。當(dāng)我們構(gòu)建了非線性優(yōu)化的代價函數(shù)后，需要求代價函數(shù)對所有優(yōu)化變量的偏導(dǎo)數(shù)，稀疏性意味著這些偏導(dǎo)數(shù)大部分為0，只有小部分不為0，這些不為0的項對應(yīng)著相機位姿與其能夠觀測到的路標(biāo)點的組合。Schur消元法利用矩陣的稀疏性求逆，是BA中求解增量方程的常用手段。
原理大概是這么個原理，具體到實際操作，又會遇到很多問題。我們以g2o圖優(yōu)化方法構(gòu)建BA為例看一看如何實現(xiàn)一個后端優(yōu)化。
三、g2o求解BA
與之前的做法一樣，用頂點表示相機位姿和路標(biāo)點，用邊表示它們之間的觀測。自定義的頂點和邊如下。
// 相機位姿頂點，維度為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)點頂點，維度為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;
}
};
// 代表觀測的邊，是二元邊，兩端分別連接相機位姿頂點和路標(biāo)點頂點
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)用運算符，也就是前面定義的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)以提高效率。
使用上面定義頂點和邊構(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ào)用了setMarginalized方法以利用矩陣的稀疏性。每條邊都調(diào)用了setRobustKernel方法添加了Huber核函數(shù)，以避免異常值對結(jié)果產(chǎn)生過大影響。
啟動優(yōu)化的代碼這里就不貼出來了。我們運行程序，采用一個未優(yōu)化的點云數(shù)據(jù)作為輸入，并將優(yōu)化后的結(jié)果輸出，將它們同時顯示在Meshlab上，效果如下。
左邊是優(yōu)化前的點云，右邊是優(yōu)化后的點云。明顯優(yōu)化后的點云看起來整齊干凈了許多，而優(yōu)化前的點云則比較雜亂。
本文使用了高翔博士的示例代碼，地址是：https://github.com/gaoxiang12/slambook/tree/master/ch10/g2o_custombundle
大家可以下載測試。
四、參考資料
《視覺SLAM十四講》第10講 后端1 高翔
