/** * Creates an estimator for the PnP problem that uses only three observations, which is the minimal case * and known as P3P. * * @param which The algorithm which is to be returned. * @param numIterations Number of iterations. Only used by some algorithms and recommended number varies * significantly by algorithm. * @return An estimator which can return multiple estimates. */ public static EstimateNofPnP computePnP_N(EnumPNP which , int numIterations ) { MotionTransformPoint<Se3_F64, Point3D_F64> motionFit = FitSpecialEuclideanOps_F64.fitPoints3D(); switch( which ) { case P3P_GRUNERT: P3PGrunert grunert = new P3PGrunert(PolynomialOps.createRootFinder(5, RootFinderType.STURM)); return new WrapP3PLineDistance(grunert,motionFit); case P3P_FINSTERWALDER: P3PFinsterwalder finster = new P3PFinsterwalder(PolynomialOps.createRootFinder(4,RootFinderType.STURM)); return new WrapP3PLineDistance(finster,motionFit); case EPNP: Estimate1ofPnP epnp = computePnP_1(which,numIterations,0); return new Estimate1toNofPnP(epnp); } throw new IllegalArgumentException("Type "+which+" not known"); }
/** * Creates an estimator for the PnP problem that uses only three observations, which is the minimal case * and known as P3P. * * <p>NOTE: Observations are in normalized image coordinates NOT pixels.</p> * * @param which The algorithm which is to be returned. * @param numIterations Number of iterations. Only used by some algorithms and recommended number varies * significantly by algorithm. * @return An estimator which can return multiple estimates. */ public static EstimateNofPnP pnp_N(EnumPNP which , int numIterations ) { MotionTransformPoint<Se3_F64, Point3D_F64> motionFit = FitSpecialEuclideanOps_F64.fitPoints3D(); switch( which ) { case P3P_GRUNERT: P3PGrunert grunert = new P3PGrunert(PolynomialOps.createRootFinder(5, RootFinderType.STURM)); return new WrapP3PLineDistance(grunert,motionFit); case P3P_FINSTERWALDER: P3PFinsterwalder finster = new P3PFinsterwalder(PolynomialOps.createRootFinder(4,RootFinderType.STURM)); return new WrapP3PLineDistance(finster,motionFit); case EPNP: Estimate1ofPnP epnp = pnp_1(which,numIterations,0); return new Estimate1toNofPnP(epnp); case IPPE: Estimate1ofEpipolar H = FactoryMultiView.homographyTLS(); return new Estimate1toNofPnP(new IPPE_to_EstimatePnP(H)); } throw new IllegalArgumentException("Type "+which+" not known"); }
public static void main( String args[] ) { // Select which algorithm to use PolynomialRoots finder = PolynomialOps.createRootFinder(6, RootFinderType.EVD); // Create an arbitrary 3rd order polynomial // f(x) = 2 + 0.2*x + 5*x^2 + 3*x^3 Polynomial poly = Polynomial.wrap( 2 , 0.2 , 5 , 3 ); // Find the roots if( !finder.process(poly) ) throw new RuntimeException("Failed to find solution!"); // Print the solution List<Complex_F64> roots = finder.getRoots(); System.out.println("Total roots found: "+roots.size()); for( Complex_F64 c : roots ) { if( !c.isReal() ) { System.out.println("root is imaginary: "+c); continue; } double value = poly.evaluate(c.real); System.out.println("Polynomial value at "+c.real+" is "+value); } } }