H矩阵求取旋转和平移分量

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

代码如下:

 

[cpp]  view plain copy
  1. int calcRTfromHomo(CvMat* H, double t[3], double rodrot[3]){  
  2.       
  3.     double r[9];  
  4.     CvMat _r = cvMat(3, 3, CV_64F, r);          //rotation matrix  
  5.     double intrinsic[9]={1, 0, 0, 0, 1, 0, 0, 0, 1};  
  6.     CvMat _M = cvMat(3, 3, CV_64F, intrinsic); //intrinsic matrix, of no use in this implementation, reserved for future use  
  7.     double ones[]={1,1,1};  
  8.     CvMat _ones = cvMat(3, 1, CV_64F, ones);      
  9. //  double rodrot[3];  
  10.     CvMat _rodrot = cvMat(3, 1, CV_64F, rodrot);  
  11.     //for SVD  
  12.     CvMat* U = cvCreateMat(3, 3, CV_64F);  
  13.     CvMat* W = cvCreateMat(3, 3, CV_64F);  
  14.     CvMat* V = cvCreateMat(3, 3, CV_64F);  
  15.     CvMat* invM = cvCreateMat(3, 3, CV_64F);  
  16.     // three columns of Homography matrix  
  17.     CvMat* h1 = cvCreateMat(3, 1, CV_64F);  
  18.     CvMat* h2 = cvCreateMat(3, 1, CV_64F);  
  19.     CvMat* h3 = cvCreateMat(3, 1, CV_64F);  
  20.     // three columns of rotation matrix  
  21.     CvMat* r1 = cvCreateMat(3, 1, CV_64F);  
  22.     CvMat* r2 = cvCreateMat(3, 1, CV_64F);  
  23.     CvMat* r3 = cvCreateMat(3, 1, CV_64F);  
  24.     // translation vector  
  25.     CvMat _t =  cvMat(3, 1, CV_64F, t);   
  26.     cvGetCol(H,h1,0);  
  27.     cvGetCol(H,h2,1);  
  28.     cvGetCol(H,h3,2);  
  29.     cvGetCol(&_r,r1,0);  
  30.     cvGetCol(&_r,r2,1);  
  31.     cvGetCol(&_r,r3,2);  
  32.       
  33.     cvInvert(&_M, invM);  
  34.     cvMatMul(invM,h1,r1);  
  35.     cvMatMul(invM,h2,r2);  
  36.     cvMatMul(invM,h3,&_t);  
  37.       
  38.     cvNormalize(r1, r1);  
  39.     cvMul(r2,&_ones,r2,1/cvNorm(r1));  
  40.     cvMul(&_t,&_ones, &_t,1/cvNorm(r1) );  
  41.     cvCrossProduct(r1, r2, r3);  
  42.     cvSVD(&_r, W, U, V, CV_SVD_V_T);  
  43.     cvMatMul(U,V,&_r);  
  44.     cvRodrigues2(&_r, &_rodrot, NULL);  
  45.     return 1;  
  46. }  

 


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

 

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

 

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

 

[c-sharp]  view plain copy
  1. if( !cvFindHomography( &_pt1, &_pt2, &_h, CV_RANSAC, 5 ))  
  2.     return 0;  

 

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

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

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

 

 

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

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

H矩阵求取旋转和平移分量_第1张图片

 

 

 

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

 

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

 

 

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

H矩阵求取旋转和平移分量_第2张图片

 

可以看出场景之间的切换基本都是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

你可能感兴趣的:(工作,null,嵌入式,平台,reference,translation)