德国bonn大学因为疫情的影响转为网上授课，著名的Cyrill Stachniss教授也把自己的Sensing and State Estimation 课程放到了网上。
可惜没有课件，只能看一点写一点笔记。
背景
基于图的SLAM是目前最流行的SLAM。
假设我们有一个机器人在房间里面走，每隔一段时间或者每隔一段距离我们记录一次机器人的位姿。
机器人从previous position走到了current position，我们可以说，pose 2 is probability one meter away from pose 1.
The constraint is a soft constraint. Because we are not certain about that.
每两个node之间的constraint 可以从Odometer，Scan-match, IMU等获得。
当某个时刻，机器人重新访问到之前已经去过的地方（Loop Closure），那我们除了之前sequencely的constrains之外，还能获得当前node跟不相邻的历史node的constraints。（Bundle Adjustment？）
在图中，每一个node是一个pose，每一个edge是node之间的constraint。
这些constraints之间是可以互相冲突的，目标就是找到a node configuration that minimizes the error introduced by the constraints。
下图是一副用odemetry和laser创建的地图，其中的黑线表示连接着nodes的constrains。
后端优化时不需要考虑地图，所以把地图去掉来看，
利用最小二乘法minimize error之后，这些重复的轨迹会靠近，接近于重叠到一起。
基于这些修正过的位姿信息，我们再建图。
这就是后端优化过程。
概念
假设节点X_i 观察到了这个角落，之后某个节点X_j也观察到了这个角落
我们把相同的观测align到一起，可以得到一个虚拟的观测 X_ij 即 X_j 相对于 X_i 的位姿。
当然我们的measurement/constraints/whatever都是有噪声的
同时我们用一个information matrix Omega_ij 来表示这个constrains的uncertainty。information matrix相当于是constrains的权重。(Information matrix?缓缓打出一个？)
（这个information matrix好像根据不同的sensor，不同的档次和不同环境是不一样的。）
如下图，x_i观测到的x_j的位姿有一个variance，（还有一个information matrix Omega_ij, which is the inverse of convariace matrix）
这个观测Z_ij跟真实的Xj有一个error，即e_ij(xi,xj)
优化目标就是优化所有的error。
构建误差方程-The Error Function
For single constraint
其中
是x_j referenced w.r.t. x_i 而
是measurement 即 how x_i see x_j（通常是ICP结果）。（完美情况应该就是Identical咯）。
t2v is “transform to vector”， a vectorized representation from homogeneous representation。因为我们要把error表示成vector形式。
Gaussian-Newton : The Overall Error Minimization Procedure
高斯牛顿迭代求近似最优解啦这里就是。
一般来说，我们要求error function对于每一个variable的partial derivative。
但是我们的error function 真的 depend on all state variables吗？
当然不，我们这里的single error_ij只跟x_i x_j相关。
这样的话，我们的Jocabian Matrix就会有很多地方是0，除了x_i x_j相关的行列。
我们Jocabian 的稀疏性就会影响到后面要用的b矩阵和H矩阵
当我们把所有的b和H累加到一起之后呢，大概长这样了。
有稀疏性就很好用咯（就真的很像Bundle Adjustment ，其实我没看懂.jpg）
简单例子
然后这里举了一个1维的例子
假设有node 和 node
还有观测
initial status
observations
information matrix how certain we are about that
the error function is our actual observations minus our predicted observations
Jacobian (derive the error function with respect to each variable.)
then we have b vector
then we have the H matrix
finally the update amount
但是咱们这里det(H)=0, 矩阵不能invert。
因为，这里我们的constraint是relative constrain，我们只知道node2相对于node1有1米远。
但是我们不知道node2具体在哪。
要解决这个问题，我们就要假设node1有一个固定值0啊1啊随意。
我们跟系统说，node1不需要更新！
（有点像高斯分布给一个先验，我们这里给一个先验，prior node）(但是为什么是这个位置设置为1)
(我算过，如果固定node2不需要更新，最后delta X就是(1 0 )^T
(0 0)一个更新(0 1)的到(0 1).符合观测。
Prior的意义
只要我们没有fix reference frame。比如GPS。我们就会一直有这个问题。
所以一般我们需要设置一个node作为reference frame（again bundle adjustment既视感。）
Suppress the H matrix
当我们决定有一些状态量不再需要更新的时候，比如不想被其他的constraint给拉偏了，我们要怎么做？
（实际上应该办不到，要更新就全部更新了，所以就要kick them out of the system？）
具体来说就是把H矩阵里面对应的行列cut out，那自然就更新不到那里拉。
Uncer
这个可以用来告诉系统，现在我的机器人在这个位姿，相对于其他位姿，我跟谁比较接近，可以用来找loop closure
Conclusion
graph slam 只考虑pose,而不像一般SLAM还要考虑land mark。（从这个角度来看就是一个全局姿态优化的bundle adjustment？好的，再次感谢当年安排我做bundle adjustment的老大。）