VINS代码解析0——预备知识记录

目录

1. 旋转矩阵与四元数

1.1 四元数和旋转矩阵基本性质

1.2 四元数和李代数更新

1.3 四元数与3D点乘积的求导

1.4 齐次坐标的理解

1.5 其他导数及常用公式

2. 计算机视觉基本原理

2.1 RANSAC

2.1.1 直接线性变换法

2.1.2 稳健估计

3. C++及基本设计模式基础

3.1 C++工厂模式

---

1. 旋转矩阵与四元数

1.1 四元数和旋转矩阵基本性质

        在SLAM中所有涉及到的四元数旋转都是单位四元数，这一点很重要，所以要单独说出来，这和SLAM中的旋转矩阵是一致的，因为旋转矩阵是正交阵，他们都不会改变状态量的大小，只是方向上的变化。四元数的表示以及基本运算书上写的很清楚，就不多说了。

1.2 四元数和李代数更新

        可以使用四元数或旋转矩阵存储旋转变量。当我们用计算出来的w对某旋转更新时，有两种不同方式：

这部分很重要，涉及到IMU积分求四元数旋转都需要这部分理论知识，其中四元数求导过程如下：

 VINS在计算IMU预积分旋转四元数的时候使用的都是第二个求到的式子，在后面的各雅克比和协方差矩阵列状态误差传播方程的时候使用的是第一个推导的式子。

1.3 四元数与3D点乘积的求导

        设Pc=q*Pw，其中Pc是相机坐标系下的点，Pw是世界坐标系下的点。由上面的公式可知，运动四元数的增量可以由δθ表示，因此求Pc对四元数增量δθ的倒数就可以表示对四元数的倒数。

这部分在求视觉雅各比矩阵的时候很有用，其实推导过程和SLAM十四讲中李代数的左乘扰动模型相似。

1.4 齐次坐标的理解

        一直对齐次坐标这个概念的理解不够彻底，只见大部分的书中说道“齐次坐标在仿射变换中非常的方便”，然后就没有了后文，今天在一个叫做“三百年 重生”的博客上看到一篇关于透视投影变换的探讨的文章，其中有对齐次坐标有非常精辟的说明，特别是针对这样一句话进行了有力的证明：“齐次坐标表示是计算机图形学的重要手段之一，它既能够用来明确区分向量和点，同时也更易用于进行仿射（线性）几何变换。”—— F.S. Hill, JR。

         由于作者对齐次坐标真的解释的不错，我就原封不动的摘抄过来：

        对于一个向量v以及基oabc，可以找到一组坐标(v1,v2,v3)，使得v = v1 a + v2 b + v3 c          （1）

        而对于一个点p，则可以找到一组坐标（p1,p2,p3），使得p – o = p1 a + p2 b + p3 c            （2），

        从上面对向量和点的表达，我们可以看出为了在坐标系中表示一个点（如p），我们把点的位置看作是对这个基的原点o所进行的一个位移，即一个向量——p – o（有的书中把这样的向量叫做位置向量——起始于坐标原点的特殊向量），我们在表达这个向量的同时用等价的方式表达出了点p：p = o + p1 a + p2 b + p3 c (3)

        (1)(3)是坐标系下表达一个向量和点的不同表达方式。这里可以看出，虽然都是用代数分量的形式表达向量和点，但表达一个点比一个向量需要额外的信息。如果我写出一个代数分量表达(1, 4, 7)，谁知道它是个向量还是个点！

我们现在把（1）（3）写成矩阵的形式：v = (v1 v2 v3 0) X (a b c o)

        p = (p1 p2 p3 1) X (a b c o),这里(a,b,c,o)是坐标基矩阵，右边的列向量分别是向量v和点p在基下的坐标。这样，向量和点在同一个基下就有了不同的表达：3D向量的第4个代数分量是0，而3D点的第4个代数分量是1。像这种这种用4个代数分量表示3D几何概念的方式是一种齐次坐标表示。

        这样，上面的(1, 4, 7)如果写成（1,4,7,0），它就是个向量；如果是(1,4,7,1)，它就是个点。下面是如何在普通坐标(Ordinary Coordinate)和齐次坐标(Homogeneous Coordinate)之间进行转换：

        (1)从普通坐标转换成齐次坐标时：

•                 如果(x,y,z)是个点，则变为(x,y,z,1);

                如果(x,y,z)是个向量，则变为(x,y,z,0)。

        (2)从齐次坐标转换成普通坐标时   

                如果是(x,y,z,1)，则知道它是个点，变成(x,y,z);

                如果是(x,y,z,0)，则知道它是个向量，仍然变成(x,y,z)

        而旋转和缩放对于向量和点都有意义，你可以用类似上面齐次表示来检测。从中可以看出，齐次坐标用于仿射变换非常方便。

        此外，对于一个普通坐标的点P=(Px, Py, Pz)，有对应的一族齐次坐标(wPx, wPy, wPz, w)，其中w不等于零。

        比如，P(1, 4, 7)的齐次坐标有(1, 4, 7, 1)、（2, 8, 14, 2）、（-0.1, -0.4, -0.7, -0.1）等等。因此，如果把一个点从普通坐标变成齐次坐标，给x,y,z乘上同一个非零数w，然后增加第4个分量w；如果把一个齐次坐标转换成普通坐标，把前三个坐标同时除以第4个坐标，然后去掉第4个分量。

        由于齐次坐标使用了4个分量来表达3D概念，使得平移变换可以使用矩阵进行，从而如F.S. Hill, JR所说，仿射（线性）变换的进行更加方便。由于图形硬件已经普遍地支持齐次坐标与矩阵乘法，因此更加促进了齐次坐标使用，使得它似乎成为图形学中的一个标准。        

        以上很好的阐释了齐次坐标的作用及运用齐次坐标的好处。其实在图形学的理论中，很多已经被封装的好的API也是很有研究的，要想成为一名专业的计算机图形学的学习者，除了知其然必须还得知其所以然。这样在遇到问题的时候才能迅速定位问题的根源，从而解决问题。

        以上是通过齐次坐标来区分向量和点的方式。从中可以思考得知，对于平移T、旋转R、缩放S这3个最常见的仿射变换，平移变换只对于点才有意义，因为普通向量没有位置概念，只有大小和方向.

1.5 其他导数及常用公式

证明过程如下：

2. 计算机视觉基本原理

2.1 RANSAC

       RANSAC在整个VINS系统中应用很频繁，举个例子，比如vins_estimator/src/initial/solve_5pts.cpp中的MotionEstimator::solveRelativeRT(const vector<pair<Vector3d, Vector3d>>& corres, Matrix3d& Rotation, Vector3d& Translation)函数中就有如下应用：

// 调用opencv接口求解E矩阵, 注意这里使用的方法不是8点法
cv::Mat E = cv::findFundamentalMat(ll, rr, cv::FM_RANSAC, 0.3 / 460, 0.99, mask);

函数作用就是通过两帧匹配的特征点计算出E矩阵。

        对于基本矩阵Funamental Matrix求解方法主要有：

（1）直接线性变换法

• 8点法
• 最小二乘法

（2）基于RANSAC的鲁棒方法

        先简单介绍一下直接线性变换法。

2.1.1 直接线性变换法

注：三个红线标注的三个等式等价。

在上述分析过程中，如果n>=8时，最小二乘法求解是否是最优估计呢？

接下来，我们重点探讨一下这个问题。

2.1.2 稳健估计

1、稳健的定义

        稳健（robust）:对数据噪声的敏感性。        

对于上述采样，如果出现外点（距离正确值较远），将会影响实际估计效果。

2、RANSAC——随机一致性采样

        RANSAC主要解决样本中的外点问题，最多可处理50%的外点情况。        

基本思想：

        RANSAC通过反复选择数据中的一组随机子集来达成目标。被选取的子集被假设为局内点，并用下述方法进行验证：

1. 有一个模型适用于假设的局内点，即所有的未知参数都能从假设的局内点计算得出。

2. 用1中得到的模型去测试所有的其它数据，如果某个点适用于估计的模型，认为它也是局内点。

3. 如果有足够多的点被归类为假设的局内点，那么估计的模型就足够合理。

4. 然后，用所有假设的局内点去重新估计模型，因为它仅仅被初始的假设局内点估计过。

5. 最后，通过估计局内点与模型的错误率来评估模型。

        这个过程被重复执行固定的次数，每次产生的模型要么因为局内点太少而被舍弃，要么因为它比现有的模型更好而被选用。

对上述步骤，进行简单总结如下：

举个例子：使用RANSAC——拟合直线

2.3 关于OpenCV中使用到RANSAC的相关函数

1. solvePnPRansac（这个VINS中没有用，VINS在vins_estimator/src/initial_sfm.cpp的GlobalSFMFrameByPnP中使用的是cv::solvePnP(pts_3_vector, pts_2_vector, K, D, rvec, t, 1);）

2. findFundamentalMat

2.2 cv::Rodrigues()函数

        在vins_estimator/src/initial/initial_sfm.cpp的GlobalSFM::solveFrameByPnP()函数中，有两个opencv的函数应用值的注意。

        一个是Eigen与cv::Mat的相互转换，函数如下：

cv::eigen2cv(R_initial, tmp_r);         // Eigen::Matrix3d ==> cv::Mat
cv::cv2eigen(r, R_pnp);                 // cv::Mat ==> Eigen::Matrix3d

应用场景是使用cv::solvePnP时，必须要使用cv::Mat格式，求得数值之后再将求得的cv::Mat转换回slam算法常用的eigen形式。

        另一个opencv函数是cv::Rodrigues()函数，代码如下：

cv::Rodrigues(rvec, r);     // 旋转：rvec为PnP求出的旋转向量形式，用Rodrigues公式转换为矩阵，见课本 P166页

具体的原理如下：

转轴 n 是矩阵 R 特征值 1 对应的特征向量。求解此方程,再归一化,就得到了旋转轴。函数原型及参数说明如下：

void Rodrigues( const CvMat* src，CvMat* dst，CvMat* jacobian=0 );

参数说明：

• src——为输入的旋转向量（3x1或者1x3）或者旋转矩阵（3x3）。该参数向量表示其旋转的角度，用向量长度表示。
• dst——为输出的旋转矩阵（3x3）或者旋转向量（3x1或者1x3）。
• jacobian——为可选的输出雅可比矩阵（3x9或者9x3），是输入与输出数组的偏导数。
   

3. C++及基本设计模式基础

3.1 C++工厂模式

        工厂模式应用模板函数如下：

// 抽象模板工厂类 
// 模板参数：AbstractProduct_t 产品抽象类 
template <class AbstractProduct_t> 
class AbstractFactory 
{ 
public: 
    virtual AbstractProduct_t *CreateProduct() = 0; 
    virtual ~AbstractFactory() {} 
}; 


// 具体模板工厂类 // 模板参数：AbstractProduct_t 产品抽象类，ConcreteProduct_t 产品具体类
template <class AbstractProduct_t, class ConcreteProduct_t> 
class ConcreteFactory : public AbstractFactory<AbstractProduct_t> 
{ 
public: 
    AbstractProduct_t *CreateProduct() 
    { 
        return new ConcreteProduct_t(); 
    } 
};

        在VINS代码中，camera_model代码中的Camera.h以及CameraFactory.h就应用了类似的这种模式。

        这里还没有写完，我们在后续过程中逐步完善基础知识。

参考文章：

1.https://blog.youkuaiyun.com/wangshuailpp/article/details/80600706

2.https://www.cnblogs.com/csyisong/archive/2008/12/09/1351372.html 对齐次坐标的理解

3.https://yongqi.blog.youkuaiyun.com/article/details/102487585  计算机视觉基本原理——RANSAC

4. https://blog.youkuaiyun.com/weixin_42905141/article/details/100677949 OpenCV学习(12)——cv::Rodrigues()函数