论文阅读：Simultaneous localization and mapping with infinite planes

面特征论文学习

论文题目	Simultaneous localization and mapping with infinite planes
创新点	最小化特征参数适用于GN,PDL等
重点阅读	Section III :mapping with infinite planes
段落结构

必备的基础知识

李代数李群相关内容
3D空间中平面的表示
描述面的参数化在3D空间中，平面的表示方法：\pi_1X+\pi_2Y+\pi_3Z+\pi_4 = 0
空间点如果使用的是齐次坐标的形式表达为\pi_1p_1+\pi_2p_2+\pi_3p_3+\pi_4p_4 =0
把齐次表达的最后一项换成1，\pi_1\frac{p_1}{p_4}+\pi_2\frac{p_2}{p_4}+\pi_3\frac{p_3}{p_4}+\pi_4\frac{p_4}{p_4} =0
更换成单位法向量表述(这时平面参数化表达有4个参数)
\mathbf{n}^{T} \mathbf{p} = d
面与面之间的转换矩阵是
\pi_w = {}^{w}\mathbf{T}_{x}^{-T}\pi_x

如何更新传感器的位姿

传感器的位姿表达，使用平移向量和四元数来表示（我们这里可以拿相机举例）
x=(\mathbf{t}, \mathbf{q}) \in \mathbb{R}^{3} \times S^{3}
在局部坐标系下的点p_x转换到世界坐标系下表示，p_w
\mathbf{p}_w={}^{w}\mathbf{T}_{x}\mathbf{p}_x
转换矩阵中的旋转矩阵是可以从四元数表达中转换得到的

Rodrigues’ formula用来做转换，旋转向量与旋转矩阵的转换关系。我们使用旋转向量的Rodrigues’ formula来更新旋转矩阵
\exp \left([\boldsymbol{\omega}]_{\times}\right)=\mathrm{I}+\sin \|\boldsymbol{\omega}\|[\hat{\boldsymbol{\omega}}]_{\times}+(1-\cos \|\boldsymbol{\omega}\|)\left(\hat{\boldsymbol{\omega}} \hat{\boldsymbol{\omega}}^{\top}-\mathrm{I}\right)
另外一种更新是使用四元数更新，

我们使用上述公式更新四元数,S^3代表由单位四元数组成的球形表面空间。\mathbf{q}^{\prime}=\exp (\boldsymbol{\omega}) \mathbf{q}

我们正常的写法e^{\omega}=\left[\sin \left(\frac{1}{2} \theta\right) \hat{\omega}, \cos \left(\frac{1}{2} \theta\right)\right]^{T}论文里面使用了辛格函数来表示。

另外我们还有一个inverse of the exponential map ，log map，用来算两个平面的距离
\theta=2 \cos ^{-1}\left(q_{w}\right)
\hat{\mathbf{\omega}}=\frac{1}{\sin \left(\cos ^{-1}\left(q_{w}\right)\right)}\left[q_{x}, q_{y}, q_{z}\right]^{\mathrm{T}}
这里我们使用q_v^2+q_w^2 =1 和 sin\theta^2+cos\theta^2 =1可以化简成论文中提到的形式。

在四元数指数映射旋转参数化的实际应用(Practical Parameterization of Rotations Using the Exponential Map)这篇论文中有提到

在实验结果中指数映射法相比较于欧拉角和四元数法,有以下优势:更强的鲁棒性,小向量可以被更好的表达,无需明确过多的限制条件,更好的建模能力,ODE简化和更好的插值性能.

论文里面还有评价各种参数化好坏的一个标准。

利用四元数去优化平面方程构建误差一般采用以下形式:
\begin{aligned}\mathbf{e}\left(\mathbf{x}_{G L}, \mathbf{z}_{\text {Global }}\right) &=\mathbf{q}\left[\mathbf{T}\left(\mathbf{x}_{G L}\right) \mathbf{p}\left(\mathbf{z}_{\text {Local }}\right)\right] \ominus \mathbf{q}\left(\mathbf{z}_{\text {Global }}\right) \\ &=\log \left\{\mathbf{q}\left[\mathbf{T}\left(\mathbf{x}_{G L}\right)^{T} \mathbf{p}\left(\mathbf{z}_{\text {Local }}\right)\right]^{-1} \mathbf{q}\left(\mathbf{z}_{\text {Global }}\right)\right\}\end{aligned}

SLAM中面特征的参数化

SLAM中面特征的参数化https://zhuanlan.zhihu.com/p/71924149,首先来个感性的认识

SLAM 建立优化模型

我们有解决的待优化变量有传感器的位姿，senor poses x_0,x_2,x_2...,平面路标\pi_1,\pi_2 ...\pi_m

这里我们的估计模型使用的是因子图的形式，Factor graph 的入门教程可以看

PPThttps://pan.baidu.com/s/1JkimXEm9weuPTDPoFhMENQ
[iSAM2 笔记:()
这里要寻找使得概率最大化的待优化变量
\Theta^{*}=\underset{\Theta}{\arg \max } \prod_{i} f_{i}\left(\Theta_{i}\right)
一般假设Gaussian measurement model，factor graph 目标函数定义取对数会转变为求最小值
f_{i}\left(\Theta_{i}\right) \propto \exp \left(-\frac{1}{2}\left\|h_{i}\left(\Theta_{i}\right)-z_{i}\right\|_{\Sigma_{i}}^{2}\right)
\underset{\Theta}{\arg \min }(-\log f(\Theta))=\underset{\Theta}{\arg \min } \frac{1}{2} \sum_{i}\left\|h_{i}\left(\Theta_{i}\right)-z_{i}\right\|_{\Sigma_{i}}^{2}
我们这里更新优化变量的方式要找到measurement Jacobian,planar measurement 模型很重要。
本质上面和点都是一样的，只是我们要使用对应的转换矩阵才行。
今天的残留问题

算法评估和误差分析

开发某类变换的估计算法需要相应的性能测试（真实的数据或者人工拟合的数据）
注意负号的区分

• 测量得到的量
• 被估计得到的量
• 量的真值加横表述

参考文献

[1]M. Kaess, “Simultaneous localization and mapping with infinite planes,” 2015 IEEE International Conference on Robotics and Automation (ICRA), 2015, pp. 4605-4611, doi: 10.1109/ICRA.2015.7139837.

[2]F. Grassia, “Practical parameterization of rotations using the exponen- tial map,” J. Graph. Tools, vol. 3, pp. 29–48, Mar. 1998.http://www.doc88.com/p-101878246674.html

英文	中文释义
DOF(degree-of-freedom)	旋转自由度
ODE(ordinary differential equation)	常微分方程
transformation matrix	变换矩阵(变换一词包含:平移,旋转,放缩,甚至仿射)
S3	单位四元数组成的球形表面空间
SO3	四维空间中的三维子空间集合
normalize	标准化,通常变模为单位长度1
end-effector	机械臂末端-与环境交互部分
singularity	奇点
hierarchies	层级结构

论文内容提及到旋转矩阵(9个参数，要加6个约束，三平移三正交)，欧拉角（gimbal lock，缺失一个自由度），四元数参数化的优劣评价
[3]http://www.euclideanspace.com/physics/kinematics/angularvelocity/QuaternionDifferentiation2.pdf
四元数求导问题
[4]四元数指数映射旋转参数化的实际应用(Practical Parameterization of Rotations Using the Exponential Map)

[5]M. Kaess, H. Johannsson, R. Roberts, V. Ila, J. J. Leonard, and F. Dellaert, “iSAM2: Incremental smoothing and mapping using the Bayes tree,” The International Journal of Robotics Research, vol. 31, pp. 217–236, Feb. 2012.