public boolean process ( List < AssociatedPair > points ) { if ( points . size ( ) < estimateHomography . getMinimumPoints ( ) ) throw new IllegalArgumentException ( "At least " + estimateHomography . getMinimumPoints ( ) + " must be provided" ) ; // center location of points in model zeroMeanWorldPoints ( points ) ; // make sure there are no accidental references to the original points points = pointsAdj . toList ( ) ; if ( ! estimateHomography . process ( points , H ) ) return false ; // make sure H[2,2] == 1 CommonOps_DDRM . divide ( H , H . get ( 2 , 2 ) ) ; // Jacobian of pi(H[u_0 1]^T) at u_0 = 0 // pi is plane to image transform J . a11 = H . unsafe_get ( 0 , 0 ) - H . unsafe_get ( 2 , 0 ) * H . unsafe_get ( 0 , 2 ) ; J . a12 = H . unsafe_get ( 0 , 1 ) - H . unsafe_get ( 2 , 1 ) * H . unsafe_get ( 0 , 2 ) ; J . a21 = H . unsafe_get ( 1 , 0 ) - H . unsafe_get ( 2 , 0 ) * H . unsafe_get ( 1 , 2 ) ; J . a22 = H . unsafe_get ( 1 , 1 ) - H . unsafe_get ( 2 , 1 ) * H . unsafe_get ( 1 , 2 ) ; // v = (H[0,1],H[1,2]) = pi(H[u_0 1]^T) at u_0 = 0 // projection of u_0 into normalized coordinates v1 = H . unsafe_get ( 0 , 2 ) ; v2 = H . unsafe_get ( 1 , 2 ) ; // Solve for rotations IPPE ( pose0 . R , pose1 . R ) ; // Solve for translations estimateTranslation ( pose0 . R , points , pose0 . T ) ; estimateTranslation ( pose1 . R , points , pose1 . T ) ; // compute the reprojection error for each pose error0 = computeError ( points , pose0 ) ; error1 = computeError ( points , pose1 ) ; // Make sure the best pose is the first one if ( error0 > error1 ) { double e = error0 ; error0 = error1 ; error1 = e ; Se3_F64 s = pose0 ; pose0 = pose1 ; pose1 = s ; } // Undo centering adjustment center3 . set ( - center . x , - center . y , 0 ) ; GeometryMath_F64 . addMult ( pose0 . T , pose0 . R , center3 , pose0 . T ) ; GeometryMath_F64 . addMult ( pose1 . T , pose1 . R , center3 , pose1 . T ) ; return true ; }
Estimates the transform from world coordinate system to camera given known points and observations . For each observation p1 = World 3D location . z = 0 is implicit . p2 = Observed location of points in image in normalized image coordinates