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

Example 11 with Vector3D_F64

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

the class TestMultiViewOps method decomposeCameraMatrix.

@Test
public void decomposeCameraMatrix() {
    // compute an arbitrary projection matrix from known values
    DMatrixRMaj K = PerspectiveOps.calibrationMatrix(200, 250, 0.0, 100, 110);
    DMatrixRMaj R = ConvertRotation3D_F64.eulerToMatrix(EulerType.XYZ, 1, 2, -0.5, null);
    Vector3D_F64 T = new Vector3D_F64(0.5, 0.7, -0.3);
    DMatrixRMaj P = new DMatrixRMaj(3, 4);
    DMatrixRMaj KP = new DMatrixRMaj(3, 4);
    CommonOps_DDRM.insert(R, P, 0, 0);
    P.set(0, 3, T.x);
    P.set(1, 3, T.y);
    P.set(2, 3, T.z);
    CommonOps_DDRM.mult(K, P, KP);
    // decompose the projection matrix
    DMatrixRMaj foundK = new DMatrixRMaj(3, 3);
    Se3_F64 foundPose = new Se3_F64();
    MultiViewOps.decomposeCameraMatrix(KP, foundK, foundPose);
    // recompute the projection matrix found the found results
    DMatrixRMaj foundKP = new DMatrixRMaj(3, 4);
    CommonOps_DDRM.insert(foundPose.getR(), P, 0, 0);
    P.set(0, 3, foundPose.T.x);
    P.set(1, 3, foundPose.T.y);
    P.set(2, 3, foundPose.T.z);
    CommonOps_DDRM.mult(foundK, P, foundKP);
    // see if the two projection matrices are the same
    assertTrue(MatrixFeatures_DDRM.isEquals(foundKP, KP, 1e-8));
}

Example 12 with Vector3D_F64

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

the class TestMultiViewOps method constraint_Trifocal_ppl.

@Test
public void constraint_Trifocal_ppl() {
    Point3D_F64 X = new Point3D_F64(0.1, -0.05, 2);
    computeLines(X, line1, line2, line3);
    // When the tensor was constructed the first view was assumed to be [I|0], which
    // is why normalized image coordinates are used for the first view
    Point2D_F64 x1 = PerspectiveOps.renderPixel(new Se3_F64(), null, X);
    Point2D_F64 x2 = PerspectiveOps.renderPixel(worldToCam2, K, X);
    Vector3D_F64 found = MultiViewOps.constraint(tensor, x1, x2, line3, null);
    assertEquals(0, found.x, 1e-12);
    assertEquals(0, found.y, 1e-12);
    assertEquals(0, found.z, 1e-12);
}

Example 13 with Vector3D_F64

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

the class TestMultiViewOps method canonicalCamera.

@Test
public void canonicalCamera() {
    DMatrixRMaj K = PerspectiveOps.calibrationMatrix(200, 250, 0.0, 100, 110);
    DMatrixRMaj R = ConvertRotation3D_F64.eulerToMatrix(EulerType.XYZ, 1, 2, -0.5, null);
    Vector3D_F64 T = new Vector3D_F64(0.5, 0.7, -0.3);
    DMatrixRMaj E = MultiViewOps.createEssential(R, T);
    DMatrixRMaj F = MultiViewOps.createFundamental(E, K);
    Point3D_F64 e1 = new Point3D_F64();
    Point3D_F64 e2 = new Point3D_F64();
    CommonOps_DDRM.scale(-2.0 / F.get(0, 1), F);
    MultiViewOps.extractEpipoles(F, e1, e2);
    DMatrixRMaj P = MultiViewOps.canonicalCamera(F, e2, new Vector3D_F64(1, 1, 1), 2);
    // recompose the fundamental matrix using the special equation for canonical cameras
    DMatrixRMaj foundF = new DMatrixRMaj(3, 3);
    DMatrixRMaj crossEpi = new DMatrixRMaj(3, 3);
    GeometryMath_F64.crossMatrix(e2, crossEpi);
    DMatrixRMaj M = new DMatrixRMaj(3, 3);
    CommonOps_DDRM.extract(P, 0, 3, 0, 3, M, 0, 0);
    CommonOps_DDRM.mult(crossEpi, M, foundF);
    // see if they are equal up to a scale factor
    CommonOps_DDRM.scale(1.0 / foundF.get(0, 1), foundF);
    CommonOps_DDRM.scale(1.0 / F.get(0, 1), F);
    assertTrue(MatrixFeatures_DDRM.isIdentical(F, foundF, 1e-8));
}

Example 14 with Vector3D_F64

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

the class TestPerspectiveOps method createCameraMatrix.

@Test
public void createCameraMatrix() {
    SimpleMatrix R = SimpleMatrix.random_DDRM(3, 3, -1, 1, rand);
    Vector3D_F64 T = new Vector3D_F64(2, 3, -4);
    SimpleMatrix K = SimpleMatrix.wrap(RandomMatrices_DDRM.triangularUpper(3, 0, -1, 1, rand));
    SimpleMatrix T_ = new SimpleMatrix(3, 1, true, new double[] { T.x, T.y, T.z });
    // test calibrated camera
    DMatrixRMaj found = PerspectiveOps.createCameraMatrix(R.getDDRM(), T, null, null);
    for (int i = 0; i < 3; i++) {
        assertEquals(found.get(i, 3), T_.get(i), 1e-8);
        for (int j = 0; j < 3; j++) {
            assertEquals(found.get(i, j), R.get(i, j), 1e-8);
        }
    }
    // test uncalibrated camera
    found = PerspectiveOps.createCameraMatrix(R.getDDRM(), T, K.getDDRM(), null);
    SimpleMatrix expectedR = K.mult(R);
    SimpleMatrix expectedT = K.mult(T_);
    for (int i = 0; i < 3; i++) {
        assertEquals(found.get(i, 3), expectedT.get(i), 1e-8);
        for (int j = 0; j < 3; j++) {
            assertEquals(found.get(i, j), expectedR.get(i, j), 1e-8);
        }
    }
}

Example 15 with Vector3D_F64

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

the class TestPositiveDepthConstraintCheck method testPositive.

/**
 * Point a point in front of both cameras and see if it returns true
 */
@Test
public void testPositive() {
    // create transform from A to B
    DMatrixRMaj R = ConvertRotation3D_F64.eulerToMatrix(EulerType.XYZ, 0, -0.05, 0, null);
    Vector3D_F64 T = new Vector3D_F64(1, 0, 0);
    Se3_F64 fromAtoB = new Se3_F64(R, T);
    // point in front of both cameras
    Point3D_F64 pt = new Point3D_F64(0, 0, 2);
    // create observations of the point in calibrated coordinates
    Point2D_F64 obsA = new Point2D_F64(0, 0);
    Point3D_F64 pt_inB = SePointOps_F64.transform(fromAtoB, pt, null);
    Point2D_F64 obsB = new Point2D_F64(pt_inB.x / pt_inB.z, pt_inB.y / pt_inB.z);
    PositiveDepthConstraintCheck alg = new PositiveDepthConstraintCheck();
    assertTrue(alg.checkConstraint(obsA, obsB, fromAtoB));
}