單目 Bundle Adjustment 求解代碼分析，相關資料請參考我的這篇部落格。完整的工程在這裡。
單目 Bundle Adjustment 求解代碼
- 相機資料
- 定義頂點加法
- 殘差計算
- Jacobians計算
-
- check jacobians
- Hessian計算
-
- Hessian矩陣模型
-
- Jacobian乘法模型
- Hessian矩陣變化過程
- Hessian矩陣最終形式
- slam的舒爾補操作
-
- Hessian矩陣的舒爾模型
- 舒爾補計算公式
- 舒爾補變換
- 變量計算
- toy example 1 marg變量測試代碼
相機資料
相關的産生仿真資料的代碼可以參考我的這篇部落格。
/*
* 産生世界坐标系下的虛拟資料: 相機姿态, 特征點, 以及每幀觀測
*/
void GetSimDataInWordFrame(vector<Frame> &cameraPoses, vector<Eigen::Vector3d> &points) {
int featureNums = 20; // 特征數目，假設每幀都能觀測到所有的特征
int poseNums = 3; // 相機數目
double radius = 8;
for (int n = 0; n < poseNums; ++n) {
double theta = n * 2 * M_PI / (poseNums * 4); // 1/4 圓弧
// 繞 z軸 旋轉
Eigen::Matrix3d R;
R = Eigen::AngleAxisd(theta, Eigen::Vector3d::UnitZ());
Eigen::Vector3d t = Eigen::Vector3d(radius * cos(theta) - radius, radius * sin(theta), 1 * sin(2 * theta));
cameraPoses.push_back(Frame(R, t));
}
// 随機數生成三維特征點
std::default_random_engine generator;
std::normal_distribution<double> noise_pdf(0., 1. / 1000.); // 2pixel / focal
for (int j = 0; j < featureNums; ++j) {
std::uniform_real_distribution<double> xy_rand(-4, 4.0);
std::uniform_real_distribution<double> z_rand(4., 8.);
Eigen::Vector3d Pw(xy_rand(generator), xy_rand(generator), z_rand(generator));
points.push_back(Pw);
// 在每一幀上的觀測量
for (int i = 0; i < poseNums; ++i) {
Eigen::Vector3d Pc = cameraPoses[i].Rwc.transpose() * (Pw - cameraPoses[i].twc);
Pc = Pc / Pc.z(); // 歸一化圖像平面
Pc[0] += noise_pdf(generator);
Pc[1] += noise_pdf(generator);
cameraPoses[i].featurePerId.insert(make_pair(j, Pc));
}
}
}
定義頂點加法
定義pose的加法運算，将pose拆分成平移和旋轉，平移直接進行加法，旋轉采用李代數進行更新。
void VertexPose::Plus(const VecX &delta) {
VecX ¶meters = Parameters();
parameters.head<3>() += delta.head<3>(); //pose的前三維平移向量
Qd q(parameters[6], parameters[3], parameters[4], parameters[5]); //pose的旋轉部分，四元數表示的
q = q * Sophus::SO3d::exp(Vec3(delta[3], delta[4], delta[5])).unit_quaternion(); // right multiplication with so3
q.normalized();
parameters[3] = q.x();
parameters[4] = q.y();
parameters[5] = q.z();
parameters[6] = q.w();
// Qd test = Sophus::SO3d::exp(Vec3(0.2, 0.1, 0.1)).unit_quaternion() * Sophus::SO3d::exp(-Vec3(0.2, 0.1, 0.1)).unit_quaternion();
// std::cout << test.x()<<" "<< test.y()<<" "<<test.z()<<" "<<test.w() <<std::endl;
}
殘差計算
計算邊的殘差。相關内容請參照我的這篇部落格的
VIO 中基于逆深度的重投影誤差
和
殘差 Jacobian 的推導
部分。
void EdgeReprojection::ComputeResidual() {
//verticies_頂點順序必須為InveseDepth、T_World_From_Body1、T_World_From_Body2。
double inv_dep_i = verticies_[0]->Parameters()[0]; //逆深度資訊
VecX param_i = verticies_[1]->Parameters();
Qd Qi(param_i[6], param_i[3], param_i[4], param_i[5]); //旋轉
Vec3 Pi = param_i.head<3>(); //平移
VecX param_j = verticies_[2]->Parameters();
Qd Qj(param_j[6], param_j[3], param_j[4], param_j[5]); //旋轉
Vec3 Pj = param_j.head<3>(); //平移
//vio中基于逆深度的沖投影誤差
Vec3 pts_camera_i = pts_i_ / inv_dep_i;
Vec3 pts_imu_i = qic * pts_camera_i + tic; //相機坐标轉換為imu坐标
Vec3 pts_w = Qi * pts_imu_i + Pi; //imu坐标系變換到世界坐标系
Vec3 pts_imu_j = Qj.inverse() * (pts_w - Pj); //世界坐标系變換到imu坐标系
Vec3 pts_camera_j = qic.inverse() * (pts_imu_j - tic); //imu坐标系變換的相機坐标系
double dep_j = pts_camera_j.z();
residual_ = (pts_camera_j / dep_j).head<2>() - pts_j_.head<2>(); /// J^t * J * delta_x = - J^t * r
// residual_ = information_ * residual_; // remove information here, we multi information matrix in problem solver
}
Jacobians計算
相關内容請參照我的這篇部落格的
殘差 Jacobian 的推導
部分。
-
殘差對j時刻路标點的jacobian
VIO學習筆記（五）—— 單目 Bundle Adjustment 求解代碼相機資料定義頂點加法殘差計算Jacobians計算Hessian計算slam的舒爾補操作toy example 1 marg變量測試代碼 - j時刻路标點對逆深度jacobian
VIO學習筆記（五）—— 單目 Bundle Adjustment 求解代碼相機資料定義頂點加法殘差計算Jacobians計算Hessian計算slam的舒爾補操作toy example 1 marg變量測試代碼
void EdgeReprojection::ComputeJacobians() {
//verticies_頂點順序必須為InveseDepth、T_World_From_Body1、T_World_From_Body2。
double inv_dep_i = verticies_[0]->Parameters()[0]; //逆深度
VecX param_i = verticies_[1]->Parameters(); //i時刻位姿
Qd Qi(param_i[6], param_i[3], param_i[4], param_i[5]);
Vec3 Pi = param_i.head<3>();
VecX param_j = verticies_[2]->Parameters(); //j時刻位姿
Qd Qj(param_j[6], param_j[3], param_j[4], param_j[5]);
Vec3 Pj = param_j.head<3>();
Vec3 pts_camera_i = pts_i_ / inv_dep_i;
Vec3 pts_imu_i = qic * pts_camera_i + tic;
Vec3 pts_w = Qi * pts_imu_i + Pi;
Vec3 pts_imu_j = Qj.inverse() * (pts_w - Pj);
Vec3 pts_camera_j = qic.inverse() * (pts_imu_j - tic);
double dep_j = pts_camera_j.z();
Mat33 Ri = Qi.toRotationMatrix();
Mat33 Rj = Qj.toRotationMatrix();
Mat33 ric = qic.toRotationMatrix();
Mat23 reduce(2, 3);
//殘差對j時刻路标點的jacobian
reduce << 1. / dep_j, 0, -pts_camera_j(0) / (dep_j * dep_j),
0, 1. / dep_j, -pts_camera_j(1) / (dep_j * dep_j);
// reduce = information_ * reduce;
//j時刻路标點對i時刻pose的jacobian
Eigen::Matrix<double, 2, 6> jacobian_pose_i;
Eigen::Matrix<double, 3, 6> jaco_i;
jaco_i.leftCols<3>() = ric.transpose() * Rj.transpose();
jaco_i.rightCols<3>() = ric.transpose() * Rj.transpose() * Ri * -Sophus::SO3d::hat(pts_imu_i);
jacobian_pose_i.leftCols<6>() = reduce * jaco_i;
//j時刻路标點對j時刻pose的jacobian
Eigen::Matrix<double, 2, 6> jacobian_pose_j;
Eigen::Matrix<double, 3, 6> jaco_j;
jaco_j.leftCols<3>() = ric.transpose() * -Rj.transpose();
jaco_j.rightCols<3>() = ric.transpose() * Sophus::SO3d::hat(pts_imu_j);
jacobian_pose_j.leftCols<6>() = reduce * jaco_j;
//j時刻路标點對逆深度jacobian
Eigen::Vector2d jacobian_feature;
jacobian_feature = reduce * ric.transpose() * Rj.transpose() * Ri * ric * pts_i_ * -1.0 / (inv_dep_i * inv_dep_i);
jacobians_[0] = jacobian_feature;
jacobians_[1] = jacobian_pose_i;
jacobians_[2] = jacobian_pose_j;
}
check jacobians
if(true)
{
std::cout << "//---------------- check jacobians -----------------//" << std::endl;
std::cout << jacobians_[0] <<std::endl;
const double eps = 1e-6;
inv_dep_i += eps;
Eigen::Vector3d pts_camera_i = pts_i_ / inv_dep_i;
Eigen::Vector3d pts_imu_i = qic * pts_camera_i + tic;
Eigen::Vector3d pts_w = Qi * pts_imu_i + Pi;
Eigen::Vector3d pts_imu_j = Qj.inverse() * (pts_w - Pj);
Eigen::Vector3d pts_camera_j = qic.inverse() * (pts_imu_j - tic);
Eigen::Vector2d tmp_residual;
double dep_j = pts_camera_j.z();
tmp_residual = (pts_camera_j / dep_j).head<2>() - pts_j_.head<2>();
tmp_residual = information_ * tmp_residual;
std::cout <<"num jacobian: "<< (tmp_residual - residual_) / eps <<std::endl;
}
比較計算結果與數值結果，一下為輸出結果
//---------------- check jacobians -----------------//
-6.26683
-11.4615
num jacobian: -6.26683
-11.4615
//---------------- check jacobians -----------------//
-1.40693
-6.67589
num jacobian: -1.40693
-6.6759
//---------------- check jacobians -----------------//
-6.21937
-9.82836
num jacobian: -6.21937
-9.82837
//---------------- check jacobians -----------------//
-1.98315
-5.9814
num jacobian: -1.98316
-5.98141
Hessian計算
void Problem::MakeHessian() {
TicToc t_h;
// 直接構造大的 H 矩陣
ulong size = ordering_generic_; //38維，３×６＋２０
// cout << "//-------------size--------------//" << endl;
// cout << size << endl;
MatXX H(MatXX::Zero(size, size));
VecX b(VecX::Zero(size));
int iiii = 1;
for (auto &edge: edges_) {
int iii = 1;
edge.second->ComputeResidual();
edge.second->ComputeJacobians();
auto jacobians = edge.second->Jacobians();
auto verticies = edge.second->Verticies();
assert(jacobians.size() == verticies.size());
for (size_t i = 0; i < verticies.size(); ++i) {
auto v_i = verticies[i];
if (v_i->IsFixed()) continue; // Hessian 裡不需要添加它的資訊，也就是它的雅克比為 0
auto jacobian_i = jacobians[i];
ulong index_i = v_i->OrderingId();
ulong dim_i = v_i->LocalDimension();
// cout << "//-------------------index_i------------------//" << endl;
// cout << index_i << endl;
MatXX JtW = jacobian_i.transpose() * edge.second->Information();
// cout << "//---------------JtW---------------------//" << endl;
// cout << JtW << endl;
for (size_t j = i; j < verticies.size(); ++j) {
auto v_j = verticies[j];
if (v_j->IsFixed()) continue;
auto jacobian_j = jacobians[j];
ulong index_j = v_j->OrderingId();
ulong dim_j = v_j->LocalDimension();
assert(v_j->OrderingId() != -1);
MatXX hessian = JtW * jacobian_j;
// 所有的資訊矩陣疊加起來
// TODO:: home work. 完成 H index 的填寫.
H.block(index_i, index_j, dim_i, dim_j).noalias() += hessian;
cout << iiii<< ": " << iii << "//--------------H---------------//" << endl;
cout << H << endl;
if (j != i) {
// 對稱的下三角
// TODO:: home work. 完成 H index 的填寫.
H.block(index_j, index_i, dim_j, dim_i).noalias() += hessian.transpose();
cout << iiii<< ": " << iii << "//--------------H---------------//" << endl;
cout << H << endl;
}
iii++;
}
b.segment(index_i, dim_i).noalias() -= JtW * edge.second->Residual();
}
// cout << "//--------------H---------------//" << endl;
// cout << H << endl;
// cout << "//--------------b---------------//" << endl;
// cout << b << endl;
iiii++;
}
Hessian_ = H;
b_ = b;
t_hessian_cost_ += t_h.toc();
// cout << "//--------------H---------------//" << endl;
// cout << H << endl;
// cout << "//--------------b---------------//" << endl;
// cout << b << endl;
// Eigen::JacobiSVD<Eigen::MatrixXd> svd(H, Eigen::ComputeThinU | Eigen::ComputeThinV);
// std::cout << svd.singularValues() <<std::endl;
if (err_prior_.rows() > 0) {
b_prior_ -= H_prior_ * delta_x_.head(ordering_poses_); // update the error_prior
}
Hessian_.topLeftCorner(ordering_poses_, ordering_poses_) += H_prior_;
b_.head(ordering_poses_) += b_prior_;
delta_x_ = VecX::Zero(size); // initial delta_x = 0_n;
}
Hessian矩陣模型
Jacobian乘法模型
Hessian矩陣變化過程
Hessian矩陣最終形式
slam的舒爾補操作
/*
* Solve Hx = b, we can use PCG iterative method or use sparse Cholesky
*/
void Problem::SolveLinearSystem() {
if (problemType_ == ProblemType::GENERIC_PROBLEM) {
// 非 SLAM 問題直接求解
// PCG solver
MatXX H = Hessian_;
for (ulong i = 0; i < Hessian_.cols(); ++i) {
H(i, i) += currentLambda_;
}
// delta_x_ = PCGSolver(H, b_, H.rows() * 2);
delta_x_ = Hessian_.inverse() * b_;
} else {
// SLAM 問題采用舒爾補的計算方式
// step1: schur marginalization --> Hpp, bpp
int reserve_size = ordering_poses_;
int marg_size = ordering_landmarks_;
// TODO:: home work. 完成矩陣塊取值，Hmm，Hpm，Hmp，bpp，bmm
MatXX Hmm = Hessian_.block(reserve_size, reserve_size, marg_size, marg_size);
MatXX Hpm = Hessian_.block(0, reserve_size, reserve_size, marg_size);
MatXX Hmp = Hessian_.block(reserve_size, 0, marg_size, reserve_size);
VecX bpp = b_.segment(0, reserve_size);
VecX bmm = b_.segment(reserve_size, marg_size);
// Hmm 是對角線矩陣，它的求逆可以直接為對角線塊分别求逆，如果是逆深度，對角線塊為1維的，則直接為對角線的倒數，這裡可以加速
MatXX Hmm_inv(MatXX::Zero(marg_size, marg_size));
for (auto landmarkVertex : idx_landmark_vertices_) {
int idx = landmarkVertex.second->OrderingId() - reserve_size;
int size = landmarkVertex.second->LocalDimension();
Hmm_inv.block(idx, idx, size, size) = Hmm.block(idx, idx, size, size).inverse();
}
// TODO:: home work. 完成舒爾補 Hpp, bpp 代碼
MatXX tempH = Hpm * Hmm_inv;
H_pp_schur_ = Hessian_.block(0, 0, ordering_poses_, ordering_poses_) - tempH * Hmp;
b_pp_schur_ = bpp - tempH * bmm;
// step2: solve Hpp * delta_x = bpp
VecX delta_x_pp(VecX::Zero(reserve_size));
// PCG Solver
for (ulong i = 0; i < ordering_poses_; ++i) {
H_pp_schur_(i, i) += currentLambda_;
}
int n = H_pp_schur_.rows() * 2; // 疊代次數
// 哈哈，小規模問題，搞 pcg 花裡胡哨
delta_x_pp = PCGSolver(H_pp_schur_, b_pp_schur_, n);
delta_x_.head(reserve_size) = delta_x_pp;
// std::cout << delta_x_pp.transpose() << std::endl;
// TODO:: home work. step3: solve landmark
VecX delta_x_ll(marg_size);
delta_x_ll = Hmm_inv * (bmm - Hmp * delta_x_pp);
delta_x_.tail(marg_size) = delta_x_ll;
}
}
Hessian矩陣的舒爾模型
舒爾補計算公式
舒爾補變換
變量計算
toy example 1 marg變量測試代碼
關于toy example 1的相關内容請參照我的這篇部落格的
toy example 1
和
回顧 toy example 1 中去除變量 x3 的操作
部分。
相關的資訊矩陣為：
void Problem::TestMarginalize() {
// Add marg test
int idx = 1; // marg 中間那個變量
int dim = 1; // marg 變量的次元
int reserve_size = 3; // 總共變量的次元
double delta1 = 0.1 * 0.1;
double delta2 = 0.2 * 0.2;
double delta3 = 0.3 * 0.3;
int cols = 3;
MatXX H_marg(MatXX::Zero(cols, cols));
H_marg << 1./delta1, -1./delta1, 0,
-1./delta1, 1./delta1 + 1./delta2 + 1./delta3, -1./delta3,
0., -1./delta3, 1/delta3;
std::cout << "---------- TEST Marg: before marg------------"<< std::endl;
std::cout << H_marg << std::endl;
// TODO:: home work. 将變量移動到右下角
/// 準備工作： move the marg pose to the Hmm bottown right
// 将 row i 移動矩陣最下面
Eigen::MatrixXd temp_rows = H_marg.block(idx, 0, dim, reserve_size);
Eigen::MatrixXd temp_botRows = H_marg.block(idx + dim, 0, reserve_size - idx - dim, reserve_size);
H_marg.block(idx, 0, dim, reserve_size) = temp_botRows;
H_marg.block(idx + dim, 0, reserve_size - idx - dim, reserve_size) = temp_rows;
// 将 col i 移動矩陣最右邊
Eigen::MatrixXd temp_cols = H_marg.block(0, idx, reserve_size, dim);
Eigen::MatrixXd temp_rightCols = H_marg.block(0, idx + dim, reserve_size, reserve_size - idx - dim);
H_marg.block(0, idx, reserve_size, reserve_size - idx - dim) = temp_rightCols;
H_marg.block(0, reserve_size - dim, reserve_size, dim) = temp_cols;
std::cout << "---------- TEST Marg: 将變量移動到右下角------------"<< std::endl;
std::cout<< H_marg <<std::endl;
/// 開始 marg ： schur
double eps = 1e-8;
int m2 = dim;
int n2 = reserve_size - dim; // 剩餘變量的次元
Eigen::MatrixXd Amm = 0.5 * (H_marg.block(n2, n2, m2, m2) + H_marg.block(n2, n2, m2, m2).transpose());
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> saes(Amm);
Eigen::MatrixXd Amm_inv = saes.eigenvectors() * Eigen::VectorXd(
(saes.eigenvalues().array() > eps).select(saes.eigenvalues().array().inverse(), 0)).asDiagonal() *
saes.eigenvectors().transpose();
// TODO:: home work. 完成舒爾補操作
Eigen::MatrixXd Arm = H_marg.block(0,n2,n2,m2);
Eigen::MatrixXd Amr = H_marg.block(n2,0,m2,n2);
Eigen::MatrixXd Arr = H_marg.block(0,0,n2,n2);
Eigen::MatrixXd tempB = Arm * Amm_inv;
Eigen::MatrixXd H_prior = Arr - tempB * Amr;
std::cout << "---------- TEST Marg: after marg------------"<< std::endl;
std::cout << H_prior << std::endl;
}
結果如下：
---------- TEST Marg: before marg------------
100 -100 0
-100 136.111 -11.1111
0 -11.1111 11.1111
---------- TEST Marg: 将變量移動到右下角------------
100 0 -100
0 11.1111 -11.1111
-100 -11.1111 136.111
---------- TEST Marg: after marg------------
26.5306 -8.16327
-8.16327 10.2041