slam中线和面特征的参数化表示方法

1 线特征的参数化表示：

1.1 Plucker坐标系表示法

普吕克坐标系使用两个向量表示直线为：

其中：

n为直线的方向向量，模长为1。

d为线与原点组成的平面的法线。可以用原点到线上一点的向量与n叉乘得出。

d与n的模的比值为原点到直线的距离。

1.2 正交表示法

使用Plucker坐标系表示在优化时不方便进行数据处理，所以要将其形式转化一下。

Plucker坐标系表示：

可以写成一个3×2的矩阵，对矩阵做QR分解得到：

分解得到的第一项是正交矩阵U ，是一个旋转矩阵。所表示的是相机/激光雷达坐标系到直线坐标系的旋转。直线坐标系的三个轴方向分别顺序对应这三项。

对于第二项，由于为直线到原点的距离，只有一个自由度，所以可以用三角函数矩阵参数化：

 因此，对直线参数进行更新时，就只需要更新第一部分的3个参数和第二部分的角度值。即为四个参数的正交表示：

1.3 两点表示法

给定两个点M和N，有：

 其中和表示原点到点的向量，m和n表示距离。则根据Plucker坐标表示有直线mn方程为：

 1.4 交面表示法

给定两个面有：

  其中和表示面的法向量，m和n表示原点到面距离。则根据Plucker坐标表示有直线mn方程为：

 2 面的参数化表示

2.1 Hesse形式

一种最常见的参数化形式，用一个平面的单位法向量和平面距离原点的距离来表示：

 其中 为平面上的一点。同时，此种表示形式左乘一个变换矩阵也可以进行坐标的变换。

 注意：

此种表示形式有4个参数，但平面只有3个自由度，存在过参数化问题。

在使用高斯牛顿优化时，计算出的Hessian矩阵会不满秩，也就不能求逆。所以只能使用列文伯格-马夸尔特算法(L-M)提供的正则化(regularization)。

也会使得代价函数在某些方向上为平面时的优化收敛速度有二阶收敛变为线性收敛。

还不能使用信赖区域法。

2.2 球坐标表示

基于上述的平面表示法，由于法向量部分为3个参数却只确定2个自由度，所以改为使用求坐标表示，化为两个参数，分别为xy平面上的θ和与z轴的夹角φ。面的最终表示形式如下：

g2o中实现了此种表示形式。 但当φ为±90°时，会产生奇异性。导致雅克比的秩从3降为2.

2.3 切平面表示

使用原点到切点的向量加上平面上一对任意正交的单位向量乘以两个参数。如下：

 表示法方向上的自由度，a1和a2表示面上的2个自由度，但此方法只适用法向量的微调。

2.4 最近点表示

此方法可以直观的表示出面的法向量和距离原点的距离。但在d为0时具有奇异性，所以要转换到不会有奇异性的坐标系之下。

2.5 单位四元数表示

 与Hesse形式相似，只是加了一步单位化。

2.6 退化二次曲面表示

用一种参数化形式同时表示点线面的参数化形式。这样的优点是可以构建出点线面之间的约束。如下：

其中 是二次曲面上的点，是二次曲面的原点。A是3*3的对称矩阵。

对A做一个特征值分解：

其中R的每一列都表示二次曲面的一个轴， 是一个特征值的对角矩阵。

这样，点就可以看成一个半径为0的球，无为0的特征值；线就可以看成一个半径为0的圆筒，一个为0的特征值；面可以看成两个匹配上的面，2个为0的特征值。

根据上述3个参数，所以任意点线面特征可以表示为：

 特征之间的转换关系为：

 2.7 其它防止过参数化的方法

其核心思想都是讲4个参数化为3个参数 。

参考：

https://zhuanlan.zhihu.com/p/149028042

 https://zhuanlan.zhihu.com/p/71924149