一起学ORBSLAM2（10）ORBSLAM中的相似变换矩阵的计算

转载请注明原创地址：https://blog.csdn.net/qq_30356613/article/category/6897125

ORBSLAM在回环检测过程中通过计算回环两帧的相机相似变换矩阵来为图优化添加边。已知3D-3D的匹配地图点，通过匹配地图点计算两关键帧之间的相似变换矩阵，主要参考文章：Closed-form solution of absolute orientation using unit quaternions.主要内容如下：

相似变换矩阵与欧式变换的区别：

 

如表所示相似变换与欧式变换相比较多了尺度因子。因此对于一个相似变换矩阵可以分为三部分旋转矩阵R，平移矩阵t和初度因子s。

1.计算旋转（只能利用三对匹配点）

假设三对匹配点在两相机坐标系下坐标对分别为：，代表第一对匹配点在第一个相机坐标系下的坐标。我们根据三对匹配点重新建立坐标系，对于三个点建立如图所示坐标系：

 

用1,2点确定x轴，1,2,3,点构成x-y平面，并以1点为原点在平面x-y内作12的垂直线为y轴，经过1点作x-y平面的垂线为z轴，构成左坐标系。其中新坐标系下：，

，同理：

向量，

则：

2.构建最小二乘计算尺度和平移向量

左右坐标系下的匹配点变换可以表示为：

此式写成误差函数表示为：

误差和可以表示成如下最小二乘形式：

我们将匹配后的所有地图3D点进行中心化：

两坐标系下中心点坐标：

将所有点进行中心化：

注意到：

 

上式共有三项，其中中间项结果为0，因为r’坐标为中心化之后的，因此其和为0，另注意到最后一项一定为大于等于0的项，因此我们只需计算第一项的最小值，然后让最后一项为0即可得到该式的最小值。并以此计算出平移矩阵

因此最小二乘问题转化为如下形式：

，因R为正交矩阵（酉矩阵），因此根据酉不变性可以得到：

展开最小二乘为：

从而可以写成：，我们将其写成二次乘方的形式：

从而得到尺度为

 

 

3.通过四元数计算旋转矩阵（ORBSLAM中旋转矩阵计算方式）

如上式，如要得到其最小值，应是D尽可能的大，，故最终可以根据此式求解R。我们用四元数q来表示旋转矩阵R，D可写成如下形式：

其中：

 

依次推导如下：

 

最终化为求解最大化的，找一q使得该式最大化。

设

 

则存在：

 

其中：

 

那么N就可以表示如下：

 

要求解的最大值，我们需要将N进行特征分解，求得最大特征值对应的特征向量，则为该式中q的解。q为四元数表示的旋转，我们将四元数转换为旋转矩阵的形式：，首先求得旋转角和旋转轴，从而得到最终的旋转矩阵。