1.hdl_graph_slam|后端优化|hdl_graph_slam_nodelet.cpp|源码解读(四)
2.ROS博客基于ROS的自动驾驶数据集可视化项目(附源代码)
3.IMU工作记录
4.Navigation2源码剖析:（二）启动
5.ROS 2中发布和可视化传感器数据实战系列之四：kitti公开数据集IMU数据的发布及在RViz2中的显示
6.ubuntu18.04 OAK-D kalibr 双目+imu联合标定

hdl_graph_slam|后端优化|hdl_graph_slam_nodelet.cpp|源码解读(四)

       hdl_graph_slam源码解读(八)：后端优化

       后端概率图构建核心：hdl_graph_slam_nodelet.cpp

       整体介绍

       这是整个系统建图的核心，综合所有信息进行优化。所有的信息都会发送到这个节点并加入概率图中。

       包含信息

       1）前端里程计传入的位姿和点云

       2）gps信息

       3）Imu信息

       4）平面拟合的参数信息

       处理信息步骤

       1）在对应的callback函数中接收信息，并放入相应的队列

       2）根据时间戳对队列中的信息进行顺序处理，加入概率图

       其他内容

       1）执行图优化，摇钱树解套指标源码这是一个定时执行的函数，闭环检测也在这个函数里

       2）生成全局地图并定时发送，即把所有关键帧拼一起，得到全局点云地图，然后在一个定时函数里发送到rviz上去

       3）在rviz中显示顶点和边，如果运行程序，会看到rviz中把概率图可视化了

       关键帧同步与优化

       cloud_callback

       cloud_callback(const nav_msgs::OdometryConstPtr& odom_msg,const sensor_msgs::PointCloud2::ConstPtr& cloud_msg)

       该函数主要是odom信息与cloud信息的同步，同步之后检查关键帧是否更新。

       关键帧判断：这里主要看关键帧设置的这两个阈值keyframe_delta_trans、keyframe_delta_angle

       变成关键帧的要求就是：/hdl_graph_slam/include/hdl_graph_slam/keyframe_updater.hpp

       优化函数

       optimization_timer_callback(const ros::TimerEvent& event)

       函数功能：将所有的位姿放在posegraph中开始优化

       loop detection 函数：主要就是将当前帧和历史帧遍历，寻找loop。

       闭环匹配与信息矩阵计算

       匹配与闭环检测

       潜在闭环完成匹配（matching 函数）

       不同loop的信息矩阵计算（hdl_graph_slam/information_matrix_calculator.cpp）

       gps对应的信息矩阵

       hdl_graph_slam/graph_slam.cpp

       添加地面约束

       使用add_se3_plane_edge函数的代码

       执行图优化

       优化函数optimization_timer_callback

       执行图优化，闭环检测检测闭环并加到了概率图中，优化前

       生成简化版关键帧，KeyFrameSnapshot用于地图拼接

       生成地图并定时发送

       生成地图：简化版关键帧拼接

       定时发送：src/hdl_graph_slam_nodelet.cpp文件中

       系统性能与扩展性

       hdl_graph_slam性能问题在于帧间匹配和闭环检测精度不足，系统代码设计好，模块化强，易于扩展多传感器数据融合。

       总结

       hdl_graph_slam后端优化是关键，涉及大量信息融合与概率图构建。系统设计清晰，扩展性强，但在性能上需改进。

ROS博客基于ROS的自动驾驶数据集可视化项目(附源代码)

       项目简介

       基于加州大学伯克利分校 MSC Lab的自动驾驶数据集，本项目旨在进行数据集的可视化。项目源代码已上传至 GitHub，英文版文章与演示视频也已准备就绪。

       数据集展示

       左侧展示了GPS信号的可视化，通过 Mapviz 工具，将行驶过程中走过的路径显示出来，左上角则呈现了车前摄像头的视角。右侧是自定义的可视化，利用绿色代表 y 轴正方向，蓝色表示 x 轴正方向。紫色圆点表示汽车行驶过程中各个方向的swapidc码支付源码加速度信息，天蓝色箭头指示汽车前进方向，绿色则代表不同强度的加速度。

       问题与解决方案

       在使用 Mapviz 可视化 GPS 信号时，遇到了数据格式不匹配的问题。通过在自定义的 package 中编写 `trans_GPS.cpp` 文件，成功实现了数据格式转换，解决了数据可视化的问题。同时，还撰写了关于 Mapviz 的基础使用教程。

       加速度信息的可视化涉及确定坐标轴方向、避免信息跳动以及直观显示加速度大小。通过在 RVIZ 中绘制 x 和 y 轴，并使用平滑器处理频繁读取的 IMU 数据，成功解决了这些问题。极坐标系的引入使得加速度大小的显示更为直观。

       汽车前进方向的可视化涉及到姿态信息的获取与 RVIZ 显示角度的调整。通过分析 IMU 的 orientation 数据，并设置 marker 的 orientation 值，实现了方向的正确显示。

       相机信息的可视化面临格式转换问题。通过使用 `image_transport` 包装解决了传感器数据格式不兼容的问题。

       总结

       在本项目中，通过学习与实践 ROS 相关知识，成功实现了自动驾驶数据集的可视化。接下来，将集中精力深入学习 OSM 的使用，并着手进行 GPS 定位与搜索的小项目开发。

IMU工作记录

       本文主要概述了STM型号的asm在OpenIMU移植项目中的工作记录，涉及两个关键环节：

       首先，我们对STM型号的asm进行了详细的检查，这是项目的基础步骤，旨在确保硬件与软件的兼容性和稳定性。在这个阶段，我们对其特性、功能和性能进行了深入研究，以确保后续开发的顺利进行。

       其次，我们进行了OpenIMU移植1. hlu project。这是一个重要的软件移植工作，目标是饭堂小程序源码将asm的硬件驱动与OpenIMU库进行整合，实现imu数据的准确采集和处理。这个阶段涉及源码的改编、配置文件的调整，以及与STM平台的接口设计，旨在最大限度地发挥asm的潜能。

       整个过程中，我们密切关注代码的兼容性、效率和稳定性，力求为用户提供高质量的imu数据。通过这两个项目的实施，我们对asm在实际应用中的表现有了深入的了解，并为后续的优化和升级积累了宝贵经验。

Navigation2源码剖析:（二）启动

       Navigation2源码剖析:（二）启动

       Nv2源码中的bringup包和svl-robot-bringup负责LgSvl仿真和Nv2项目的启动，它们是整个工程的入口。

       主车设计采用两轮差分驱动，如Turtlebot3，由两个动力轮控制轮速，实现前进和转向，万向轮作为支撑。其控制模型基于开环系统，可通过添加负反馈形成闭环，以提高控制精度。

       Nv2的传感器配置包括2D激光雷达（Lidar）、深度相机和imu模块。Lidar用于建图、定位和代价地图生成，depth-camera提供障碍物信息，imu则用于里程计数据的计算和漂移校正。在Gazebo仿真中，IMU直接作为输入。

       在LGCloi中，已预置6种传感器，选择Nav2-PointCloud或Navigation2配置，主要区别在于Lidar数据类型。为适配Nv2需求，需使用pointcloud_to_laserscan包将PointCloud2转换为LaserScan类型，这一过程涉及数据压缩和转换，如图[5]所示。

       svl-robot-bringup和nav2_bringup模块在项目启动过程中起关键作用，详细内容可参考相关附录[4]。平行宇宙解说源码

ROS 2中发布和可视化传感器数据实战系列之四：kitti公开数据集IMU数据的发布及在RViz2中的显示

       在ROS 2中发布和可视化传感器数据实战系列之四：kitti公开数据集IMU数据的发布及在RViz2中的显示

       本文将指导您如何在ROS 2中发布kitti公开数据集的IMU数据，并在RViz2中进行可视化显示。在ROS 2中，RViz2默认不支持IMU数据的可视化。因此，我们将通过添加Github网站上的imu_tools开源存储库中的rviz_imu_plugin插件软件包，将IMU数据中的线性加速度、角速度、方位等数据在RViz2中进行可视化显示。以下是详细步骤：

       1. **发布IMU数据**：使用Python编写一个ROS 2发布者节点，读取kitti数据集中oxts/data子目录中的.txt文件，从中提取方位、角速度和线性加速度数据，转换为sensor_msgs/Imu消息类型进行发布。确保在发布消息时仅包含方位、角速度和线性加速度数据及其协方差矩阵。

       2. **安装rviz_imu_plugin插件**：从imu_tools存储库中获取rviz_imu_plugin插件，按照自述文档中的指示进行二进制包安装或源代码编译安装，并设置环境变量。

       3. **在RViz2中显示IMU数据**：在RViz2中添加(rviz_imu_plugin)插件后，可以将发布到话题上的IMU消息数据进行可视化显示。

       注意，RViz2在ROS 2中对于IMU消息的直接支持不足，因此需要通过上述步骤进行自定义配置。通过这些步骤，您可以有效地在RViz2中展示kitti数据集中的IMU数据。

       **IMU简介**：

       - IMU（Inertial Measurement Unit）是测量物体三轴角速度和加速度的设备，内部包含三轴陀螺仪和三轴加速度计。

       - IMU数据包含线性加速度、角速度、方位等信息，可用于自动驾驶、机器人和工业自动化等领域。

       - IMU传感器在定位领域的地位不容忽视，尤其与全球导航卫星系统（GNSS）结合使用时，能提供高度准确的定位信息。

       **IMU在ROS 2中的应用**：

       - ROS 2中发布IMU数据并利用rviz_imu_plugin插件进行可视化显示，是实现自动驾驶、机器人应用中定位与姿态控制的关键步骤。

       - 本文通过具体示例和代码指导，网页源码和ip帮助开发者在ROS 2环境中实现IMU数据的发布与可视化，为相关应用提供技术支持。

       **总结**：

       - 通过本实战系列之四，您将掌握如何在ROS 2中发布kitti公开数据集的IMU数据，并在RViz2中进行可视化显示。

       - 了解IMU及其在ROS 2中的应用，对于自动驾驶、机器人和工业自动化领域的开发人员而言，是一次宝贵的学习经验。

ubuntu. OAK-D kalibr 双目+imu联合标定

       将OAK相机接入ROS系统，进行SLAM定位建图导航等算法时，确保双目相机与IMU的标定参数准确，对于提升定位建图的精度和算法的鲁棒性至关重要。原厂提供的OAK相机仅包含双目的标定数据，因此，本教程使用第三方标定工具Kalibr，在ROS系统中完成双目标定、IMU标定以及IMU+双目的联合标定，以满足算法需求。

       在执行标定过程前，首先确保已安装ROS melodic和depthai_ros环境，参考教程链接进行安装配置。接着，利用源码地址克隆Kalibr，使用catkin_make命令编译相关依赖，如Ceres Solver和code_utils，并根据教程调整code_utils文件中的代码引用。

       标定过程中，需采集数据，具体分为双目、IMU以及双目+IMU三个部分。双目数据需在白色背景下录制，确保场景清晰。IMU数据录制时，保持相机静止状态，并确保时间超过2个小时。双目+IMU数据采集时，使用特定的配置文件路径进行标定，输出cam_chain.yaml、imu_param.yaml文件，以及生成camchain-imucam.yaml结果文件。

       通过Kalibr工具，完成双目和IMU的单独标定及联合标定，输出关键参数文件。完成整个标定流程后，可参阅OAK中国官网获取最新教程和更新信息。如有技术咨询或项目合作需求，请通过指定链接与OAK官方联系，加入OAK官方微信群，获取最新资讯和社区支持。

       感谢您对OAK中国和Kalibr标定工具的关注与支持，如您喜欢本教程，欢迎分享或点赞，您的鼓励是我们持续改进的动力。

MIT cheetah源码业务层逻辑简介

       MIT Cheetah机器人源码揭示了其业务层逻辑的全面体系结构。自从MIT公开了Cheetah Mini的完整资料，包括主控源代码、电机驱动源代码、控制板硬件PCB设计以及本体结构资料后，这款腿部型机器人的研发在国内受到了广泛关注，从而催生了多个基于此平台的机器人研发团队。

       整体系统由个电机驱动单元、一个数据转接板SPIne、一台主控电脑、一个手柄以及一个缺省状态的IMU组成。个电机驱动单元通过CAN总线和数据转接板SPIne连接，分别控制着Cheetah腿部的关节电机，每条腿由三个关节组成。SPIne模块由两个STM芯片构成，负责主控数据的分发到驱动以及驱动反馈数据的打包。主控部分采用开源ethercat协议栈soem，支持两种通信方式：spi通信频率为Hz，ethercat通信频率为Hz。目前推测SPIne上可能仅支持spi通信。主控部分是一个计算机，通过USB连接手柄，实现手动控制，并包含上位机软件及仿真器代码，用于配置主控单元的控制参数和下发指令信息。

       主控部分业务逻辑主要通过多态设计实现多种控制类型，包括MIT_Controller、MiniCheetahSpi_Controller、JPos_Controller等。用户可根据已有功能模块继承基类RobotController，在Cheetah Software/user目录下创建自定义控制器。JPos_Controller提供参考示例，算法完整实现则需参考MIT_Controller模块调用。

       程序运行模式分为仿真模式和实际控制模式，通过main_helper函数进行加载启动。HardwareBridge实现加载实际控制程序流程，从Cheetah3HardwareBridge.run()开始，执行控制器硬件初始化、配置参数加载以及算法功能模块初始化，随后启动多个任务，包括可视化线程、日志线程、手柄通信线程、IMU通信线程、关节电机通信线程和周期回调主控线程。

       主控线程周期回调执行关键操作，包括更新数据、步态规划、外部输入转换、状态机运行以及控制数据更新。具体操作如下：

       更新数据：通过运动学和雅可比计算，将电机传回的关节角度和角速度信息转换为机器人腿部末端的速度和位置信息。

       步态规划：对机器人步态进行规划，内容涉及算法细节，后续将单独分析。

       外部输入转换：将外部指令转换为机器人本体的位姿控制信息，包括机器人位姿和位姿速度，共计组外部控制量。

       状态机运行：执行机器人集成动作的状态机，进行动力学、步态规划、MPC控制等核心算法计算，周期性更新legController中command信息，通过调用legController中的updateCommand更新电机控制相关通信数据寄存器。

       控制数据更新：将机器人控制核心的输出控制数据写入相关寄存器，通过spi接口输入到电机驱动，控制电机运行。

       对于仿真部分，由于需要接入罗技F手柄才能进行仿真。因未配备手柄，源代码被相应修改，以便实现仿真运行。

白话VINS-Mono之外参标定（二）

       在深入探讨Vins-mono系统中的外参标定部分之前，我们先回顾一下上篇文章中预积分的基本概念。接下来，我们将从实际应用出发，深入解析Vins-mono系统中关于外参标定的原理与源码。

       Vins-mono作为紧耦合视觉IMU系统，在实现中通过在SLAM过程中进行相机与IMU的标定，以应对没有标定信息的情况。这种设计的一大优势在于系统能够动态计算相机与IMU之间的标定值。尽管标定过程并非绝对精确，但在后续的后端优化中会持续调整这些值。

       配置文件中支持输入精确的外参标定值，通过设置config.yaml中的ESTIMATE_ESTRINSIC参数来决定。根据yaml设置，有三种情况可供选择：计算相机（Camera）坐标系到IMU坐标系的相对旋转矩阵。这一过程主要在CalibrationExRotation函数中实现。

       在Vins-mono中，标定过程是独立于初始化的一部分，但它是系统启动前的关键步骤。在processimg函数中，初始化之前，即真正的初始化前，需要执行CalibrationExRotation函数。

       虽然Vins-mono文章中并未引入新的在线标定相机与IMU方法，而是基于文献（[8]）中提出的“monocular视觉惯性状态估计的在线初始化和相机IMU外参标定”。基于Vins-mono的代码实现，我们重新整理了文献中Fig. 3的图，并将其转化为图2，旨在从理论到代码详细解析标定过程。

       结合图2，相邻相机关键帧对应的pose可以通过两种方式来构建方程：使用八点法（solveRelativeR）和结合已知的[公式]标定转换（solveRelativeRT）。理论上，通过假设相对旋转量为[公式]，可以构建方程并求解。为了深入理解求解过程，我们将参考文献中的式子4~9。

       在求解过程中，考虑到使用对极约束算法时不可避免的匹配错误（outlier），在A矩阵中加入权重计算，以提高在线标定结果的鲁棒性。加权计算方式近似于Huber norm计算（参考式8、9）。

       在CalibrationExRotation核心函数中，实现流程遵循上述式子4~9的步骤。solveRelativeR与solveRelativeRT函数之间的区别在于，后者在多个判断内点个数操作中有所不同。重要的是，ric.inverse()* delta_q_imu * ric这一表达式将式4转换为[公式]，通过在循环中不断计算相邻帧特征点对应变换，逐渐构建Ax=0形式的方程。

       对于求解Ax=0问题的SVD分解，它是在矩阵非方阵时的特征值分解拓展，可以提取矩阵的主要特征。通过SVD分解，我们可以将问题转化为求解特定的特征值和特征向量，进而求解方程。

       在理解SVD分解为何可以求解Ax=0问题时，关键在于其几何意义。SVD分解将任意矩阵通过一系列旋转和平移转换为对角矩阵，其中的奇异值表示椭球体轴的长度。通过最小奇异值，我们可以求解出最优解，即Ax=0的非零解。

       综上所述，Vins-mono系统中的外参标定过程通过一系列理论解析和代码实现，确保了相机与IMU之间标定值的动态调整和优化。通过对关键步骤的深入理解，我们可以更好地掌握SLAM系统中这一重要模块的工作原理。

ORB-SLAM3 源码剖析：IMU 预积分

       IMU的数据结构在ORB-SLAM3中用于表示机体坐标系中的测量值。在特定时刻，加速度计测量线加速度和陀螺仪测量角速度。假设这些测量值包含高斯白噪声，且偏置建模为随机游走，其导数也是高斯白噪声。将重力转换到机体坐标系后，得到连续视觉帧间的IMU预积分结果。这些预积分包括旋转、速度和位置测量，以及整个测量向量的协方差矩阵。

       在ORB-SLAM3中，每帧的IMU预积分在tracking线程中计算，具体由Tracking::PreintegrateIMU()函数执行。每帧间的IMU测量通过src/ImuTypes.cc中的Preintegrated::IntegrateNewMeasurement()进行积分。主要步骤如下：首先进行偏置校正，然后计算位置、速度的增量，接着计算旋转的增量。旋转变化量以李代数中的旋转向量表示，并通过指数映射转换为旋转矩阵。旋转矩阵按旋转顺序右乘。最后，更新协方差矩阵，并调整与偏置修正相关的位置、速度和旋转雅可比。

       IMU的偏置校正、测量、标定和预积分类定义在include/ImuTypes.h文件中。

       值得注意的是，对于初学者，了解GDB调试方法是提高ORB-SLAM3源码理解效率的重要步骤。GDB提供了一系列功能，允许开发者在运行程序时设置断点、查看变量值、追踪程序执行流程等，从而深入分析代码行为和潜在问题。