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

       终于有时间更新实战篇了，本节实战几乎包含了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.pswp.cn/web/91697.shtml
繁体地址，请注明出处：http://hk.pswp.cn/web/91697.shtml

如若内容造成侵权/违法违规/事实不符，请联系多彩编程网进行投诉反馈email:809451989@qq.com，一经查实，立即删除！

相关文章

知识随记-----Qt 实战教程：使用 QNetworkAccessManager 发送 HTTP POST

知识随记-----Qt 实战教程：使用 QNetworkAccessManager 发送 HTTP POST

文章目录Qt 网络编程&#xff1a;使用 QNetworkAccessManager 实现 HTTP POST 请求概要整体架构流程技术名词解释技术细节注意事项&#xff1a;Qt 网络编程&#xff1a;使用 QNetworkAccessManager 实现 HTTP POST 请求 概要 本文介绍如何使用 Qt 框架的网络模块&#xff08;…
阅读更多...
wordpress批量新建产品分类

wordpress批量新建产品分类

1、下载安装插件&#xff1a;bulk-category-import-export2、激活插件后&#xff0c;左侧点击插件下的导入&#xff0c;选择product categories&#xff0c;点击下一步3、这里可以选择导入的分类列表文件&#xff0c;可以选择分隔符&#xff0c;CSV文件默认为‘&#xff0c;’要…
阅读更多...
CentOS 镜像源配置与 EOL 后的应对策略

CentOS 镜像源配置与 EOL 后的应对策略

引言 本文将详细介绍如何使用 阿里云开源镜像站 配置 CentOS 的各类软件源&#xff0c;包括基础源、历史归档源&#xff08;vault&#xff09;、ARM 架构源、Stream 版本以及调试信息源&#xff08;debuginfo&#xff09;&#xff0c;并重点讲解在 CentOS 8 停止维护后&#x…
阅读更多...
CTF实战：用Sqlmap破解表单输入型SQL注入题（输入账号密码/usernamepassword）

CTF实战：用Sqlmap破解表单输入型SQL注入题（输入账号密码/usernamepassword）

目录 引言 步骤1&#xff1a;用Burp Suite捕获表单请求 步骤2&#xff1a;用Sqlmap获取数据库名称 参数解释&#xff1a; 输出示例&#xff08;根据题目环境调整&#xff09;&#xff1a; 步骤3&#xff1a;获取目标数据库中的表名 参数解释&#xff1a; 输出示例&#…
阅读更多...
质数时间（二分查找）

质数时间（二分查找）

题目描述如果把一年之中的某个时间写作 a 月 b 日 c 时 d 分 e 秒的形式&#xff0c;当这五个数都为质数时&#xff0c;我们把这样的时间叫做质数时间&#xff0c;现已知起始时刻是 2022 年的 a 月 b 日 c 时 d 分 e 秒&#xff0c;终止时刻是 2022 年的 u 月 v 日 w 时 x 分 y…
阅读更多...
Python训练Day29

Python训练Day29

浙大疏锦行 类的装饰器装饰器思想的进一步理解&#xff1a;外部修改、动态类方法的定义&#xff1a;内部定义和外部定义
阅读更多...
新手DBA实战指南：如何使用gh-ost实现MySQL无锁表结构变更

新手DBA实战指南：如何使用gh-ost实现MySQL无锁表结构变更

新手DBA实战指南：如何使用gh-ost实现MySQL无锁表结构变更 作为DBA，大表结构变更(DDL)一直是令人头疼的问题。传统的ALTER TABLE操作会锁表，严重影响业务连续性；而常见的pt-online-schema-change工具虽然能实现在线变更，但依赖触发器机制，在高并发场景下性能表现不佳。本…
阅读更多...
OSPF综合

OSPF综合

一、实验拓扑二、实验需求1、R4为ISP&#xff0c;其上只配置IP地址&#xff1b;R4与其他所直连设备间均使用公有IP&#xff1b; 2、R3-R5、R6、R7为MGRE环境&#xff0c;R3为中心站点&#xff1b; 3、整个OSPF环境IP基于172.16.0.0/16划分&#xff1b;除了R12有两个环回&#x…
阅读更多...
技术面试知识点详解 - 从电路到编程的全栈面经

技术面试知识点详解 - 从电路到编程的全栈面经

技术面试知识点详解 - 从电路到编程的全栈面经 目录 模拟电路基础数字电路原理电源设计相关编程语言基础数据库与并发网络协议基础算法与数据结构 模拟电路基础 1. 放大电路类型判断 这是模拟电路面试的经典题目&#xff0c;通过电压放大倍数判断放大电路类型&#xff1a; …
阅读更多...
LangGraph认知篇-Command函数

LangGraph认知篇-Command函数

Command简述 在 LangGraph 中&#xff0c;Command 是一个极具实用性的功能&#xff0c;它能够将控制流&#xff08;边&#xff09;和状态更新&#xff08;节点&#xff09;巧妙地结合起来。这意味着开发者可以在同一个节点中&#xff0c;既执行状态更新操作&#xff0c;又决定下…
阅读更多...
【目标检测】小样本度量学习

【目标检测】小样本度量学习

小样本度量学习&#xff08;Few-Shot Metric Learning&#xff09;通常用于分类任务​&#xff08;如图像分类&#xff09;&#xff0c;但它也可以与目标检测&#xff08;Object Detection&#xff09;结合&#xff0c;解决小样本目标检测&#xff08;Few-Shot Object Detectio…
阅读更多...
cmd怎么取消关机命令

cmd怎么取消关机命令

在 Windows 的命令提示符&#xff08;CMD&#xff09;中取消已计划的关机操作&#xff0c;可以通过 shutdown 命令的 ​**-a**​ 参数实现。以下是具体步骤&#xff1a;​操作方法​​打开 CMD​按下 Win R 组合键&#xff0c;输入 cmd 并回车&#xff0c;打开命令提示符窗口。…
阅读更多...
网易云音乐硬刚腾讯系！起诉SM娱乐滥用市场支配地位

网易云音乐硬刚腾讯系！起诉SM娱乐滥用市场支配地位

企查查APP显示&#xff0c;近日&#xff0c;法院公开杭州乐读科技有限公司、杭州网易云音乐科技有限公司起诉SM ENTERTAINMENT CO. 、卡斯梦&#xff08;上海&#xff09;文化传播有限公司等开庭信息&#xff0c;案由涉及滥用市场支配地位纠纷。公告显示&#xff0c;该案件计划…
阅读更多...
[css]切角

[css]切角

使用css实现一个切角的功能&#xff0c;有以下几种方案&#xff1a; <div class"box"></div>方案一&#xff1a;linear-gradient linear-gradient配合backgroud-image可以实现背景渐变的效果。linear-gradient的渐变过渡区的占比是总的空间&#xff08;高…
阅读更多...
分享一个可以测试离线服务器性能的脚本

分享一个可以测试离线服务器性能的脚本

在日常运维工作中&#xff0c;经常会遇到系统性能莫名跟不上业务需求的情况&#xff1a;服务器响应变慢、应用加载卡顿、资源占用异常飙升等问题频繁出现&#xff0c;却难以快速问题根源究竟在CPU过载、内存泄漏、磁盘I/O阻塞还是网络带宽瓶颈。这种时候&#xff0c;特别需要一…
阅读更多...
Python Pandas.unique函数解析与实战教程

Python Pandas.unique函数解析与实战教程

Python Pandas.unique 函数解析与实战教程 摘要 本文章旨在全面地解析 pandas 库中的 unique 函数。pandas.unique 是一个用于从一维数组型（array-like）对象中提取唯一值的高效工具。我们将从其核心功能、函数签名、参数详解、返回值类型，到关键行为特性（如顺序保留、缺失…
阅读更多...
排序算法入门：直接插入排序详解

排序算法入门：直接插入排序详解

这里写目录标题介绍原理代码实现分析介绍 直接插入排序是一种简单直观的排序算法&#xff0c;适用于小规模数据或基本有序的数据集。其核心思想是构建有序序列&#xff0c;对于未排序数据&#xff0c;在已排序序列中从后向前扫描&#xff0c;找到相应位置并插入。 原理 我们…
阅读更多...
ClickHouse MergeTree引擎：从核心架构到三级索引实战

ClickHouse MergeTree引擎：从核心架构到三级索引实战

摘要 MergeTree是ClickHouse最核心的存储引擎&#xff0c;采用列式存储LSM-Tree架构设计&#xff0c;支持高效的数据写入、合并和查询。本文将全面解析MergeTree引擎的基础概念、数据流、核心架构、索引系统以及常见问题。 基础篇&#xff1a; 一、MergeTree引擎基础概念 1. 定…
阅读更多...
电脑手机热点方式通信（上）

电脑手机热点方式通信（上）

电脑连接手机热点时的无线链路情况&#xff1a; 电脑上网时&#xff08;从服务器下载数据&#xff0c;或者上传指令、数据&#xff09;&#xff0c;首先电脑与手机之间基于WiFi协议在2.4G频段或者5G频段通信&#xff0c;然后手机与基站之间再基于4G LTE或者5G NR协议在2412MHz…
阅读更多...
MySQL CPU占用过高排查指南

MySQL CPU占用过高排查指南

MySQL CPU 占用过高时&#xff0c;排查具体占用资源的表需结合系统监控、数据库分析工具和 SQL 诊断命令。&#x1f50d; ​一、快速定位问题根源​​确认 MySQL 进程占用 CPU​使用 top 或 htop 命令查看系统进程&#xff0c;确认是否为 mysqld 进程导致 CPU 飙升。若 MySQL 进…
阅读更多...
最新文章

Copyright @ 2022~2023