1
在MakeHseesian()函数中;
// 所有的信息矩阵叠加起来
// TODO:: home work. 完成 H index 的填写.
// 确定不会有错误的产生，那么可以使用noaliasd()函数来避免产生临时变量,使用noalias()函数来声明这里没有混淆直接赋值
//这里index_i、index_j是对应矩阵块的位置，i代表行。j代表列 此时对应的是对角线部分加上三角部分
H.block(index_i, index_j, dim_i, dim_j).noalias() += hessian;
if (j != i) {
// 对称的下三角 index_j, index_i对应的值下三角的行和列
// TODO:: home work. 完成 H index 的填写.
H.block(index_j, index_i, dim_j, dim_i).noalias() += hessian.transpose();//
}
在SolveLinearSystem()函数中:
// 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,reserve_size,reserve_size) - tempH * Hmp;
b_pp_schur_ = bpp - tempH * bpp;
// 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; // 迭代次数
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;
PCG求解：添加链接描述
2
在TestMarginalize()函数中：
// 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,reserve_size - idx - dim,reserve_size) = temp_botRows;
H_marg.block(reserve_size - dim,0,dim,reserve_size) = temp_rows;
/// 开始 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;
补充：参考博客 添加链接描述
代码中求Amm的逆Amm_inv用到了特征值分解的方法
其中Eigen::SelfAdjointEigenSolver解释为：
A matrix A A A is selfadjoint if it equals its adjoint. For real matrices, this means that the matrix is symmetric: it equals its transpose.
这里感觉应该说的是共轭矩阵(Hermite矩阵)而不是自伴随矩阵（伴随矩阵是那个由代数余子式构成的与逆矩阵有关的矩阵）
2
文献阅读
On the Comparison of Gauge Freedom Handling in
Optimization-based Visual-Inertial State Estimation
基于量规自由处理的基于优化的视觉惯性状态估计的比较
参考：文献阅读
3在代码中给第一帧和第二帧添加 prior 约束,并比较为 prior 设定不同权重时, BA 求解收敛精度和速度
edge_prior.cpp中的部分源码：
//
// Created by wenbo on 2020/12/4.
//
//计算和先验的残差
void EdgeSE3Prior::ComputeResidual()
{
VecX param_i = verticies_[0]->Parameters();//得到第一个顶点的参数
Qd Qi(param_i[6], param_i[3], param_i[4], param_i[5]);//获得第一个顶点的旋转-Qi
Vec3 Pi = param_i.head<3>();//获得第一个顶点的平移-Pi
// std::cout << Qi.vec().transpose() <<" "<< Qp_.vec().transpose()<<std::endl;
// rotation error
#ifdef USE_SO3_JACOBIAN //判断是否使用SO3 雅克比
Sophus::SO3d ri(Qi);//第一个顶点的旋转-ri
Sophus::SO3d rp(Qp_);//第一个顶点的先验旋转-rp
Sophus::SO3d res_r = rp.inverse() * ri;//第一个顶点的先验的旋转和第一个顶点的旋转的残差
residual_.block<3,1>(0,0) = Sophus::SO3d::log(res_r);//残差取李代数
#else
residual_.block<3,1>(0,0) = 2 * (Qp_.inverse() * Qi).vec();//这里是直接用四元数吧
#endif
// translation error
residual_.block<3,1>(3,0) = Pi - Pp_;//第一个顶点的先验平移和第一个顶点的平移的残差
// std::cout << residual_.transpose() <<std::endl;
}
//计算和先验残差的雅克比
void EdgeSE3Prior::ComputeJacobians()
{
VecX param_i = verticies_[0]->Parameters();
Qd Qi(param_i[6], param_i[3], param_i[4], param_i[5]);//
// w.r.t. pose i
Eigen::Matrix<double, 6, 6> jacobian_pose_i = Eigen::Matrix<double, 6, 6>::Zero();
#ifdef USE_SO3_JACOBIAN
Sophus::SO3d ri(Qi);//第一个顶点的旋转-ri
Sophus::SO3d rp(Qp_);//第一个顶点先验的旋转-rp
Sophus::SO3d res_r = rp.inverse() * ri;//第一个顶点的先验的旋转和第一个顶点的旋转的残差
// http://rpg.ifi.uzh.ch/docs/RSS15_Forster.pdf 公式A.32
jacobian_pose_i.block<3,3>(0,3) = Sophus::SO3d::JacobianRInv(res_r.log());
#else
jacobian_pose_i.block<3,3>(0,3) = Qleft(Qp_.inverse() * Qi).bottomRightCorner<3, 3>();//四元数的这个公式不太懂
#endif
jacobian_pose_i.block<3,3>(3,0) = Mat33::Identity();
jacobians_[0] = jacobian_pose_i;
// std::cout << jacobian_pose_i << std::endl;
}
在main()函数中构建循环遍历不同的权重，对不同的权重下的优化进行对比，将不同权重下的迭代次数，求解时间，路标点(逆深度)、相机平移、相机旋转的均方根误差 存在csv_data中
源码如下：
int main() {
// 准备数据
vector<Frame> cameras;
vector<Eigen::Vector3d> points;
GetSimDataInWordFrame(cameras, points);//cameras, points这里面的数据都是真实的
Eigen::Quaterniond qic(1, 0, 0, 0);//imu与相机变换的旋转
Eigen::Vector3d tic(0, 0, 0);//imu与相机变换的平移
///存csv文件
std::ofstream csv_data;
csv_data.open("data.csv", ios_base::out);
// 设置不同权重 咱们就是想看不同权重下 先验方法的效果
// vector<double> Wparray{0, 3, 7, 10, 50, 100, 300, 500, 1000, 5000};
vector<double> Wparray{
0, 1e-5, 1e-4, 1e-3, 1e-2, 0.05, 1e-1, 0.5, 1e0, 5, 1e1,
50, 1e2, 1e3, 1e4, 1e5, 1e6, 1e7, 1e8, 1e9, 1e10};
我个人的理解是 这里的边由两种边构成
1、第一帧和第二帧的先验边
2、各个相机关于同一个路标点的边
for (int i = 0; i < Wparray.size(); ++i)//遍历所有的权重
{
// 构建 problem
Problem problem(Problem::ProblemType::SLAM_PROBLEM);//slam问题
// 所有 Pose
vector<shared_ptr<VertexPose> > vertexCams_vec; //容器vertexCams_vec里存放指向顶点的指针
for (size_t i = 0; i < cameras.size(); ++i)
{
shared_ptr<VertexPose> vertexCam(new VertexPose()); //vertexCam指向各个顶点
Eigen::VectorXd pose(7);//pose得到各个顶点的相机里的具体参数
pose << cameras[i].twc, cameras[i].qwc.x(), cameras[i].qwc.y(), cameras[i].qwc.z(), cameras[i].qwc.w();
vertexCam->SetParameters(pose);//
// if(i < 2)
// vertexCam->SetFixed();//这里不用fix的方法
problem.AddVertex(vertexCam);//添加pose顶点
vertexCams_vec.push_back(vertexCam);//将顶点放在容器里
}
/// 根据论文中 gauge prior 方法额外添加第一帧、第二帧的两个先验项，等效于在 H_11 H_22 上加上一矩阵 H_prior 设置不同的信息矩阵比较结果
double Wp = Wparray[i];// 0 3 7 10 50 100 300 500 1000 5000
在第一帧和第二帧加先验 等效于在 H_11 H_22 上加上一矩阵 H_prior
for (size_t i = 0; i < 2; ++i)
{
shared_ptr<EdgeSE3Prior> edge_prior(new EdgeSE3Prior(cameras[i].twc, cameras[i].qwc));//把第一帧和第二帧的旋转和平移放进去
std::vector<std::shared_ptr<Vertex> > edge_prior_vertex;//容器edge_prior_vertex存放先验的顶点
edge_prior_vertex.push_back(vertexCams_vec[i]);//先验顶点
edge_prior->SetVertex(edge_prior_vertex);//设置先验顶点
edge_prior->SetInformation(edge_prior->Information() * Wp);//设置先验信息矩阵
problem.AddEdge(edge_prior);//加入先验边 先验边里面弄好了残差，雅克比的计算了
}
// 所有 Point 及 edge
std::default_random_engine generator;//随机数产生器
std::normal_distribution<double> noise_pdf(0, 1.);//噪声的正态分布
double noise = 0;
vector<double> noise_invd;//容器noise_invd存放各个路标点的逆深度
vector<shared_ptr<VertexInverseDepth> > allPoints;//容器allPoints
/// 遍历所有landmark
for (size_t i = 0; i < points.size(); ++i)
{
//假设所有特征点的起始帧为第0帧， 逆深度容易得到
Eigen::Vector3d Pw = points[i];//世界下的路标点
Eigen::Vector3d Pc = cameras[0].Rwc.transpose() * (Pw - cameras[0].twc);//相机下的路标点
noise = noise_pdf(generator);//随机产生噪声
double inverse_depth = 1. / (Pc.z() + noise);//带噪声的逆深度
// double inverse_depth = 1. / Pc.z();//不带噪声的逆深度
noise_invd.push_back(inverse_depth);
// 初始化特征 vertex
shared_ptr<VertexInverseDepth> verterxPoint(new VertexInverseDepth());
VecX inv_d(1);//inv_d尺寸是2？
inv_d << inverse_depth;//把第一 和第二的逆深度放在 inv_d
verterxPoint->SetParameters(inv_d);//把逆深度设置为verterxPoint
problem.AddVertex(verterxPoint);//加入顶点
allPoints.push_back(verterxPoint);
// 每个特征对应的投影误差, 第 0 帧为起始帧
/// 每个landmark在每个相机中观测
for (size_t j = 1; j < cameras.size(); ++j)
{
// 在相机cameras[0]、cameras[j]下观测的特征点，归一化平面坐标
Eigen::Vector3d pt_i = cameras[0].featurePerId.find(i)->second;
Eigen::Vector3d pt_j = cameras[j].featurePerId.find(i)->second;
shared_ptr<EdgeReprojection> edge(new EdgeReprojection(pt_i, pt_j));//算出同一个特征在不同相机下的投影误差
edge->SetTranslationImuFromCamera(qic, tic);//imu与相机的变换？？ 相机到imu
std::vector<std::shared_ptr<Vertex> > edge_vertex;//相机与相机
edge_vertex.push_back(verterxPoint);//此时的路标点的逆深度
edge_vertex.push_back(vertexCams_vec[0]);//此时路标点被第0个相机看到，这里的路标点以第0帧为起始帧
edge_vertex.push_back(vertexCams_vec[j]);//此时路标点被第j相机看到
edge->SetVertex(edge_vertex);//设置边的顶点
problem.AddEdge(edge);//加入边
}
}
problem.Solve(15);//优化
std::cout << "\nCompare MonoBA results after opt..." << std::endl;
for (size_t k = 0; k < allPoints.size(); k+=1) {
std::cout << "after opt, point " << k << " : gt " << 1. / points[k].z() << " ,noise "
<< noise_invd[k] << " ,opt " << allPoints[k]->Parameters() << std::endl;
}
std::cout<<"------------ pose translation ----------------"<<std::endl;
for (int i = 0; i < vertexCams_vec.size(); ++i) {
std::cout<<"translation after opt: "<< i <<" :"<< vertexCams_vec[i]->Parameters().head(3).transpose() << " || gt: "<<cameras[i].twc.transpose()<<std::endl;
}
/// 优化完成后，第一帧相机的 pose 平移（x,y,z）不再是原点 0,0,0. 说明向零空间发生了漂移。
/// 解决办法： fix 第一帧和第二帧，固定 7 自由度。 或者加上非常大的先验值。
std::cout << "\n------------Test add different prior weight------------" << std::endl;
cout << "weight:" << Wp << endl;
//计算landmark逆深度的总误差
double sumErrorlandmark = 0;
for (int l = 0; l < allPoints.size(); ++l) {
double error = 1./points[l].z() - allPoints[l]->Parameters()[0];//allPoints[l]->Parameters()[0]为优化后各个路标点的逆深度
sumErrorlandmark += error * error;
}
//计算landmark均方根误差
sumErrorlandmark += sqrt(sumErrorlandmark / allPoints.size());//landmark均方根误差
cout << "Landmark root mean squared error(RMSE):" << sumErrorlandmark << endl;
double sumErrorCamTran = 0;
double sumErrorCamRot = 0;
//优化后的相机位姿
double qx = vertexCams_vec[0]->Parameters()[3];
double qy = vertexCams_vec[0]->Parameters()[4];
double qz = vertexCams_vec[0]->Parameters()[5];
double qw = vertexCams_vec[0]->Parameters()[6];
Sophus::SE3d cam0_opt(Qd(qw, qx, qy, qz), vertexCams_vec[0]->Parameters().head(3));
//真实的相机位姿
Sophus::SE3d cam0_gt(cameras[0].qwc, cameras[0].twc);
// gt2opt 将opt坐标系下位姿变化到gt中的变换矩阵，或者解释为gt坐标系变换到到opt坐标系的变换矩阵
//用于将优化后的位姿与真实位姿对齐，以第一帧为标准
Sophus::SE3d gt2opt(cam0_gt.inverse() * cam0_opt);
//计算相机pose的误差
for (int i = 1; i < vertexCams_vec.size(); ++i) {
qx = vertexCams_vec[i]->Parameters()[3];
qy = vertexCams_vec[i]->Parameters()[4];
qz = vertexCams_vec[i]->Parameters()[5];
qw = vertexCams_vec[i]->Parameters()[6];
//cami_opt是从第2个相机开始的优化后的位姿
Sophus::SE3d cami_opt(Qd(qw, qx, qy, qz), vertexCams_vec[i]->Parameters().head(3));
cami_opt = gt2opt * cami_opt;//这里得到真实坐标系下的优化后的位姿
double error2 = (cami_opt.translation() - cameras[i].twc).norm();
sumErrorCamTran += error2 * error2;
double error3 = Sophus::SO3d::log(Sophus::SO3d(cameras[i].Rwc.inverse() * cami_opt.rotationMatrix())).norm();
sumErrorCamRot += error3 * error3;
}
sumErrorCamRot = sqrt(sumErrorCamRot / (vertexCams_vec.size() - 1));//旋转均方根误差
sumErrorCamTran = sqrt(sumErrorCamTran / (vertexCams_vec.size() - 1));//平移均方根误差
cout << "Camera pose translation RMSE:" << sumErrorCamTran << endl;
cout << "Camera pose rotation RMSE:" << sumErrorCamRot << endl << endl;
//把对应权重，迭代次数，求解时间，路标点(逆深度)、相机平移、相机旋转的均方根误差 存在csv_data中
csv_data << Wp << "," << problem.sumiter << "," << problem.solvetime << "," <<
sumErrorlandmark << "," << sumErrorCamTran << "," << sumErrorCamRot << endl;
global_vertex_id = 0;
}
// problem.TestMarginalize();
return 0;
}