Examples with Se3_F64 georegression.struct.se.Se3_F64 used on opensource projects

Example 11 with Se3_F64

use of georegression.struct.se.Se3_F64 in project BoofCV by lessthanoptimal.

the class TestMultiViewOps method createEssential.

@Test
public void createEssential() {
    DMatrixRMaj R = ConvertRotation3D_F64.eulerToMatrix(EulerType.XYZ, 0.05, -0.04, 0.1, null);
    Vector3D_F64 T = new Vector3D_F64(2, 1, -3);
    T.normalize();
    DMatrixRMaj E = MultiViewOps.createEssential(R, T);
    // Test using the following theorem:  x2^T*E*x1 = 0
    Point3D_F64 X = new Point3D_F64(0.1, 0.1, 2);
    Point2D_F64 x0 = PerspectiveOps.renderPixel(new Se3_F64(), null, X);
    Point2D_F64 x1 = PerspectiveOps.renderPixel(new Se3_F64(R, T), null, X);
    double val = GeometryMath_F64.innerProd(x1, E, x0);
    assertEquals(0, val, 1e-8);
}

Example 12 with Se3_F64

use of georegression.struct.se.Se3_F64 in project BoofCV by lessthanoptimal.

the class TestMultiViewOps method constraint_epipolar.

@Test
public void constraint_epipolar() {
    Point3D_F64 X = new Point3D_F64(0.1, -0.05, 2);
    Point2D_F64 p1 = PerspectiveOps.renderPixel(new Se3_F64(), K, X);
    Point2D_F64 p2 = PerspectiveOps.renderPixel(worldToCam2, K, X);
    DMatrixRMaj E = MultiViewOps.createEssential(worldToCam2.R, worldToCam2.T);
    DMatrixRMaj F = MultiViewOps.createFundamental(E, K);
    assertEquals(0, MultiViewOps.constraint(F, p1, p2), 1e-8);
}

Example 13 with Se3_F64

use of georegression.struct.se.Se3_F64 in project BoofCV by lessthanoptimal.

the class TestMultiViewOps method decomposeEssential.

@Test
public void decomposeEssential() {
    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);
    List<Se3_F64> found = MultiViewOps.decomposeEssential(E);
    // the scale factor is lost
    T.normalize();
    int numMatched = 0;
    for (Se3_F64 m : found) {
        DMatrixRMaj A = new DMatrixRMaj(3, 3);
        CommonOps_DDRM.multTransA(R, m.getR(), A);
        if (!MatrixFeatures_DDRM.isIdentity(A, 1e-8)) {
            continue;
        }
        Vector3D_F64 foundT = m.getT();
        foundT.normalize();
        if (foundT.isIdentical(T, 1e-8))
            numMatched++;
    }
    assertEquals(1, numMatched);
}

Example 14 with Se3_F64

use of georegression.struct.se.Se3_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 15 with Se3_F64

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