学习笔记----直线的普吕克坐标表示和正交优化

内容全部翻译自PL-VIO: Tightly-Coupled Monocular Visual–Inertial Odometry Using Point and Line Features部分章节，方便查阅

一、以普吕克坐标表示3D直线

由于直线有四个自由度，紧凑参数化直线需要四个参数，普吕克坐标假设直线无限延长，针对三维直线提出两种参数化方法，普吕克坐标由六个参数组成，利用正交表示的紧凑性对其进行优化

图a表示相机帧c观测现实中的一条直线L，普吕克坐标表示这条直线为：

其中n向量表示直线与原点（即观测点）所在平面的法向量，d表示直线方向向量，图b是在两个连续相机帧观察到同一条直线在相机上的投影图解。

1.坐标变换：从世界坐标系w到相机坐标系c的转换矩阵为

 得到普吕克坐标表示的线在两个坐标系的转换为

2.计算L的普吕克坐标

当在两个相机视角观察到新的线特征时，计算普吕克坐标比较容易，如上图b所示，相机帧在c1和c2帧分别观察到同一条线L

线的一个分量在归一化图像平面可以通过两个端点表示

 以上两个端点加上坐标原点可以表示平面

根据原点坐标得到平面的约束公式，其中平面参数由两端点计算获得

 因此，如果给定同一条直线的两个观测平面Π1、Π2，则可以通过以下公式的计算获得直线的普吕克坐标表达：

注意：由于普吕克坐标为单位向量组成，还需要进行归一化

二 、用正交优化普吕克坐标

接下来通过正交表达来优化普吕克坐标，（普吕克坐标和正交表示可以相互转化）正交表达式为

如图a表示在三维线上定义一个坐标系，归一化的法向量和方向向量是坐标系的两个轴，第三个轴是前两个轴的矢量相交结果，定义线坐标系和相机坐标系之间的旋转定义为U：

 U和普吕克坐标的转换关系：

利用三角函数表示：

 其中W包含原点到3D线的距离d，U和W的定义包含四个自由度，在优化过程中用：来表示优化变量最小表示

正交化后获得优化后的普吕克坐标

其中L'和L之间存在比例因子

 三、线特征测量模型

线的重投影误差定义为从端点到投影线的距离，针对针孔相机来说，一条3D空间的线L通过以下投影公式投影到相机平面上：

其中 K 为线特征的投影矩阵，当将直线投影到归一化平面时，K是单位矩阵，投影公式中只涉及到直线普吕克坐标中的平面法向量n，因此投影坐标仅与法向量有关

当给定直线在世界坐标系下普吕克坐标L和正交表达O时，首先将其通过上述的坐标转换公式转换为相机坐标系，然后将其投影到相机平面上获得投影线段 I ，将第 i 相机帧的重投影误差定义为：

 

其中第一项代表从端点 s 到投影线段的距离

接下来就是进行Jacobian，推导过程见贺博论文附录