一文詳解bundle adjustment

作者：李城
來源：微信公眾號|3D視覺工坊（系投稿）
3D視覺精品文章匯總：https://github.com/qxiaofan/awesome-3D-Vision-Papers/

 

‍

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 的公式還是從共線條件方程出發:

 

四種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/openMVGhttps://github.com/colmap/colmap
• 單傑. 光束法平差簡史與概要. 武漢大學學報·信息科學版, 2018, 43(12): 1797-1810.

備注：作者也是我們「3D視覺從入門到精通」特邀嘉賓：一個超干貨的3D視覺學習社區本文僅做學術分享，如有侵權，請聯系刪文。