Examples with Point3D_F64 georegression.struct.point.Point3D_F64 used on opensource projects

Example 56 with Point3D_F64

use of georegression.struct.point.Point3D_F64 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);
}

Example 57 with Point3D_F64

use of georegression.struct.point.Point3D_F64 in project BoofCV by lessthanoptimal.

the class TestDistanceTranGivenRotSq method testNoisy.

@Test
public void testNoisy() {
    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 + 1;
    obs.observation.y = X.y / X.z + 1;
    alg.setRotation(keyToCurr.getR());
    alg.setModel(keyToCurr.getT());
    assertTrue(alg.computeDistance(obs) > 1e-8);
}

Example 58 with Point3D_F64

use of georegression.struct.point.Point3D_F64 in project BoofCV by lessthanoptimal.

the class TestRectifyCalibrated method compareWithKnown.

/**
 * Compare results from rectified transform and a set of camera which are already rectified.
 */
@Test
public void compareWithKnown() {
    DMatrixRMaj K = new DMatrixRMaj(3, 3, true, 300, 0, 200, 0, 400, 205, 0, 0, 1);
    // transforms are world to camera, but I'm thinking camera to world, which is why invert
    Se3_F64 poseR1 = createPose(0, 0, 0, 0.1, 0, 0.1).invert(null);
    Se3_F64 poseR2 = createPose(0, 0, 0, 1, 0, 0.1).invert(null);
    // only rotate around the y-axis so that the rectified coordinate system will have to be
    // the same as the global
    Se3_F64 poseA1 = createPose(0, 0.05, 0, 0.1, 0, 0.1).invert(null);
    Se3_F64 poseA2 = createPose(0, -0.1, 0, 1, 0, 0.1).invert(null);
    RectifyCalibrated alg = new RectifyCalibrated();
    alg.process(K, poseA1, K, poseA2);
    // original camera matrix
    DMatrixRMaj foundP1 = PerspectiveOps.createCameraMatrix(poseA1.getR(), poseA1.getT(), K, null);
    DMatrixRMaj foundP2 = PerspectiveOps.createCameraMatrix(poseA2.getR(), poseA2.getT(), K, null);
    // apply rectification transform
    DMatrixRMaj temp = new DMatrixRMaj(3, 4);
    CommonOps_DDRM.mult(alg.getRect1(), foundP1, temp);
    foundP1.set(temp);
    CommonOps_DDRM.mult(alg.getRect2(), foundP2, temp);
    foundP2.set(temp);
    CommonOps_DDRM.scale(0.1 / Math.abs(foundP1.get(2, 3)), foundP1);
    Point3D_F64 X = new Point3D_F64(0, 0, 3);
    // compare results, both should match because of rotation only being around y-axis
    assertEquals(PerspectiveOps.renderPixel(poseR1, K, X).x, PerspectiveOps.renderPixel(foundP1, X).x, 1e-5);
    assertEquals(PerspectiveOps.renderPixel(poseR1, K, X).y, PerspectiveOps.renderPixel(foundP1, X).y, 1e-5);
    assertEquals(PerspectiveOps.renderPixel(poseR2, K, X).x, PerspectiveOps.renderPixel(foundP2, X).x, 1e-5);
    assertEquals(PerspectiveOps.renderPixel(poseR2, K, X).y, PerspectiveOps.renderPixel(foundP2, X).y, 1e-5);
}

Example 59 with Point3D_F64

use of georegression.struct.point.Point3D_F64 in project BoofCV by lessthanoptimal.

the class TestRectifyFundamental method createScene.

public void createScene() {
    DMatrixRMaj K = new DMatrixRMaj(3, 3, true, 500, 0, 250, 0, 520, 270, 0, 0, 1);
    // define the camera's motion
    motion = new Se3_F64();
    motion.getR().set(ConvertRotation3D_F64.eulerToMatrix(EulerType.XYZ, -0.01, 0.1, 0.05, null));
    motion.getT().set(-0.5, 0.1, -0.05);
    DMatrixRMaj E = MultiViewOps.createEssential(motion.getR(), motion.getT());
    F = MultiViewOps.createFundamental(E, K);
    // randomly generate points in space
    List<Point3D_F64> worldPts = GeoTestingOps.randomPoints_F64(-1, 1, -1, 1, 2, 3, N, rand);
    // transform points into second camera's reference frame
    pairs = 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);
        pairs.add(pair);
        GeometryMath_F64.mult(K, pair.p1, pair.p1);
        GeometryMath_F64.mult(K, pair.p2, pair.p2);
    }
}

Example 60 with Point3D_F64

use of georegression.struct.point.Point3D_F64 in project BoofCV by lessthanoptimal.

the class TestRectifyFundamental method checkEpipoles.

/**
 * Checks to see that the epipoles go to infinity after applying the transforms
 */
@Test
public void checkEpipoles() {
    createScene();
    // extract eipoles
    Point3D_F64 epipole1 = new Point3D_F64();
    Point3D_F64 epipole2 = new Point3D_F64();
    MultiViewOps.extractEpipoles(F, epipole1, epipole2);
    // compute the rectification transforms
    RectifyFundamental alg = new RectifyFundamental();
    alg.process(F, pairs, 500, 520);
    DMatrixRMaj R1 = alg.getRect1();
    DMatrixRMaj R2 = alg.getRect2();
    // sanity check
    assertTrue(Math.abs(epipole1.z) > 1e-8);
    assertTrue(Math.abs(epipole2.z) > 1e-8);
    // see if epipoles are projected to infinity
    GeometryMath_F64.mult(R1, epipole1, epipole1);
    GeometryMath_F64.mult(R2, epipole2, epipole2);
    assertEquals(0, epipole1.z, 1e-12);
    assertEquals(0, epipole2.z, 1e-12);
}