/** * Creates two 3D point clouds for the left and right camera using the known calibration points and camera * calibration. Then find the optimal rigid body transform going from the right to left views. * * @return Transform from right to left view. */ private Se3_F64 computeRightToLeft() { // location of points in the world coordinate system List<Point2D_F64> points2D = layout; List<Point3D_F64> points3D = new ArrayList<>(); for( Point2D_F64 p : points2D ) { points3D.add( new Point3D_F64(p.x,p.y,0)); } // create point cloud in each view List<Point3D_F64> left = new ArrayList<>(); List<Point3D_F64> right = new ArrayList<>(); for( int i = 0; i < viewLeft.size(); i++ ) { Se3_F64 worldToLeft = viewLeft.get(i); Se3_F64 worldToRight = viewRight.get(i); // These points can really be arbitrary and don't have to be target points for( Point3D_F64 p : points3D ) { Point3D_F64 l = SePointOps_F64.transform(worldToLeft, p, null); Point3D_F64 r = SePointOps_F64.transform(worldToRight, p, null); left.add(l); right.add(r); } } // find the transform from right to left cameras return FitSpecialEuclideanOps_F64.fitPoints3D(right,left); }
/** * Creates two 3D point clouds for the left and right camera using the known calibration points and camera * calibration. Then find the optimal rigid body transform going from the right to left views. * * @return Transform from right to left view. */ private Se3_F64 computeRightToLeft() { // location of points in the world coordinate system List<Point2D_F64> points2D = layout; List<Point3D_F64> points3D = new ArrayList<>(); for( Point2D_F64 p : points2D ) { points3D.add( new Point3D_F64(p.x,p.y,0)); } // create point cloud in each view List<Point3D_F64> left = new ArrayList<>(); List<Point3D_F64> right = new ArrayList<>(); for( int i = 0; i < viewLeft.size(); i++ ) { Se3_F64 worldToLeft = viewLeft.get(i); Se3_F64 worldToRight = viewRight.get(i); // These points can really be arbitrary and don't have to be target points for( Point3D_F64 p : points3D ) { Point3D_F64 l = SePointOps_F64.transform(worldToLeft, p, null); Point3D_F64 r = SePointOps_F64.transform(worldToRight, p, null); left.add(l); right.add(r); } } // find the transform from right to left cameras return FitSpecialEuclideanOps_F64.fitPoints3D(right,left); }
/** * 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"); }