码迷,mamicode.com
首页 > 其他好文 > 详细

一文详解bundle adjustment

时间:2020-12-14 13:48:28      阅读:5      评论:0      收藏:0      [点我收藏+]

标签:image   检测   块代码   parameter   empty   常用   template   lzw   上海交大   

作者:李城

点击上方“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 的公式还是从共线条件方程出发:

技术图片

技术图片

 

四种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视觉学习社区本文仅做学术分享,如有侵权,请联系删文。下载1在「3D视觉工坊」公众号后台回复:3D视觉即可下载 3D视觉相关资料干货,涉及相机标定、三维重建、立体视觉、SLAM、深度学习、点云后处理、多视图几何等方向。
下载2在「3D视觉工坊」公众号后台回复:3D视觉github资源汇总即可下载包括结构光、标定源码、缺陷检测源码、深度估计与深度补全源码、点云处理相关源码、立体匹配源码、单目、双目3D检测、基于点云的3D检测、6D姿态估计源码汇总等。
下载3在「3D视觉工坊」公众号后台回复:相机标定即可下载独家相机标定学习课件与视频网址;后台回复:立体匹配即可下载独家立体匹配学习课件与视频网址。

重磅!3DCVer-学术论文写作投稿 交流群已成立

扫码添加小助手微信,可申请加入3D视觉工坊-学术论文写作与投稿 微信交流群,旨在交流顶会、顶刊、SCI、EI等写作与投稿事宜。

同时也可申请加入我们的细分方向交流群,目前主要有3D视觉CV&深度学习SLAM三维重建点云后处理自动驾驶、CV入门、三维测量、VR/AR、3D人脸识别、医疗影像、缺陷检测、行人重识别、目标跟踪、视觉产品落地、视觉竞赛、车牌识别、硬件选型、学术交流、求职交流、ORB-SLAM系列源码交流、深度估计等微信群。

 

一定要备注:研究方向+学校/公司+昵称,例如:”3D视觉 + 上海交大 + 静静“。请按照格式备注,可快速被通过且邀请进群。原创投稿也请联系。

技术图片

▲长按加微信群或投稿

技术图片

▲长按关注公众号

3D视觉从入门到精通知识星球:针对3D视觉领域的知识点汇总、入门进阶学习路线、最新paper分享、疑问解答四个方面进行深耕,更有各类大厂的算法工程人员进行技术指导。与此同时,星球将联合知名企业发布3D视觉相关算法开发岗位以及项目对接信息,打造成集技术与就业为一体的铁杆粉丝聚集区,近2000星球成员为创造更好的AI世界共同进步,知识星球入口:

学习3D视觉核心技术,扫描查看介绍,3天内无条件退款

技术图片 

圈里有高质量教程资料、可答疑解惑、助你高效解决问题觉得有用,麻烦给个赞和在看~  

一文详解bundle adjustment

标签:image   检测   块代码   parameter   empty   常用   template   lzw   上海交大   

原文地址:https://www.cnblogs.com/YongQiVisionIMAX/p/14105783.html

(0)
(0)
   
举报
评论 一句话评论(0
登录后才能评论!
© 2014 mamicode.com 版权所有  联系我们:gaon5@hotmail.com
迷上了代码!