catia怎么将特征参数化_VSLAM中特征点的参数化表示

VSLAM中特征点的参数化表示有很多，最直接的是用三维坐标XYZ来表示，但通常大家更喜欢用逆深度表示，因为逆深度优势在于能够建模无穷远点。Open VINS文档中给出了五种特征参数化表示：Global XYZ，Global Inverse Depth，Anchored XYZ，Anchored Inverse Depth，Anchored Inverse Depth (MSCKF Version)，区别在于：

Global vs Anchored：特征点的表示是全局坐标系的坐标还是局部相机坐标系的坐标。
XYZ vs Inverse Depth：使用的XYZ还是逆深度
Two different Inverse Depth：两种不同类型的逆深度参数

特征的参数化表示即以何种方式表示特征点的位置，在优化中它决定了特征以何种参数进行的迭代更新

，在EKF中它决定了以何种参数构建高斯模型

。不论在优化还是EKF中，我们关心的都是特征在图像上的投影与特征参数之间的关系（Jacobian）。

本文理论推导全部参考Open VINS，感兴趣的可以直接阅读原版，链接如下：

Measurement Update Derivations " Camera Measurement Updatedocs.openvins.com

1. 特征点重投影过程

设k时刻特征在图像上的投影为

，特征参数为

，特征参数到图像投影的转换过程为：

特征参数转换为特征点的全局坐标：
特征点的全局坐标转到观测相机坐标系：
相机坐标系投影到normalize平面：
normalize平面转到图像平面：

根据链式法则有：

下面分别计算每一步的转换关系及Jacobian。

a. normalize平面转到图像平面

对于Normalize平面上的观测点

，先畸变得到畸变观测点

，再通过内参矩阵转成图像平面上的观测点

。

转换关系：

对于Radial畸变模型：

对于Fisheye畸变模型：

Jacobian：

对于Radial畸变模型：
对于Fisheye畸变模型：

b. 透视投影

转换关系：

Jacobian：

c. 刚性变换

转换关系：

Jacobian：

2. 特征参数表示

针对不同的特征参数表示

，都先将其转换成世界系下的XYZ，即

，再投影到图像上。这样做的好处在于

到

的转换关系和Jacobian保持不变，只需要构造不同的

并推导

，就能很方便求得

与

的关系。

a. Global vs Anchored

Global XYZ表示为：

，很容易写出Jacobian为：

Anchored XYZ表示选择某个相机坐标系作为Anbchor坐标系

，用特征在

下的位置作为参数表示，Anchored XYZ的表示为：

，很容易写出Jacobian为：

值得一提的是参考文档中将世界系中的相机位姿拆分成了IMU位姿以及相机到IMU的外参，这样是为了分别对IMU位姿和相机外参求Jacobian，这里只讨论了对特征点的Jacobian，所以未进行拆分。

b. 球坐标逆深度 vs MSCKF逆深度

逆深度顾名思义是用

作为特征参数，根据逆深度

和一个单位方向向量(baering)

共同确定3D空间中的一个点：

逆深度的好处是能够表示无穷远点(

)，当特征点较远时，

数值比较大，直接估计

会存在数值问题，此外，有论文觉得将逆深度建模成高斯分布

比直接将深度建模成高斯分布

效果要更好。

单位方向向量

的构造有两种，一种是《Inverse Depth Parametrization for Monocular SLAM》中提出的球坐标表示，另一种是《 A multi-state constraint kalman filter for vision-aided inertial navigation》（MSCKF）中提出的表示。

球坐标表示：

MSCKF表示：

可以看到MSCKF的

表示比球坐标

表示要简洁得多，但MSCKF逆深度有个缺点是当

时，

，存在数值奇异，所以只能用在相机坐标系，因为相机系下特征点的深度都大于0。而球坐标逆深度仅在

都趋近于0时才存在数值奇异，所以能用在全局坐标系。

将Achored和逆深度结合起来得到Anchored逆深度表示：