秃头队长

机器人SLAM 中的位姿变化关系与常用消息记录。

首先贴上简单的验证代码，方便大家验证，欢迎提出问题一起交流讨论～目录一丶代码1. 工程一：transform 发布节点2. 工程二：transform 接收节点二丶测试结果分析1. ros::Time::now() 获取当前的时间2. ros::Time(0) 使用缓冲中最新的tf数据3. ros::Time::now() + ros::Time(0) 等待到最新时间，再取缓存数据一丶代码1. 工程一：transform 发布节点/***************** tf发布文件 *******

参考论文：Planar Odometry from a Radial Laser Scanner. A Range Flow-based ApproachRF2O根据传感器速度制定范围流约束方程，并最小化所得几何约束的鲁棒函数以获得运动估计。设R(t，α)R(t，α)R(t，α)为范围扫描，其中ttt是时间，α∈[0，N)⊂Rα \in [0，N)⊂Rα∈[0，N)⊂R是扫描坐标，N是扫描的大小。 任何点P相对于连接到传感器的局部参考系的位置由其极坐标(r，θ)(r，θ)(r，θ)给出，如果从激光雷达

目录1.CMakeLists 起飞2.main 函数3.计算激光里程计的主循环3.1 创建图像金字塔 createImagePyramid3.2 金字塔处理的八个步骤3.2.1 计算坐标3.2.2 找到数据为0的点3.2.3 计算梯度3.2.1 金字塔缩放后的数据裁剪  很早之前看过这篇论文，但是当时还是以多线激光雷达为主，最近又回到了2D激光里程计，所以有重新看了这部分代码，顺便记录一下！之前的原理部分可参考：RF2O激光里程计算法原理1.CMakeLists 起飞先从 CMakeLists 开

摘要：  本文介绍了应用于室内环境二维激光扫描的不同线提取算法的实验评估。选择并测试了移动机器人和计算机视觉中的六种流行算法。通过使用不同平台从两个办公环境中收集的 真实激光 数据用于实验中，以评估算法。提出并讨论了几种比较标准，以突出每种算法的优缺点，包括速度、复杂性、正确性和精度。使用标准统计方法将算法的结果与基本事实进行比较。进行了扩展的案例研究，以进一步评估 SLAM 应用程序中的算法1 介绍  对于移动机器人来说，能够在已知或未知的环境中自主导航和定位是很重要的。精确的位姿估计始终是任何导航

继续在家读文献～ 希望下周能继续打工，最近还是更多的关注激光里程计相关的方向。论文地址：Removal-First Tightly-coupled Lidar Inertial Odometry inHigh Dynamic Environments摘要：同步定位和建图（SLAM）被认为是智能车辆和移动机器人的基本能力。然而，当前大多数激光雷达 SLAM 方法都是基于静态环境的假设。因此，在具有多个移动对象的动态环境中定位实际上是不可靠的。论文提出了一种动态 SLAM 框架 RF-LIO，它建立在 L

被疫情包围了！在家多看看论文吧～论文地址：A Visual Inertial Odometry Framework for 3D Points, Lines and Planes【摘要】：恢复连续相机位姿之间的刚性配准是 3D 重建、SLAM 和视觉里程计的核心。配准依赖于计算连续相机图像中可区分的 2D 特征以确定特征对应的能力，这在特征较差的环境（即低纹理和/或低光环境）中非常具有挑战性。在本文中，我们旨在解决视觉惯性里程计（VIO）在特征较差的环境中恢复连续相机姿势之间的刚性配准的挑战。除了惯性传

参考文章：旋转的左乘与右乘对于一个变换矩阵 TTT，具体的定义因人而异，例如一个变换 TwcT_{wc}Twc​，代表了从相机坐标系到世界坐标系的变换，但是其基准（base）坐标系有时候定义为 www，也可以定义为 ccc，基准坐标系的不同，直接导致变换相反。例如对于下图中的三个简单的坐标系，分别为相机坐标系 c1，c2c1，c2c1，c2，世界坐标系 www，其中点 PPP 在坐标系 c1c1c1 下。接下来定义变换矩阵，为了简化计算，其中三个坐标系没有旋转，只需要考虑平移。对于变换矩阵 TwcT

SLAM的后端优化学习笔记对于相机位姿的变换可以通过旋转矩阵或者四元数进行表示，对于旋转矩阵的定义满足：R{∣R∣=1RRT=I R \begin{cases} |R| = 1 \\ RR^T = I\\ \end{cases} R{∣R∣=1RRT=I​即RRR 为正交矩阵，且行列式为1 。 旋转矩阵的行列式为什么等于1?除了旋转矩阵外，还需要仿射矩阵，仿射矩阵

对于窗口中连续两帧图像 bkb_{k}bk​ 和 bk+1b_{k+1}bk+1​，之前已经通过视觉 sfm 得到了两帧相对于滑窗第一帧的旋转qc0bk和qc0bk+1\mathbf{q}_{c_{0} b_{k}}和\mathbf{q}_{c_{0} b_{k+1}}qc0​bk​​和qc0​bk+1​​，此时之前通过 IMU 预积分得到这两帧旋转的预积分γ^bkbk+1\hat{\gamma}_{b_k b_{k+1}}γ^​bk​bk+1​​（先前不带 bias 的预积分）。此时，我们可以建立约束方

在realsence上测试运行vins时，后端优化部分进程挂掉，如下图所示，看提示是string的问题，通过调试定位到描述子模板文件名出现问题。通过打印看出是工程目录出现问题，因此在导入时将描述子文件换成绝对路径，问题解决！/home/robot/vins/src/VINS-Mono/pose_graph/../support_files/brief_pattern.ymlBriefExtractor extractor(BRIEF_PATTERN_FILE.c_str());BriefExtr

基础理论介绍：扩展卡尔曼滤波EKF用于多源传感器融合相关 robot pose ekf 代码解析：robot_pose_ekf源码解读 - zhxue_11Navigation源码阅读之robot_pose_ekf - 若愚和小巧robot_pose_ekf源码解读 - 首飞爱玩机器人本篇博客主要是记录自己 EKF 的学习过程，以理论逻辑为主线，代码实现为辅助理解如何从头开始实现一个基于扩展卡尔曼滤波的数据。待更…...

参考文章：1.张兴学. 基于因子图的多传感器信息融合导航算法研究[D].哈尔滨工业大学,2018.2.SLAM 中的位姿图优化，BA优化，因子图优化，非线性优化之间的区别3.因子图的理论基础4.机器人感知：因子图在SLAM中的应用(读书笔记)第一至三章SLAM中的定位问题，是根据先验信息（上一时刻的位姿和已知路标点信息）以及传感器的观测值（当前时刻测到的路标点信息），推断出该时刻的位姿以及未知路标点位置。对于这样一个概率推理问题，可通过因子图简化。因子图是概率图模型的一种表示方式，表示了全局.

TARE: A Hierarchical Framework for Efficiently Exploring Complex 3D Environments论文地址：https://www.ri.cmu.edu/wp-content/uploads/2021/06/RSS_2021.pdfgithub：https://github.com/caochao39/tare_planner我们的方法的好处是，它可以比目前的最先进的算法更快地探索3D空间。该方法的优点是基于分层框架，将处理分离为两个级别。

参考翻译项目主页：https://github.com/teddyluo/motion-planning-chs核心步骤：调用hybridAStar()函数获取一条路径获取路径点(3D Node) -> 原始路径对路径依据Voronoi图进行平滑->平滑路径 // FIND THE PATH Node3D* nSolution = Algorithm::hybridAStar(nStart, nGoal, nodes3D, nodes2D, width, height,

为了保证激光雷达的360°环境覆盖，准备在车体上安装三个雷神16线的激光雷达，以前二后一的布局分布，大致位置情况如下所示：其中蓝色区域为雷达1所保留的点云区域，黄色区域为雷达2所保留的点云区域，绿色区域为雷达3所保留的点云区域，从而实现整车的周围环境的全覆盖。但是这种情况会降低了多个激光雷达的鲁棒性，万一某个激光雷达出现故障或者污渍遮挡，就会出现数据点云盲区，因此后面会尝试融合多个激光雷达的点云数据。关于激光雷达的IP配置问题，雷神16线的激光雷达出厂默认配置ip为192.168.1.200。UDP设

在ORB-SLAM中的特征提取和描述子计算，调用Opencv库函数完成，在脱离Opencv的过程中，遇到的BUG和问题简单的记录一下。首先定义自己的Mat和Keypoint以及在测试中所需要的转换工具。typedef unsigned char uchar;typedef vector<vector<uchar>> VecMat;struct mPoint2f{ float x; float y;};typedef struct _mKeyPoint

LVI-SAM建立在一个因子图之上，由两个子系统组成：视觉惯性系统(VIS)和激光雷达惯性系统(LIS)。这两个子系统采用紧密耦合的方式设计，其中VIS利用LIS估计来初始化。利用激光雷达测量来提取视觉特征的深度信息，提高了视觉识别的精度。反过来，LIS利用VIS估计来进行初始估计，进行 Scan-Matching。回环首先由VIS识别，然后由LIS进一步细化。当两个子系统中的一个出现故障时，LVI-SAM也可以正常工作，在无纹理和无特征环境中增强了它的鲁棒性。LVI-SAM的系统结构。该系统接收来

以双目IMU为例，即以stereo_inertial_tum_vi为入口，在该函数中通过LoadIMU()加载IMU数据至vImuMeas容器中。然后开始进入SLAM系统的双目跟踪。SLAM.TrackStereo(imLeft,imRight,tframe,vImuMeas);如果是单目VIO模式，把IMU数据存储到mlQueueImuData中，if (mSensor == System::IMU_STEREO) for(size_t i_imu = 0; i_imu <

参考论文：MLPNP - A REAL-TIME MAXIMUM LIKELIHOOD SOLUTION TO THE PERSPECTIVE-N-POINT PROBLEMPnP的任务是找到一个旋转R∈SO(3)R \in SO(3)R∈SO(3)和平移t∈R3t \in \mathbb R^3t∈R3，将世界坐标系下的点pip_ipi​映射到相机坐标系下的观测点viv_ivi​，即：λivi=Rpi+t(1)\lambda_iv_i=Rp_i+t \tag{1}λi​vi​=Rpi​+t(1)这里

高斯牛顿求解流程有如下最小二乘系统，对应的模型如下所示：ξ=argminξ12∑i∥ri∥∑i2\xi = \underset{\xi}{argmin}{1\over 2}\sum_i\lVert r_i\rVert^2_{\sum_i}ξ=ξargmin​21​i∑​∥ri​∥∑i​2​对上式展开求导，令其导数为零，可得到Hx=bHx=bHx=b的形式，上式对应的高斯牛顿求解 normal equation：J⊤Σ−1J⏟H or Aδξ=−J⊤Σ−1r⏟b\underbrace{J^\topΣ^{-

首先关于什么是协方差矩阵与信息矩阵。在零均值的多元高斯分布中有如下概率形式：p(x)=1Zexp(−12x⊤Σ−1x)p(x)={1\over Z}exp(-{1\over 2}x^⊤Σ^{−1}x)p(x)=Z1​exp(−21​x⊤Σ−1x)其中ΣΣΣ为协方差矩阵，而协方差矩阵的逆Λ=Σ−1Λ = Σ−1Λ=Σ−1则为信息矩阵。比如在XXX为三维变量时，协方差矩阵Σij=E(xixj)Σ_{ij}=E(x_ix_j)Σij​=E(xi​xj​)为对应元素求期望。样例：viv_ivi​相互独立，且各

我们工作的主要贡献是:考虑IMU噪声的概率模型的情况下，将视觉-惯性初始化问题表述为只考虑惯性的最优估计问题。我们一次性求解了所有的惯性参数，避免了解耦估计所产生的不一致性。这使得所有的估计都一致。我们不做任何初始速度和姿态的假设，这使得我们的方法适用于任何初始情况。我们不假设IMU偏差为零，相反，我们将它们的已知信息编码为被我们的MAP估计所利用的概率先验。一丶 最大后验初始化 MAP（maximum-a-posteriori）适当考虑了所有传感器的噪声特性，得到了所有变量的最大后验联合估

VIO 残差函数的构建基于滑动窗口的 VIO Bundle Adjustment，为了节约计算量采用滑动窗口形式的 Bundle Adjustment，在 i 时刻，滑动窗口内待优化的系统状态量定义如下：χ=[Xn,Xn+1,...,Xn+N,λm,λm+1,...,λm+M]\chi = [X_n,X_{n+1},...,X_{n+N},\lambda{m},\lambda{m+1},...,\lambda{m+M}]χ=[Xn​,Xn+1​,...,Xn+N​,λm,λm+1,...,λm+M]

第一讲 概述与课程介绍VIO 概述VIO：（Visual-Inertial Odometry）以视觉与 IMU 融合实现里程计IMU（Inertial Measurement Unit），惯性测量单元典型 6 轴 IMU 以较高频率（≥ 100Hz）返回被测量物体的角速度与加速度。高频率运动。受自身温度、零偏、振动等因素干扰，积分得到的平移和旋转容易漂移。可通过标定去除干扰。六自由度 IMU 本身由一个陀螺仪和一个加速度计组成，分别测量自身的角速度和加速度。视觉 Visual Odome

来自论文：A Lightweight and Accurate Localization Algorithm Using Multiple Inertial Measurement Units一丶摘要本文通过融合来自多个惯性测量单元（IMU）和传感器的信息，提出了一种新颖的惯性辅助定位方法。 IMU是一种低成本的运动传感器，可对移动平台的角速度和重力补偿的线性加速度进行测量，并广泛用于现代定位系统中。迄今为止，大多数现有的惯性辅助定位方法仅利用一个IMU。尽管单个IMU定位可以为不同的使用案例提供可.

总结课程《深蓝学院移动机器人路径规划》1.Introduction光滑轨迹生成边界条件：起始状态，终止状态中继节点：机器人经过的路标点waypoint平滑准则：评测生成的轨迹是否光滑2.Minimum Snap Optimization（微分平坦）Differential Flatness四旋翼的12个状态和输入可以写为4个平坦变量及其导数的代数函数。简单来说就是把无人机X=[x,y,z,ϕ,θ,ψ,x˙,y˙,z˙,wx,wy,wz]X=[x,y,z, \phi,\theta,\.

相关参考文章：RTAB-MAP原理详解RTABMAP-ROS RGB-D的建图原理SLAM: RtabMap中文解析RTAB-Map学习和测试详解​Rtab-Map的思想是：假设更频繁的被访问的定位点比其他的定位点更易于形成闭环。这样一个定位点被连续访问的次数就可以用来衡量其易于形成闭环的权重。当需要从WM(Working Memory)转移定位点到LTM（Long-Term Memory）中时，优先选择具有最低权重的定位点。如果具有最低权重的定位点又有多个时，优先选择被存储时间最长的那一个.

总结课程《深蓝学院移动机器人路径规划》Kinodynamic : Kinematic + Dynamic1.State Lattice Planning汽车质点模型不满足动力学约束，需要满足两点之间是可行的运动连接。1.离散机器人控制空间2.离散机器人状态空间考虑动力学约束后，建立一个机器人模型，由状态和输入描述。S˙=f(s,u)\dot{S}=f(s,u)S˙=f(s,u)当给系统不同的输入，持续一定的时间T。（离散机器人控制空间）（没有任务导向性，效率相对低。）当选择两个状态.

总结课程《深蓝学院移动机器人路径规划》1.概率路图 Probabilistic Road Map分为学习阶段，查询阶段。通过采样的方式代替整个2D栅格图，通过一个图结构简化地图。学习阶段：用一定的采样方式撒点，把落在障碍物中的点剔除，完成采样。采样后通过线段连接采样点，连接时需要考虑距离限制以及障碍物限制。查询阶段：构建完成一张图后，可以通过D或者A*去查找路径。概率完备，高效。2.RRT(Rapidly-exploring Random Tree)相对于PRM，更针对的去寻找从起.

1.Kalman Filters在跟踪一辆运行的车时，当σ\sigmaσ越小，表明确定因素越多。测量值更新使用贝叶斯规则，而预测是全概率公式，也是一种加权平均，他们之间通过高斯实现。假设我们已知先验告诉我们有辆车的位置分布如下黑色线所示，一个较宽的高斯分布，其均值为μ\muμ。而测量值信息告诉的车辆位置如蓝色线所示，其均值为vvv，测量值拥有更小的协方差。相比较，该例子中测量值所给出的位置信息更多。因此，利用先验和测量值信息，所得到的新的高斯分布如下，均指在两者之间，但相比较组成他的两个高斯分布

github地址：Fast LOAM (Lidar Odometry And Mapping)Fast LOAM提供了mapping和localization的两个节点，目前只使用其定位部分，以velodyne为例分析一下源码。 <node pkg="floam" type="floam_odom_estimation_node" name="floam_odom_estimation_node" output="screen"/> <node pkg="floam" t

@masterthesis{Hörner2016, author = {Jiří Hörner}, title = {Map-merging for multi-robot system}, address = {Prague}, year = {2016}多机器人探索拥有比单个机器人明显的优势，可以去做一些单个机器人无法做的事情。在全分布式系统中，消除了单点故障。多机器人群可以克服导航和建图算法中的底层缺陷，特别当使用异构机器人群时，当某处一个机器人被困住了，就会被使用不同算法的机器人取代。当前.

参考课程《深蓝学院激光SLAM》曾书格 总结一、首先数据入口在文件global_trajectory_builder中，在其头文件中仅创建两个初始实例化函数：std::unique_ptr<TrajectoryBuilderInterface> CreateGlobalTrajectoryBuilder2D()std::unique_ptr<TrajectoryBuilderInterface> CreateGlobalTrajectoryBuilder3D()主要内容.

用RGBD模拟激光雷达数据：depthimage_to_laserscanROS导航-向cost-map中添加超声波障碍图层

一丶ORB SLAM2参考README文件Add the path including Examples/ROS/ORB_SLAM2 to the ROS_PACKAGE_PATH environment variable.Open .bashrc file and add at the end the following line. Replace PATH by the folder where you cloned ORB_SLAM2:export ROS_PACKAGE_PATH=${R

《深蓝学院-激光SLAM》课程一丶激光雷达传感器介绍1.三角测距其中假设右边为一个激光发射器，左边为摄像头接受右方反射的光。其中已知L=L1+L2L=L_1+L_2L=L1​+L2​，因此：L1=dtanα,L2=dtanβL_1 = {d \over tan \alpha }, L_2 = {d \over tan \beta }L1​=tanαd​,L2​=tanβd​L=L1+L2=dtanα+dtanβL=L_1+L_2={d \over tan \alpha}+ {d \over t.

// 求解线性最小二乘 //correct_matrix = (A.transpose()*A).inverse()*A.transpose()*b; Eigen::VectorXd correct_vector = A.ldlt().solve(b); correct_matrix = Eigen::Map<Eigen::Matrix3d>(correct_vector.data()); correct_matrix = correct_matrix.tran.

深蓝学院《激光SLAM》-曾书格轮式里程计运动模型及标定轮式里程计相比IMU是一个光电编码器测量轮子转动距离，不像IMU受到漂移影响。一丶轮式里程计模型1.两轮差分底盘的运动学模型因为底盘中心做圆弧运动，因此围绕同一圆心做圆弧运动角速度相同。在实际情况中，左右轮线速度和两轮距离b可能是未知的， 以角速度表示会显示的带机器轮子半径的参数，而标定就是要求解该半径。2.航迹推算（Dead Reckoning）二丶轮式里程计标定1.线性最小二乘最小二乘求解的绝大多数为超定方程组。多.

参考文献：《车载激光扫描仪外参数标定方法研究》 康永伟，钟若飞，吴 俣https://kns.cnki.net/kcms/detail/detail.aspx?filename=HWYJ2008S3060&dbcode=CJFQ&dbname=CJFD2008&v=车载多传感器集成系统中激光扫描仪与 GPS、 IMU （惯性测量单元）联合使用， GPS 得到的是 GPS 天线中心位置在 WGS-84 高斯克吕格三度带投影下的坐标；IMU 得到的是 IMU 本身的姿态信息。.