voidDecomposeEssentialMatrix(
const Eigen : : Matrix3d& E, Eigen : : Matrix3d? R1, 
Eigen : : Matrix3d? R2, Eigen : : Vector3d? t )
{
// 根據(jù)對(duì)極約束得到的帶噪聲的E做SVD分解 
          Eigen::JacobiSVD svd(
          E, Eigen : : ComputeFullU | Eigen : : ComputeFullV ) ; 
          Eigen::Matrix3d U = svd.matrixU(); 
          Eigen::Matrix3d V = svd.matrixV().transpose();


// 保證行列式符號(hào)為正
if (U.determinant() < 0) {
                  U ?= ?1;
          }
if (V.determinant() < 0) {
                  V ?= ?1;
          }


          Eigen : : Matrix3d W;
          W<< 0, 1, 0, ?1, 0, 0, 0, 0, 1;


          ?R1 = U ? W ? V; 
          ?R2=U?W.transpose() ?V; 
          ?t = U. col (2). normalized ();
}


void PoseFromEssentialMatrix ( const Eigen : : Matrix3d& E, 
const std : : vector& points1 ,
const std : : vector& points2 ,
Eigen : : Matrix3d? R, Eigen : : Vector3d? t ,
std : : vector? points3D ) { 7
CHECK_EQ(points1 . size () , points2 . size ());


Eigen::Matrix3d R1;
Eigen::Matrix3d R2; 
DecomposeEssentialMatrix(E, &R1, &R2, t );


// Generate all possible projection matrix combinations.
const std : : array R_cmbs{{R1, R2, R1, R2}}; 
const std : : array t_cmbs{{?t , ?t , ??t , ??t }};
...
}