《主动驾驶与机器人中的SLAM技术》ch8：基于预积分和图优化的紧耦合 LIO 系 ...

        和组合导航一样，也可以通过预积分 IMU 因子加上雷达残差来实现基于预积分和图优化的紧耦合 LIO 系统。一些今世的 Lidar SLAM 系统也采用了这种方式。相比滤波器方法来说，预积分因子可以更方便地整合到现有的优化框架中，从开发到实现都更为便捷。
        1 预积分 LIO 系统的履历

        在实现当中，预积分的使用方式是相当机动的，要设置的参数也比 EKF 系统更多。比方 LIO-SAM 把预积分因子与雷达里程计的因子相结合，来构建整个优化问题。而在 VSLAM 系统里，也可以把预积分因子与重投影误差结合起往复求解 Bundle Adjustment。我们在此先容一些预积分应用上的履历：

• 1. 预积分因子通常关联两个关键帧的高维状态（典型的 15 维状态
  
  ）。在转换为图优化问题时，我们可以选择①把各极点分开处理，比方 SE(3) 一个极点，v 占一个极点，然后让一个预积分边关联到 8 个极点上；②也可以选择把高维状态写成一个极点，而预积分边关联两个极点，但雅可比矩阵含有大量的零块。在本节实际操纵中，我们选择前一种做法，即极点种类数量较多，但边的维度较低的写法。这里使用和 《主动驾驶与机器人中的SLAM技术》ch4：预积分学 中一样的散装的形式。
  
• 2. 由于预积分因子关联的变量较多，且观丈量大部分是状态变量的差值，我们应该对状态变量有富足的观测和束缚，否则整个状态变量容易在零空间内自由变动。比方预积分的速度观测
  
  形貌了两个关键帧速度之差。如果我们将两个关键帧的速度都增量固定值，也可以让速度项误差保持不变，而在位移项施加一些调解，还能让位移部分观测保持不变。因此，在实际使用中，我们会给前一个关键帧施加先验束缚，给后一个关键帧施加观测束缚，让整个优化问题限制在肯定的范围内。
  
• 3. 预积分的图优化模子如下图。我们在对两个关键帧盘算优化时，为上一个关键帧添加一个先验因子，然后在两个帧间添加预积分因子和零偏随机游走因子，末了在下一个关键帧中添加 NDT 观测的位姿束缚。在本轮优化完成后，我们利用边缘化方法，求出下一关键帧位姿的协方差矩阵，作为下一轮优化的先验因子来使用。
  

• 4. 这个图优化模子和第 4 章中的 GINS 系统非常相似。但是我们应当注意到，雷达里程计的观测位姿是依赖预测数据（初始值）的，这和 RTK 的位姿观测（绝对位姿观测）有着本质区别。如果 RTK 信号精良，我们可以认为 RTK 的观测有着固定的精度，此时滤波器和图优化器都可以保证在位移和旋转方面收敛。然而，如果雷达里程计使用一个禁绝确的预测位姿，它很有大概给出一个不正常的观测位姿，进而使整个 LIO 发散。这也导致了基于图优化的 LIO 系统调试难度要明显大于 GINS 系统。
  
• 5. 为了重复使用 《主动驾驶与机器人中的SLAM技术》ch8：基于 IESKF 的紧耦合 LIO 系统 中的代码，我们仍然使用前文所用的 LIO 框架，只是将原先 IESKF 处理的预测和观测部分，变为预积分器的预测和观测部分（在实际的系统中，也可以将滤波器作为前端，把图优化当成关键帧后端来使用）。整个 LIO 的盘算框架图如下图所示。我们会在两个点云之间使用预积分举行优化。当然，正如我们前面所说，预积分的使用方式十分机动，读者不必拘泥于我们的实现方式，也可以使用更长时间的预积分优化，大概将 NDT 内部的残差放到图优化中。但相对的，由于预积分因子关联的极点较多，实际调试会比力困难，容易造成误差发散的环境。从一个现有系统出发再举行后端优化是个不错的选择。
  

        2 预积分图优化的极点

        这里图优化的极点 和 《主动驾驶与机器人中的SLAM技术》ch4：基于预积分和图优化的 GINS 中一样，为 15 维的位姿（

）、速度、陀螺仪零偏、加速度计零偏四种极点，不再过多先容。
        3 预积分图优化的边 

        这里的图优化边包括：

• 预积分边（观测值维度为 9 维的多元边）：ch4：预积分学 中先容。
  
• 零偏随机游走边（观测值维度为 3 维的双元边）：ch4：基于预积分和图优化的 GINS 中先容。
  
• 先验因子边（观测值维度为 15 维的多元边）：ch4：基于预积分和图优化的 GINS 中先容。
  
• NDT 观测边（观测值维度为 6 维的单元边）：和双天线的 GNSS 观测边同等，在 ch4：基于预积分和图优化的 GINS 中先容。
  

        3.1 NDT 残差边（观测值维度为 3 维的单元边）

        注意（前面提到的）： 广义地说，只要我们筹划的状态估计系统考虑了各传感器内涵的性质，而非模块化地将它们的输出举行融合，就可以称为紧耦合系统。比方，考虑了 IMU 观测噪声和零偏的系统，就可以称为 IMU的（或 INS 的）紧耦合；考虑了激光的配准残差，就可以称为激光的紧耦合；考虑了视觉特征点的重投影误差，大概考虑了 RTK 的细分状态、搜星数等信息，就可以称为视觉或 RTK 的紧耦合。
        在 ch8：基于 IESKF 的紧耦合 LIO 系统 中，我们即考虑了 IMU 的观测噪声和零偏，又考虑了激光的配准残差（NDT 残差），所以可以称之为紧耦合的 LIO 系统；但是在这里，我们只考虑的 IMU 的观测噪声和零偏，并没有考虑点云的配准残差，严格来说不能称之为紧耦合的 LIO 系统。但是在 slam_in_autonomous_driving/src/common/g2o_types.h 和 slam_in_autonomous_driving/src/ch7/ndt_inc.cc中，实现了 NDT残差边（EdgeNDT类）和 根据估计的NDT建立edges的函数（IncNdt3d::BuildNDTEdges())，本章中并没有使用这里先容的NDT残差边，后续可将其加入到图优化中。
        残差的界说：
       假设图 8.3 中的上一个关键帧是 

 时间，下一个关键帧是 

 时间。 

 时间点云中的某一个点点

 经过 预积分预测得到的

 时间的位姿 

（ 

） 的转换后，会落在目的点云中的某一个体素内，假设这个体素的正态分布参数为

。此时，该点的残差 

 为 转换后的点的坐标和体素中的正态分布参数中的均值之差，即：

        残差对状态变量的雅可比矩阵：

1. slam_in_autonomous_driving/src/common/g2o_types.h
3. /**
4. * NDT误差模型
5. * 残差是 Rp+t-mu，info为NDT内部估计的info
6. * 观测值维度为 3 维的单元边
7. */
8. class EdgeNDT : public g2o::BaseUnaryEdge<3, Vec3d, VertexPose> {
9.    public:
10.     EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
11.     EdgeNDT() = default;
13.     /// 需要查询NDT内部的体素，这里用一个函数式给设置过去
14.     // 该函数已实现，在 IncNdt3d::BuildNDTEdges() 函数内部
15.     using QueryVoxelFunc = std::function<bool(const Vec3d& query_pt, Vec3d& mu, Mat3d& info)>;
17.     EdgeNDT(VertexPose* v0, const Vec3d& pt, QueryVoxelFunc func) {
18.         setVertex(0, v0);
19.         pt_ = pt;
20.         query_ = func;
22.         Vec3d q = v0->estimate().so3() * pt_ + v0->estimate().translation();
23.         if (query_(q, mu_, info_)) {
24.             setInformation(info_);
25.             valid_ = true;
26.         } else {
27.             valid_ = false;
28.         }
29.     }
31.     bool IsValid() const { return valid_; }
33.     Mat6d GetHessian() {
34.         linearizeOplus();
35.         return _jacobianOplusXi.transpose() * info_ * _jacobianOplusXi;
36.     }
38.     /// 残差计算
39.     void computeError() override {
40.         VertexPose* v0 = (VertexPose*)_vertices[0];
41.         Vec3d q = v0->estimate().so3() * pt_ + v0->estimate().translation();
43.         if (query_(q, mu_, info_)) {
44.             _error = q - mu_;
45.             setInformation(info_);
46.             valid_ = true;
47.         } else {
48.             valid_ = false;
49.             _error.setZero();
50.             setLevel(1);
51.         }
52.     }
54.     /// 线性化
55.     void linearizeOplus() override {
56.         if (valid_) {
57.             VertexPose* v0 = (VertexPose*)_vertices[0];
58.             SO3 R = v0->estimate().so3();
60.             _jacobianOplusXi.setZero();
61.             _jacobianOplusXi.block<3, 3>(0, 0) = -R.matrix() * SO3::hat(pt_);  // 对R
62.             _jacobianOplusXi.block<3, 3>(0, 3) = Mat3d::Identity();            // 对p
63.         } else {
64.             _jacobianOplusXi.setZero();
65.         }
66.     }
68.     virtual bool read(std::istream& in) { return true; }
69.     virtual bool write(std::ostream& out) const { return true; }
71.    private:
72.     QueryVoxelFunc query_;
73.     Vec3d pt_ = Vec3d::Zero();
74.     Vec3d mu_ = Vec3d::Zero();
75.     Mat3d info_ = Mat3d::Identity();
76.     bool valid_ = false;
77. };

复制代码

        根据估计的NDT（local map）建立 NDT残差边 ：

1. slam_in_autonomous_driving/src/ch7/ndt_inc.cc
3. /**
4. * 根据估计的NDT建立edges
5. * @param v     ：输入参数，位姿顶点
6. * @param edges ：输出参数，全部的有效的NDT残差边
7. */
8. void IncNdt3d::BuildNDTEdges(sad::VertexPose* v, std::vector<EdgeNDT*>& edges) {
9.     assert(grids_.empty() == false);
10.     SE3 pose = v->estimate();
12.     /// 整体流程和NDT一致，只是把查询函数放到edge内部，建立和v绑定的边
13.     for (const auto& pt : source_->points) {
14.         Vec3d q = ToVec3d(pt);
15.         auto edge = new EdgeNDT(v, q, [this](const Vec3d& qs, Vec3d& mu, Mat3d& info) -> bool {
16.             Vec3i key = CastToInt(Vec3d(qs * options_.inv_voxel_size_));
18.             auto it = grids_.find(key);
19.             /// 这里要检查高斯分布是否已经估计
20.             if (it != grids_.end() && it->second->second.ndt_estimated_) {
21.                 auto& v = it->second->second;  // voxel
22.                 mu = v.mu_;
23.                 info = v.info_;
24.                 return true;
25.             } else {
26.                 return false;
27.             }
28.         });
30.         if (edge->IsValid()) {
31.             edges.emplace_back(edge);
32.         } else {
33.             delete edge;
34.         }
35.     }
36. }

复制代码

   
        4 基于预积分和图优化 LIO 系统的实现

        基于预积分的紧耦合 LioPreinteg类 持有一个 IncNdt3d 对象，一个 IMUPreintegration 对象，一个 MessageSync 对象 处理同步之后的点云和 IMU。该类处理流程非常简单：当 MeasureGroup 到达后，在 IMU 未初始化时，使用第 3 章的静止初始化来估计 IMU 零偏。初始化完毕后，预积分 IMU 数据举行预测，再用预测数据对点云去畸变，末了对去畸变的点云做配准。

1. void LioPreinteg::ProcessMeasurements(const MeasureGroup &meas) {
2.     LOG(INFO) << "call meas, imu: " << meas.imu_.size() << ", lidar pts: " << meas.lidar_->size();
3.     measures_ = meas;
5.     if (imu_need_init_) {
6.         // 初始化IMU系统
7.         TryInitIMU();
8.         return;
9.     }
11.     // 利用IMU数据进行状态预测
12.     Predict();
14.     // 对点云去畸变
15.     Undistort();
17.     // 配准
18.     Align();
19. }

复制代码

        4.1 IMU 静止初始化

        IMU 的静止初始化与《主动驾驶与机器人中的SLAM技术》ch3：惯性导航与组合导航 中先容的大要同等。当 MeasureGroup 到达后，在 IMU 未初始化时，调用 StaticIMUInit::AddIMU() 函数举行 IMU的静止初始化。
        当 IMU 初始化成功时，在当前 MeasureGroup 中使用 IMU 静止初始化结果初始化了 陀螺仪和加速度计的噪声标准差、初始的 

、预积分类IMUPreintegration（在其构造中使用陀螺仪和加速度计的噪声方差初始化了 IMU 丈量噪声的协方差矩阵）。
  

1. void LioPreinteg::TryInitIMU() {
2.     for (auto imu : measures_.imu_) {
3.         imu_init_.AddIMU(*imu);
4.     }
6.     if (imu_init_.InitSuccess()) {
7.         // 读取初始零偏，设置ESKF
8.         // 噪声由初始化器估计
9.         options_.preinteg_options_.noise_gyro_ = sqrt(imu_init_.GetCovGyro()[0]);
10.         options_.preinteg_options_.noise_acce_ = sqrt(imu_init_.GetCovAcce()[0]);
11.         options_.preinteg_options_.init_ba_ = imu_init_.GetInitBa();
12.         options_.preinteg_options_.init_bg_ = imu_init_.GetInitBg();
14.         preinteg_ = std::make_shared<IMUPreintegration>(options_.preinteg_options_);
15.         imu_need_init_ = false;
17.         current_nav_state_.v_.setZero();
18.         current_nav_state_.bg_ = imu_init_.GetInitBg();
19.         current_nav_state_.ba_ = imu_init_.GetInitBa();
20.         current_nav_state_.timestamp_ = measures_.imu_.back()->timestamp_;
21.         
22.         last_nav_state_ = current_nav_state_;
23.         last_imu_ = measures_.imu_.back();
25.         LOG(INFO) << "IMU初始化成功";
26.     }
27. }

复制代码

       4.2 使用预积分预测

        和基于 IESKF 的紧耦合 LIO 系统不同，这里使用了 IMU 预积分举行预测：

1. void LioPreinteg::Predict() {
2.     // 这里会清空 imu_states_ ，所以在每接收一个 MeasureGroup 时，imu_states_ 中会存储 measures_.imu_.size() + 1 个数据，用于去畸变
3.     imu_states_.clear();
4.     imu_states_.emplace_back(last_nav_state_);
6.     /// 对IMU状态进行预测
7.     for (auto &imu : measures_.imu_) {
8.         if (last_imu_ != nullptr) {
9.             preinteg_->Integrate(*imu, imu->timestamp_ - last_imu_->timestamp_);
10.         }
12.         last_imu_ = imu;
13.         imu_states_.emplace_back(preinteg_->Predict(last_nav_state_, imu_init_.GetGravity()));
14.     }
15. }

复制代码

        4.3 使用 IMU 预测位姿举行运动补偿

        和 《主动驾驶与机器人中的SLAM技术》ch8：基于 IESKF 的紧耦合 LIO 系统 中先容的一样，不再先容。
        4.4 位姿配准部分

        在配定时，使用预积分给出的预测位姿作为增量NDT里程计的初始位姿输入，迭代得到优化后的位姿，将优化后的位姿作为观测值举行优化（即作为 

 的初始估计值）。

1. void LioPreinteg::Align() {
2.     FullCloudPtr scan_undistort_trans(new FullPointCloudType);
3.     pcl::transformPointCloud(*scan_undistort_, *scan_undistort_trans, TIL_.matrix().cast<float>());
4.     scan_undistort_ = scan_undistort_trans;
6.     current_scan_ = ConvertToCloud<FullPointType>(scan_undistort_);
8.     // voxel 之
9.     pcl::VoxelGrid<PointType> voxel;
10.     voxel.setLeafSize(0.5, 0.5, 0.5);
11.     voxel.setInputCloud(current_scan_);
13.     CloudPtr current_scan_filter(new PointCloudType);
14.     voxel.filter(*current_scan_filter);
16.     /// the first scan
17.     if (flg_first_scan_) {
18.         ndt_.AddCloud(current_scan_);
20.         // my 我认为这里应该添加如下代码
21.         // current_nav_state_ = imu_states_.back();
22.         // NormalizeVelocity();
23.         // last_nav_state_ = current_nav_state_;
25.         // 重置预积分 preinteg_
26.         preinteg_ = std::make_shared<IMUPreintegration>(options_.preinteg_options_);
27.         flg_first_scan_ = false;
28.         return;
29.     }
31.     // 后续的scan，使用NDT配合pose进行更新
32.     LOG(INFO) << "=== frame " << frame_num_;
33.     ndt_.SetSource(current_scan_filter);
35.     current_nav_state_ = preinteg_->Predict(last_nav_state_, imu_init_.GetGravity());
36.     ndt_pose_ = current_nav_state_.GetSE3();
38.     // 使用 IMU 预积分预测值作为配准初始值
39.     ndt_.AlignNdt(ndt_pose_);
41.     Optimize();
43.     // 若运动了一定范围，则把点云放入地图中
44.     SE3 current_pose = current_nav_state_.GetSE3();
45.     SE3 delta_pose = last_ndt_pose_.inverse() * current_pose;
47.     if (delta_pose.translation().norm() > 0.3 || delta_pose.so3().log().norm() > math::deg2rad(5.0)) {
48.         // 将地图合入NDT中
49.         CloudPtr current_scan_world(new PointCloudType);
50.         pcl::transformPointCloud(*current_scan_filter, *current_scan_world, current_pose.matrix());
51.         ndt_.AddCloud(current_scan_world);
52.         last_ndt_pose_ = current_pose;
53.     }
55.     // 放入UI
56.     if (ui_) {
57.         ui_->UpdateScan(current_scan_, current_nav_state_.GetSE3());  // 转成Lidar Pose传给UI
58.         ui_->UpdateNavState(current_nav_state_);
59.     }
61.     frame_num_++;
62. }

复制代码

        4.5 图优化部分

        图优化部分基本上和 ch4：基于预积分和图优化的 GINS 一样，不同之处在于一下几点：

• 1.使用了NDT优化后的位姿作为
  
   时间位姿极点的初始估计值，而没有使用预积分预测的位姿；
  

1.     // 本时刻顶点，pose, v, bg, ba
2.     auto v1_pose = new VertexPose();
3.     v1_pose->setId(4);
4.     // 注意：这里使用NDT优化后的位姿作为 j 时刻位姿的初始估计值
5.     v1_pose->setEstimate(ndt_pose_);  // NDT pose作为初值
6.     // v1_pose->setEstimate(current_nav_state_.GetSE3());  // 预测的pose作为初值
7.     optimizer.addVertex(v1_pose);

复制代码

• 2.在优化过程中，使用 setFixed() 函数将
  
   时间的 
  
   和
  
   节点视为固定节点，不举行优化；
  

1.     // 在优化过程中，将 i 时刻的bg和ba节点视为固定节点，不进行优化
2.     v0_bg->setFixed(true);
3.     v0_ba->setFixed(true);

复制代码

• 3.对于
  
  ，我们想将 
  
   中的
  
   举行边缘化（对应 Hessian 矩阵中左上角 15x15 的小块），得到 
  
   时间状态的信息矩阵（15x15维），作为下一轮优化时（
  
   时间和
  
   时间） 
  
   时间的先验因子的信息矩阵。在本博客的 4.6 末节中具体先容；
  
• 4.对速度举行了限制，将其限制在正常区间。
  

1. void LioPreinteg::NormalizeVelocity() {
2.     /// 限制v的变化
3.     /// 一般是-y 方向速度
4.     // 将车体坐标系下 y 方向的分速度限制在 （-2 到 0 之间）
5.     Vec3d v_body = current_nav_state_.R_.inverse() * current_nav_state_.v_;
6.     if (v_body[1] > 0) {
7.         v_body[1] = 0;
8.     }
9.     // 将车体坐标系下 z 方向的分速度限制为 0
10.     v_body[2] = 0;
12.     if (v_body[1] < -2.0) {
13.         v_body[1] = -2.0;
14.     }
16.     // 将车体坐标系下 x 方向的分速度限制在（-0.1 到 0.1 之间）
17.     if (v_body[0] > 0.1) {
18.         v_body[0] = 0.1;
19.     } else if (v_body[0] < -0.1) {
20.         v_body[0] = -0.1;
21.     }
23.     current_nav_state_.v_ = current_nav_state_.R_ * v_body;
24. }

复制代码

        优化部分代码如下所示：

1. void LioPreinteg::Optimize() {    // 调用g2o求解优化问题    // 上一个state到本时间state的预积分因子，本时间的NDT因子    LOG(INFO) << " === optimizing frame " << frame_num_ << " === "              << ", dt: " << preinteg_->dt_;    /// NOTE 这些东西是对参数非常敏感的。相差几个数量级的话，容易出现优化不动的环境    using BlockSolverType = g2o::BlockSolverX;    using LinearSolverType = g2o::LinearSolverEigen<BlockSolverType::PoseMatrixType>;    auto *solver = new g2o::OptimizationAlgorithmLevenberg(        g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));    g2o::SparseOptimizer optimizer;    optimizer.setAlgorithm(solver);    // 上时间极点， pose, v, bg, ba    auto v0_pose = new VertexPose();    v0_pose->setId(0);    v0_pose->setEstimate(last_nav_state_.GetSE3());    optimizer.addVertex(v0_pose);    auto v0_vel = new VertexVelocity();    v0_vel->setId(1);    v0_vel->setEstimate(last_nav_state_.v_);    optimizer.addVertex(v0_vel);    auto v0_bg = new VertexGyroBias();    v0_bg->setId(2);    v0_bg->setEstimate(last_nav_state_.bg_);    optimizer.addVertex(v0_bg);    auto v0_ba = new VertexAccBias();    v0_ba->setId(3);    v0_ba->setEstimate(last_nav_state_.ba_);    optimizer.addVertex(v0_ba);    // 本时刻顶点，pose, v, bg, ba
2.     auto v1_pose = new VertexPose();
3.     v1_pose->setId(4);
4.     // 注意：这里使用NDT优化后的位姿作为 j 时刻位姿的初始估计值
5.     v1_pose->setEstimate(ndt_pose_);  // NDT pose作为初值
6.     // v1_pose->setEstimate(current_nav_state_.GetSE3());  // 预测的pose作为初值
7.     optimizer.addVertex(v1_pose);    auto v1_vel = new VertexVelocity();    v1_vel->setId(5);    v1_vel->setEstimate(current_nav_state_.v_);    optimizer.addVertex(v1_vel);    auto v1_bg = new VertexGyroBias();    v1_bg->setId(6);    v1_bg->setEstimate(current_nav_state_.bg_);    optimizer.addVertex(v1_bg);    auto v1_ba = new VertexAccBias();    v1_ba->setId(7);    v1_ba->setEstimate(current_nav_state_.ba_);    optimizer.addVertex(v1_ba);    // imu factor    auto edge_inertial = new EdgeInertial(preinteg_, imu_init_.GetGravity());    edge_inertial->setVertex(0, v0_pose);    edge_inertial->setVertex(1, v0_vel);    edge_inertial->setVertex(2, v0_bg);    edge_inertial->setVertex(3, v0_ba);    edge_inertial->setVertex(4, v1_pose);    edge_inertial->setVertex(5, v1_vel);    auto *rk = new g2o::RobustKernelHuber();    rk->setDelta(200.0);    edge_inertial->setRobustKernel(rk);    optimizer.addEdge(edge_inertial);    // 零偏随机游走    auto *edge_gyro_rw = new EdgeGyroRW();    edge_gyro_rw->setVertex(0, v0_bg);    edge_gyro_rw->setVertex(1, v1_bg);    edge_gyro_rw->setInformation(options_.bg_rw_info_);    optimizer.addEdge(edge_gyro_rw);    auto *edge_acc_rw = new EdgeAccRW();    edge_acc_rw->setVertex(0, v0_ba);    edge_acc_rw->setVertex(1, v1_ba);    edge_acc_rw->setInformation(options_.ba_rw_info_);    optimizer.addEdge(edge_acc_rw);    // 上一帧pose, vel, bg, ba的先验    auto *edge_prior = new EdgePriorPoseNavState(last_nav_state_, prior_info_);    edge_prior->setVertex(0, v0_pose);    edge_prior->setVertex(1, v0_vel);    edge_prior->setVertex(2, v0_bg);    edge_prior->setVertex(3, v0_ba);    optimizer.addEdge(edge_prior);    /// 使用NDT的pose举行观测    auto *edge_ndt = new EdgeGNSS(v1_pose, ndt_pose_);    edge_ndt->setInformation(options_.ndt_info_);    optimizer.addEdge(edge_ndt);    if (options_.verbose_) {        LOG(INFO) << "last: " << last_nav_state_;        LOG(INFO) << "pred: " << current_nav_state_;        LOG(INFO) << "NDT: " << ndt_pose_.translation().transpose() << ","                  << ndt_pose_.so3().unit_quaternion().coeffs().transpose();    }    // 在优化过程中，将 i 时刻的bg和ba节点视为固定节点，不进行优化
8.     v0_bg->setFixed(true);
9.     v0_ba->setFixed(true);    // go    optimizer.setVerbose(options_.verbose_);    optimizer.initializeOptimization();    optimizer.optimize(20);    // get results    last_nav_state_.R_ = v0_pose->estimate().so3();    last_nav_state_.p_ = v0_pose->estimate().translation();    last_nav_state_.v_ = v0_vel->estimate();    last_nav_state_.bg_ = v0_bg->estimate();    last_nav_state_.ba_ = v0_ba->estimate();    current_nav_state_.R_ = v1_pose->estimate().so3();    current_nav_state_.p_ = v1_pose->estimate().translation();    current_nav_state_.v_ = v1_vel->estimate();    current_nav_state_.bg_ = v1_bg->estimate();    current_nav_state_.ba_ = v1_ba->estimate();    if (options_.verbose_) {        LOG(INFO) << "last changed to: " << last_nav_state_;        LOG(INFO) << "curr changed to: " << current_nav_state_;        LOG(INFO) << "preinteg chi2: " << edge_inertial->chi2() << ", err: " << edge_inertial->error().transpose();        LOG(INFO) << "prior chi2: " << edge_prior->chi2() << ", err: " << edge_prior->error().transpose();        LOG(INFO) << "ndt: " << edge_ndt->chi2() << "/" << edge_ndt->error().transpose();    }    /// 重置预积分    options_.preinteg_options_.init_bg_ = current_nav_state_.bg_;    options_.preinteg_options_.init_ba_ = current_nav_state_.ba_;    preinteg_ = std::make_shared<IMUPreintegration>(options_.preinteg_options_);    // gauss-newton 迭代中累加Hessian和error,计算dx类似。一共 5 种类型的边，在累加Hessian都要考虑上。
10.     // 计算当前时刻先验
11.     // 构建hessian
12.     // 15x2，顺序：v0_pose, v0_vel, v0_bg, v0_ba, v1_pose, v1_vel, v1_bg, v1_ba
13.     //            0       6        9     12     15        21      24     27
14.     Eigen::Matrix<double, 30, 30> H;
15.     H.setZero();
17.     // ①添加 预积分因子的 Hessian 矩阵
18.     H.block<24, 24>(0, 0) += edge_inertial->GetHessian();
20.     // ②添加 陀螺仪零偏随机游走因子 的 Hessian 矩阵
21.     Eigen::Matrix<double, 6, 6> Hgr = edge_gyro_rw->GetHessian();
22.     // 行: bg1 列: bg1
23.     H.block<3, 3>(9, 9) += Hgr.block<3, 3>(0, 0);
24.     // 行: bg1 列: bg2
25.     H.block<3, 3>(9, 24) += Hgr.block<3, 3>(0, 3);
26.     // 行: bg2 列: bg1
27.     H.block<3, 3>(24, 9) += Hgr.block<3, 3>(3, 0);
28.     // 行: bg2 列: bg2
29.     H.block<3, 3>(24, 24) += Hgr.block<3, 3>(3, 3);
31.     // ③添加 加速度计零偏随机游走因子 的 Hessian 矩阵
32.     Eigen::Matrix<double, 6, 6> Har = edge_acc_rw->GetHessian();
33.     H.block<3, 3>(12, 12) += Har.block<3, 3>(0, 0);
34.     H.block<3, 3>(12, 27) += Har.block<3, 3>(0, 3);
35.     H.block<3, 3>(27, 12) += Har.block<3, 3>(3, 0);
36.     H.block<3, 3>(27, 27) += Har.block<3, 3>(3, 3);
38.     // ④添加 先验因子 的 Hessian 矩阵
39.     H.block<15, 15>(0, 0) += edge_prior->GetHessian();
40.     // ⑤添加 NDT 观测因子的 Hessian 矩阵
41.     H.block<6, 6>(15, 15) += edge_ndt->GetHessian();
43.     // 边缘化(利用 H 的稀疏性加速 HΔx=g 的求解的方法。视觉SLAM十四讲 p245)
44.     // 边缘化(在本轮优化完成后，利用边缘化的方法，求出下一个关键帧位姿的协方差，作为下一轮优化的先验因子的信息矩阵使用。sad p245)
45.     H = math::Marginalize(H, 0, 14);
46.     prior_info_ = H.block<15, 15>(15, 15);    if (options_.verbose_) {        LOG(INFO) << "info trace: " << prior_info_.trace();        LOG(INFO) << "optimization done.";    }    NormalizeVelocity();    last_nav_state_ = current_nav_state_;}

复制代码

        4.6 边缘化

        优化完毕后，把 5 种因子（预积分因子、2个零偏随机游走因子、先验因子和NDT观测因子）的海塞 (Hessian) 矩阵按照次序累加组合成一个大的 Hessian 矩阵

，对于

，我们想将 

 中的

 边缘化（对应 Hessian 矩阵中左上角 15x15 的小块，要求其逆），得到 

 时间状态的信息矩阵（15x15维），作为下一轮优化时（

 时间和

 时间） 

 时间的先验因子的信息矩阵。
        累加 5 种因子的 Hessian 矩阵一个大的 Hessian 矩阵

 代码如下：

1.     // gauss-newton 迭代中累加Hessian和error,计算dx类似。一共 5 种类型的边，在累加Hessian都要考虑上。
2.     // 计算当前时刻先验
3.     // 构建hessian
4.     // 15x2，顺序：v0_pose, v0_vel, v0_bg, v0_ba, v1_pose, v1_vel, v1_bg, v1_ba
5.     //            0       6        9     12     15        21      24     27
6.     Eigen::Matrix<double, 30, 30> H;
7.     H.setZero();
9.     // ①添加 预积分因子的 Hessian 矩阵
10.     H.block<24, 24>(0, 0) += edge_inertial->GetHessian();
12.     // ②添加 陀螺仪零偏随机游走因子 的 Hessian 矩阵
13.     Eigen::Matrix<double, 6, 6> Hgr = edge_gyro_rw->GetHessian();
14.     // 行: bg1 列: bg1
15.     H.block<3, 3>(9, 9) += Hgr.block<3, 3>(0, 0);
16.     // 行: bg1 列: bg2
17.     H.block<3, 3>(9, 24) += Hgr.block<3, 3>(0, 3);
18.     // 行: bg2 列: bg1
19.     H.block<3, 3>(24, 9) += Hgr.block<3, 3>(3, 0);
20.     // 行: bg2 列: bg2
21.     H.block<3, 3>(24, 24) += Hgr.block<3, 3>(3, 3);
23.     // ③添加 加速度计零偏随机游走因子 的 Hessian 矩阵
24.     Eigen::Matrix<double, 6, 6> Har = edge_acc_rw->GetHessian();
25.     H.block<3, 3>(12, 12) += Har.block<3, 3>(0, 0);
26.     H.block<3, 3>(12, 27) += Har.block<3, 3>(0, 3);
27.     H.block<3, 3>(27, 12) += Har.block<3, 3>(3, 0);
28.     H.block<3, 3>(27, 27) += Har.block<3, 3>(3, 3);
30.     // ④添加 先验因子 的 Hessian 矩阵
31.     H.block<15, 15>(0, 0) += edge_prior->GetHessian();
32.     // ⑤添加 NDT 观测因子的 Hessian 矩阵
33.     H.block<6, 6>(15, 15) += edge_ndt->GetHessian();
35.     // 边缘化(利用 H 的稀疏性加速 HΔx=g 的求解的方法。视觉SLAM十四讲 p245)
36.     // 边缘化(在本轮优化完成后，利用边缘化的方法，求出下一个关键帧位姿的协方差，作为下一轮优化的先验因子的信息矩阵使用。sad p245)
37.     H = math::Marginalize(H, 0, 14);
38.     prior_info_ = H.block<15, 15>(15, 15);

复制代码

          将 

 中的

 边缘化，即消去对应的大 Hessian 矩阵

 中左上角 15x15 的小块，取边缘化后的 

对应的子矩阵，即矩阵右下角15x15 的小块作为下一轮优化的先验因子的信息矩阵使用：

1.     // 边缘化(利用 H 的稀疏性加速 HΔx=g 的求解的方法。视觉SLAM十四讲 p245)
2.     // 边缘化(在本轮优化完成后，利用边缘化的方法，求出下一个关键帧位姿的协方差，作为下一轮优化的先验因子的信息矩阵使用。sad p245)
3.     H = math::Marginalize(H, 0, 14);
4.     prior_info_ = H.block<15, 15>(15, 15);

复制代码

        边缘化的目的如下，要将通过函数形参 start 和 end 选定的 

 对应的小矩阵块 b 消去：

1. a  | ab | ac       a*  | 0 | ac*
2. ba | b  | bc  -->  0   | 0 | 0
3. ca | cb | c        ca* | 0 | c*

复制代码

• 1.通过函数形参 start 和 end 选定待边缘化的 
  
   对应的矩阵块 b；
  
• 2.将 b 矩阵块移动到矩阵 H 的右下角，即对应的 
  
   也在
  
   末了；
  

1. a  | ab | ac       a  | ac | ab
2. ba | b  | bc  -->  ca | c  | cb
3. ca | cb | c        ba | bc | b

复制代码

• 3.对 b 矩阵块举行奇异值分解求其伪逆。
  
  ，
  
   ；
  
• 4.使用如下公式更新 H 矩阵；
  

         TODO：待增补

• 5.将更新后的 H 矩阵规复为初始次序。
  

1. a*  | ac* | 0       a*  | 0 | ac*
2. ca* | c*  | 0  -->  0   | 0 | 0
3. 0   | 0   | 0       ca* | 0 | c*

复制代码

        具体代码如下： 

1. /**
2. * 边缘化。视觉SLAM十四讲。p 249
3. * @param H
4. * @param start
5. * @param end
6. * @return
7. */
8. inline Eigen::MatrixXd Marginalize(const Eigen::MatrixXd& H, const int& start, const int& end) {
9.     // ① b 矩阵块为需要边缘化的矩阵块（通过 start 和 end 确定）
10.     // Goal
11.     // a  | ab | ac       a*  | 0 | ac*
12.     // ba | b  | bc  -->  0   | 0 | 0
13.     // ca | cb | c        ca* | 0 | c*
15.     // Size of block before block to marginalize
16.     const int a = start;
17.     // Size of block to marginalize
18.     const int b = end - start + 1;
19.     // Size of block after block to marginalize
20.     const int c = H.cols() - (end + 1);
22.     // ② 将 b 矩阵块移动到右下角
23.     // Reorder as follows:
24.     // a  | ab | ac       a  | ac | ab
25.     // ba | b  | bc  -->  ca | c  | cb
26.     // ca | cb | c        ba | bc | b
28.     Eigen::MatrixXd Hn = Eigen::MatrixXd::Zero(H.rows(), H.cols());
29.     // block函数：block(startRow, startCol, rows, cols);
30.     if (a > 0) {
31.         Hn.block(0, 0, a, a) = H.block(0, 0, a, a);
32.         Hn.block(0, a + c, a, b) = H.block(0, a, a, b);
33.         Hn.block(a + c, 0, b, a) = H.block(a, 0, b, a);
34.     }
35.     if (a > 0 && c > 0) {
36.         Hn.block(0, a, a, c) = H.block(0, a + b, a, c);
37.         Hn.block(a, 0, c, a) = H.block(a + b, 0, c, a);
38.     }
39.     if (c > 0) {
40.         Hn.block(a, a, c, c) = H.block(a + b, a + b, c, c);
41.         Hn.block(a, a + c, c, b) = H.block(a + b, a, c, b);
42.         Hn.block(a + c, a, b, c) = H.block(a, a + b, b, c);
43.     }
44.     Hn.block(a + c, a + c, b, b) = H.block(a, a, b, b);
46.     // ③ 对 b 矩阵块进行奇异值分解求其伪逆。A = U*Σ*V^T    A^-1 = V*Σ^-1*U^T
47.     // Perform marginalization (Schur complement)
48.     Eigen::JacobiSVD<Eigen::MatrixXd> svd(Hn.block(a + c, a + c, b, b), Eigen::ComputeThinU | Eigen::ComputeThinV);
49.     // 返回奇异值矩阵 Σ，即对角矩阵，其中每个对角元素都是 b 矩阵块 的奇异值。
50.     Eigen::JacobiSVD<Eigen::MatrixXd>::SingularValuesType singularValues_inv = svd.singularValues();
51.     // 计算 Σ^-1
52.     for (int i = 0; i < b; ++i) {
53.         if (singularValues_inv(i) > 1e-6) singularValues_inv(i) = 1.0 / singularValues_inv(i);
54.         else
55.             singularValues_inv(i) = 0;
56.     }
57.     // 使用奇异值分解法求 b 矩阵块的伪逆。A^-1 = V*Σ^-1*U^T
58.     Eigen::MatrixXd invHb = svd.matrixV() * singularValues_inv.asDiagonal() * svd.matrixU().transpose();
59.     // ④ 更新 H 矩阵
60.     // H11 = H11 - H12 * H22^-1 * H21
61.     // H22 = 0
62.     // H12 = 0
63.     // H21 = 0
64.     Hn.block(0, 0, a + c, a + c) =
65.         Hn.block(0, 0, a + c, a + c) - Hn.block(0, a + c, a + c, b) * invHb * Hn.block(a + c, 0, b, a + c);
66.     Hn.block(a + c, a + c, b, b) = Eigen::MatrixXd::Zero(b, b);
67.     Hn.block(0, a + c, a + c, b) = Eigen::MatrixXd::Zero(a + c, b);
68.     Hn.block(a + c, 0, b, a + c) = Eigen::MatrixXd::Zero(b, a + c);
70.     // ⑤将更新后的 H 矩阵恢复为原顺序
71.     // Inverse reorder
72.     // a*  | ac* | 0       a*  | 0 | ac*
73.     // ca* | c*  | 0  -->  0   | 0 | 0
74.     // 0   | 0   | 0       ca* | 0 | c*
75.     Eigen::MatrixXd res = Eigen::MatrixXd::Zero(H.rows(), H.cols());
76.     if (a > 0) {
77.         res.block(0, 0, a, a) = Hn.block(0, 0, a, a);
78.         res.block(0, a, a, b) = Hn.block(0, a + c, a, b);
79.         res.block(a, 0, b, a) = Hn.block(a + c, 0, b, a);
80.     }
81.     if (a > 0 && c > 0) {
82.         res.block(0, a + b, a, c) = Hn.block(0, a, a, c);
83.         res.block(a + b, 0, c, a) = Hn.block(a, 0, c, a);
84.     }
85.     if (c > 0) {
86.         res.block(a + b, a + b, c, c) = Hn.block(a, a, c, c);
87.         res.block(a + b, a, c, b) = Hn.block(a, a + c, c, b);
88.         res.block(a, a + b, b, c) = Hn.block(a + c, a, b, c);
89.     }
91.     res.block(a, a, b, b) = Hn.block(a + c, a + c, b, b);
93.     return res;
94. }

复制代码

免责声明：如果侵犯了您的权益，请联系站长，我们会及时删除侵权内容，谢谢合作！更多信息从访问主页：qidao123.com:ToB企服之家，中国第一个企服评测及商务社交产业平台。