protected void estimateP3P ( int excluded ) { // the point used to check the solutions is the last one inputP3P . clear ( ) ; for ( int i = 0 ; i < 4 ; i ++ ) { if ( i != excluded ) { inputP3P . add ( points . get ( i ) ) ; } } // initial estimate for the pose solutions . reset ( ) ; if ( ! p3p . process ( inputP3P , solutions ) ) { // System.err.println("PIP Failed!?! That's weird"); return ; } for ( int i = 0 ; i < solutions . size ; i ++ ) { double error = computeErrors ( solutions . get ( i ) ) ; // see if it's better and it should save the results if ( error < bestError ) { bestError = error ; bestPose . set ( solutions . get ( i ) ) ; } } }
Estimates the pose using P3P from 3 out of 4 points . Then use all 4 to pick the best solution