从Homography矩阵得到旋转和平移量(From Homography to Rotation and Translation)

时间:2023-02-09 09:58:12

最近在做机器人导航项目,于是就顺手在OpenCV里面将以前看过但是不太理解的从Homography求Rotation和Translation实现了一下

代码如下:

 

 


代码原理的话可以参考[1],实现的时候还碰到了一个小问题,即Rotation矩阵三列归一化的时候都是除以第一列矩阵的Norm,当时自己实现的时候没有发现,误用了三列各自的Norm,结果一直没有找到错误,后来参考了[2]的代码才得以解决。

 

函数的输出一个是Translation 向量,另一个是Rodrigues变换之后的向量,该向量的方向代表旋转轴,模代表旋转的角度,具体数学表示见参考[3].

 

函数中的输入Homography矩阵,可以用cvFindHomography求出。至于如何从特征点求得Homography可以参考opencv2.0里find_obj例程的下面这段代码:

 

 

下面是上面这个函数的一个应用,也是正在进行中的一个工作:(sigh, csdn博客竟然不支持嵌入式视频功能。。。太不给力了,真的是要找下家了。。)

墙外的可以点击:http://www.youtube.com/watch?v=YjrTFO0ftDQ

墙内的youku视频正在上传。。。这里上传好慢。。。

 

 

视频是一个移动机器人在遥控器控制下用BumbleBee2立体摄像头拍的一段视频处理后的结果。

下面是iRobot机器人平台,低角度拍摄很是霸气哈哈。上面有很多传感器,我只用了BumbleBee2:

从Homography矩阵得到旋转和平移量(From Homography to Rotation and Translation)

 

 

 

目前的话,还有两个问题:

 

1. 一个由于SURF点是每个frame都要重新计算的,因此计算出来Homography矩阵不一定准确,表现在视频上的话会产生映射的抖动(见视频mapping的框),而且在特征点比较少的时候,这种抖动尤为明显(可以见当机器人从室内转到走廊这段,由于走廊的SURF点明显减少,映射框剧烈抖动)。这种抖动用optical flow应该可以解决。不过在目前应用中应该也不是特别大的问题,而且个人认为这种连续性需要到scene understanding这个更高的层次解决,而不是死抠这里一些只关注细节的方法。

 

 

2. 另外一个问题是关于Rotation向量的,从这段视频中可以提取出50个连续的场景(目前程序里不同场景的定义两个场景之间匹配的SURF点小于整个场景SURF点的1/8,这个参数是可调的),不同场景之间的旋转和平移量见下图:

从Homography矩阵得到旋转和平移量(From Homography to Rotation and Translation)

 

可以看出场景之间的切换基本都是Translation(T向量的z都是1,所以我省略了), Rotation的量很小(最大也只不过是0.2弧度左右),这与机器人在场景里面的运动相符

但是可以看Rotation向量,计算出来的值似乎是显示所有的旋转都是绕着第三个量也就是Z轴进行的,而在实际过程中,旋转似乎是主要绕着Y轴进行的。

当然根据这个结果我们可以大致绘制出机器人的行进路线图,背后的原理似乎还有点那么不通,感觉这是由于我对于Affine Space和Othodogical Space没有完全理解导致的。求高人解答。

 

*******************************************************************

 

刚刚去问了老师。。。整个彻底错了。。。立体的场景不是一个平面,中间的点是不能用Homography来映射的,这个地方是需要用Fundamental矩阵的。这也是为什么Homography抖动以及求出Rotation Translation有误的原因。

 

基础不牢,地动山摇,本文权当反面教材吧。

 

********************************************************************

再补充20110525:

经验证 利用宽松限制(比如允许误差在10像素以内)的RANSAC求Homography是可以粗略筛选outlier匹配对的,效果比用RANSAC做FindFundmental好。因为Homography在两幅视图里是点对应,而Fundamental在两幅视图里是点和极线对应

 

_________________________________________________________________

 

Reference:

[1]. Chapter 11, Learning OpenCV: P384-P391

[2]. https://gist.github.com/740979

[3]. http://opencv.willowgarage.com/documentation/camera_calibration_and_3d_reconstruction.html#rodrigues2