当前位置： 首页 > ai >正文

SLAM中的非线性优化-2D图优化之零空间实战（十六）

ai 2025/8/11 12:50:57

       终于有时间更新实战篇了，本节实战几乎包含了SLAM后端的所有技巧，其中包括：舒尔补/先验Factor/鲁棒核函数/FEJ/BA优化等滑动窗口法的相关技巧，其中构建2D轮式里程计预积分以及绝对位姿观测的10帧滑动窗口，并边缘化最老帧，其中所有雅可比及其残差都可以在本系列博客之前章节找到对应的数学原理，完整版代码可在如下地址找到

slam_optimizer: 个人CSDN博客《SLAM中非线性优化的从古至今》对应的源码，该系列博客地址：https://blog.csdn.net/zl_vslam/category_12872677.html - Gitee.comhttps://gitee.com/zl_vslam/slam_optimizer/tree/master/2d_optimize/ch16

接下来请看实战。

一. 主函数

int main() {  double acc_noise_std = 0.1;double init_bg = 0.0;std::vector<double> timestamps;std::vector<Eigen::Vector3d> imu_data;  std::vector<Eigen::Vector3d> odometry_data;std::vector<Eigen::Vector3d> vel_data;  std::vector<Eigen::Vector3d> pose_data;std::vector<Eigen::Vector3d> gps_data;read_simulate(init_bg, timestamps, imu_data, odometry_data, vel_data, pose_data, gps_data);// initial wheel WheelOptions wheel_options;wheel_options.sigma_x = 1e-8;wheel_options.sigma_y = 1e-8;wheel_options.sigma_t = 1e-8;std::shared_ptr<WheelPreIntegration> wheel_preintegration = std::make_shared<WheelPreIntegration>(wheel_options);Optimizer optimizer(wheel_preintegration);// initial stateState state;  memset(&state, 0.0, sizeof(state));std::vector<Eigen::Vector3d> pose_gt = pose_data;state.timestamp = timestamps[0];state.p = pose_gt[0].head<2>();state.R = SO2(pose_gt[0][2]);state.type = "s";State last_state = state; Eigen::Vector3d last_pose;int id = optimizer.AddVertex(state, true);int last_id = id; for(unsigned int i = 1; i < timestamps.size(); i++) {cout << "\ncurr index ==== : " << i << std::endl;double delta_time = timestamps[i]-timestamps[i-1];// cout << "delta_time ==== : " << delta_time << std::endl;Eigen::Vector3d last_odometry_o = odometry_data[i-1];Eigen::Vector3d curr_odometry_o = odometry_data[i];Eigen::Vector3d delta_odometry = curr_odometry_o - last_odometry_o;std::cout << " last_odometry_o =======: " << last_odometry_o.transpose() << std::endl;std::cout << " curr_odometry_o =======: " << curr_odometry_o.transpose() << std::endl;std::cout << " delta_odometry =======: " << delta_odometry.transpose() << std::endl;Eigen::Vector3d last_pose = pose_data[i-1];Eigen::Vector3d curr_pose = pose_data[i];SE2 last_odometry = SE2(last_odometry_o[0], last_odometry_o[1], last_odometry_o[2]);SE2 current_odometry = SE2(curr_odometry_o[0], curr_odometry_o[1], curr_odometry_o[2]);wheel_preintegration->Integrate(last_odometry, current_odometry);State& last_state_t = state;wheel_preintegration->Predict(last_state_t);state.type = "s";id = optimizer.AddVertex(state);State last_pose_state;last_pose_state.timestamp = timestamps[i-1];last_pose_state.p = last_pose.head<2>();last_pose_state.R = SO2(last_pose[2]);last_pose_state.type = "p";State pose_state;pose_state.timestamp = timestamps[i];pose_state.p = curr_pose.head<2>();pose_state.R = SO2(curr_pose[2]);	pose_state.type = "p";if(i > 0) {Matrix3d info = Matrix3d::Identity();State wheel_state;wheel_state.timestamp = timestamps[i];wheel_state.p = wheel_preintegration->GetTranslation();wheel_state.R = wheel_preintegration->GetSO2();wheel_state.type = "w";info = wheel_preintegration->GetCov().inverse();optimizer.AddEdge(PoseGraphEdge(id-1, id, wheel_state, info));Matrix3d pose_info = 100 * Matrix3d::Identity();double last_pose_sigma = 1e-15;double curr_pose_sigma = 1e-15;Eigen::Matrix<double, 3, 3> last_pose_covariance = Eigen::Matrix<double, 3, 3>::Identity();last_pose_covariance(0,0) = std::pow(last_pose_sigma,2);last_pose_covariance(1,1) = std::pow(last_pose_sigma,2);last_pose_covariance(2,2) = std::pow(last_pose_sigma,2);Eigen::Matrix<double, 3, 3> last_information_matrix = last_pose_covariance.inverse();Eigen::Matrix<double, 3, 3> curr_pose_covariance = Eigen::Matrix<double, 3, 3>::Identity();curr_pose_covariance(0,0) = std::pow(curr_pose_sigma,2);curr_pose_covariance(1,1) = std::pow(curr_pose_sigma,2);curr_pose_covariance(2,2) = std::pow(curr_pose_sigma,2);Eigen::Matrix<double, 3, 3> curr_information_matrix = curr_pose_covariance.inverse();optimizer.AddEdge(PoseGraphEdge(id-1, id-1, last_pose_state, last_information_matrix));optimizer.AddEdge(PoseGraphEdge(id, id, pose_state, curr_information_matrix));std::cout << " wheel dp_ =======: " << wheel_preintegration->dp_.transpose() << std::endl;std::cout << " wheel dR_ =======: " << wheel_preintegration->dR_.log() << std::endl;optimizer.Optimize(20);optimizer.UpdateState(state, id);wheel_preintegration = std::make_shared<WheelPreIntegration>(wheel_options);cout << "After step " << i << ":" << endl;optimizer.PrintPoses();cout << "----------------------" << endl;// cout << "curr_odometry ==== : " << curr_odometry_o.transpose() << std::endl;// cout << "delta_odometry ==== : " << delta_odometry.transpose() << std::endl;// cout << "curr_velocity ==== : " << curr_velocity.transpose() << std::endl;cout << "gt_pose ==== : " << curr_pose.transpose() << std::endl;// std::cout << "optimize state.R.log() ============= : " << state.R.log() << std::endl;std::cout << "optimize state.pose ============= : " << state.p.transpose() << " " << state.R.log() << std::endl;// std::cout << "optimize state.cov ============= : \n" << state.cov << std::endl;last_id = id;last_state = state;usleep(150000);		}}return 0;
}

与之前相似，相信大家能够明白

二. 轮式里程计预积分

void WheelPreIntegration::Integrate(const SE2& last_odometry, const SE2& current_odometry) {// preintegrationSE2 odok = current_odometry - last_odometry;Eigen::Vector2d odork = odok.translation();Eigen::Matrix2d Phi_ik = dR_.matrix();dp_ += Phi_ik * odork;dR_ = dR_ * odok.so2();// std::cout << " Phi_ik =======: " << Phi_ik << std::endl;// std::cout << " odork =======: " << odork << std::endl;// std::cout << " wheel dp_0 =======: " << dp_.transpose() << std::endl;// std::cout << " wheel dR_0 =======: " << dR_.log() << std::endl;Eigen::Matrix3d Ak = Eigen::Matrix3d::Identity();Eigen::Matrix3d Bk = Eigen::Matrix3d::Identity();Ak.block<2,1>(1,0) = Phi_ik * Eigen::Vector2d(-odork[1], odork[0]);Bk.block<2,2>(1,1) = Phi_ik;// Ak.block<2,1>(0,2) = Phi_ik * Eigen::Vector2d(-odork[1], odork[0]);// Bk.block<2,2>(0,0) = Phi_ik;Eigen::Matrix3d Sigma_vk = Eigen::Matrix3d::Identity();Sigma_vk(0,0) = options_.sigma_t * options_.sigma_t;Sigma_vk(1,1) = options_.sigma_x * options_.sigma_x;Sigma_vk(2,2) = options_.sigma_y * options_.sigma_y;cov_ =  Ak * cov_ * Ak.transpose() + Bk * Sigma_vk * Bk.transpose();
}

这段代码的原理在之前章节也可以找到

三. 预积分Factor

void WheelFactor::ComputeResidualAndJacobianPoseGraph(const State& last_state, const State& state, Eigen::Matrix<double, 3, 1> &residual, Eigen::Matrix<double, 3, 3> &jacobianXi, Eigen::Matrix<double, 3, 3> &jacobianXj) {Eigen::Matrix2d Ri = SO2(last_state.R).matrix();Eigen::Vector2d ri = last_state.p;double ai = SO2(last_state.R).log();double aj = SO2(state.R).log();Eigen::Vector2d rj = state.p;// 公式（24）residual.head<2>() = Ri.transpose() * (rj-ri) - wheel_preintegration_->dp_;residual[2] = SO2(aj - ai - wheel_preintegration_->dR_.log()).log();// compute jacobian matrixEigen::Vector2d rij = rj-ri;Eigen::Vector2d rij_x(-rij[1], rij[0]);// 公式（25）jacobianXi.block<2,2>(0,0) = -Ri.transpose();jacobianXi.block<2,1>(0,2) = -Ri.transpose() * rij_x;jacobianXi.block<1,2>(2,0).setZero();jacobianXi(2,2) = -1;jacobianXj.setIdentity();jacobianXj.block<2,2>(0,0) = Ri.transpose();
}

四.位姿Factor

// predict - obs
void PoseObserve::ComputeResidualAndJacobianPoseGraph(const State& state, const State& pose_state, Eigen::Matrix<double, 3, 1> &residualXi, Eigen::Matrix<double, 3, 3> &jacobianXi) {Eigen::Vector2d res_p = state.p - pose_state.p;double res_theta = math_utils::theta_normalize(SO2(state.R).log() - SO2(pose_state.R).log());residualXi.block<2,1>(0,0) = res_p;residualXi(2,0) = res_theta;// cout << "residualXi ================== : " << residualXi << endl;jacobianXi = Eigen::Matrix<double, 3, 3>::Identity();// cout << "jacobianXi == : \n" << jacobianXi << endl;
}

五. 相关修改位置

        for (size_t edge_idx : related_edges) {const auto& edge = edges_[edge_idx];if (!vertices_.count(edge.id1) || !vertices_.count(edge.id2)) continue;State xi = vertices_.at(edge.id1), xj = vertices_.at(edge.id2);Vector3d e;Eigen::Matrix<double, 3, 3> jacobianXi, jacobianXj;if(edge.measurement.type == "w") {wheel_factor_->ComputeResidualAndJacobianPoseGraph(xi, xj, e, jacobianXi, jacobianXj);MatrixXd weighted_H;VectorXd weighted_e;std::tie(weighted_H, weighted_e) = robust_kernel_->robustify(e);Matrix3d Omega_robust = edge.information * weighted_H;Vector3d e_robust = weighted_e;int idx1 = local_indices[edge.id1], idx2 = local_indices[edge.id2];// cout << "w idx1 ==== : " << idx1 << " , " << idx2 << std::endl;H_local.block<3, 3>(3*idx1, 3*idx1) += jacobianXi.transpose() * Omega_robust * jacobianXi;H_local.block<3, 3>(3*idx1, 3*idx2) += jacobianXi.transpose() * Omega_robust * jacobianXj;H_local.block<3, 3>(3*idx2, 3*idx1) += jacobianXj.transpose() * Omega_robust * jacobianXi;H_local.block<3, 3>(3*idx2, 3*idx2) += jacobianXj.transpose() * Omega_robust * jacobianXj;b_local.segment<3>(3*idx1) += jacobianXi.transpose() * Omega_robust * e_robust;b_local.segment<3>(3*idx2) += jacobianXj.transpose() * Omega_robust * e_robust;}// cout << "edge.id1 ==== : " << edge.id1 << std::endl;// cout << "edge.id2 ==== : " << edge.id2 << std::endl;if(edge.measurement.type == "p") {if(edge.id1 == edge.id2) {PoseObserve::ComputeResidualAndJacobianPoseGraph(xi, edge.measurement, e, jacobianXi);MatrixXd weighted_H;VectorXd weighted_e;std::tie(weighted_H, weighted_e) = robust_kernel_->robustify(e);Matrix3d Omega_robust = edge.information * weighted_H;Vector3d e_robust = weighted_e;int idx1 = local_indices[edge.id1], idx2 = local_indices[edge.id2];// cout << "p idx1 ==== : " << idx1 << " , " << idx2 << std::endl;H_local.block<3, 3>(3*idx1, 3*idx1) += jacobianXi.transpose() * Omega_robust * jacobianXi;// H_local.block<3, 3>(3*idx1, 3*idx2) += jacobianXi.transpose() * Omega_robust * jacobianXj;// H_local.block<3, 3>(3*idx2, 3*idx1) += jacobianXj.transpose() * Omega_robust * jacobianXi;// H_local.block<3, 3>(3*idx2, 3*idx2) += jacobianXj.transpose() * Omega_robust * jacobianXj;b_local.segment<3>(3*idx1) += jacobianXi.transpose() * Omega_robust * e_robust;// b_local.segment<3>(3*idx2) += jacobianXj.transpose() * Omega_robust * e_robust;}}    }

主要是替换上节代码相应位置的残差及雅可比，其它基本没有改动，就不作过多说明了

总结

       经过这样一个10帧的滑动窗口法，算法即可获得优化效果，其中需要调节协方差相关参数，即可影响结果，本代码中，融合的是真值位姿，因此最终结果也收敛到了真值上，大家若想体验其它效果，可自行调节sigma之类的参数，本节更新较晚，主要还是代码调试需要时间，本节更新完，2D图优化也将进入终极篇了，终极篇预告：2D视觉惯性VIO，敬请期待

http://www.xdnf.cn/news/16835.html

相关文章：

  • Selenium自动化：轻松实现网页操控
  • 归并排序（简单讲解）
  • MySQL 基础
  • linux source命令使用详细介绍
  • 浅拷贝与深拷贝的区别
  • Vue 响应式基础全解析2
  • Python Pandas.unique函数解析与实战教程
  • 24黑马SpringCloud的Docker本地目录挂载出现相关问题解决
  • 《JMM 与 happens-before 原则：并发编程的核心内存语义》
  • 网络常识-子网掩码
  • 暑期算法训练.13
  • stm32F407 实现有感BLDC 六步换相 cubemx配置及源代码（二）
  • 电脑系统中的BCD
  • 排序算法-堆排序
  • ARMv8/v9架构FAR_EL3寄存器介绍
  • Android 13/14/15 默认授权应用权限的实现方法
  • 《深潜React列表渲染：调和算法与虚拟DOM Diff的优化深解》
  • 开疆智能Profinet转Modbus网关连接信捷PLC从站配置案例
  • WPFC#超市管理系统（4）入库管理
  • oect刷入arm系统安装docker
  • 【Redis数据结构详解】特点、用途与实际应用
  • CCF IVC 2025“汽车安全攻防赛” -- Crypto -- WriteUp
  • VAST视频广告技术实现：从零开始搭建视频广告投放系统
  • 文件同步神器-rsync命令讲解
  • linux编译基础知识-库文件标准路径
  • Oracle 11g RAC集群部署手册（一）
  • imx6ull-驱动开发篇6——Linux 设备树语法
  • K8S部署ELK（二）：部署Kafka消息队列
  • NVIDIA GPU架构
  • 四、Portainer图形化管理实战与Docker镜像原理