/** * 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"); }
public static void main( String args[] ) { Polynomial a = Polynomial.wrap(5,-2,3,0.5,30.4); Polynomial b = Polynomial.wrap(-0.4,8.4,-2.3); System.out.println("a = "+a); System.out.println("b = "+b); System.out.println("a + b = "+PolynomialOps.add(a,b,null)); System.out.println("a * b = "+PolynomialOps.multiply(a, b, null)); // Declare storage for the quotient and the remainder Polynomial q = new Polynomial(10); Polynomial r = new Polynomial(10); PolynomialOps.divide(a, b, q,r); System.out.println("a / b = ( "+q+" , "+r+" )"); System.out.println("Derivative a = "+PolynomialOps.derivative(a,null)); } }
/** * Find real roots for the specified polynomial. * * @param poly Polynomial which has less than or equal to the number of coefficients specified in this class's * constructor. */ public void process( Polynomial poly ) { sturm.initialize(poly); if( searchRadius <= 0 ) numRoots = sturm.countRealRoots(Double.NEGATIVE_INFINITY,Double.POSITIVE_INFINITY); else numRoots = sturm.countRealRoots(-searchRadius,searchRadius); if( numRoots == 0 ) return; if( searchRadius <= 0 ) handleAllRoots(); else { boundEachRoot(-searchRadius,searchRadius,0,numRoots); } // improve the accuracy for( int i = 0; i < numRoots; i++ ) { roots[i] = PolynomialOps.refineRoot(poly, roots[i], maxRefineIterations); } }
PolynomialOps.derivative(poly, previous); PolynomialOps.divide(sequence[0], previous, result, next); PolynomialOps.divide(previous, next, sequence[i-1], result); } else { sequence[i+1].setTo(result); PolynomialOps.divide(next, result, sequence[i], previous); sequenceLength = i+2;
/** * Randomly generate polynomials. First divide then multiply them back together to reconstruct * the original. */ @Test public void divide_then_multiply() { for( int i = 2; i < 10; i++ ) { Polynomial n = createRandom(i); for( int j = 1; j < i; j++ ) { Polynomial d = createRandom(j); Polynomial q = new Polynomial(10); Polynomial r = new Polynomial(10); PolynomialOps.divide(n,d,q,r); Polynomial a = PolynomialOps.multiply(d,q,null); Polynomial b = PolynomialOps.add(a,r,null); assertTrue(b.isIdentical(n, 1e-8)); } } }
derivative(poly,deriv);
@Test public void divide() { // numerator and denominator, intentionally add trailing zeros to skew things up Polynomial n = Polynomial.wrap(1,2,3,0); Polynomial d = Polynomial.wrap(-3,1,0); // quotient and remainder Polynomial q = new Polynomial(10); Polynomial r = new Polynomial(10); // expected solutions Polynomial expectedQ = Polynomial.wrap(11,3,0); Polynomial expectedR = Polynomial.wrap(34); PolynomialOps.divide(n,d,q,r); assertEquals(3,q.size); assertEquals(1,r.size); assertTrue(expectedQ.isIdentical(q, 1e-8)); assertTrue(expectedR.isIdentical(r, 1e-8)); }
@Test public void rootsSmallReal() { for( int numCoef = 2; numCoef < 6; numCoef++ ) { Polynomial poly = new Polynomial(numCoef); for( int trial = 0; trial < 20; trial++ ) { for( int i = 0; i < numCoef; i++ ) { poly.c[i] = 10*(rand.nextDouble()-0.5); } List<Double> roots = computeRealRoots(poly); // the root of the polynomial should be zero. higher degree polynomials have // more stability problems for( double d : roots ) { assertEquals(0,poly.evaluate(d),1e-8); } int expectedRoots = PolynomialOps.countRealRoots(poly); assertTrue(roots.size() == expectedRoots); } } }
@Test public void quadraticVertex() { double a = -0.5; double b = 2.0; double x = -b/(2*a); double found = PolynomialOps.quadraticVertex(a, b); assertEquals(x, found, 1e-8); }
@Test public void cubicRealRoot() { Polynomial p = Polynomial.wrap(4,-5,2,1.3); double root = PolynomialOps.cubicRealRoot(p.c[0],p.c[1],p.c[2],p.c[3]); double eval = p.evaluate(root); assertEquals(0, eval, 1e-8); } }
@Test public void multiply() { Polynomial a = PolynomialOps.multiply(Polynomial.wrap(2), Polynomial.wrap(3), null); Polynomial b = PolynomialOps.multiply(Polynomial.wrap(),Polynomial.wrap(3),null); Polynomial c = PolynomialOps.multiply(Polynomial.wrap(),Polynomial.wrap(),null); Polynomial d = PolynomialOps.multiply(Polynomial.wrap(4),Polynomial.wrap(2,3),null); Polynomial e = PolynomialOps.multiply(Polynomial.wrap(2,3),Polynomial.wrap(4),null); Polynomial f = PolynomialOps.multiply(Polynomial.wrap(2,3),Polynomial.wrap(4,8),null); assertTrue(Polynomial.wrap(6).isIdentical(a, 1e-8)); assertTrue(b.size == 0); assertTrue(c.size == 0); assertTrue(Polynomial.wrap(8, 12).isIdentical(d, 1e-8)); assertTrue(Polynomial.wrap(8, 12).isIdentical(e, 1e-8)); assertTrue(Polynomial.wrap(8, 28, 24).isIdentical(f, 1e-8)); }
@Test public void add() { Polynomial a = PolynomialOps.add(Polynomial.wrap(0.5,1,1),Polynomial.wrap(0.5,1,2),null); Polynomial b = PolynomialOps.add(Polynomial.wrap(0.5,1),Polynomial.wrap(0.5,1,2),null); Polynomial c = PolynomialOps.add(Polynomial.wrap(0.5,1,1),Polynomial.wrap(0.5,1),null); assertTrue(Polynomial.wrap(1, 2, 3).isIdentical(a, 1e-8)); assertTrue(Polynomial.wrap(1, 2, 2).isIdentical(b, 1e-8)); assertTrue(Polynomial.wrap(1, 2, 1).isIdentical(c, 1e-8)); }
PolynomialOps.derivative(poly, previous); PolynomialOps.divide(sequence[0], previous, result, next); PolynomialOps.divide(previous, next, sequence[i-1], result); } else { sequence[i+1].setTo(result); PolynomialOps.divide(next, result, sequence[i], previous); sequenceLength = i+2;
derivative(poly,deriv);
@Test public void rootsLargeReal() { for( int numCoef = 10; numCoef < 15; numCoef++ ) { Polynomial poly = new Polynomial(numCoef); for( int trial = 0; trial < 20; trial++ ) { for( int i = 0; i < numCoef; i++ ) { poly.c[i] = 2000*(rand.nextDouble()-0.5); } List<Double> roots = computeRealRoots(poly); // the root of the polynomial should be zero. higher degree polynomials have // more stability problems for( double d : roots ) { assertEquals(0,poly.evaluate(d),1e-8); } int expectedRoots = PolynomialOps.countRealRoots(poly); assertTrue(roots.size() == expectedRoots); } } }
/** * Compute the sturm sequence using a straight forward method */ private List<Double> computeSturm( Polynomial poly , double x ) { Polynomial d = new Polynomial(poly.size); PolynomialOps.derivative(poly, d); List<Double> found = new ArrayList<Double>(); found.add( poly.evaluate(x)); found.add( d.evaluate(x)); Polynomial q = new Polynomial(poly.size); Polynomial r = new Polynomial(poly.size); Polynomial p1 = new Polynomial(poly.size); Polynomial p2 = new Polynomial(poly.size); p1.setTo(poly); p2.setTo(d); do { PolynomialOps.divide(p1,p2,q,r); for( int i = 0; i < r.size; i++ ) { r.c[i] = -r.c[i]; } found.add(r.evaluate(x)); p1.setTo(p2); p2.setTo(r); } while( r.computeDegree() > 0 ); return found; }
@Test public void derivative() { Polynomial p = Polynomial.wrap(2,3,4,5); p.size = 2; Polynomial d = new Polynomial(10); PolynomialOps.derivative(p,d); assertTrue(d.isIdentical(Polynomial.wrap(3), 1e-8)); p.size = 3; PolynomialOps.derivative(p,d); assertTrue(d.isIdentical(Polynomial.wrap(3, 8), 1e-8)); }
/** * 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"); }
/** * Find real roots for the specified polynomial. * * @param poly Polynomial which has less than or equal to the number of coefficients specified in this class's * constructor. */ public void process( Polynomial poly ) { sturm.initialize(poly); if( searchRadius <= 0 ) numRoots = sturm.countRealRoots(Double.NEGATIVE_INFINITY,Double.POSITIVE_INFINITY); else numRoots = sturm.countRealRoots(-searchRadius,searchRadius); if( numRoots == 0 ) return; if( searchRadius <= 0 ) handleAllRoots(); else { boundEachRoot(-searchRadius,searchRadius,0,numRoots); } // improve the accuracy for( int i = 0; i < numRoots; i++ ) { roots[i] = PolynomialOps.refineRoot(poly, roots[i], maxRefineIterations); } }
@Test public void basicTest() { for( int numCoef = 2; numCoef < 6; numCoef++ ) { Polynomial poly = new Polynomial(numCoef); for( int i = 0; i < numCoef; i++ ) { poly.c[i] = 10*(rand.nextDouble()-0.5); } PolynomialRoots alg = new RootFinderCompanion(); assertTrue(alg.process(poly)); List<Complex_F64> roots = alg.getRoots(); int numReal = 0; for( Complex_F64 c : roots ) { if( c.isReal() ) { assertEquals(0,poly.evaluate(c.real),1e-8); numReal++; } } int expectedRoots = PolynomialOps.countRealRoots(poly); assertTrue(numReal == expectedRoots); } } }