本文转载自 金木炎 的博客,仅供个人学习。感谢博主的无私分享,如有侵权,敬请告知
这篇将要讲orbslam2相机初始位置估计的另一种方法:单应矩阵H(使用场景:平面,视差小)和map point的求法
(一):单应矩阵
在计算机视觉中,平面的单应性被定义为一个平面到另外一个平面的投影映射。因此一个二维平面上的点映射到摄像机成像仪上的映射就是平面单应性的例子。如果点Q到成像仪上的点q的映射使用齐次坐标,这种映射可以用矩阵相乘的方式表示。若有一下定义:
Q=[X Y Z 1]T
q=[x y 1]T
q=sHQ
这里引入参数s,它是任意尺度的比例(目的是使得单应性定义到该尺度比例)。通常根据习惯放在H的外面。
H有两部分组成:用于定位观察的物体平面的物理变换和使用摄像机内参数矩阵的投影。
物理变换部分是与观测到的图像平面相关的部分旋转R和部分平移t的影响之和,表示如下
W=[R t]
这里R为3*3大小的矩阵,t表示一个一个3维的列矢量。
摄像机内参数矩阵用M表示,那么我们重写单应性如下:
q=sWMQ where W是相机内参矩阵
我们知道单应性研究的是一个平面上到另外一个平面的映射,那么上述公式中的~Q,就可以简化为平面坐标中的~Q',即我们使Z=0。即物体平面上的点我们用x,y表示,相机平面上的点,我们也是用二维点表示。我们去掉了Z方向的坐标,那么相对于旋转矩阵R,R可以分解为R=[r1 r2 r3],那么r3也就不要了。
其中H为:
H=sM[r1 r2 t]
是一个3×3大小的矩阵.
故最终的单应性矩阵可表示如下:
q=sHQ我们可以从H矩阵中恢复R,T。但是同样存在尺度问题(指T),所以只有在初始位置估计的时候才用到E或H。以后一般都用OpenCV的PNP求解也就是3D-2D的映射。
(二)我们除了要求相机的pose以外,还要求map point。下面来讲解map point的求法。
所以,通过本征矩阵得到R,t以后,要计算摄像机坐标系 C2 外参数矩阵的程序如下:
有了外参矩阵,我们就可以得到这些点坐标的关系:
在以上计算P三维坐标的推导过程中,可以看到和本征矩阵E是息息相关的,E和我们的尺度紧密相连,所以计算出来的深度和尺度scale也是直接相关的。同时,根据这种三角法(triangulation )计算的深度,其实是不怎么靠谱的,一般只是拿这个做一个初始值。
两个射线夹角越小,误差协方差越大。所以点到光心连线组成的射线向量在orbslam中是有明确记录的。