protected void IPPE ( DMatrixRMaj R1 , DMatrixRMaj R2 ) { // Equation 23 - Compute R_v from v double norm_v = Math . sqrt ( v1 * v1 + v2 * v2 ) ; if ( norm_v <= UtilEjml . EPS ) { // the plane is fronto-parallel to the camera, so set the corrective rotation Rv to identity. // There will be only one solution to pose. CommonOps_DDRM . setIdentity ( R_v ) ; } else { compute_Rv ( ) ; } // [B|0] = [I2|-v]*R_v compute_B ( B , R_v , v1 , v2 ) ; CommonOps_DDF2 . invert ( B , B ) ; // A = inv(B)*J CommonOps_DDF2 . mult ( B , J , A ) ; // Find the largest singular value of A double gamma = largestSingularValue2x2 ( A ) ; // Compute R22 from A CommonOps_DDF2 . scale ( 1.0 / gamma , A , R22 ) ; // B = I2 - R22^T * Rss CommonOps_DDF2 . setIdentity ( B ) ; CommonOps_DDF2 . multAddTransA ( - 1 , R22 , R22 , B ) ; double b1 = Math . sqrt ( B . a11 ) ; double b2 = Math . signum ( B . a12 ) * Math . sqrt ( B . a22 ) ; // [c;a] = [R22;b^T]*[1;0] cross [R22;b^T]*[0;1] l0 . set ( R22 . a11 , R22 . a21 , b1 ) ; l1 . set ( R22 . a12 , R22 . a22 , b2 ) ; ca . cross ( l0 , l1 ) ; // ca = [c;a] // This will be the solution for the two rotation matrices // R1 = R_v*[R22, +c; b^T , a ] constructR ( R1 , R_v , R22 , b1 , b2 , ca , 1 , tmp ) ; constructR ( R2 , R_v , R22 , b1 , b2 , ca , - 1 , tmp ) ; }
Solves the IPPE problem