一文詳解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 的公式還是從共線條件方程出發:
四種Bundle adjustment 演算法程式碼
這裡程式碼主要從四個方面來介紹:
- 優化相機內參及畸變係數,相機的pose(6dof)和landmark 代價函式寫法如下:
template<typenameCameraModel>
classBundleAdjustmentCostFunction{
public:
explicitBundleAdjustmentCostFunction(constEigen::Vector2d&point2D)
:observed_x_(point2D(0)),observed_y_(point2D(1)){}
//建構函式傳入的是觀測值
staticceres::CostFunction*Create(constEigen::Vector2d&point2D){
return(newceres::AutoDiffCostFunction<
BundleAdjustmentCostFunction<CameraModel>,2,4,3,3,
CameraModel::kNumParams>(
newBundleAdjustmentCostFunction(point2D)));
}
//優化量:2代表誤差方程個數;4代表pose中的姿態資訊,用四元數表示;3代表pose中的位置資訊;3代表landmark
自由度;CameraModel::kNumParams是相機內參和畸變係數,其取決於相機模型是what
//opertator過載函式的引數即是待優化的量
template<typenameT>
booloperator()(constT*constqvec,constT*consttvec,
constT*constpoint3D,constT*constcamera_params,
T*residuals)const{
//Rotateandtranslate.
Tprojection[3];
ceres::UnitQuaternionRotatePoint(qvec,point3D,projection);
projection[0]+=tvec[0];
projection[1]+=tvec[1];
projection[2]+=tvec[2];
//Projecttoimageplane.
projection[0]/=projection[2];
projection[1]/=projection[2];
//Distortandtransformtopixelspace.
CameraModel::WorldToImage(camera_params,projection[0],projection[1],
&residuals[0],&residuals[1]);
//Re-projectionerror.
residuals[0]-=T(observed_x_);
residuals[1]-=T(observed_y_);
returntrue;
}
private:
constdoubleobserved_x_;
constdoubleobserved_y_;
};
寫好了代價函式,下面就是需要把引數都加入殘差塊,讓ceres自動求導,程式碼如下:
ceres::Problemproblem;
ceres::CostFunction*cost_function=nullptr;
ceres::LossFunction*p_LossFunction=
ceres_options_.bUse_loss_function_?
newceres::HuberLoss(Square(4.0))
:nullptr;//關於為何使用損失函式,因為現實中並不是所有觀測過程中的噪聲都服從
//gaussiannoise的（或者可以說幾乎沒有），
//遇到有outlier的情況，這些方法非常容易掛掉，
//這時候就得用到robuststatistics裡面的
//robustcost(*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=
newceres::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=
newceres::SubsetParameterization(6,vec_constant_extrinsic);
problem.SetParameterization(parameter_block,subset_parameterization);
}
- 優化相機內參及畸變係數,pose 是常量不優化 和3D landmark. 代價函式寫法如下:
//相機模型
template<typenameCameraModel>
classBundleAdjustmentConstantPoseCostFunction{
public:
BundleAdjustmentConstantPoseCostFunction(constEigen::Vector4d&qvec,
constEigen::Vector3d&tvec,
constEigen::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)){}
staticceres::CostFunction*Create(constEigen::Vector4d&qvec,
constEigen::Vector3d&tvec,
constEigen::Vector2d&point2D){
return(newceres::AutoDiffCostFunction<
BundleAdjustmentConstantPoseCostFunction<CameraModel>,2,3,
CameraModel::kNumParams>(
newBundleAdjustmentConstantPoseCostFunction(qvec,tvec,point2D)));
}
template<typenameT>
booloperator()(constT*constpoint3D,constT*constcamera_params,
T*residuals)const{
constTqvec[4]={T(qw_),T(qx_),T(qy_),T(qz_)};
//Rotateandtranslate.
Tprojection[3];
ceres::UnitQuaternionRotatePoint(qvec,point3D,projection);
projection[0]+=T(tx_);
projection[1]+=T(ty_);
projection[2]+=T(tz_);
//Projecttoimageplane.
projection[0]/=projection[2];
projection[1]/=projection[2];
//Distortandtransformtopixelspace.
CameraModel::WorldToImage(camera_params,projection[0],projection[1],
&residuals[0],&residuals[1]);
//Re-projectionerror.
residuals[0]-=T(observed_x_);
residuals[1]-=T(observed_y_);
returntrue;
}
private:
constdoubleqw_;
constdoubleqx_;
constdoubleqy_;
constdoubleqz_;
constdoubletx_;
constdoublety_;
constdoubletz_;
constdoubleobserved_x_;
constdoubleobserved_y_;
};
接下來problem 加入殘差塊程式碼如下:
ceres::Problemproblem;
ceres::CostFunction*cost_function=nullptr;
ceres::LossFunction*p_LossFunction=
ceres_options_.bUse_loss_function_?
newceres::HuberLoss(Square(4.0))
:nullptr;//關於為何使用損失函式,因為現實中並不是所有觀測過程中的噪聲都服從
//gaussiannoise的（或者可以說幾乎沒有），
//遇到有outlier的情況，這些方法非常容易掛掉，
//這時候就得用到robuststatistics裡面的
//robustcost(*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世界共同進步，知識星球入口：