一文詳解bundle adjustment
來(lái)源：公眾號(hào)|3D視覺(jué)工坊（系投稿）
作者：李城
「3D視覺(jué)工坊」技術(shù)交流群已經(jīng)成立，目前大約有12000人，方向主要涉及3D視覺(jué)、CV&深度學(xué)習(xí)、SLAM、三維重建、點(diǎn)云后處理、自動(dòng)駕駛、CV入門(mén)、三維測(cè)量、VR/AR、3D人臉識(shí)別、醫(yī)療影像、缺陷檢測(cè)、行人重識(shí)別、目標(biāo)跟蹤、視覺(jué)產(chǎn)品落地、視覺(jué)競(jìng)賽、車(chē)牌識(shí)別、硬件選型、學(xué)術(shù)交流、求職交流、ORB-SLAM系列源碼交流、深度估計(jì)等。工坊致力于干貨輸出，不做搬運(yùn)工，為計(jì)算機(jī)視覺(jué)領(lǐng)域貢獻(xiàn)自己的力量！歡迎大家一起交流成長(zhǎng)~
添加小助手微信：CV_LAB，備注學(xué)校/公司+姓名+研究方向即可加入工坊一起學(xué)習(xí)進(jìn)步。
QQ群「3D視覺(jué)研習(xí)社」，群號(hào)：574432628
bundle adjustment 的歷史發(fā)展
bundle adjustment,中文名稱(chēng)是光束法平差,經(jīng)典的BA目的是優(yōu)化相機(jī)的pose和landmark,其在SfM和SLAM 領(lǐng)域中扮演者重要角色.目前大多數(shù)書(shū)籍或者參老文獻(xiàn)將其翻譯成"捆綁調(diào)整"是不太嚴(yán)謹(jǐn)?shù)淖龇?bundle adjustment 最早是19世紀(jì)由搞大地測(cè)量學(xué)(測(cè)繪學(xué)科)的人提出來(lái)的,19世紀(jì)中期的時(shí)候，geodetics的學(xué)者就開(kāi)始研究large scale triangulations（大型三角剖分）。20世紀(jì)中期，隨著camera和computer的出現(xiàn)，photogrammetry(攝影測(cè)量學(xué))也開(kāi)始研究adjustment computation，所以他們給起了個(gè)名字叫bundle adjustment(隸屬攝影測(cè)量學(xué)科前輩的功勞)。21世紀(jì)前后，robotics領(lǐng)域開(kāi)始興起SLAM，最早用的recursive bayesian filter（遞歸貝葉斯濾波），后來(lái)把問(wèn)題搞成個(gè)graph然后用least squares方法求解,bundle adjusment歷史發(fā)展圖如下:
bundle adjustment 其本質(zhì)還是離不開(kāi)最小二乘原理(Gauss功勞)(幾乎所有優(yōu)化問(wèn)題其本質(zhì)都是最小二乘),目前bundle adjustment 優(yōu)化框架最為代表的是ceres solver和g2o(這里主要介紹ceres solver).據(jù)說(shuō)ceres的命名是天文學(xué)家Piazzi閑暇無(wú)事的時(shí)候觀測(cè)一顆沒(méi)有觀測(cè)到的星星,最后用least squares算出了這個(gè)小行星的軌道,故將這個(gè)小行星命名為ceres.
Bundle adjustment 的算法理論
觀測(cè)值:像點(diǎn)坐標(biāo) 優(yōu)化量(平差量):pose 和landmark 因?yàn)橐坏┥婕捌讲?就必定有如下公式:觀測(cè)值+觀測(cè)值改正數(shù)=近似值+近似值改正數(shù),那么bundle adjustment 的公式還是從共線(xiàn)條件方程出發(fā):
四種Bundle adjustment 算法代碼
這里代碼主要從四個(gè)方面來(lái)介紹:
優(yōu)化相機(jī)內(nèi)參及畸變系數(shù),相機(jī)的pose(6dof)和landmark 代價(jià)函數(shù)寫(xiě)法如下:
template?<typename?CameraModel>class?BundleAdjustmentCostFunction?{
?public:
??explicit?BundleAdjustmentCostFunction(const?Eigen::Vector2d&?point2D)
??????:?observed_x_(point2D(0)),?observed_y_(point2D(1))?{}//構(gòu)造函數(shù)傳入的是觀測(cè)值??static?ceres::CostFunction*?Create(const?Eigen::Vector2d&?point2D)?{
????return?(new?ceres::AutoDiffCostFunction<
????????????BundleAdjustmentCostFunction<CameraModel>,?2,?4,?3,?3,
????????????CameraModel::kNumParams>(
????????new?BundleAdjustmentCostFunction(point2D)));
??}//優(yōu)化量:2代表誤差方程個(gè)數(shù);4代表pose中的姿態(tài)信息,用四元數(shù)表示;3代表pose中的位置信息;3代表landmark自由度;CameraModel::kNumParams是相機(jī)內(nèi)參和畸變系數(shù),其取決于相機(jī)模型是what//?opertator?重載函數(shù)的參數(shù)即是待優(yōu)化的量??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_;
};
寫(xiě)好了代價(jià)函數(shù),下面就是需要把參數(shù)都加入殘差塊,讓ceres自動(dòng)求導(dǎo),代碼如下:
ceres::Problem?problem;
ceres::CostFunction*?cost_function?=?nullptr;?
ceres::LossFunction?*?p_LossFunction?=
????ceres_options_.bUse_loss_function_??
??????new?ceres::HuberLoss(square(4.0))
??????:?nullptr;?//?關(guān)于為何使用損失函數(shù),因?yàn)楝F(xiàn)實(shí)中并不是所有觀測(cè)過(guò)程中的噪聲都服從???????//gaussian?noise的（或者可以說(shuō)幾乎沒(méi)有），??????//遇到有outlier的情況，這些方法非常容易掛掉，??????//這時(shí)候就得用到robust?statistics里面的??????//robust?cost(*cost也可以叫做loss,?統(tǒng)計(jì)學(xué)那邊喜歡叫risk)?function了，??????//比較常用的有huber, cauchy等等。cost_function?=?BundleAdjustmentCostFunction<CameraModel>::Create(point2D.XY());?//將優(yōu)化量加入殘差塊problem_->AddResidualBlock(cost_function,?p_LossFunction,?qvec_data,
?????????????????????????????????tvec_data,?point3D.XYZ().data(),
?????????????????????????????????camera_params_data);
?
如上,case1 的bundle adjustment 就搭建完成!
優(yōu)化相機(jī)內(nèi)參及畸變系數(shù),pose subset parameterization(pose 信息部分參數(shù)優(yōu)化)和3D landmark,當(dāng) 只優(yōu)化姿態(tài)信息時(shí)候,problem需要添加的代碼如下:
????//這里姿態(tài)又用歐拉角表示????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函數(shù)????????ceres::SubsetParameterization?*subset_parameterization?=
??????????new?ceres::SubsetParameterization(6,?vec_constant_extrinsic);
????????problem.SetParameterization(parameter_block,?subset_parameterization);
?????}?
?????
優(yōu)化相機(jī)內(nèi)參及畸變系數(shù),pose subset parameterization(pose 信息部分參數(shù)優(yōu)化)和3D landmark,當(dāng) 只優(yōu)化位置信息時(shí)候,problem需要添加的代碼如下(同上面代碼,只需修改一行):
????//這里姿態(tài)又用歐拉角表示????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);
?????}?
?????
優(yōu)化相機(jī)內(nèi)參及畸變系數(shù),pose 是常量不優(yōu)化 和3D landmark. 代價(jià)函數(shù)寫(xiě)法如下:
//相機(jī)模型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_;
};
接下來(lái)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;?//?關(guān)于為何使用損失函數(shù),因?yàn)楝F(xiàn)實(shí)中并不是所有觀測(cè)過(guò)程中的噪聲都服從???????//gaussian?noise的（或者可以說(shuō)幾乎沒(méi)有），??????//遇到有outlier的情況，這些方法非常容易掛掉，??????//這時(shí)候就得用到robust?statistics里面的??????//robust?cost(*cost也可以叫做loss,?統(tǒng)計(jì)學(xué)那邊喜歡叫risk)?function了，??????//比較常用的有huber, cauchy等等。cost_function?=?BundleAdjustmentConstantPoseCostFunction<CameraModel>::Create(?\
????????????image.Qvec(),?image.Tvec(),?point2D.XY());//觀測(cè)值輸入??//將優(yōu)化量加入殘差塊?problem_->AddResidualBlock(cost_function,?loss_function,?\
??????????????point3D.XYZ().data(),?camera_params_data);//被優(yōu)化量加入殘差-3D點(diǎn)和相機(jī)內(nèi)參?
以上就是四種BA 的case 當(dāng)然還可以有很多變種,比如gps約束的BA(即是附有限制條件的間接平差),比如 固定3D landmark,優(yōu)化pose和相機(jī)參數(shù)和畸變系數(shù)
參考資料
colmap openmvg 源代碼,github 地址:https://github.com/openMVG/openMVGhttps://github.com/colmap/colmap
單杰. 光束法平差簡(jiǎn)史與概要. 武漢大學(xué)學(xué)報(bào)·信息科學(xué)版, 2018, 43(12): 1797-1810.
備注：作者也是我們「3D視覺(jué)從入門(mén)到精通」特邀嘉賓：一個(gè)超干貨的3D視覺(jué)學(xué)習(xí)社區(qū)
本文僅做學(xué)術(shù)分享，如有侵權(quán)，請(qǐng)聯(lián)系刪文。
