常规的动态物体过滤方法按策略可以分为两类：（一）SLAM过程中在线去除动态点云，考虑了过往帧的信息(局部信息)；（二）用后处理的方式，考虑整个地图提供的信息(全局信息)，去除动态点云。

第一类策略可以细分为以下三种方法

1. segmentation-based。该类方法通常基于聚类，比如，Litomisky等人基于确定视角下的特征分布直方图(VFH, Viewpoint Feature Histogram)来从静态聚类中区分出动态聚类；Yin等人则认为相邻帧配准过程中匹配误差较大的点很可能是动态点，用这些点作为种子进行区域生长，搜索出的聚类既是动态聚类；此外，Yoon等人也提出了一种基于区域生长过滤动态聚类的方案。基于分割的方法中不得不提的还有基于深度学习的语义分割方法(deep-learning based semantic segmentation)，语义分割直接label出了哪些点是动态物体，建图算法只需要直接弃掉这些点即可，简单粗暴。但是，深度学习只能分割出训练过的动态类别，对其它类别的动态物体则无能为力。

2. ray tracing based method 这类方法非常典型地要结合栅格去实现，可以是普通的占据栅格或者八叉树栅格。其基本原理是，激光点打到的cell，hits计数+1，激光光束穿过的cell，misses计数+1，通过hits和misses计算cell的占据概率，占据概率低于阈值，则抹除掉这个cell内的所有点。这种方法利用了动态点只会短暂地hit到某个cell，这个cell在随后的大部分时间里都会被miss的特点。这种方法的 缺点是消耗计算资源，因为一个激光点不仅产生了一个hit，同时又产生非常多的misses。当然，这个方法也可以用后处理的方式去做，比如Cartographer 3D

3. visibility-based (基于可见性的方法)。这类方法的基本假设是：如果一个新激光点会穿过某个旧激光点的位置（共线），那么这个旧激光点就是动态点。这个假设逻辑上说得通，但实现起来有两个问题：其一，入射角接近90度时的误杀问题，如下图所示，红色箭头指向的旧点因为与新激光点（五角星）光路很接近，会被误杀掉，考虑到激光点测量本身的角度误差、测距误差、光斑影响等，这种误杀会更严重。其二，遮挡问题，比如对于一些大型动态物体，它们完全挡住了激光雷达的视线，激光雷达没有机会看到这些动态物体后方的静态物体，意味着这些动态点永远不会被新的激光点穿过，此时就绝无可能把这些动态点滤除掉了。

cartographer处理动态障碍的逻辑太简单了，效果不好，需要研究其他方法。

The obstacle_detector package provides utilities to detect and track obstacles from data provided by 2D laser scanners. Detected obstacles come in a form of line segments or circles. The package was designed for a robot equipped with two laser scanners therefore it contains several additional utilities.

这个包也可以用于合并scan

算法顺序： two laser scans -> scans merger -> merged scan or pcl -> obstacle extractor -> obstacles -> obstacle tracker -> refined obstacles

resulting laser scan divides the area into finite number of circular sectors and put one point (or actually one range value) in each section occupied by some measured points

This node converts messages of type sensor_msgs/LaserScan from topic scan or messages of type sensor_msgs/PointCloud from topic pcl into obstacles which are published as messages of custom type obstacles_detector/Obstacles under topic raw_obstacles.

• obstacle_tracker node

使用卡尔曼滤波追踪和过滤圆形障碍，订阅消息类型obstacle_detector/Obstacles from topic raw_obstacles and publishes messages of the same type under topic tracked_obstacles. The tracking algorithm is applied only to the circular obstacles, hence the segments list in the published message is simply a copy of the original segments. The tracked obstacles are supplemented with additional information on their velocity.

自定义的消息

• CircleObstacle

1
2
3
4
5
6
7
8

// center of circular obstacle,
geometry_msgs/Point  center
// linear velocity of circular obstacle,
geometry_msgs/Vector3  velocity
// radius of circular obstacle with added safety margin,
float64 radius
// measured radius of obstacle without the safety margin.
float64 true_radius 

• SegmentObstacle

1
2
3
4

// first point of the segment (in counter-clockwise direction)
geometry_msgs/Point first_point
// end point of the segment
geometry_msgs/Point last_point

• Obstacles

1
2
3

Header header
obstacle_detector/SegmentObstacle[]  segments
obstacle_detector/CircleObstacle[]  circles

obstacle_detector也有一定误检测概率，可能把真实静态障碍检测为行人。

参考：
Github的obstacle_detector
Cartographer行人点云过滤建图
Paper Review: Detection and Tracking of 2D Geometric Obstacles from LRF Data

cartographer的内容实在太庞大了，还有去除建图时动态障碍的模块。

在使用Cartographer建图时，存在环境中行人较多，或者建图时机器人周围人员一直跟随，导致最终建图后地图上存在行人噪点。

Cartographer在构建submap以及整体map时，实质是对栅格地图概率的更新，每一个node在栅格概率上的累加，可以去除建图时一般移动物体所产生的噪点。只要不是长时间停留在雷达探测范围内，栅格地图的概率会及时更新，最终生成的地图不会带有行人和车辆。但是实际建图中经常会出现多人跟在机器人周围建图或恰巧机器人旋转时周围人为移动，旋转时node快速生成，导致该部分submap中存在行人噪点，如果后期不重复走这条路径，最终生成的map会存在噪点，影响地图美观，以及路径规划是被视为障碍物。

cartographer在建图后比较每个栅格被击中和被路过的次数来判断动静态栅格。参数voxel_filter_and_remove_moving_objects和类OutlierRemovingPointsProcessor，可以用来配置Voxel过滤数据，并仅传递我们认为在非运动对象上的点。

追本溯源，发现这一机制只在cartographer_assets_writer中，顺序是 assets_writer_main.cc —— assets_writer.cc —— points_processor_pipeline_builder.cc中的RegisterBuiltInPointsProcessors —— OutlierRemovingPointsProcessor类

cartographer_assets_writer 使用.bag中的数据团和.pbstream中的轨迹. 配置文件可以用来上色，过滤，导出SLAM点云数据成不同的格式。

逻辑

其实很简单，有三个阶段，筛选出 hit > 0 的 voxel，计算经过这些voxel的 rays-cast 次数，可以按miss集合理解，然后比较rays和hits数目关系

1
2
3
4
5
6
7
8
9
10
11
12
13

// 第二阶段
// 如果栅格之前的 hit>0，那么就 ray 次数增加
if (voxels_.value(index).hits > 0)
{
  ++voxels_.mutable_value(index)->rays;
}

// 第三阶段
// 如果点的 rays > 3*hits 那么就认为是动态障碍物
if (!(voxel.rays < miss_per_hit_limit_ * voxel.hits))
{
    to_remove.insert(i);
}

运行

assets_writer_backup_2d.launch如下

1
2
3
4
5
6
7
8
9
10
11
12

<launch>
  <arg name="config_file" default="assets_writer_backpack_2d.lua"/>
  <node name="cartographer_assets_writer" pkg="cartographer_ros" required="true"
      type="cartographer_assets_writer" args="
          -configuration_directory $(find cartographer_ros)/configuration_files
          -configuration_basename $(arg config_file)
          -urdf_filename $(find cartographer_ros)/urdf/backpack_2d.urdf
          -bag_filenames $(arg bag_filenames)
          -pose_graph_filename $(arg pose_graph_filename)"
      output="screen">
  </node>
</launch>

参考：如何使用Cartographer生成的地图

—-title: SLAM中的图优化公式推导
date: 2022-09-20 22:17:51
categories:

- 算法推导

这里可以联系cartographer的前端理解。

以下为ubuntu18版本的g2o对应的代码，和16版不兼容。TEB的常见安装问题就是16版到18版不兼容

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17

// 求解器一般有两种, TEB用的是 Levenberg
typedef g2o::BlockSolver<g2o::BlockSolverTraits<3, 3>> Block; // 每个误差项优化变量维度为3，误差值维度为1

Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>();

Block* solver_ptr = new Block(std::unique_ptr<Block::LinearSolverType>(linearSolver) );

g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(std::unique_ptr<Block>(solver_ptr) );

g2o::SparseOptimizer optimizer;
// setAlgorithm()需要传递一个优化算法
optimizer.setAlgorithm(solver);
optimizer.setVerbose(true);
optimizer->initMultiThreading();

// ......
optimizer.optimize(maxIterations); 	//开始优化，设置迭代次数

• static bool initMultiThreading();

Eigen3.1开始，要求we call an initialize函数，如果从不同线程调用Eigen
如果g2o基于OpenMP编译，而且Eigen版本高于3.1，则这个函数调用 Eigen::initParallel

LinearSolver

它用来求解线性方程$H\Delta X = -b$ ，可以从PCG、CSparse、Choldmod、Dense、Eigen选择求解方法。

• LinearSolverCSparse：使用CSparse法。继承自LinearSolverCCS

• LinearSolverCholmod ：使用sparse cholesky分解法。继承自LinearSolverCCS

• LinearSolverDense ：使用dense cholesky分解法。继承自LinearSolver

• LinearSolverEigen： 依赖项只有eigen，使用eigen中的sparse Cholesky 求解，因此编译好后可以方便的在其他地方使用，性能和CSparse差不多。继承自LinearSolver

• LinearSolverPCG ：使用preconditioned conjugate gradient 法，继承自LinearSolver

选哪个求解器很重要，有一次使用LinearSolverDense，结果260秒才迭代一次，换成LinearSolverCSparse，0.4秒就迭代一次

TEB是涉及到CSparse和Choldmod，默认使用前者

解析器solver的另一种创建方式

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17

// 加载几组 solver
G2O_USE_OPTIMIZATION_LIBRARY(pcg)
G2O_USE_OPTIMIZATION_LIBRARY(cholmod)
G2O_USE_OPTIMIZATION_LIBRARY(csparse)
G2O_USE_OPTIMIZATION_LIBRARY(dense)
G2O_USE_OPTIMIZATION_LIBRARY(eigen)


g2o::OptimizationAlgorithmFactory *solver_factory = g2o::OptimizationAlgorithmFactory::instance();
// 记录所有可用的 solver
std::ofstream fileout("/home/user/test_nodes/solvers.txt");
solver_factory->listSolvers(fileout);
// or lm_var_cparse
g2o::OptimizationAlgorithmProperty solver_property;
g2o::OptimizationAlgorithm *solver = solver_factory->construct("lm_var", solver_property);
std::unique_ptr<g2o::SparseOptimizer> optimizer(new g2o::SparseOptimizer());
optimizer->setAlgorithm(solver);

construct函数比上面的LinearSolverDense系列更灵活，设置求解器更精准。

使用listSolvers输出所有可用solver到文件，这个结果是和使用了多少G2O_USE_OPTIMIZATION_LIBRARY有关的，上面的5个应该涵盖了所有求解器，结果非常多。比如不加载G2O_USE_OPTIMIZATION_LIBRARY(eigen)，那么lm_var不可用，会报错 SOLVER FACTORY WARNING: Unable to create solver lm_var

listSolvers会输出所有求解器名称和解释，比如

1

lm_var    Eigen 	Levenberg: Cholesky solver using Eigen's Sparse Cholesky methods (variable blocksize)

优化结果

optimizer.setVerbose(true)后才能看到优化过程

或者g2o::SparseOptimizer::chi2()，优化前后各调用一次，可得到优化前后误差变化

1
2
3
4
5

iteration= 0	 chi2= 6973711.783675	 time= 0.228985	 cumTime= 0.228985	 edges= 3995	 schur= 0	 lambda= 42.450053	 levenbergIter= 1
iteration= 1	 chi2= 477444.019837	 time= 0.219176	 cumTime= 0.448161	 edges= 3995	 schur= 0	 lambda= 14.150018	 levenbergIter= 1
								......
iteration= 20	 chi2= 10344.665262	 time= 0.200148	 cumTime= 4.63974	 edges= 3995	 schur= 0	 lambda= 1.634041	 levenbergIter= 1
iteration= 21	 chi2= 10344.665262	 time= 0.461369	 cumTime= 5.10111	 edges= 3995	 schur= 0	 lambda= 112290463347.367386	 levenbergIter= 8

位姿 —-> 转换矩阵

1
2
3
4
5
6
7
8
9
10

Eigen::Matrix3d PoseToTrans(Eigen::Vector3d x)
{
    std::cout << x << std::endl;
    Eigen::Matrix3d trans;
    trans << cos(x(2)), -sin(x(2)), x(0),
        	 sin(x(2)), cos(x(2)), x(1),
        		 0,         0, 	    1;

    return trans;
}

转换矩阵 —-> 位姿

1
2
3
4
5
6
7
8
9

Eigen::Vector3d TransToPose(Eigen::Matrix3d trans)
{
    Eigen::Vector3d pose;
    pose(0) = trans(0, 2);
    pose(1) = trans(1, 2);
    pose(2) = atan2(trans(1, 0), trans(0, 0));

    return pose;
}

计算整个 pose-graph 的误差

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23

double ComputeError(std::vector<Eigen::Vector3d>& Vertexs,
    std::vector<Edge>& Edges)
{
    double sumError = 0;
    for (int i = 0; i < Edges.size(); i++) {
        Edge tmpEdge = Edges[i];
        Eigen::Vector3d xi = Vertexs[tmpEdge.xi];
        Eigen::Vector3d xj = Vertexs[tmpEdge.xj];
        Eigen::Vector3d z  = tmpEdge.measurement;
        Eigen::Matrix3d infoMatrix = tmpEdge.infoMatrix;

        Eigen::Matrix3d Xi = PoseToTrans(xi);
        Eigen::Matrix3d Xj = PoseToTrans(xj);
        Eigen::Matrix3d Z = PoseToTrans(z);

        Eigen::Matrix3d Ei = Z.inverse() * Xi.inverse() * Xj;

        Eigen::Vector3d ei = TransToPose(Ei);

        sumError += ei.transpose() * infoMatrix * ei;
    }
    return sumError;
}

照搬其他人的讲解。

方程个数 > 未知数个数，也就是”超定”线性方程组。方程个数比未知数个数多，可以想象不同方程之间会出现”冲突”的现象，也就是说导致整个方程组没有精确解。但我们只要设定一个误差标准，找到一个让这个误差标准值最小的近似解也是完全可以接收的。即针对超定不相容线性方程组，利用最小二乘误差标准求解近似值。

1.1 首先构建位姿之间的关系，即图的边

下面和开头的非线性最小二乘的推导一样了

问题：用最小二乘拟合直线，什么时候会失败？

答: 矩阵 $A^TA$ 不可逆的情况。 我的理解是把最小二乘按正规方程解法，也就是先获得 $A^TAx = A^Tb$，这是一个 n×n
的线性方程组，称为正规方程组。显然解为 $x = (A^TA)^{-1}A^Tb$

图优化SLAM通过减去原始传感器的测量，构建了简化的估计问题。这些原始的测量由图中的边取代，边可以看做“虚拟测量”(virtual measurements)。图中的两个节点之间的边是用两个位姿之间的相对位置的概率分布表示的，这一概率分布的条件是它们的相互测量。观测模型 $p(z_t | \ x_t, m_t)$ 是多模态的，因此高斯分布的假设不成立。这意味着一次观测 $z_t$ 可能影响图中连接不同位姿的多条边。 graph connectivity 应当用概率分布来描述。如果直接处理这个多模态问题，将出现极大的复杂度。我们需要将地图变成node和edge的拓扑结构。因此，需要确定不同node之间的edge约束。

我们应当判断一次观测导致的最可能约束，这取决于机器人位姿的概率分布。这一问题称作data association，常由SLAM后端解决。前端需要对机器人轨迹 $p(X_{1:T} \ | \ Z_{1:T}, \ u_{1:T})$ 上的条件先验进行一致估计，这就要求机器人运动过程中对前端和后端穿插地执行。因此后端的准确性和效率对SLAM系统非常重要。

如果观测值受局部高斯噪声影响，而且data association已知，基于图的建图算法的目标就是机器人轨迹的高斯估计后验。包括计算这个高斯分布的均值，然后以均值作为节点的configuration，这样最大化了观测的似然。

• 图优化中，顶点是优化项，而边是约束项。优化过程就是通过全局调整优化项，使约束项的和最小。
• Pose(x)与Landmark(m)构成了图的顶点集
• 边(Edge)存在两种。第一种是Pose到Pose，另一种是Pose到 Landmark

• 图优化中边表示约束，Pose到Pose的边与机器人的运动模型相关，机器人从$x_0$运动到$x_1$，形成了这条边。同时在运动模型的作用下，可以获得机器人在$x_0$，控制量为$u_1$时的状态 g。该值与“真实”的Pose 的差，构成了这条边的约束。由于边的约束是标量，因此取这个差的平方。R表示为运动模型的协方差。公式可以写作 $(x_1 - g)^T R^{-1}(x_1 - g)$

• Pose到Landmark的边与机器人的观测模型相关，机器人从 $x_{1}$观测到 $m_{1}$，就会形成一条边。同时在观测模型 $h_{(measure, Pose)}$ 的作用下，可以获得机器人在 $x_{1}$ 时对 $m_1$ 的预测 $h_{(m_{1}, x_{1})}$。该值与此时真正观测到的值 $z_{1}$ 的差，构成了这条边的约束。同样的，取这个差的平方作为约束值。$Q$ 表示为观测模型的协方差。公式 $[z_1 - h(m_1,x_1)]^TQ^{-1}[z_1 - h(m_1,x_1)]$

• 完整的约束为两种约束的和 $J_{GraghSLAM}$

SLAM的图优化问题就是寻找合适的 Pose(x), Landmark(m)，使 $J_{GraghSLAM}$ 最小。

看到预测值和观测值的差，很容易想到Cartographer，PoseExtrapolator提供预测值，Scan Matcher提供观测值。

或者可以这样理解：


我们用$e(x_i,x_j,z_{ij})$ 表示预测值和观测值之间的差，假设其满足高斯分布 $N(0, \Sigma)$，或者说观测值的分布是以真值为中心的多元高斯分布。 我们的目标是让预测值和观测值尽可能趋近于0（均值），也就是让 $e$ 出现在概率最大的地方，这就是个最大似然的问题。 我们让概率密度函数取最大值

把图中的x换成 $e(x_i,x_j,z_{ij})$， 均值取0，得到下面的最小二乘形式

最小二乘问题的目标函数的变量维度很高，但每个误差项都是简单的，仅与少量变量有关。因此为了直观的展示变量与变量之间的关系，可以把优化问题用图的方式来展现。其中顶点表示优化变量，边表示误差项。

图优化的流程

1. 选择你想要的图里的顶点与边的类型，确定它们的参数化形式

2. 往图里加入实际的顶点和边

3. 选择初值，开始迭代

4. 每一步迭代中，计算对应于当前估计值的雅可比矩阵和海塞矩阵

5. 求解稀疏线性方程 $H_k \Delta x = -b_k$ ，得到梯度方向

6. 继续用高斯牛顿法(GN)或列文列文伯格-马夸尔特法(LM)进行迭代。如果迭代结束，返回优化值。

SLAM中的图优化

滤波类算法最大的问题，是无法处理大尺度场景的建图。由于滤波类算法是基于递归计算的，下一时刻的估计值依赖于上一时刻的估计，因而在大尺度场景下，由于系统参数和传感器观测数据的不准确性，会造成误差慢慢累积，一旦当前时刻的估计出现偏差，无法修正该误差，最终无法获得一致性的地图。

另外滤波方法对计算量和内存消耗都很大，使用图优化方法则效率较高，这种方法位姿不是实时纠正，而是将数据记录下来，最后进行计算优化。

机器人的位姿是一个顶点(vertex)，位姿之间的关系构成边(edge)，边表示两个位姿的空间约束(相对位姿和对应方差)。比如t+1时刻和t时刻之间的里程计关系构成边，或者由视觉计算出来的位姿转换矩阵也可以构成边。图构建完成后，就要调整机器人的位姿去尽量满足这些边构成的约束。图优化就是构建图并调整各顶点的位姿，使预测和观测的误差最小。每一条边的信息矩阵就是协方差矩阵的逆。协方差越小，说明这次测量越准，信息权重就越大。

前端： 从里程计和激光雷达数据中求得位姿，构建图。机器人位姿当做顶点，位姿间关系当做边，常常是传感器信息的堆积
后端： 在图构建好以后，采用回环检测，出现了回环边说明有误差。构造闭环约束，调整机器人位姿，通过最小化观测和估计残差求得优化后的位姿。

通过帧间匹配得到 $X_8$ 和 $X_2$之间的相对位姿，称为观测值

观测方程也有很多形式，如：

• 机器人两个Pose之间的变换；
• 机器人在某个Pose处用激光测量到了某个空间点，得到了它离自己的距离与角度；
• 机器人在某个Pose处用相机观测到了某个空间点，得到了它的像素坐标；

同样，它们的具体形式很多样化，这允许我们在讨论slam问题时，不局限于某种特定的传感器或姿态表达方式。

这个减法只是通用的，在SLAM里可能没有定义减法。

算法流程

• 初始化

首先定义两个粒子集，开始都是空集，一个用于重采样缓冲的set_a，一个是最终需要的set_b，对每份粒子集合创建kdtree。算法的目的是用粒子集合来表示置信度。

先在AMCL里给定一个初始位姿值和协方差矩阵，粒子数为M。一开始，每一个粒子的权重值都是1/M，位姿的分布是一个高斯分布，每个粒子的初始位姿记做(x,y,theta)，其均值是AMCL的初始位姿，方差是从协方差矩阵获得的。将每个粒子插入到kdtree中，对粒子滤波器的粒子集进行聚类。

• 运动模型

概率模型是 $p(x_t\ | \ x_{t-1},\ u_t)$， 使用里程计模型进行估计。对每一个粒子更新位姿。位姿是从运动模型中采样获得的，机器人从t-1时刻运动到t时刻，获得相对运动参数rot1,trans,rot2

最终，对于每个粒子的位姿， x加上相对平移量在x轴的分量(乘以cos), y加上相对平移量在y轴的分量， theta加上两次旋转的参数。这些分量除了跟rot1,trans,rot2有关，还跟AMCL中的四个运动噪声参数alpha1~alpha4有关

• 测量模型

概率模型是 $p(z_t\ | \ x_t)$，使用似然域模型，用于更新每一个粒子的权重。

似然域测量模型认为测量的结果包含一个高斯分布噪声和随机噪声。激光测量距离（x,y）变换到map坐标系。找到距离激光点最近的障碍，查表获得它们的最近距离dist。这个dist也是高斯噪声的均值，所以dist越大，打到障碍物的可能性越小，则粒子不可信，权重越小。

$\omega = \prod_{i=1}^N \left\{ z_{hit}*prob(dist_i, \sigma_{hit})+\frac{z_{rand}}{z_{max}} \right\}\ \qquad \qquad$ N为一帧激光的点数

似然域测量模型比较理想，因此在场景有些变化，和动态人存在干扰的情况，估计的pose精度会下降。放大测量方差或许会有效果，最好要是能引入类似非线性优化中的核函数（损失函数）或许不错。

• 失效恢复的准备

把每个粒子添加到临时粒子集中，也就是说set_a内的粒子的权重是不同的，更新粒子集的平均权重。根据alpha_slow和alpha_fast计算短期和长期的似然平均

• 重采样

从缓冲的粒子集中替换所有粒子，抽取粒子的概率取决于粒子的权重，重采样之后，粒子的数量不变，权重较小的粒子将被过滤掉，权重较大的粒子将被复制

上面的算法还有两个问题： 1. 粒子重采样的退化问题；2. 固定的粒子群导致无用的计算，影响效率。
每个粒子集合中包含的粒子数量很大，粒子的权重即使在多次重采样之后还是很小，导致位置上相邻的粒子的权重差值不大，从而最后输出的预测位姿不准确。

因此AMCL加入了新的结构体 Cluster，表示相邻粒子的集合，具有权重与位姿等属性，与粒子相似。而其位姿是通过该 Cluster中包含的粒子的位姿加权平均得到。开始时 Cluster的数量与粒子数量相等，即每一个 Cluster中只有一个粒子。当粒子在地图上产生聚合后，Cluster的数量就会下降，此时选取权重最大的 Cluster 作为输出结果。

使用KD树存储粒子信息，并且进行聚类分析。树的叶子节点是每一个粒子，而非叶子节点则保存的是该节点的分叉判断的标准。生成树之后，进行粒子聚类，循环遍历每一个叶子节点，在叶子节点的key值的 ±1 范围内的粒子都归为一类。也就是手，每个粒子周围27个粒子若有相连的，就将它们全部连起来，形成聚类。

处理每一个粒子，先生成一个(0,1)的随机数rand，如果 rand< w_diff = (1 - w_fast / w_slow)，那么当前粒子的位姿是随机值。

把上面找出的所有粒子添加到set_b，set_b的粒子个数是M，多数粒子是原来set_a中权重比较大的粒子，而且可能重复。让set_b中的粒子权重全是1 / M

AMCL定位不准的3个依据:

1. 协方差矩阵的对角线元素小于阈值
2. 当前粒子群的个数大于阈值
3. 收敛标志 pf->converged 为false

AMCL用到的算法:

• sample_motion_model_odometry
• likelihood_field_range_finder_model
• Augmented_MCL
• KLD_Sampling_MCL

梯度

梯度是一个矢量，曲面上每点的梯度是常数。

曲面中点的方向导数有无数个，当方向导数与梯度方向一致时，该导数值取得最大，等价于该点在梯度方向具有最快的变化值。梯度方向是函数值增加最快的方向，梯度的反方向是函数值减小最快的方向。

参考：如何直观形象地理解方向导数与梯度以及它们之间的关系？

海森矩阵

Hessian矩阵是半正定的

对一元函数f(x)来说，就极值而言，一阶导数为0是极值点的必要但不充分条件，一阶导数为0且二阶导数大于0是极小值的充要条件。用二阶泰勒展开就能理解。

对于多元变量，二阶高斯公式展开如下：

$H$ 作为海森矩阵，也可以写作$\nabla^2 f(x_0)$，可以理解为把梯度向量推广为二阶形式，梯度向量本身也是Jacobian矩阵的一种特例。

把$(x-x_0)$写作 $\Delta x$ ，类似一元函数，我们希望二次项 $\Delta x^T H \Delta x \ge 0$对任意的$\Delta x$ 成立，这就等价于 $H$ 半正定。

当$\nabla f(x_0)=0 \ 且 \ \Delta x^T H \Delta x > 0$时，也就是海森矩阵正定，$x_0$是$f(x)$的局部极小点。但这是充分不必要条件，有的点x可能是极小点，但该点的海森矩阵不是正定的。

但是当$\Delta x^T H \Delta x = 0$时，无法判断此点是否为极值点，所以牛顿法及阻尼牛顿法的缺陷就在这里，因为$H$是半正定的，所以不能保证每次都能收敛到极小值点。

海森矩阵正定，则函数为凸函数

凸函数$f(x)$的任何局部极小点 $\hat x$ 都是该函数的一个全局极小点。