weixin_42650596的博客

扫描到扫描匹配是最基本的扫描匹配方法，通过比较当前扫描数据与上一扫描数据之间的差异来估计机器人的位姿变化。常见的算法包括迭代最近点算法(ICP, Iterative Closest Point)和PL-ICP(Point-to-Line ICP)，其中ICP适用于点云数据的精确匹配，而PL-ICP则更适合处理线性特征较多的场景。似然场扫描匹配方法基于概率模型，将激光雷达的扫描数据与地图中的栅格或点云进行比较，从而计算出当前位姿的可能性。ICP通过迭代优化两个点云之间的相对位姿，以实现精确的匹配和定位。

将一段时间内的IMU测量数据(如加速度和角速度)累积起来，形成一个相对运动的增量模型。这种方法避免了直接对每一时刻的状态进行积分，而是通过累积增量来更新状态，从而简化了计算过程。

重点：

fast-lio系列介绍。

假设一个非线性运动模型矫正点云由于运动产生地畸变，基于IMU预积分估计的运动可作为激光里程计优化的初值。然后再基于激光里程计估计因子图中IMU的零偏。在机器人的轨迹估计时，基于因子图，我们有效地使用雷达和imu的测量值进行多传感器融合。融合来自不同源的多种因子集合在因子图中共同优化。a）IMU预积分约束，（b）激光里程计约束，（C）GPS因子约束，（d）回环检测因子(基于欧式距离进行实现)。

faster-lio主要优化地图格式，采用体素增量(ivox)作为地图结构，并完成地图中点的增删改查。后端：利用反向传播构建点云残差和IMU积分的先验误差基于优化模型利用IKEF进行迭代优化求解。前端主要是提取面特征、线特征，并利用IMU数据估计先验位姿；反向传播的目的就是为了去除运动畸变，构建更准确的点云残差。前端：提取点云特征中面特征和线特征，

其实现原理：根据初始位姿和点云，以初始位姿为中心，在指定范围内枚举出所有候选位姿，并生成每个位姿对应的点云。位置、角度两残差顶多就是对匹配位姿的约束，防止点云匹配的结果和初值差太多，真正的扫描匹配的主角是点云匹配残差。需要CSM粗匹配把位姿推测器传来的预测值更新为一个好的初值，如果没有CSM粗匹配，就用位姿推测器传来的预测值作初值。cartographer的扫描匹配主要通过CSM粗匹配或者位姿推测器提供Ceres精匹配的初值后，通过Ceres精匹配优化将相关数据插入到子图中。

基于右手坐标系(右手拇指指向旋转轴u 的正方向，这时其它四个手指弯曲的方向即为旋转的正方向．在上图中即为逆时针方向)，假设我们有一个经过原点的(如果旋转轴不经过原点我们可以先将旋转轴平移到原点，进行旋转，再平移回原处)旋转轴。如果有一个复数𝑧 = 𝑎+𝑏𝑖，那么𝑧 与任意一个复数𝑐 相乘都会将𝑐 逆时针旋转θ= atan2(𝑏, 𝑎) 度，并将其缩放||z||倍。注意，其实z旋转矩阵如果写成复数形式的话就是复数(复数形式)z = cos(θ) + 𝑖 sin(θ)．。如果||𝑧|| =

ROS中常见的坐标系包括map坐标系、odom坐标系、base_link(base_footprint)坐标系和base_laser坐标系等；一般的连接关系都是map->odom->base_link->base_laser。

该框架的特点：该框架易于使用、可移植、广泛优化、低计算时间；对于任何一个优化问题，我们首先需要对问题进行建模，之后采用合适的优化方法，进行求解。所以在代码中使用优化库进行优化时，需要包含基本内容：建模、优化、求导方法。Ceres Solver是一个开源的c++库，用于建模和解决大型、复杂的优化问题。g2o是一个开源的通用框架，用于优化可以定义为图形的非线性函数，它的优点是易于扩展，高效，适用于广泛的问题。gtsam应用：GTSAM与各种传感器前端一起使用，SVO的变体使用GTSAM作为里程计的后端。

流程：通过利用当前节点和构建子图(节点组成)、landmark、IMU和里程计数据构建。，结合线程池中将整体优化流程形成各个任务节点，并利用ceres库构建。

cartographer后端的误差和约束类型

当程序运行时，创建一定数量的线程，此时所有线程都处于空闲状态。当有新的任务进来时，任务将挂在任务队列中，等待着线程池中空闲线程获取，任务处理完后，该线程被放到线程池中。