如果想成为中心，那么就到中心去吧。

有运动就有畸变。

在LIO（激光惯性里程计）系统中，将GPS信息融合到里程计中，借助GTSAM（Georgia Tech Smoothing and Mapping）库进行因子图优化，可以有效提升全局定位精度。当进入到GPS信号不好或缺失的环境中，需要借助融合后的里程计反算经纬度，来获取高精度的定位信息。利用GeographicLib第三方库将经纬度投影到局部笛卡尔坐标系中。

将lio-sam作为里程计，在move_base中实现2D导航。

自我车辆运动会导致从附加激光雷达传感器收集的点云数据失真。失真程度取决于自车速度和激光雷达传感器的扫描速率。机械激光雷达传感器通过旋转反射激光脉冲的镜子来扫描环境，并生成周围环境的点云数据。该镜子的旋转速度决定了传感器的扫描速率。激光雷达传感器生成点云数据，假设每次测量都是从同一视点捕获的，但自我车辆运动会改变镜子的旋转，从而改变传感器捕获数据的视点。假设视点和实际视点之间的差异会导致生成的点云失真。该图显示了当自我车辆移动时如何发生失真以及如何通过使用点云中每个点的自我车辆姿态来补偿失真的顶视图。

多传感器硬同步（Hardware Synchronization）的过程是指通过硬件信号使多个传感器在同一时刻进行数据采集，以确保不同传感器的数据具有相同的时间基准。以下是多传感器硬同步的简要步骤：主时钟源（同步信号源）：选择一个主时钟源，通常是高精度时钟（如GPS、PPS信号或高频时钟脉冲），作为所有传感器的时间基准。该时钟源通过硬件接口（如信号线）向其他传感器发送同步信号。传感器接口设置：各个传感器需要有同步接口，通常通过硬件触发输入（trigger input）或外部时钟输入端口接受同步信号。

在激光SLAM（Simultaneous Localization and Mapping）中，使用彩色相机为激光点云染色是一个常见的做法。这种技术结合了激光雷达的高精度距离测量和相机的丰富色彩信息，使得生成的点云不仅包含空间位置信息，还包含颜色信息，从而更直观和细节丰富。传感器校准： 目的是确定激光雷达和相机之间的外部参数（旋转和平移矩阵），以及相机的内部参数（焦距、主点、畸变系数等）。

树的每个内部节点代表点云的一个子集，并根据选定的坐标轴分为两个子节点。因此，点云分析的第一步也是主要步骤是将点配对以进行有意义的比较。当数据点分布不均匀时，空间的某些区域可能比其他区域拥有更多的点。然后算法递归遍历包含最近点的子节点，直到找到最近点。当树中的某些节点比其他节点拥有更多的点时，就会发生这种情况。尽管网格采样是点配对的替代方法，但由于子采样过程（可能包括平均），它仍然会比点配对产生更粗糙的数据。最后，它返回最近点的坐标。另一方面，与网格采样相比，点配对排除了平均，并产生更深入、更细粒度的结果。

PS:可能遇到的问题 CV_* ’was not declared in this scope，opencv4部分命名发生变换，将CV_WINDOW_AUTOSIZE改为WINDOW_AUTOSIZE；CV_TM_SQDIFF_NORMED改为TM_SQDIFF_NORMED。FAST-LIVO通过使用LiDAR点云建立环境地图，IMU提供高频率的动态信息，视觉传感器提供纹理信息，综合这些数据源来提高定位的精度和鲁棒性。这种多传感器融合的方法使其在动态和复杂环境中也能保持高精度的位姿估计。

读取华测设备组合导航数据，将其发布成ros中的odometry格式话题。

4D毫米波雷达SLAM（Simultaneous Localization and Mapping，即同时定位与建图）是一种利用4D毫米波雷达传感器来进行环境感知并构建地图的技术。4D毫米波雷达不仅能够测量目标物体的距离和角度，还能够获取速度和高度信息，从而提供更丰富的环境感知数据。4D毫米波雷达在传统毫米波雷达的基础上，增加了对目标物体高度和速度的测量，即除了传统的3D信息（距离、方位角和俯仰角）外，还能提供速度和高度信息。这增加了雷达的环境感知能力，使其在复杂环境下也能有效工作。

为了复用上个生命周期录制的轨迹，我需要用到重定位功能，现有的开源方案中，可以实现该功能，但存在以下问题：在预先构建的地图之外，无法实现定位功能。只在初始化原点位姿的时候，进行重定位，使得当前生命周期内的原点和预先构建的地图原点一致，后续操作和lio-sam一样。在rviz中添加2d pose estimate。初始化后进行增量建图。

打开界面后-》编辑-》首选项-》插件。检索“opendata”后安装。导入与影像相同的空间参考。新建shapefile。右击文件另存为osm。

这个转换系统能够将局部坐标（相对于原点的X, Y, Z坐标）转换回全球坐标系统。将局部里程计数据反算成经纬度坐标的过程涉及几个关键步骤，主要是将相对于某个局部参考点的坐标（例如在机器人、无人车或无人机的导航系统中使用的坐标）转换回全球GPS坐标系统（WGS-84经纬度）。为了能够将局部里程计数据转换为经纬度，首先需要知道局部坐标系统的原点（即参考点）在全球坐标系统中的位置。对于从里程计获得的每个局部坐标点，使用步骤2中设置的坐标转换系统将其转换为经纬度。使用转换模型将局部坐标转换为全球坐标。

PS：将雷达和imu相对固定后，拆下来，原地晃啊晃，直到xyz进度达到百分百，参考gif。标定完成后，会在result文件夹下生成result.txt。

在mapviz中加载地图瓦片，可以用于目视多传感器融合定位结果的好坏。

主要参数文件来自这里https://github.com/wh200720041/warehouse_simulation_toolkit/tree/master/param。move_base的输入话题有三个：/odom，/map, /scan，前两个做重映射即可，后面一个在pointcloud_to_laserscan包中发布了。参考坐标系向lio_sam配置对其，在lio_sam中，坐标系为。在move_base设置为。

将投影后的二维点云转换为栅格地图（occupancy grid）。在栅格地图中，每个单元格（grid cell）表示空间中的一定区域，单元格的值表示该区域被占用的概率。在ROS中，将三维点云转换为二维栅格地图是一个常见的需求，尤其是在机器人导航和路径规划中。去除离群点：移除离群的噪声点，常用的方法有统计分析滤波（Statistical Outlier Removal）。降采样：减少点云的数据量以提高后续处理的效率，常用的方法包括体素网格滤波（Voxel Grid Filter）。

在LIO-SAM的基础上进行重定位，主要是指在已经建立的地图上进行位置的快速定位，这对于机器人在已知环境中的快速启动或者在丢失定位后的恢复尤为重要。LIO-SAM是一个高效的激光雷达增量式SLAM系统，其重定位功能需要额外的策略和步骤来实现。

Gazebo 仿真提供了一个高效且成本低廉的平台，使研究人员和开发者能够在安全且可控的虚拟环境中设计、测试和优化机器人系统。它允许快速原型制作和迭代，精确控制测试条件，并能模拟复杂或危险的场景，从而避免了在真实环境中可能遇到的风险和成本。

小车搭载了2D激光雷达，realsense深度相机。在小车上添加相机，请参考这篇。在农场的场景中加入小车。

将地理坐标（纬度、经度、高度）转换为地心地固坐标（ECEF坐标系下的x、y、z坐标），这里_earth是一个Geocentric对象，用于进行地心坐标的计算。总的来说，Reset函数通过定义的纬度、经度和高度，初始化一个本地笛卡尔坐标系，计算了从地理坐标到该本地笛卡尔坐标系的转换所需的所有参数和矩阵。总体来说，IntForward函数实现了从地理坐标系到局部笛卡尔坐标系的转换，包括平移和旋转，使得局部坐标系的原点位于给定的地理坐标点，并且根据地球的形状和局部原点的方位进行适当的旋转。

在inputOdom函数中，即使没有新的GPS数据，只要有里程计（odom）数据输入，globalP就会根据当前的OdomP、OdomQ（odom的位置和旋转）以及WGPS_T_WVIO（上一次GPS数据与VIO之间的变换矩阵）来更新。因此，即使没有新的GPS数据，只要odom数据持续输入，globalP就会实时更新。但是，如果长时间没有接收到GPS数据，而odom数据持续更新，那么定位的准确性可能会逐渐降低，因为累积的odom误差没有通过新的GPS数据进行纠正。计算视觉里程计和GPS之间的最终变换矩阵。

ORB-SLAM3是一种基于特征的SLAM（同时定位与地图构建）系统，它可以在多种环境中实时运行，包括在单目、立体和RGB-D摄像头输入下。这个系统是ORB-SLAM系列的第三个版本，其前身为ORB-SLAM和ORB-SLAM2。ORB-SLAM3的主要特点包括：多种传感器输入：ORB-SLAM3支持多种类型的摄像头输入，包括单目、双目（立体摄像头）和RGB-D摄像头。这意味着系统可以适应不同的应用场景和硬件配置。

ORBSLAM3 是一个最新的视觉SLAM（Simultaneous Localization and Mapping）系统，它建立在成功的 ORBSLAM2 系统的基础上，引入了多种新的特性和改进。它是目前最先进的视觉SLAM系统之一，它的性能和灵活性为研究人员和开发者提供了强大的工具，用于构建复杂的定位和地图构建应用。通过这样的策略，VINS-Fusion能够利用GPS数据来提高在大尺度环境下的定位精度，并在视觉信息不足或遮挡的情况下保持系统的稳定。这通常涉及到计算地图与地球坐标系之间的初始变换。

VINS-Fusion 是一个开源的实时多传感器状态估计库，主要由香港科技大学的沈邵劼教授领导的研究团队开发。它是 VINS-Mono（单目视觉惯性系统）的扩展，支持多种传感器组合，如双目、立体相机和IMU。

在许多机器人操作系统（例如ROS，Robot Operating System）中，里程计信息可以通过订阅一个话题（topic）来获得。在每次回调函数被里程计消息触发时，从消息中提取位置和方向信息，然后将这些信息写入打开的文本文件中。在你的ROS包中，编写一个订阅者（subscriber）节点，这个节点订阅里程计话题（通常是 /odom），并在回调函数中处理接收到的数据。在订阅者节点中，打开一个文本文件用于写入。当记录完成或者节点关闭时，确保关闭文本文件以保持数据的完整性。当距离过小时，不与记录。

具体下载教程可去官网查看（https://ucr-robotics.github.io/Citrus-Farm-Dataset/download.html）官方给出了两种网盘下载方式。

这个bag在容器中，无需自己另外下载。根据相机类型选择运行的语句。从vscode中进入容器。拉取自带数据集的镜像。

在进行目标跟踪时，算法实时测量得到的目标平面位置，是具有误差的，连续观测，所形成的轨迹如下图所示，需要对其进行噪声滤除。这篇博客将使用卡尔曼滤波，对轨迹进行优化。优化的结果为黄色线。

利用imu和GPS数据进行组合导航，在我的数据集上复现效果较差。局部细节（红色线表示GNSS，绿色线表示组合导航结果）添加imu和gps话题参数，修改imu的内参。

H。

g2o实现拟合圆

事实上，图优化中一条边可以连接一个、两个或多个顶点，这主要反映在每个误差与多少个优化变量有关。在稍微有些玄妙的说法中，我们把它叫做超边（Hyper Edge），整个图叫做超图（Hyper Graph）。而每个带噪声的数据点，构成了一个个误差项，也就是图优化的边。但这里的边与我们平时想的边不太一样，它们是一元边（Unary Edge），即只连接一个顶点——因为我们整个图只有一个顶点。这个过程中，只要记住节点为优化变量，边为误差项即可。弄清了这个图模型之后，接下来就是在 g2o 中建立该模型，进行优化了。

惯性测量单元(Inertial measurement unit，IMU)，是测量物体三轴姿态角以及加速度的装置。IMU通常包含陀螺仪 (Gyroscope)、加速度计 (Accelermeters)，有的还包含磁力计(Magnetometers)。陀螺仪用来测量三轴的角度/角速度，加速度计用来测量三轴的加速度，磁力计提供磁场朝向信息。IMU在手机、VR，航空航天领域都有着广泛应用。比如手机的记步功能就是利用IMU的算法达成；VR中IMU可实现随头部姿态变化而切换场景；

对极几何是两个透视几何模型间的几何约束关系，主要用于实现基于三角测量的双目立体视觉、深度估计等，对极几何约束只能实现点到线的映射，因此约束条件弱于透视几何。透视相机光心与物点确定的平面称为极平面，对于不同物点，极平面绕基线旋转，极线绕极点矩形，如图3所示。透视几何的缺陷是图像深度信息的丢失，如图1所示，根据相似变换关系，视线上的若干平面都映射为成像面上的一个平面。R称为本质矩阵(Essential Matrix)，表征了同一物点在两个透视相机成像面上像点的几何约束关系。，左右两相机间的变换关系为。

SLAM”是指在绘制环境地图的同时，对某些传感器相对于周围环境进行同步定位（即位置/方向）的方法。这是机器人和自主系统领域的一个活跃的研究领域。

农业任务自动化速度的加快要求现场机器人采用高精度和鲁棒的定位系统。同时定位和映射（SLAM）方法不可避免地会在探索性轨迹上积累漂移，并且主要依赖于位置重新访问和循环闭合来保持一个有界的全局定位误差。环状闭合技术在农田中具有显著的挑战性，因为不同视野的局部视觉外观非常相似，并且很容易由于天气的影响而发生改变。在实践中，一个合适的替代方案是将全球传感器定位系统与其他机器人传感器联合使用。在本文中，我们提出并实现了GNSS、立体视图和惯性测量的定位融合。

ROS 中的 robots_localization 包是一个非常有用的包，可以使用各种卡尔曼滤波器融合任意数量的传感器！我们将使用它将全局姿势数据（x、y、z 和orientation）与机器人上的现有传感器融合，以实现更强大的定位！

【代码】两种测量方式的自适应卡尔曼滤波数据融合。