bundle adjustment 详解

3D视觉工坊和计算机视觉工坊优先发表自己文章(博客优先级滞后)

bundle adjustment 的历史发展

bundle adjustment,中文名称是光束法平差,经典的BA目的是优化相机的pose和landmark,其在SfM和SLAM 领域中扮演者重要角色.目前大多数书籍或者参老文献将其翻译成"捆绑调整"是不太严谨的做法.bundle adjustment 最早是19世纪由搞大地测量学(测绘学科)的人提出来的,19世纪中期的时候，geodetics的学者就开始研究large scale triangulations（大型三角剖分）。20世纪中期，随着camera和computer的出现，photogrammetry(摄影测量学)也开始研究adjustment computation，所以他们给起了个名字叫bundle adjustment(隶属摄影测量学科前辈的功劳)。21世纪前后，robotics领域开始兴起SLAM，最早用的recursive bayesian filter（递归贝叶斯滤波），后来把问题搞成个graph然后用least squares方法求解,bundle adjusment历史发展图如下:

bundle adjustment 其本质还是离不开最小二乘原理(Gauss功劳)(几乎所有优化问题其本质都是最小二乘),目前bundle adjustment 优化框架最为代表的是ceres solver和g2o(这里主要介绍ceres solver).据说ceres的命名是天文学家Piazzi闲暇无事的时候观测一颗没有观测到的星星,最后用least squares算出了这个小行星的轨道,故将这个小行星命名为ceres.

Bundle adjustment 的算法理论

观测值:像点坐标
优化量(平差量):pose 和landmark
因为一旦涉及平差,就必定有如下公式:观测值+观测值改正数=近似值+近似值改正数,那么bundle adjustment 的公式还是从共线条件方程出发:

x−x0=−fa1(XA−XS)+b1(YA−YS)+c1(ZA−LS)a3(XA−XS)+b3(YA−YS)+c3(ZA−ZS)y−y0=−fa2(XA−XS)+b2(YA−YS)+c2(ZA−ZS)a3(XA−XS)+b3(YA−YS)+c3(ZA−ZS)\begin{array}{l} x-x_{0}=-f \frac{a_{1}\left(X_{A}-X_{S}\right)+b_{1}\left(Y_{A}-Y_{S}\right)+c_{1}\left(Z_{A}-L_{S}\right)}{a_{3}\left(X_{A}-X_{S}\right)+b_{3}\left(Y_{A}-Y_{S}\right)+c_{3}\left(Z_{A}-Z_{S}\right)} \\ y-y_{0}=-f \frac{a_{2}\left(X_{A}-X_{S}\right)+b_{2}\left(Y_{A}-Y_{S}\right)+c_{2}\left(Z_{A}-Z_{S}\right)}{a_{3}\left(X_{A}-X_{S}\right)+b_{3}\left(Y_{A}-Y_{S}\right)+c_{3}\left(Z_{A}-Z_{S}\right)} \end{array} x−x0=−fa3(XA−XS)+b3(YA−YS)+c3(ZA−ZS)a1(XA−XS)+b1(YA−YS)+c1(ZA−LS)y−y0=−fa3(XA−XS)+b3(YA−YS)+c3(ZA−ZS)a2(XA−XS)+b2(YA−YS)+c2(ZA−ZS)

优化函数(误差方程)如下,其中uij是像点坐标,Cj是相机投影矩阵,Xi是三维点坐标:

min⁡∑i=1n∑j=1m(uij−π(Cj,Xi))2\min \sum_{i=1}^{n} \sum_{j=1}^{m}\left(u_{i j}-\pi\left(C_{j}, X_{i}\right)\right)^{2} mini=1∑nj=1∑m(uij−π(Cj,Xi))2

四种Bundle adjustment 算法代码

这里代码主要从四个方面来介绍:

优化相机内参及畸变系数,相机的pose(6dof)和landmark
代价函数写法如下:

template <typename CameraModel>
class BundleAdjustmentCostFunction {public:explicit BundleAdjustmentCostFunction(const Eigen::Vector2d& point2D): observed_x_(point2D(0)), observed_y_(point2D(1)) {}
//构造函数传入的是观测值static ceres::CostFunction* Create(const Eigen::Vector2d& point2D) {return (new ceres::AutoDiffCostFunction<BundleAdjustmentCostFunction<CameraModel>, 2, 4, 3, 3,CameraModel::kNumParams>(new BundleAdjustmentCostFunction(point2D)));}
//优化量:2代表误差方程个数;4代表pose中的姿态信息,用四元数表示;3代表pose中的位置信息;3代表landmark
自由度;CameraModel::kNumParams是相机内参和畸变系数,其取决于相机模型是what// opertator 重载函数的参数即是待优化的量template <typename T>bool operator()(const T* const qvec, const T* const tvec,const T* const point3D, const T* const camera_params,T* residuals) const {// Rotate and translate.T projection[3];ceres::UnitQuaternionRotatePoint(qvec, point3D, projection);projection[0] += tvec[0];projection[1] += tvec[1];projection[2] += tvec[2];// Project to image plane.projection[0] /= projection[2];projection[1] /= projection[2];// Distort and transform to pixel space.CameraModel::WorldToImage(camera_params, projection[0], projection[1],&residuals[0], &residuals[1]);// Re-projection error.residuals[0] -= T(observed_x_);residuals[1] -= T(observed_y_);return true;}private:const double observed_x_;const double observed_y_;
};

写好了代价函数,下面就是需要把参数都加入残差块,让ceres自动求导,代码如下:

ceres::Problem problem;
ceres::CostFunction* cost_function = nullptr;
ceres::LossFunction * p_LossFunction =ceres_options_.bUse_loss_function_ ?new ceres::HuberLoss(Square(4.0)): nullptr; // 关于为何使用损失函数,因为现实中并不是所有观测过程中的噪声都服从 //gaussian noise的（或者可以说几乎没有），//遇到有outlier的情况，这些方法非常容易挂掉，//这时候就得用到robust statistics里面的//robust cost(*cost也可以叫做loss, 统计学那边喜欢叫risk) function了，//比较常用的有huber, cauchy等等。
cost_function = BundleAdjustmentCostFunction<CameraModel>::Create(point2D.XY());
//将优化量加入残差块
problem_->AddResidualBlock(cost_function, p_LossFunction, qvec_data,tvec_data, point3D.XYZ().data(),camera_params_data);

如上,case1 的bundle adjustment 就搭建完成!

优化相机内参及畸变系数,pose subset parameterization(pose 信息部分参数优化)和3D landmark,当
只优化姿态信息时候,problem需要添加的代码如下:

    //这里姿态又用欧拉角表示map_poses[indexPose] = {angleAxis[0], angleAxis[1], angleAxis[2], t(0), t(1), t(2)};double * parameter_block = &map_poses.at(indexPose)[0];problem.AddParameterBlock(parameter_block, 6);std::vector<int> vec_constant_extrinsic;vec_constant_extrinsic.insert(vec_constant_extrinsic.end(), {3,4,5});if (!vec_constant_extrinsic.empty()){// 主要用到ceres的SubsetParameterization函数ceres::SubsetParameterization *subset_parameterization =new ceres::SubsetParameterization(6, vec_constant_extrinsic);problem.SetParameterization(parameter_block, subset_parameterization);}

优化相机内参及畸变系数,pose subset parameterization(pose 信息部分参数优化)和3D landmark,当
只优化位置信息时候,problem需要添加的代码如下(同上面代码,只需修改一行):

    //这里姿态又用欧拉角表示map_poses[indexPose] = {angleAxis[0], angleAxis[1], angleAxis[2], t(0), t(1), t(2)};double * parameter_block = &map_poses.at(indexPose)[0];problem.AddParameterBlock(parameter_block, 6);std::vector<int> vec_constant_extrinsic;vec_constant_extrinsic.insert(vec_constant_extrinsic.end(), {1,2,3});if (!vec_constant_extrinsic.empty()){ceres::SubsetParameterization *subset_parameterization =new ceres::SubsetParameterization(6, vec_constant_extrinsic);problem.SetParameterization(parameter_block, subset_parameterization);}

优化相机内参及畸变系数,pose 是常量不优化和3D landmark.
代价函数写法如下:

//相机模型
template <typename CameraModel>
class BundleAdjustmentConstantPoseCostFunction {public:BundleAdjustmentConstantPoseCostFunction(const Eigen::Vector4d& qvec,const Eigen::Vector3d& tvec,const Eigen::Vector2d& point2D): qw_(qvec(0)),qx_(qvec(1)),qy_(qvec(2)),qz_(qvec(3)),tx_(tvec(0)),ty_(tvec(1)),tz_(tvec(2)),observed_x_(point2D(0)),observed_y_(point2D(1)) {}static ceres::CostFunction* Create(const Eigen::Vector4d& qvec,const Eigen::Vector3d& tvec,const Eigen::Vector2d& point2D) {return (new ceres::AutoDiffCostFunction<BundleAdjustmentConstantPoseCostFunction<CameraModel>, 2, 3,CameraModel::kNumParams>(new BundleAdjustmentConstantPoseCostFunction(qvec, tvec, point2D)));}template <typename T>bool operator()(const T* const point3D, const T* const camera_params,T* residuals) const {const T qvec[4] = {T(qw_), T(qx_), T(qy_), T(qz_)};// Rotate and translate.T projection[3];ceres::UnitQuaternionRotatePoint(qvec, point3D, projection);projection[0] += T(tx_);projection[1] += T(ty_);projection[2] += T(tz_);// Project to image plane.projection[0] /= projection[2];projection[1] /= projection[2];// Distort and transform to pixel space.CameraModel::WorldToImage(camera_params, projection[0], projection[1],&residuals[0], &residuals[1]);// Re-projection error.residuals[0] -= T(observed_x_);residuals[1] -= T(observed_y_);return true;}private:const double qw_;const double qx_;const double qy_;const double qz_;const double tx_;const double ty_;const double tz_;const double observed_x_;const double observed_y_;
};

接下来problem 加入残差块代码如下:

ceres::Problem problem;
ceres::CostFunction* cost_function = nullptr;
ceres::LossFunction * p_LossFunction =ceres_options_.bUse_loss_function_ ?new ceres::HuberLoss(Square(4.0)): nullptr; // 关于为何使用损失函数,因为现实中并不是所有观测过程中的噪声都服从 //gaussian noise的（或者可以说几乎没有），//遇到有outlier的情况，这些方法非常容易挂掉，//这时候就得用到robust statistics里面的//robust cost(*cost也可以叫做loss, 统计学那边喜欢叫risk) function了，//比较常用的有huber, cauchy等等。
cost_function = BundleAdjustmentConstantPoseCostFunction<CameraModel>::Create( \image.Qvec(), image.Tvec(), point2D.XY());//观测值输入
//将优化量加入残差块problem_->AddResidualBlock(cost_function, loss_function, \point3D.XYZ().data(), camera_params_data);//被优化量加入残差-3D点和相机内参

以上就是四种BA 的case 当然还可以有很多变种,比如gps约束的BA(即是附有限制条件的间接平差),比如
固定3D landmark,优化pose和相机参数和畸变系数

参考资料

colmap openmvg 源代码,github 地址:

https://github.com/openMVG/openMVG

https://github.com/colmap/colmap
单杰. 光束法平差简史与概要. 武汉大学学报·信息科学版, 2018, 43(12): 1797-1810.