一、什么是后端优化
上一篇文章介绍了视觉里程计的设计与实现，也就是所谓的“前端”。既然有前端就一定有后端，本文就来介绍一下SLAM的后端优化。
前端的视觉里程计可以给出一个增量式的地图，但由于不可避免的误差累计，这个地图在长时间内是不准确的。而SLAM致力于构建一个长生命周期的可靠的解决方案，因此只有前端是远远不够的。当地图增长到一定程度后，累计误差会使后来的数据越来越不准确。这时我们需要把所有地图数据放到一起做一次完整的优化，从而降低各部分的误差。
后端优化有很多种方案，过去采用以扩展卡尔曼滤波（Extended Kalman Filter，EKF）为主的滤波器方案，现在大多都采用非线性优化方案。EKF由于假设了马尔可夫性质，只利用前一状态来估计当前状态的值，这有点像视觉里程计中只考虑相邻两帧的关系一样，很难做到全局的优化。而现在常用的非线性优化方法，则是把所有数据都考虑进来，放在一起优化，虽然会增大计算量，但效果好得多。
二、Bundle Adjustment（BA）
本文主要来介绍一下采用Bundle Adjustment的非线性优化方法。
其实在之前的文章《3D-2D相机位姿估计》和《3D-3D相机位姿估计》中，我们都用了BA来做非线性优化，但只是优化相邻两张图片间的位姿和路标点。而现在，对于后端优化来说，我们需要优化整个地图的全部位姿和全部路标点，数据量比之前大了不知多少倍。
虽然理论上来说，数据量大并不影响BA方法。但唯一的障碍是数据量大会导致计算时间急剧增大。因为在用梯度下降法求解时，每一轮迭代至少要解一个线性方程组，这就等同于求一个矩阵的逆。矩阵求逆的时间复杂度是O(n3)，于是巨大的数据量导致这个矩阵维度极高，从而使求解用时大的离谱。这也就解释了为什么EKF曾经是后端优化的主流，因为它计算量小呀。
那为什么非线性优化又后来居上了呢？
21世纪以来，人们逐渐意识到如果矩阵具有一定形式的稀疏性，可以加速求逆的过程。而SLAM后端的非线性优化恰恰可以利用这一性质！
SLAM的相机位姿和路标点其实具有非常特殊的结构，并非随机产生。相机位姿和路标点之间是多对多的关系，一个相机位姿可以观测到多个路标点，一个路标点也可以被多个相机位姿观测到。由于相机的大范围运动，局部区域的路标点只会被局部的几个相机位姿观测到，而其它大部分相机位姿都观测不到这些点，这是产生稀疏性的根源。当我们构建了非线性优化的代价函数后，需要求代价函数对所有优化变量的偏导数，稀疏性意味着这些偏导数大部分为0，只有小部分不为0，这些不为0的项对应着相机位姿与其能够观测到的路标点的组合。Schur消元法利用矩阵的稀疏性求逆，是BA中求解增量方程的常用手段。
原理大概是这么个原理，具体到实际操作，又会遇到很多问题。我们以g2o图优化方法构建BA为例看一看如何实现一个后端优化。
三、g2o求解BA
与之前的做法一样，用顶点表示相机位姿和路标点，用边表示它们之间的观测。自定义的顶点和边如下。
// 相机位姿顶点，维度为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;
}
};
// 路标点顶点，维度为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;
}
};
// 代表观测的边，是二元边，两端分别连接相机位姿顶点和路标点顶点
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的自动求导，第一个参数对象必须包含一个重载的函数调用运算符，也就是前面定义的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只有数值求导，所以这里用了Ceres自动求导以提高效率。
使用上面定义顶点和边构建图优化问题的代码如下。
// 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) ;
}
}
这里，每个路标点都调用了setMarginalized方法以利用矩阵的稀疏性。每条边都调用了setRobustKernel方法添加了Huber核函数，以避免异常值对结果产生过大影响。
启动优化的代码这里就不贴出来了。我们运行程序，采用一个未优化的点云数据作为输入，并将优化后的结果输出，将它们同时显示在Meshlab上，效果如下。
左边是优化前的点云，右边是优化后的点云。明显优化后的点云看起来整齐干净了许多，而优化前的点云则比较杂乱。
本文使用了高翔博士的示例代码，地址是：https://github.com/gaoxiang12/slambook/tree/master/ch10/g2o_custombundle
大家可以下载测试。
四、参考资料
《视觉SLAM十四讲》第10讲 后端1 高翔
