qq_29320587的博客

在本文中，我们提出了一个通过smoothing and mapping实现的紧耦合激光惯性里程计框架，来解决上面提到的问题。首先我们假设一个非线性运动模型用于点云运动畸变的矫正，主要是使用IMU测量值估计雷达的运动。除了用于矫正点云之外，基于IMU估计的运动还可作为激光里程计优化的初值。然后再基于激光里程计估计因子图中IMU的零偏。在机器人的轨迹估计时，基于因子图，我们可以有效的使用雷达和imu的测量值进行多传感器融合，比如引入位置识别，引入GPS位置信息和指南针的朝向信息等绝对测量值。

【代码】非线性最小二乘法之Dog-Leg。

1、实际使用中使用梯度下降法找到的通常是局部极小值，而不是全局最小值。2、具体找到的是哪个极小值和初始点位置有很大关系。3、如果函数是凸函数，那么梯度下降法可以保证收敛到全局最优解（最小值）4、梯度下降法收敛速度通常比较慢5、梯度下降法中搜索步长 λ 很关键，步长太长会导致找不到极值点甚至震荡发散，步长太小收敛非常慢。因此，前面的例子中，迷路的探险者可能花费了大量时间找到的却是一个假的山脚（山腰的某个低洼处，局部最小值）。

cartographer学习笔记（二）这一篇主要分析node_main.cc的代码。

关于cartographer的学习笔记。

NDT 算法的基本思想是，先对待配准点云进行栅格化处理，将其划分为指定大小的网格，通过正态分布的方 式，构建每个网格的概率分布函数，之后优化求解出最优变换参数，使得源点云概率密度分布达到最大，以实现两个点云之间的最佳匹配。

使用便携式激光测距仪（又称Lidar）和SLAM（simultaneous localization and mapping）是获取建筑平面图的有效方法。实时生成和可视化楼层平面图有助于操作员评估捕获数据的质量和覆盖范围。而这种便携式的平台计算资源是有限的。而作者现在要介绍他们使用的方法，他们的建图平台是装着lidar的背包，实现实时建图和5cm的分辨率，主要方法是通过计算scan-to-submap匹配作为约束。建筑平面图对各种应用都很有用，现在的方法都是用激光、卷尺来测量，或者结合CAD图。

【代码】LIO-SAM代码逐行解读（6）-mapOptmization.cpp (2)

定义了一个新的数据类型，包含 x,y,z,roll,pitch,yaw, intensity, time。

IMUPreintegration 订阅 lio_sam/mapping/odometry_incremental 和 imu_correct , 使用gtsam进行imu与激光里程计的紧耦合优化，发布优化后的 odometry/imu_incremental。TransformFusion 订阅 IMUPreintegration发布的odometry/imu_incremental，以及激光里程计lio_sam/mapping/odometry。

我们在前述的博客中介绍了LIO-SAM的一些准备工作、点云预处理等内容。之后，整理为cloud_info消息类型进行发布，以供后续特征点提取进行使用。接下来，我们进行后续的工作，根据点的平滑度来进行特征点提取，包括面片点与角点。这一部分还是比较简单明了的，总体来说就是先计算平滑度，接着根据平滑度的值排序，选取其中的面片点与角点。

本篇博客中，我们主要讨论接收点云后投影到一个平面上，为后续的特征提取做准备的相关内容，称之为“点云预处理”部分。此部分主要使用imu数据，进行激光雷达数据的运动畸变去除与点云投影。

针对Shan Tixiao开源的经典LIO-SLAM算法框架LIO-SAM源代码进行逐行解读，总结。这里是第一部分，主要是开始的一些准备工作，包括 utility.h文件，cloud_info.msg文件等。

粒子滤波是一种基于蒙特卡洛模拟的非线性滤波方法，其核心思想是用随机采样的粒子表达概率密度分布。粒子滤波的三个重要步骤为：1）粒子采样，从建议分布中抽取一组粒子；2）粒子加权，根据观测概率分布，重要性分布以及贝叶斯公式计算每个粒子的权值；3）估计输出，输出系统状态的均值协方差等。此外 ，为了应对粒子退化现象，还采用了重采样等策略。

ESKF(error state Kalman filter)是Kalman滤波的一种特殊形式，相比于KF或EKF,UKF等，ESKF有以下优势:1.error state 总是接近于0，Kalman Filter工作在原点附近, 故远离奇异值、万向节锁，并且保证了线性化的合理性和有效性;2.error state 总是很小，因此二阶项都可以忽略，因此雅可比矩阵的计算会很简单，很迅速;

由于扩展卡尔曼滤波可能存在线性化误差，且一般情况下雅克比矩阵不易实现，增加了算法的计算复杂度。无迹卡尔曼滤波不采用泰勒展开实现非线性系统线性化，而是采用来处理均值和协方差的非线性传递问题。（UKF算法是对非线性函数的概率密度分布进行近似，用一系列确定样本来逼近状态的后验概率密度，而不是对非线性函数进行近似，不需要对雅克比矩阵进行求导。

使用泰勒展开使得非线性函数近似线性化之后，就满足KF滤波线性化的条件了，所以后面的预测以及更新过程与KF相同。在SLAM系统里，很难满足KF中线性化的假设，SLAM系统里的状态转移函数和观测模型均是非线性的，即。EKF的做法是将上述的非线性函数进行泰勒展开并取前面两项近似为线性函数，即，

以上所有公式中，你只需要用到式(7)、(18)、(19)。(如果忘了的话，你可以根据式(4)和(15)重新推导一下)我们可以用这些公式对任何线性系统建立精确的模型，对于非线性系统来说，我们使用扩展卡尔曼滤波，区别在于EKF多了一个把预测和测量部分进行线性化的过程。

因为我使用的robosense 16线激光雷达，而lio-sam支持的是velodyne或者ouster，lio-sam中的imageProjection.cpp订阅了‘velodyne’ or 'ouster’类型的激光。如下所示中需要加入修改，将robosense的数据类型，修改为velodyne的类型，rslidar点云格式vlp-16数据格式缓存pointcloud去除运动畸变需要的数据。