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

Example 6 with Point2D3D

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

the class CommonMotionNPoint method generateScene.

protected void generateScene(int N, Se3_F64 motion, boolean planar) {
    this.motion = motion;
    // randomly generate points in space
    if (planar) {
        worldPts = CommonHomographyChecks.createRandomPlane(rand, 3, N);
    } else {
        worldPts = GeoTestingOps.randomPoints_F64(-1, 1, -1, 1, 2, 3, N, rand);
    }
    cameraPts = new ArrayList<>();
    // transform points into second camera's reference frame
    assocPairs = new ArrayList<>();
    pointPose = new ArrayList<>();
    for (Point3D_F64 p1 : worldPts) {
        Point3D_F64 p2 = SePointOps_F64.transform(motion, p1, null);
        AssociatedPair pair = new AssociatedPair();
        pair.p1.set(p1.x / p1.z, p1.y / p1.z);
        pair.p2.set(p2.x / p2.z, p2.y / p2.z);
        assocPairs.add(pair);
        pointPose.add(new Point2D3D(pair.p2, p1));
        cameraPts.add(p2);
    }
}

Example 7 with Point2D3D

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

the class TestPnPDistanceReprojectionSq method checkErrorSingle.

/**
 * Provide an observation with a known solution and see if it is computed correctly
 */
@Test
public void checkErrorSingle() {
    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);
    // Point location in world frame
    Point3D_F64 X = new Point3D_F64(0.1, -0.04, 2.3);
    double deltaX = 0.1;
    double deltaY = -0.2;
    // create a noisy observed
    Point2D_F64 observed = PerspectiveOps.renderPixel(worldToCamera, K, X);
    observed.x += deltaX;
    observed.y += deltaY;
    // convert to normalized image coordinates
    PerspectiveOps.convertPixelToNorm(K, observed, observed);
    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));
    double expected = deltaX * deltaX + deltaY * deltaY;
    assertEquals(expected, found, 1e-8);
}

Example 8 with Point2D3D

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

the class TestPnPJacobianRodrigues method compareToNumerical.

private void compareToNumerical(double noise) {
    Se3_F64 worldToCamera = new Se3_F64();
    ConvertRotation3D_F64.eulerToMatrix(EulerType.XYZ, 0.1, 1, -0.2, worldToCamera.getR());
    worldToCamera.getT().set(-0.3, 0.4, 1);
    List<Point2D3D> observations = new ArrayList<>();
    for (int i = 0; i < numPoints; i++) {
        Point2D3D p = new Point2D3D();
        p.location.set(rand.nextGaussian() * 0.1, rand.nextGaussian() * 0.2, 3 + rand.nextGaussian());
        p.observation = PerspectiveOps.renderPixel(worldToCamera, null, p.location);
        p.observation.x += rand.nextGaussian() * noise;
        p.observation.y += rand.nextGaussian() * noise;
        observations.add(p);
    }
    PnPJacobianRodrigues alg = new PnPJacobianRodrigues();
    alg.setObservations(observations);
    func.setObservations(observations);
    double[] param = new double[codec.getParamLength()];
    codec.encode(worldToCamera, param);
    // DerivativeChecker.jacobianPrint(func, alg, param, 1e-6);
    assertTrue(DerivativeChecker.jacobian(func, alg, param, 1e-6));
}

Example 9 with Point2D3D

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

the class TestPnPResidualReprojection method basicTest.

@Test
public void basicTest() {
    Se3_F64 motion = new Se3_F64();
    ConvertRotation3D_F64.eulerToMatrix(EulerType.XYZ, 0.1, 1, -0.2, motion.getR());
    motion.getT().set(-0.3, 0.4, 1);
    Point3D_F64 world = new Point3D_F64(0.5, -0.5, 3);
    Point2D_F64 obs = new Point2D_F64();
    Point3D_F64 temp = SePointOps_F64.transform(motion, world, null);
    obs.x = temp.x / temp.z;
    obs.y = temp.y / temp.z;
    PnPResidualReprojection alg = new PnPResidualReprojection();
    // compute errors with perfect model
    double[] error = new double[alg.getN()];
    alg.setModel(motion);
    int index = alg.computeResiduals(new Point2D3D(obs, world), error, 0);
    assertEquals(alg.getN(), index);
    assertEquals(0, error[0], 1e-8);
    assertEquals(0, error[1], 1e-8);
    // compute errors with an incorrect model
    motion.getR().set(2, 1, 2);
    alg.setModel(motion);
    index = alg.computeResiduals(new Point2D3D(obs, world), error, 0);
    assertEquals(alg.getN(), index);
    assertTrue(Math.abs(error[0]) > 1e-8);
    assertTrue(Math.abs(error[1]) > 1e-8);
}

Example 10 with Point2D3D

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

the class TestDistanceTranGivenRotSq method testPerfect.

@Test
public void testPerfect() {
    Se3_F64 keyToCurr = new Se3_F64();
    keyToCurr.getR().set(ConvertRotation3D_F64.eulerToMatrix(EulerType.XYZ, 0.05, -0.03, 0.02, null));
    keyToCurr.getT().set(0.1, -0.1, 0.01);
    Point3D_F64 X = new Point3D_F64(0.1, -0.05, 3);
    Point2D3D obs = new Point2D3D();
    obs.location = X.copy();
    SePointOps_F64.transform(keyToCurr, X, X);
    obs.observation.x = X.x / X.z;
    obs.observation.y = X.y / X.z;
    alg.setRotation(keyToCurr.getR());
    alg.setModel(keyToCurr.getT());
    assertEquals(0, alg.computeDistance(obs), 1e-8);
}