public void process ( List < Point3D_F64 > worldPts , List < Point2D_F64 > observed , Se3_F64 solutionModel ) { if ( worldPts . size ( ) < 4 ) throw new IllegalArgumentException ( "Must provide at least 4 points" ) ; if ( worldPts . size ( ) != observed . size ( ) ) throw new IllegalArgumentException ( "Must have the same number of observations and world points" ) ; // select world control points using the points statistics selectWorldControlPoints ( worldPts , controlWorldPts ) ; // compute barycentric coordinates for the world control points computeBarycentricCoordinates ( controlWorldPts , alphas , worldPts ) ; // create the linear system whose null space will contain the camera control points constructM ( observed , alphas , M ) ; // the camera points are a linear combination of these null points extractNullPoints ( M ) ; // compute the full constraint matrix, the others are extracted from this if ( numControl == 4 ) { L_full . reshape ( 6 , 10 ) ; y . reshape ( 6 , 1 ) ; UtilLepetitEPnP . constraintMatrix6x10 ( L_full , y , controlWorldPts , nullPts ) ; // compute 4 solutions using the null points estimateCase1 ( solutions . get ( 0 ) ) ; estimateCase2 ( solutions . get ( 1 ) ) ; estimateCase3 ( solutions . get ( 2 ) ) ; // results are bad in general, so skip unless needed // always considering this case seems to hurt runtime speed a little bit if ( worldPts . size ( ) == 4 ) estimateCase4 ( solutions . get ( 3 ) ) ; } else { L_full . reshape ( 3 , 6 ) ; y . reshape ( 3 , 1 ) ; UtilLepetitEPnP . constraintMatrix3x6 ( L_full , y , controlWorldPts , nullPts ) ; estimateCase1 ( solutions . get ( 0 ) ) ; estimateCase2 ( solutions . get ( 1 ) ) ; if ( worldPts . size ( ) == 3 ) estimateCase3_planar ( solutions . get ( 2 ) ) ; } computeResultFromBest ( solutionModel ) ; }
Compute camera motion given a set of features with observations and 3D locations