Examples with Point2D3D boofcv.struct.geo.Point2D3D used on opensource projects

Example 26 with Point2D3D

use of boofcv.struct.geo.Point2D3D in project BoofCV by lessthanoptimal.

the class ExamplePnP method createObservations.

/**
 * Generates synthetic observations randomly in front of the camera.  Observations are in normalized image
 * coordinates and not pixels!  See {@link PerspectiveOps#convertPixelToNorm} for how to go from pixels
 * to normalized image coordinates.
 */
public List<Point2D3D> createObservations(Se3_F64 worldToCamera, int total) {
    Se3_F64 cameraToWorld = worldToCamera.invert(null);
    // transform from pixel coordinates to normalized pixel coordinates, which removes lens distortion
    Point2Transform2_F64 pixelToNorm = LensDistortionOps.narrow(intrinsic).undistort_F64(true, false);
    List<Point2D3D> observations = new ArrayList<>();
    Point2D_F64 norm = new Point2D_F64();
    for (int i = 0; i < total; i++) {
        // randomly pixel a point inside the image
        double x = rand.nextDouble() * intrinsic.width;
        double y = rand.nextDouble() * intrinsic.height;
        pixelToNorm.compute(x, y, norm);
        // Randomly pick a depth and compute 3D coordinate
        double Z = rand.nextDouble() + 4;
        double X = norm.x * Z;
        double Y = norm.y * Z;
        // Change the point's reference frame from camera to world
        Point3D_F64 cameraPt = new Point3D_F64(X, Y, Z);
        Point3D_F64 worldPt = new Point3D_F64();
        SePointOps_F64.transform(cameraToWorld, cameraPt, worldPt);
        // Save the perfect noise free observation
        Point2D3D o = new Point2D3D();
        o.getLocation().set(worldPt);
        o.getObservation().set(norm.x, norm.y);
        observations.add(o);
    }
    return observations;
}

Example 27 with Point2D3D

use of boofcv.struct.geo.Point2D3D in project BoofCV by lessthanoptimal.

the class ExamplePnP method estimateOutliers.

/**
 * Uses robust techniques to remove outliers
 */
public Se3_F64 estimateOutliers(List<Point2D3D> observations) {
    // We can no longer trust that each point is a real observation.  Let's use RANSAC to separate the points
    // You will need to tune the number of iterations and inlier threshold!!!
    Ransac<Se3_F64, Point2D3D> ransac = FactoryMultiViewRobust.pnpRansac(new ConfigPnP(intrinsic), new ConfigRansac(300, 1.0));
    // Observations must be in normalized image coordinates!  See javadoc of pnpRansac
    if (!ransac.process(observations))
        throw new RuntimeException("Probably got bad input data with NaN inside of it");
    System.out.println("Inlier size " + ransac.getMatchSet().size());
    Se3_F64 worldToCamera = ransac.getModelParameters();
    // You will most likely want to refine this solution too.  Can make a difference with real world data
    RefinePnP refine = FactoryMultiView.refinePnP(1e-8, 200);
    Se3_F64 refinedWorldToCamera = new Se3_F64();
    // notice that only the match set was passed in
    if (!refine.fitModel(ransac.getMatchSet(), worldToCamera, refinedWorldToCamera))
        throw new RuntimeException("Refined failed! Input probably bad...");
    return refinedWorldToCamera;
}

Example 28 with Point2D3D

use of boofcv.struct.geo.Point2D3D in project BoofCV by lessthanoptimal.

the class ExamplePnP method addOutliers.

/**
 * Adds some really bad observations to the mix
 */
public void addOutliers(List<Point2D3D> observations, int total) {
    int size = observations.size();
    for (int i = 0; i < total; i++) {
        // outliers will be created by adding lots of noise to real observations
        Point2D3D p = observations.get(rand.nextInt(size));
        Point2D3D o = new Point2D3D();
        o.observation.set(p.observation);
        o.location.x = p.location.x + rand.nextGaussian() * 5;
        o.location.y = p.location.y + rand.nextGaussian() * 5;
        o.location.z = p.location.z + rand.nextGaussian() * 5;
        observations.add(o);
    }
    // randomize the order
    Collections.shuffle(observations, rand);
}

Example 29 with Point2D3D

use of boofcv.struct.geo.Point2D3D in project BoofCV by lessthanoptimal.

the class TestPnPDistanceReprojectionSq method checkErrorArray.

/**
 * Check the array error function for accuracy
 */
@Test
public void checkErrorArray() {
    double[] expected = new double[5];
    DMatrixRMaj K = new DMatrixRMaj(3, 3, true, 100, 0.01, 200, 0, 150, 200, 0, 0, 1);
    Se3_F64 worldToCamera = new Se3_F64();
    worldToCamera.getT().set(0.1, -0.1, 0.2);
    List<Point2D3D> obs = new ArrayList<>();
    for (int i = 0; i < expected.length; i++) {
        Point3D_F64 X = new Point3D_F64(rand.nextGaussian() * 0.2, rand.nextGaussian() * 0.2, 2.3 + rand.nextGaussian() * 0.2);
        // create a noisy observed
        Point2D_F64 observed = PerspectiveOps.renderPixel(worldToCamera, K, X);
        double deltaX = rand.nextGaussian() * 0.2;
        double deltaY = rand.nextGaussian() * 0.2;
        observed.x += deltaX;
        observed.y += deltaY;
        // convert to normalized image coordinates
        PerspectiveOps.convertPixelToNorm(K, observed, observed);
        obs.add(new Point2D3D(observed, X));
        expected[i] = deltaX * deltaX + deltaY * deltaY;
    }
    PnPDistanceReprojectionSq alg = new PnPDistanceReprojectionSq(K.get(0, 0), K.get(1, 1), K.get(0, 1));
    alg.setModel(worldToCamera);
    double[] found = new double[5];
    alg.computeDistance(obs, found);
    for (int i = 0; i < found.length; i++) {
        assertEquals(expected[i], found[i], 1e-8);
    }
}

Example 30 with Point2D3D

use of boofcv.struct.geo.Point2D3D in project BoofCV by lessthanoptimal.

the class TestPnPDistanceReprojectionSq method checkBehindCamera.

/**
 * A very large error should be returned if the point appears behind the second camera
 */
@Test
public void checkBehindCamera() {
    DMatrixRMaj K = new DMatrixRMaj(3, 3, true, 100, 0.01, 200, 0, 150, 200, 0, 0, 1);
    Se3_F64 worldToCamera = new Se3_F64();
    worldToCamera.getT().set(0.1, -0.1, -2.5);
    // Point location in world frame
    Point3D_F64 X = new Point3D_F64(0.1, -0.04, 2.3);
    Point2D_F64 observed = PerspectiveOps.renderPixel(worldToCamera, K, X);
    PnPDistanceReprojectionSq alg = new PnPDistanceReprojectionSq(K.get(0, 0), K.get(1, 1), K.get(0, 1));
    alg.setModel(worldToCamera);
    double found = alg.computeDistance(new Point2D3D(observed, X));
    assertTrue(Double.MAX_VALUE == found);
}