西部数据网站助手,北京招标网官网,在线查看qq空间网站,合肥网站改版目录 1 预积分 LIO 系统的经验 2 预积分图优化的顶点 3 预积分图优化的边 3.1 NDT 残差边#xff08;观测值维度为 3 维的单元边#xff09; 4 基于预积分和图优化 LIO 系统的实现 4.1 IMU 静止初始化 4.2 使用预积分预测 4.3 使用 IMU 预测位姿进行运动补偿 4.4 位姿配准部…目录 1 预积分 LIO 系统的经验 2 预积分图优化的顶点 3 预积分图优化的边 3.1 NDT 残差边观测值维度为 3 维的单元边 4 基于预积分和图优化 LIO 系统的实现 4.1 IMU 静止初始化 4.2 使用预积分预测 4.3 使用 IMU 预测位姿进行运动补偿 4.4 位姿配准部分 4.5 图优化部分 4.6 边缘化 4.6.1 边缘化公式 4.6.2 边缘化过程 和组合导航一样也可以通过预积分 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 中的上一个关键帧是 时刻下一个关键帧是 时刻。 时刻点云中的某一个点点 经过 预积分预测得到的 时刻的位姿 的转换后会落在目标点云中的某一个体素内假设这个体素的正态分布参数为 。此时该点的残差 为 转换后的点的坐标和体素中的正态分布参数中的均值之差即 残差对状态变量的雅可比矩阵 slam_in_autonomous_driving/src/common/g2o_types.h/*** NDT误差模型* 残差是 Rpt-muinfo为NDT内部估计的info* 观测值维度为 3 维的单元边*/
class EdgeNDT : public g2o::BaseUnaryEdge3, Vec3d, VertexPose {public:EIGEN_MAKE_ALIGNED_OPERATOR_NEW;EdgeNDT() default;/// 需要查询NDT内部的体素这里用一个函数式给设置过去// 该函数已实现在 IncNdt3d::BuildNDTEdges() 函数内部using QueryVoxelFunc std::functionbool(const Vec3d query_pt, Vec3d mu, Mat3d info);EdgeNDT(VertexPose* v0, const Vec3d pt, QueryVoxelFunc func) {setVertex(0, v0);pt_ pt;query_ func;Vec3d q v0-estimate().so3() * pt_ v0-estimate().translation();if (query_(q, mu_, info_)) {setInformation(info_);valid_ true;} else {valid_ false;}}bool IsValid() const { return valid_; }Mat6d GetHessian() {linearizeOplus();return _jacobianOplusXi.transpose() * info_ * _jacobianOplusXi;}/// 残差计算void computeError() override {VertexPose* v0 (VertexPose*)_vertices[0];Vec3d q v0-estimate().so3() * pt_ v0-estimate().translation();if (query_(q, mu_, info_)) {_error q - mu_;setInformation(info_);valid_ true;} else {valid_ false;_error.setZero();setLevel(1);}}/// 线性化void linearizeOplus() override {if (valid_) {VertexPose* v0 (VertexPose*)_vertices[0];SO3 R v0-estimate().so3();_jacobianOplusXi.setZero();_jacobianOplusXi.block3, 3(0, 0) -R.matrix() * SO3::hat(pt_); // 对R_jacobianOplusXi.block3, 3(0, 3) Mat3d::Identity(); // 对p} else {_jacobianOplusXi.setZero();}}virtual bool read(std::istream in) { return true; }virtual bool write(std::ostream out) const { return true; }private:QueryVoxelFunc query_;Vec3d pt_ Vec3d::Zero();Vec3d mu_ Vec3d::Zero();Mat3d info_ Mat3d::Identity();bool valid_ false;
}; 根据估计的NDTlocal map建立 NDT残差边
slam_in_autonomous_driving/src/ch7/ndt_inc.cc/**
* 根据估计的NDT建立edges
* param v 输入参数位姿顶点
* param edges 输出参数全部的有效的NDT残差边
*/
void IncNdt3d::BuildNDTEdges(sad::VertexPose* v, std::vectorEdgeNDT* edges) {assert(grids_.empty() false);SE3 pose v-estimate();/// 整体流程和NDT一致只是把查询函数放到edge内部建立和v绑定的边for (const auto pt : source_-points) {Vec3d q ToVec3d(pt);auto edge new EdgeNDT(v, q, [this](const Vec3d qs, Vec3d mu, Mat3d info) - bool {Vec3i key CastToInt(Vec3d(qs * options_.inv_voxel_size_));auto it grids_.find(key);/// 这里要检查高斯分布是否已经估计if (it ! grids_.end() it-second-second.ndt_estimated_) {auto v it-second-second; // voxelmu v.mu_;info v.info_;return true;} else {return false;}});if (edge-IsValid()) {edges.emplace_back(edge);} else {delete edge;}}
} 4 基于预积分和图优化 LIO 系统的实现 基于预积分的紧耦合 LioPreinteg类 持有一个 IncNdt3d 对象一个 IMUPreintegration 对象一个 MessageSync 对象 处理同步之后的点云和 IMU。该类处理流程非常简单当 MeasureGroup 到达后在 IMU 未初始化时使用第 3 章的静止初始化来估计 IMU 零偏。初始化完毕后预积分 IMU 数据进行预测再用预测数据对点云去畸变最后对去畸变的点云做配准。
void LioPreinteg::ProcessMeasurements(const MeasureGroup meas) {LOG(INFO) call meas, imu: meas.imu_.size() , lidar pts: meas.lidar_-size();measures_ meas;if (imu_need_init_) {// 初始化IMU系统TryInitIMU();return;}// 利用IMU数据进行状态预测Predict();// 对点云去畸变Undistort();// 配准Align();
} 4.1 IMU 静止初始化 IMU 的静止初始化与《自动驾驶与机器人中的SLAM技术》ch3惯性导航与组合导航 中介绍的大体一致。当 MeasureGroup 到达后在 IMU 未初始化时调用 StaticIMUInit::AddIMU() 函数进行 IMU的静止初始化。 当 IMU 初始化成功时在当前 MeasureGroup 中使用 IMU 静止初始化结果初始化了 陀螺仪和加速度计的噪声标准差、初始的 、预积分类IMUPreintegration在其构造中使用陀螺仪和加速度计的噪声方差初始化了 IMU 测量噪声的协方差矩阵。 void LioPreinteg::TryInitIMU() {for (auto imu : measures_.imu_) {imu_init_.AddIMU(*imu);}if (imu_init_.InitSuccess()) {// 读取初始零偏设置ESKF// 噪声由初始化器估计options_.preinteg_options_.noise_gyro_ sqrt(imu_init_.GetCovGyro()[0]);options_.preinteg_options_.noise_acce_ sqrt(imu_init_.GetCovAcce()[0]);options_.preinteg_options_.init_ba_ imu_init_.GetInitBa();options_.preinteg_options_.init_bg_ imu_init_.GetInitBg();preinteg_ std::make_sharedIMUPreintegration(options_.preinteg_options_);imu_need_init_ false;current_nav_state_.v_.setZero();current_nav_state_.bg_ imu_init_.GetInitBg();current_nav_state_.ba_ imu_init_.GetInitBa();current_nav_state_.timestamp_ measures_.imu_.back()-timestamp_;last_nav_state_ current_nav_state_;last_imu_ measures_.imu_.back();LOG(INFO) IMU初始化成功;}
} 4.2 使用预积分预测 和基于 IESKF 的紧耦合 LIO 系统不同这里使用了 IMU 预积分进行预测
void LioPreinteg::Predict() {// 这里会清空 imu_states_ 所以在每接收一个 MeasureGroup 时imu_states_ 中会存储 measures_.imu_.size() 1 个数据用于去畸变imu_states_.clear();imu_states_.emplace_back(last_nav_state_);/// 对IMU状态进行预测for (auto imu : measures_.imu_) {if (last_imu_ ! nullptr) {preinteg_-Integrate(*imu, imu-timestamp_ - last_imu_-timestamp_);}last_imu_ imu;imu_states_.emplace_back(preinteg_-Predict(last_nav_state_, imu_init_.GetGravity()));}
} 4.3 使用 IMU 预测位姿进行运动补偿 和 《自动驾驶与机器人中的SLAM技术》ch8基于 IESKF 的紧耦合 LIO 系统 中介绍的一样不再介绍。 4.4 位姿配准部分 在配准时使用预积分给出的预测位姿作为增量NDT里程计的初始位姿输入迭代得到优化后的位姿将优化后的位姿作为观测值进行优化即作为 的初始估计值。
void LioPreinteg::Align() {FullCloudPtr scan_undistort_trans(new FullPointCloudType);pcl::transformPointCloud(*scan_undistort_, *scan_undistort_trans, TIL_.matrix().castfloat());scan_undistort_ scan_undistort_trans;current_scan_ ConvertToCloudFullPointType(scan_undistort_);// voxel 之pcl::VoxelGridPointType voxel;voxel.setLeafSize(0.5, 0.5, 0.5);voxel.setInputCloud(current_scan_);CloudPtr current_scan_filter(new PointCloudType);voxel.filter(*current_scan_filter);/// the first scanif (flg_first_scan_) {ndt_.AddCloud(current_scan_);// my 我认为这里应该添加如下代码// current_nav_state_ imu_states_.back();// NormalizeVelocity();// last_nav_state_ current_nav_state_;// 重置预积分 preinteg_preinteg_ std::make_sharedIMUPreintegration(options_.preinteg_options_);flg_first_scan_ false;return;}// 后续的scan使用NDT配合pose进行更新LOG(INFO) frame frame_num_;ndt_.SetSource(current_scan_filter);current_nav_state_ preinteg_-Predict(last_nav_state_, imu_init_.GetGravity());ndt_pose_ current_nav_state_.GetSE3();// 使用 IMU 预积分预测值作为配准初始值ndt_.AlignNdt(ndt_pose_);Optimize();// 若运动了一定范围则把点云放入地图中SE3 current_pose current_nav_state_.GetSE3();SE3 delta_pose last_ndt_pose_.inverse() * current_pose;if (delta_pose.translation().norm() 0.3 || delta_pose.so3().log().norm() math::deg2rad(5.0)) {// 将地图合入NDT中CloudPtr current_scan_world(new PointCloudType);pcl::transformPointCloud(*current_scan_filter, *current_scan_world, current_pose.matrix());ndt_.AddCloud(current_scan_world);last_ndt_pose_ current_pose;}// 放入UIif (ui_) {ui_-UpdateScan(current_scan_, current_nav_state_.GetSE3()); // 转成Lidar Pose传给UIui_-UpdateNavState(current_nav_state_);}frame_num_;
} 4.5 图优化部分 图优化部分基本上和 ch4基于预积分和图优化的 GINS 一样不同之处在于一下几点
1.使用了NDT优化后的位姿作为 时刻位姿顶点的初始估计值而没有使用预积分预测的位姿 // 本时刻顶点pose, v, bg, baauto v1_pose new VertexPose();v1_pose-setId(4);// 注意这里使用NDT优化后的位姿作为 j 时刻位姿的初始估计值v1_pose-setEstimate(ndt_pose_); // NDT pose作为初值// v1_pose-setEstimate(current_nav_state_.GetSE3()); // 预测的pose作为初值optimizer.addVertex(v1_pose);
2.在优化过程中使用 setFixed() 函数将 时刻的 和 节点视为固定节点不进行优化 // 在优化过程中将 i 时刻的bg和ba节点视为固定节点不进行优化v0_bg-setFixed(true);v0_ba-setFixed(true);
3.对于我们想将 中的 边缘化对应 Hessian 矩阵中 15x15 的 要求其逆从而消去 边缘化 得到 时刻状态的信息矩阵更新后的 15x15维 的 作为下一轮优化时 时刻和 时刻 时刻的先验因子的信息矩阵。在本博客的 4.6 小节中详细介绍4.对速度进行了限制将其限制在正常区间。
void LioPreinteg::NormalizeVelocity() {/// 限制v的变化/// 一般是-y 方向速度// 将车体坐标系下 y 方向的分速度限制在 -2 到 0 之间Vec3d v_body current_nav_state_.R_.inverse() * current_nav_state_.v_;if (v_body[1] 0) {v_body[1] 0;}// 将车体坐标系下 z 方向的分速度限制为 0v_body[2] 0;if (v_body[1] -2.0) {v_body[1] -2.0;}// 将车体坐标系下 x 方向的分速度限制在-0.1 到 0.1 之间if (v_body[0] 0.1) {v_body[0] 0.1;} else if (v_body[0] -0.1) {v_body[0] -0.1;}current_nav_state_.v_ current_nav_state_.R_ * v_body;
} 优化部分代码如下所示
void LioPreinteg::Optimize() {// 调用g2o求解优化问题// 上一个state到本时刻state的预积分因子本时刻的NDT因子LOG(INFO) optimizing frame frame_num_ , dt: preinteg_-dt_;/// NOTE 这些东西是对参数非常敏感的。相差几个数量级的话容易出现优化不动的情况using BlockSolverType g2o::BlockSolverX;using LinearSolverType g2o::LinearSolverEigenBlockSolverType::PoseMatrixType;auto *solver new g2o::OptimizationAlgorithmLevenberg(g2o::make_uniqueBlockSolverType(g2o::make_uniqueLinearSolverType()));g2o::SparseOptimizer optimizer;optimizer.setAlgorithm(solver);// 上时刻顶点 pose, v, bg, baauto 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, baauto v1_pose new VertexPose();v1_pose-setId(4);// 注意这里使用NDT优化后的位姿作为 j 时刻位姿的初始估计值v1_pose-setEstimate(ndt_pose_); // NDT pose作为初值// v1_pose-setEstimate(current_nav_state_.GetSE3()); // 预测的pose作为初值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 factorauto 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节点视为固定节点不进行优化v0_bg-setFixed(true);v0_ba-setFixed(true);// gooptimizer.setVerbose(options_.verbose_);optimizer.initializeOptimization();optimizer.optimize(20);// get resultslast_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_sharedIMUPreintegration(options_.preinteg_options_);// gauss-newton 迭代中累加Hessian和error,计算dx类似。一共 5 种类型的边在累加Hessian都要考虑上。// 计算当前时刻先验// 构建hessian// 15x2顺序v0_pose, v0_vel, v0_bg, v0_ba, v1_pose, v1_vel, v1_bg, v1_ba// 0 6 9 12 15 21 24 27Eigen::Matrixdouble, 30, 30 H;H.setZero();// ①添加 预积分因子的 Hessian 矩阵H.block24, 24(0, 0) edge_inertial-GetHessian();// ②添加 陀螺仪零偏随机游走因子 的 Hessian 矩阵Eigen::Matrixdouble, 6, 6 Hgr edge_gyro_rw-GetHessian();// 行: bg1 列: bg1 H.block3, 3(9, 9) Hgr.block3, 3(0, 0);// 行: bg1 列: bg2H.block3, 3(9, 24) Hgr.block3, 3(0, 3);// 行: bg2 列: bg1H.block3, 3(24, 9) Hgr.block3, 3(3, 0);// 行: bg2 列: bg2H.block3, 3(24, 24) Hgr.block3, 3(3, 3);// ③添加 加速度计零偏随机游走因子 的 Hessian 矩阵Eigen::Matrixdouble, 6, 6 Har edge_acc_rw-GetHessian();H.block3, 3(12, 12) Har.block3, 3(0, 0);H.block3, 3(12, 27) Har.block3, 3(0, 3);H.block3, 3(27, 12) Har.block3, 3(3, 0);H.block3, 3(27, 27) Har.block3, 3(3, 3);// ④添加 先验因子 的 Hessian 矩阵H.block15, 15(0, 0) edge_prior-GetHessian();// ⑤添加 NDT 观测因子的 Hessian 矩阵H.block6, 6(15, 15) edge_ndt-GetHessian();// 边缘化(利用 H 的稀疏性加速 HΔxg 的求解的方法。视觉SLAM十四讲 p245)// 边缘化(在本轮优化完成后利用边缘化的方法求出下一个关键帧位姿的协方差作为下一轮优化的先验因子的信息矩阵使用。sad p245)H math::Marginalize(H, 0, 14);prior_info_ H.block15, 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 边缘化 4.6.1 边缘化公式 对于 将其变为如下形式其中下方的 为待边缘化的增量 对线性方程组进行高斯消元目标是消去右上角的 部分将其变为 0。 整理得 消元之后方程组的第一行就变成和 无关的项。单独把它拿出来得到关于 部分的增量方程 这样就将求解 线性方程组的问题转换为先求解 在将其带入方程组求解 的问题。这个过程称为边缘化Marginalization或者 Schur 消元。即先边缘化 求出 再求 的过程。 4.6.2 边缘化过程 图优化完毕后把 5 种因子预积分因子、2个零偏随机游走因子、先验因子和NDT观测因子的海塞 (Hessian) 矩阵按照顺序累加组合成一个大的 Hessian 矩阵对于我们想将 中的 边缘化对应 Hessian 矩阵中 15x15 的 要求其逆从而消去 边缘化 得到 时刻状态的信息矩阵更新后的 15x15维 的 作为下一轮优化时 时刻和 时刻 时刻的先验因子的信息矩阵。 累加 5 种因子的 Hessian 矩阵一个大的 Hessian 矩阵 代码如下 // gauss-newton 迭代中累加Hessian和error,计算dx类似。一共 5 种类型的边在累加Hessian都要考虑上。// 计算当前时刻先验// 构建hessian// 15x2顺序v0_pose, v0_vel, v0_bg, v0_ba, v1_pose, v1_vel, v1_bg, v1_ba// 0 6 9 12 15 21 24 27Eigen::Matrixdouble, 30, 30 H;H.setZero();// ①添加 预积分因子的 Hessian 矩阵H.block24, 24(0, 0) edge_inertial-GetHessian();// ②添加 陀螺仪零偏随机游走因子 的 Hessian 矩阵Eigen::Matrixdouble, 6, 6 Hgr edge_gyro_rw-GetHessian();// 行: bg1 列: bg1 H.block3, 3(9, 9) Hgr.block3, 3(0, 0);// 行: bg1 列: bg2H.block3, 3(9, 24) Hgr.block3, 3(0, 3);// 行: bg2 列: bg1H.block3, 3(24, 9) Hgr.block3, 3(3, 0);// 行: bg2 列: bg2H.block3, 3(24, 24) Hgr.block3, 3(3, 3);// ③添加 加速度计零偏随机游走因子 的 Hessian 矩阵Eigen::Matrixdouble, 6, 6 Har edge_acc_rw-GetHessian();H.block3, 3(12, 12) Har.block3, 3(0, 0);H.block3, 3(12, 27) Har.block3, 3(0, 3);H.block3, 3(27, 12) Har.block3, 3(3, 0);H.block3, 3(27, 27) Har.block3, 3(3, 3);// ④添加 先验因子 的 Hessian 矩阵H.block15, 15(0, 0) edge_prior-GetHessian();// ⑤添加 NDT 观测因子的 Hessian 矩阵H.block6, 6(15, 15) edge_ndt-GetHessian();// 边缘化(利用 H 的稀疏性加速 HΔxg 的求解的方法。视觉SLAM十四讲 p245)// 边缘化(在本轮优化完成后利用边缘化的方法求出下一个关键帧位姿的协方差作为下一轮优化的先验因子的信息矩阵使用。sad p245)H math::Marginalize(H, 0, 14);prior_info_ H.block15, 15(15, 15); 将 中的 边缘化取边缘化后的 对应的子矩阵作为下一轮优化的先验因子的信息矩阵使用 // 边缘化(利用 H 的稀疏性加速 HΔxg 的求解的方法。视觉SLAM十四讲 p245)// 边缘化(在本轮优化完成后利用边缘化的方法求出下一个关键帧位姿的协方差作为下一轮优化的先验因子的信息矩阵使用。sad p245)H math::Marginalize(H, 0, 14);prior_info_ H.block15, 15(15, 15); 边缘化的目标如下要将通过函数形参 start 和 end 选定的 增量范围边缘化
a | ab | ac a* | 0 | ac*
ba | b | bc -- 0 | 0 | 0
ca | cb | c ca* | 0 | c*
1.通过函数形参 start 和 end 选定待边缘化的 增量范围对应矩阵块 b2.将 移动到 的下方即对应的 b 矩阵块也移动到矩阵 H 的右下角
a | ab | ac a | ac | ab
ba | b | bc -- ca | c | cb
ca | cb | c ba | bc | b
3.对 b 矩阵块进行奇异值分解求其伪逆即 。 4.使用如下公式更新 H 矩阵 注意有于在这个部分后续不会使用 所以将其全部置 0 只更新 并将其作为下一轮优化时 时刻和 时刻 时刻的先验因子的信息矩阵。。
5.将更新后的 H 矩阵恢复为初始顺序。
a* | ac* | 0 a* | 0 | ac*
ca* | c* | 0 -- 0 | 0 | 0
0 | 0 | 0 ca* | 0 | c* 具体代码如下
/*** 边缘化。视觉SLAM十四讲。p 249* param H* param start* param end* return*/
inline Eigen::MatrixXd Marginalize(const Eigen::MatrixXd H, const int start, const int end) {// ① 通过函数形参 start 和 end 选定待边缘化的 Δx_(Scher) 增量范围对应矩阵块 b// Goal// a | ab | ac a* | 0 | ac*// ba | b | bc -- 0 | 0 | 0// ca | cb | c ca* | 0 | c*// Size of block before block to marginalize const int a start;// Size of block to marginalizeconst int b end - start 1;// Size of block after block to marginalizeconst int c H.cols() - (end 1);// ② 将 Δx_(Scher) 移动到 Δx 的下方即对应的 b 矩阵块也移动到矩阵 H 的右下角// Reorder as follows:// a | ab | ac a | ac | ab// ba | b | bc -- ca | c | cb// ca | cb | c ba | bc | bEigen::MatrixXd Hn Eigen::MatrixXd::Zero(H.rows(), H.cols());// block函数block(startRow, startCol, rows, cols);if (a 0) {Hn.block(0, 0, a, a) H.block(0, 0, a, a);Hn.block(0, a c, a, b) H.block(0, a, a, b);Hn.block(a c, 0, b, a) H.block(a, 0, b, a);}if (a 0 c 0) {Hn.block(0, a, a, c) H.block(0, a b, a, c);Hn.block(a, 0, c, a) H.block(a b, 0, c, a);}if (c 0) {Hn.block(a, a, c, c) H.block(a b, a b, c, c);Hn.block(a, a c, c, b) H.block(a b, a, c, b);Hn.block(a c, a, b, c) H.block(a, a b, b, c);}Hn.block(a c, a c, b, b) H.block(a, a, b, b);// ③ 对 b 矩阵块进行奇异值分解求其伪逆即 H22^-1。A U*Σ*V^T A^-1 V*Σ^-1*U^T// Perform marginalization (Schur complement)Eigen::JacobiSVDEigen::MatrixXd svd(Hn.block(a c, a c, b, b), Eigen::ComputeThinU | Eigen::ComputeThinV);// 返回奇异值矩阵 Σ即对角矩阵其中每个对角元素都是 b 矩阵块 的奇异值。Eigen::JacobiSVDEigen::MatrixXd::SingularValuesType singularValues_inv svd.singularValues();// 计算 Σ^-1for (int i 0; i b; i) {if (singularValues_inv(i) 1e-6) singularValues_inv(i) 1.0 / singularValues_inv(i);elsesingularValues_inv(i) 0;}// 使用奇异值分解法求 b 矩阵块的伪逆。A^-1 V*Σ^-1*U^TEigen::MatrixXd invHb svd.matrixV() * singularValues_inv.asDiagonal() * svd.matrixU().transpose();// ④ 使用公式更新 H 矩阵// H11 H11 - H12 * H22^-1 * H21// H12 0// H21 H21// H22 H22Hn.block(0, 0, a c, a c) 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);Hn.block(a c, a c, b, b) Eigen::MatrixXd::Zero(b, b);Hn.block(0, a c, a c, b) Eigen::MatrixXd::Zero(a c, b);Hn.block(a c, 0, b, a c) Eigen::MatrixXd::Zero(b, a c);// ⑤将更新后的 H 矩阵恢复为初始顺序// Inverse reorder// a* | ac* | 0 a* | 0 | ac*// ca* | c* | 0 -- 0 | 0 | 0// 0 | 0 | 0 ca* | 0 | c*Eigen::MatrixXd res Eigen::MatrixXd::Zero(H.rows(), H.cols());if (a 0) {res.block(0, 0, a, a) Hn.block(0, 0, a, a);res.block(0, a, a, b) Hn.block(0, a c, a, b);res.block(a, 0, b, a) Hn.block(a c, 0, b, a);}if (a 0 c 0) {res.block(0, a b, a, c) Hn.block(0, a, a, c);res.block(a b, 0, c, a) Hn.block(a, 0, c, a);}if (c 0) {res.block(a b, a b, c, c) Hn.block(a, a, c, c);res.block(a b, a, c, b) Hn.block(a, a c, c, b);res.block(a, a b, b, c) Hn.block(a c, a, b, c);}res.block(a, a, b, b) Hn.block(a c, a c, b, b);return res;
}
