https://blog.csdn.net/weixin_36773706/article/details/122440411第二部分：第三方库应用
miniSAM库：提供了python和numpy的API，提供了线性代数求解器（包括QR分解）
ceres，g2o：通用的线性最小二乘库
gtsam：因子图优化库
给定一个位姿图：
第一步，建因子图
首先要定义传感器的噪音模型，因为每一个因子对应的是观测量，其概率模型是传感器概率模型来决定的。
第二步：加因子
先验因子：固定求解位置。
具体编写：graph是一个容器，容器中加了先验因子或其他因子。构建每个因子过程中，有三项主要输入，第一项是所连的状态量，表示办法是key（一个数字加一个字符）；第二个是观测量，传感器实际给出的测量值。第三个是误差函数的分布（传感器模型）。
对于先验因子：是实际的位姿减去观测的位姿。
对于里程计因子：比如X2 - X1- Z，即实际的里程计值减去观测的里程计值。
然后还要给变量初始化值，因为用的是迭代法求解，有初始值很重要， 在minisam中有一个容器存储初始值， 告诉求解器需要从这个值开始求解。
第三步：选择非线性最小二乘求解器，或线性求解器，如QR分解、Cholesky。
另外：考虑新的传感器，如相机、雷达等自定义传感器模型的自定义因子如何做？
定义一个类来定义error function，输入是系统状态（给定系统状态），返回误差值。
误差函数的J矩阵，如果有求解好的解析式就直接放上去，或者通过数值差分求解J矩阵。ceres支持自动微分求解。解析解比较快，数值解比较慢。
minisam速度跟ceres g2o 差不多，比GTSAM快一些。同时为方便做迁移，还提供了python接口。
总结因子图优点：求解速度快，编程方便。
第三部分：实际应用
四维重建在农田中的应用和如何用因子图解决机器人运动规划的问题。
四维重建使用因子图的思路：
即农业场景作物监测中，如何用因子图加速重建的过程。
四维重建希望得到精确的时序信息，这涉及如何将三维重建应用到连续变化的持续信息中，这就引出了四维重建，加上了时间维度。通过GPS或者点云配准得到的坐标系是相对不准的。如果若干三维重建在同一个坐标系，那么不同时间的三维空间的同个点位置都是一样的，就可以得到在同个点上三维重建的时序信息。
实际上第一步做的是三维重建，包括前端和后端，前端是原始传感器的处理，做点云匹配，然后前端数据送给因子图的后端，后端包括了若干种因子，其中包括Visual landmarks by structure-less vision factor （路标点因子）。
因为空间中有很多点，单独求解太消耗内存，所以就引入了路标点因子，做到不需要求解路标点，就可以求解系统状态。求解了系统状态后再求解路标点，这样节省内存，不需要显性存在路标点。
总结四维重建：建立若干三维重建在空间中的关系，利用因子图去优化，得到空间中连续时间的四维重建。假如给定两个三维重建，找某个空间中的三维点，用图像特征点匹配的方法，将两个三维重建的三维点进行匹配，匹配后只剩下一个点，就是空间中同个三维点，为公共的目标点，通过这种办法将两个目标点合并成一个点， 变成公共的因子，就将两个三维重建合并为一个四维重建，类推可以建立出大的四维重建，对因子图进行整体优化， 通过公共的坐标点将所有的三维重建限制在了一个坐标系下。三维重建中例如ICP点云匹配是有误差的，每一组点云都会有误差，直接匹配的话是会匹配不上甚至会有变形的， 如果去做四维重建的话，因为有对应的公共路标点去约束，再去做四维重建，这样也会有误差，但是通过公共路标点的限制下误差是相同的，因为是在公用的大的因子图里面去优化，得到的误差也是一样的。
第二个案例，运动规划。motion planning in robotics
因子图有比较好的特性，在其他领域也有应用，比如可以来加速运动规划的过程。
一个运动规划问题：有一个机器人在有障碍物的环境里，知道初始位置和障碍物，希望到达一个位置，找到一个轨迹（避开所有障碍物），不是实时做规避，是用优化或采样的办法提前给出轨迹。
其轨迹特点和要求：1.不能跟障碍物有碰撞（feasibility）
2.没有碰撞的情况，尽可能最优（时间最短，路径最短，能量最低，尽量平滑）
两类方法：1.采样法 2.优化法：速度快
套用因子图定义的联合概率分布求解一个最大概率，来实现运动规划。MAP最大后验分布包括两部分，prior 分布（先验,对应第二个要求:最优，尽可能平滑），likelihood似然分布(对应第一个要求，不能有碰撞，相当于SLAM里面的观测量，就是没有碰撞的概率尽可能高）。乘起来求解最优化问题，就是求解运动规划问题。即希望离障碍物越远，碰撞的概率越低。
计算过程中将机器人的形状用若干球体去近似。球体与空间中点的距离：点与球心的距离减去半径。这个机器人碰撞的概率就是若干球体碰撞的概率的乘积。对于轨迹的圆滑，用的是高斯过程先验GP prior，是定速先验值（整个过程速度变化小，加速度最低，则施加的力是最小的，最优的一个情况）。对于每一个时间状态，每一个时间节点计算与环境中物体碰撞的概率，两个节点间计算加速度最小高斯过程先验值，加起来就是大的因子图去优化，得到的就是最优的没有碰撞的轨迹。
但优化办法常遇到的问题是遇到局部最低值（最优），很难前进到全局最优，卡在一个有碰撞的地方，这个时候就优化失败了
因子图的扩展：如何将增量优化扩展应用到重规划Replanning的问题中， 假设有初始位置和目标位置，规划了一个轨迹，执行到中间，换了目标位置，需要重新做轨迹的规划。如果将这个问题构建成因子图的话，重规划的问题相当于只改变了两个因子， 包括目标节点因子和当前状态的因子（还要加一个当前状态的约束，作为先验），其他因子不需要变，实际上优化的是变化量非常小的一个图，这给使用增量因子图推理很大的空间。如果用isam处理少量变化的图，比从零计算后半段的图效率更高。用贝叶斯树的办法， 如果只改变最后的节点和加一个当前位置的约束，贝叶斯树只是少量的变化， 需要改变的位置越靠后，需要重新规划的轨迹越短，贝叶斯树改变的越少，计算量越低。因子图只有少量的变化，增量规划可以获得大的提升。
其他问题：
1.对于概率来讲，在贝叶斯网络， 这些概率（有效的概率）的积分加起来是1（满足概率为1的要求）。而因子是相乘起来， 对于因子没有概率为1的要求。
2.实时做运动规划，地图在实时变的话， 目标函数也要实时变，J矩阵的每一个error在每个时间点上需要重新求，要有实时的因子图优化过程。
3.GP指的是高斯过程。GP factor约束的是两个系统状态之间的加速度。速度差越大，GP prior产生越大的error， 概率就越低， error function 的cost就会增加。
4.双机协同SLAM因子图DDF-SAM：属于分布式因子图， 每个机器人处理自身的数据，处理自身的因子图，有个公共图，处理发送过来压缩过的数据。
而集中式因子图就是若干个机器人产生的因子汇总到一个大的后端服务器里面进行优化，有一个整体的大的图。
5.一直增量求解的矩阵会越来越密，需要reordering才能保证性能。
6.因子图求解的过程转换成一个最小二乘，这只是描述问题的一种方式，将其描述成了概率问题。在因子图里面只需要定义传感器的概率特性，而计算和其他优化器没有什么区别。贝叶斯树是因子图的一种表示形式。贝叶斯树是树状结构，如果只增加了少量变量，只需要增加树的少许节点。
7.因子图本身表示的是J矩阵，是分解前的形式，而贝叶斯树表示的是分解后的形式，分解后的R矩阵对应的是树状结构。贝叶斯树存储的是分解后的因子图，只增加了少许变量，对应的R矩阵也增加了一点点，反映到树上也是少许。而对于回环问题，整个R矩阵都要改变，贝叶斯树也一样。
8.因子图并不适合所有的问题，有内存随着时间增大而消耗增多的问题，单纯的IMU用卡尔曼滤波可以解决。
9.对于整个图来讲，先验信息是第一个时间的信息或者最后一个。对于运动规划来讲，先验信息是整个过程的加速度最小的信息，而SLAM中是某个时间的信息。
10.误差函数用高斯模型是方便数学处理，假设是指数函数会比较简单，实际上可以用鲁棒的模型。用高斯模型会比较直观。传感器噪音模型的单位跟测量值的单位是一致的，跟协方差也是一个单位。
11.如果内存小可以用滑窗优化，增量优化需要保存所有数据（存在树里面，随着图逐渐增长，图是在逐渐增长的），滑窗优化随着优化滑窗的过去，数据就边缘化掉了。
增量优化的优点是考虑历史信息，精度更高，但内存也消耗更大。在很大回环的情况下，增量优化求解可能会有很大的问题。
滑窗优化优点：低计算量，低内存，没有历史信息
12.根据需求选择合适的求解器方案，设计好对应的因子（建模包装成因子图的形式），多读论文，熟悉这种思维方式。
