Fusiello极线校正方法的原理及C++实现

    在三维重建中，极线校正是很重要的一个环节，其作用就是把左右两个图像的进行变换，使其左右图像的极线左右平行。通俗一点说也就是，使左图像中某一行（或列）中的每一个点，都出现在右图像中对应的行（或列）中。这样做可以极大地减少立体匹配的计算量。

    在opencv中其实也是自带了极线校正的API，如stereoRectify()函数，能够很好的进行极线校正。但是这篇文章是对Fusiello极线校正经典方法的介绍，所以就不过多的说opencv里的东西。

    Fusiello极线校正方法的原理其实挺简单的，下面是核心代码，对着代码将他的作用。Ps：这里不讲基本的立体几何知识。

原理可以看论文A compact algorithm for rectification of stereo pairs

void mystereoRectify(Mat Po1, Mat Po2,
		     Vector2d dL, Vector2d dR,
		     Mat &Tran1, Mat &Tran2,
		     Mat &Pn1, Mat &Pn2
		    )
{
  Mat c1,c2,X,v1,v2,v3,R1,T1,R2,T2,K1,K2,R;
  
  //分解P矩阵为K，R，T
  art(Po1,K1,R1,T1);
  art(Po2,K2,R2,T2);
  
  //计算两相机光心坐标
  c1 = - Po1.colRange(0,3).inv() * Po1.colRange(3,4);
  c2 = - Po2.colRange(0,3).inv() * Po2.colRange(3,4);
  
  v1 = c1-c2;
  v2 = R1.rowRange(2,3).t().cross(v1);
  v3 = v1.cross(v2);
  
  vconcat(v1.t()/norm(v1),v2.t()/norm(v2),X);
  vconcat(X,v3.t()/norm(v3),R);
      
  K.at(0,1)=0;
  
  K1.at(0,2) = K1.at(0,2) + dL(0,0);
  K1.at(1,2) = K1.at(1,2) + dL(1,0);
  K2.at(0,2) = K2.at(0,2) + dR(0,0);
  K2.at(1,2) = K2.at(1,2) + dR(1,0);
  
  hconcat(R, -R*c1, X);
  Pn1 = K1 * X;
  
  hconcat(R, -R*c2, X);
  Pn2 = K2 * X;
  
  Tran1 = Pn1.colRange(0,3) * Po1.colRange(0,3).inv();
  Tran2 = Pn2.colRange(0,3) * Po2.colRange(0,3).inv();
}

首先是函数的原型

void mystereoRectify(Mat Po1, Mat Po2,         //Po1，Po2分别是左右图像对应的投影矩阵
		     Vector2d dL, Vector2d dR, //dL和dR是用于设置图像变换过程中的中心平移距离
		     Mat &Tran1, Mat &Tran2,   //Tran1和Tran2是输出
		     Mat &Pn1, Mat &Pn2        //Pn1和Pn2是左右图像校正后的投影矩阵
		    )

  art(Po1,K1,R1,T1);//分解投影矩阵P为K本质矩阵，R旋转矩阵，T平移矩阵
  art(Po2,K2,R2,T2);

  c1 = - Po1.colRange(0,3).inv() * Po1.colRange(3,4);//计算出两个相机坐标的光心坐标
  c2 = - Po2.colRange(0,3).inv() * Po2.colRange(3,4);

  v1 = c1-c2;                           //以c1和c2的连线作为新的X轴
  v2 = R1.rowRange(2,3).t().cross(v1);  //用新的X轴和原来的Z轴叉乘计算出新的Y轴
  v3 = v1.cross(v2);                    //用新的X轴和Y轴叉乘计算出新的Z轴
  
  vconcat(v1.t()/norm(v1),v2.t()/norm(v2),X);//合并成新的旋转矩阵R
  vconcat(X,v3.t()/norm(v3),R);

  K1.at(0,1)=0; 
  K2.at(0,1)=0;
  K1.at(0,2) = K1.at(0,2) + dL(0,0); //按照输入的dL和dR中的平移值修改K矩阵中的cx和cy
  K1.at(1,2) = K1.at(1,2) + dL(1,0); //从而实现图像的平移
  K2.at(0,2) = K2.at(0,2) + dR(0,0); //主要的目的是尽量让变换后的图像保持在图像中心，
  K2.at(1,2) = K2.at(1,2) + dR(1,0); //避免变换后损失过多的图像

  hconcat(R, -R*c1, X);//根据新的本质矩阵K和旋转矩阵计算出新的投影矩阵，平移矩阵T不变
  Pn1 = K1 * X;
  
  hconcat(R, -R*c2, X);
  Pn2 = K2 * X;

  Tran1 = Pn1.colRange(0,3) * Po1.colRange(0,3).inv();//分别计算出左右两图的校正矩阵
  Tran2 = Pn2.colRange(0,3) * Po2.colRange(0,3).inv();

校正结果如下：

上面两个是极线校正后的图像，下面两个是原图像。

效果还是不错的，这个Fusiello极线校正方法虽然很简单，但是也存在一定的BUG，我发现一个就是必须要保证输入的左图像为真实的左图像，具体点说，就是左右两张图像不能颠倒，左图像必须是在现实中的左边拍的照，右图像必须是在现实物体右边拍的照。调转了就会导致校正后的图像完全颠倒过来，原因呢也想过，主要是因为左右图像调转后光心也左右互换，然后校正的x轴与正常放置校正的x轴是完全相反的，所以会导致图像的颠倒。

完整的源码下载链接：http://download.csdn.net/download/fsencen/10268864