手写VIO后端分析（手写VIO总结五）
手写VIO后端分析（手写VIO总结五）
文章目录
- 1、计算视觉重投影残差理论部分
- 2、残差雅可比的推导
- 3.作业
- 3.1.1 如何拼接信息矩阵
- 3.1.2 利用舒尔补问题加速问题的求解
- 3.2完成滑动窗口算法测试函数
手写VIO作业总结(一)
手写VIO作业总结(二)
手写VIO作业总结(三)
手写VIO作业总结(四)
1、计算视觉重投影残差理论部分
/* std::vector> verticies_; // 该边对应的顶点
VecX residual_; // 残差
std::vector jacobians_; // 雅可比，每个雅可比维度是 residual x vertex[i]
MatXX information_; // 信息矩阵
VecX observation_; // 观测信息
*/
void EdgeReprojection::ComputeResidual() {
/**
* note: Qi,Pi;Qj,Pj不是相机到世界坐标系的变换矩阵，而是imu坐标系到世界坐标系的变换矩阵
*/
double inv_dep_i = verticies_[0]->Parameters()[0];
std::cout<<"inv_dep_i: "<<inv_dep_i<<std::endl;
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>();
/**
* pts_i_:相机在第i帧图像中的归一化坐标
* pts_camera_i：相机坐标
* pts_w:世界坐标
* pts_imu_i:把相机坐标系下的坐标变换到imu坐标系下
*/
Vec3 pts_camera_i = pts_i_ / inv_dep_i;
// std::cout<<"pts_i_: "<
// std::cout<<"pts_camera_i: "<
Vec3 pts_imu_i = qic * pts_camera_i + tic;//camrea->imu
Vec3 pts_w = Qi * pts_imu_i + Pi;//imu->word
Vec3 pts_imu_j = Qj.inverse() * (pts_w - Pj);//word->imu
Vec3 pts_camera_j = qic.inverse() * (pts_imu_j - tic);//imu->camera
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
// std::cout<<"residual_: "<
// residual_ = information_ * residual_; // remove information here, we multi information matrix in problem solver
}
2、残差雅可比的推导
具体代码实现：
void EdgeReprojection::ComputeJacobians() {
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);
//计算雅可比矩阵分为两步走：
/**
* 1.投影方程关于相机坐标系下的三维点的导数即rc对fcj求导
*/
reduce << 1. / dep_j, 0, -pts_camera_j(0) / (dep_j * dep_j), //fx=fy=1
0, 1. / dep_j, -pts_camera_j(1) / (dep_j * dep_j);
// reduce = information_ * reduce;
/**
* 2.fcj对各个状态量求导
*/
Eigen::Matrix<double, 2, 6> jacobian_pose_i;
Eigen::Matrix<double, 3, 6> jaco_i;
//a.对i时刻位移求导
jaco_i.leftCols<3>() = ric.transpose() * Rj.transpose();//RbcT*RwbT
//b.对i时刻角度增量求导
jaco_i.rightCols<3>() = ric.transpose() * Rj.transpose() * Ri * -Sophus::SO3d::hat(pts_imu_i);
//Sophus::SO3d::hat(pts_imu_i)表示pts_imu_i向量的反对称矩阵
jacobian_pose_i.leftCols<6>() = reduce * jaco_i;//链式求导
Eigen::Matrix<double, 2, 6> jacobian_pose_j;
Eigen::Matrix<double, 3, 6> jaco_j;
//a.对j时刻位移求导
jaco_j.leftCols<3>() = ric.transpose() * -Rj.transpose();
//b.对j时刻角度增量求导
jaco_j.rightCols<3>() = ric.transpose() * Sophus::SO3d::hat(pts_imu_j);
jacobian_pose_j.leftCols<6>() = reduce * jaco_j;
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 -----------------
// {
// std::cout << jacobians_[0] <
// 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 <
// }
}
3.作业
3.1.1 如何拼接信息矩阵
void Problem::MakeHessian() {
TicToc t_h;
// 直接构造大的 H 矩阵
ulong size = ordering_generic_;
MatXX H(MatXX::Zero(size, size));
VecX b(VecX::Zero(size));
for (auto &edge: edges_) {
edge.second->ComputeResidual();
edge.second->ComputeJacobians();
auto jacobians = edge.second->Jacobians();
auto verticies = edge.second->Verticies();
assert(jacobians.size() == verticies.size());
std::cout<<"verticies.size()"<<verticies.size()<<std::endl;
for (size_t i = 0; i < verticies.size(); ++i)
{
auto v_i = verticies[i]; //verticies:边对应的顶点
if (v_i->IsFixed()) continue; // Hessian 里不需要添加它的信息，也就是它的雅克比为 0
auto jacobian_i = jacobians[i];
ulong index_i = v_i->OrderingId();
ulong dim_i = v_i->LocalDimension();
MatXX JtW = jacobian_i.transpose() * edge.second->Information();
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(?,?, ?, ?).noalias() += hessian;
std::cout<<"hessian"<<hessian<<std::endl;
std::cout<<index_i<<" "<<index_j<<" "<<dim_i<<" "<<dim_j<<std::endl;
H.block(index_i,index_j,dim_i,dim_j).noalias()+=hessian;//.noalias()声明矩阵运算中不会出现混淆问题，提高速度
if (j != i)
{
// 对称的下三角
// TODO:: home work. 完成 H index 的填写.
H.block(index_j,index_i,dim_j,dim_i).noalias() += hessian.transpose();
}
}
b.segment(index_i, dim_i).noalias() -= JtW * edge.second->Residual();
}
}
Hessian_ = H;
b_ = b;
t_hessian_cost_ += t_h.toc();
std::cout<<"H matrix"<<"\n"<<H.matrix()<<std::endl;
// Eigen::JacobiSVD svd(H, Eigen::ComputeThinU | Eigen::ComputeThinV);
// std::cout << svd.singularValues() <
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;
}
注释
Eigen中segment和block的区别
segment适用于一维矩阵;block适用于矩阵块
假如H矩阵为38*38
则H（0,0,6,6）表示的是以前两个坐标为起点，维度为6×6;
b.segment(1,2)表示的是取以1为起点，维度为2
Eigen::Matrix<double, 3, 1> Vec3;
Vec3<<1,2,3;
std::cout<<Vec3<<std::endl;
std::cout<<Vec3.segment(1,2)<<std::endl;
3.1.2 利用舒尔补问题加速问题的求解
/*
* 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_;//18
int marg_size = ordering_landmarks_;//20
// std::cout<<"reserve_size: "<
// std::cout<<"marg_size: "<
// 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));
std::cout<<"idx_landmark_vertices_"<<idx_landmark_vertices_.size()<<std::endl;
for (auto landmarkVertex : idx_landmark_vertices_)
{
int idx = landmarkVertex.second->OrderingId() - reserve_size;
//landmarkVertex.second->OrderingId() landmark的ID号
int size = landmarkVertex.second->LocalDimension();
// std::cout<<"test"<OrderingId()<
// std::cout<<"idx: "<
// std::cout<<"size: "<
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(reserve_size,reserve_size,marg_size,marg_size) - tempH * Hmp;
b_pp_schur_ = bpp - tempH * bmm;//TODO::bmm理论有点问题
// step2: solve Hpp * delta_x = bpp
VecX delta_x_pp(VecX::Zero(reserve_size));
// PCG Solver
//TODO:: 了解一下PCG solver
for (ulong i = 0; i < ordering_poses_; ++i)
{
H_pp_schur_(i, i) += currentLambda_;
}
int n = H_pp_schur_.rows() * 2; // 迭代次数
delta_x_pp = PCGSolver(H_pp_schur_, b_pp_schur_, n); // 哈哈，小规模问题，搞 pcg 花里胡哨
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;
}
}
大家补充好尽量在终端编译，我用clion老是报错
终端使用cmake编译成功了，一脸懵
上图是固定相机的第一帧和第二帧的相机位姿，下图是没有固定之后的结果，可以看到第一帧相机的pose平移不再是原点（0,0,0），说明向零空间发生了偏移。
3.2完成滑动窗口算法测试函数
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());
// std::cout<<"Amm"<
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();
// std::cout<<"Amm_inv"<
// std::cout<<1./Amm<
// 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;
}
运行结果：
