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

Example 6 with TrifocalTensor

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

the class MultiViewOps method createTrifocal.

/**
 * <p>
 * Creates a trifocal tensor from two rigid body motions.  This is for the calibrated camera case.
 * </p>
 *
 * <p>
 * NOTE: View 1 is the world coordinate system.
 * </p>
 *
 * @param P2 Transform from view 1 to view 2.
 * @param P3 Transform from view 1 to view 3.
 * @param ret Storage for trifocal tensor.  If null a new instance will be created.
 * @return The trifocal tensor
 */
public static TrifocalTensor createTrifocal(Se3_F64 P2, Se3_F64 P3, TrifocalTensor ret) {
    if (ret == null)
        ret = new TrifocalTensor();
    DMatrixRMaj R2 = P2.getR();
    DMatrixRMaj R3 = P3.getR();
    Vector3D_F64 T2 = P2.getT();
    Vector3D_F64 T3 = P3.getT();
    for (int col = 0; col < 3; col++) {
        DMatrixRMaj T = ret.getT(col);
        int index = 0;
        for (int i = 0; i < 3; i++) {
            double a_left = R2.unsafe_get(i, col);
            double a_right = T2.getIndex(i);
            for (int j = 0; j < 3; j++) {
                T.data[index++] = a_left * T3.getIndex(j) - a_right * R3.unsafe_get(j, col);
            }
        }
    }
    return ret;
}

Example 7 with TrifocalTensor

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

the class MultiViewOps method createTrifocal.

/**
 * <p>
 * Creates a trifocal tensor from two camera matrices.
 * </p>
 *
 * <p>
 * IMPORTANT: It is assumed that the first camera has the following camera matrix P1 = [I|0],
 * where I is an identify matrix.
 * </p>
 *
 * @param P2 Camera matrix from view 1 to view 2
 * @param P3 Camera matrix from view 1 to view 3
 * @param ret Storage for trifocal tensor.  If null a new instance will be created.
 * @return The trifocal tensor
 */
public static TrifocalTensor createTrifocal(DMatrixRMaj P2, DMatrixRMaj P3, TrifocalTensor ret) {
    if (ret == null)
        ret = new TrifocalTensor();
    for (int col = 0; col < 3; col++) {
        DMatrixRMaj T = ret.getT(col);
        int index = 0;
        for (int i = 0; i < 3; i++) {
            double a_left = P2.get(i, col);
            double a_right = P2.get(i, 3);
            for (int j = 0; j < 3; j++) {
                T.data[index++] = a_left * P3.get(j, 3) - a_right * P3.get(j, col);
            }
        }
    }
    return ret;
}

Example 8 with TrifocalTensor

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

the class TestMultiViewOps method extractFundamental_threeview.

@Test
public void extractFundamental_threeview() {
    DMatrixRMaj found2 = new DMatrixRMaj(3, 3);
    DMatrixRMaj found3 = new DMatrixRMaj(3, 3);
    TrifocalTensor input = tensor.copy();
    MultiViewOps.extractFundamental(input, found2, found3);
    // make sure the input was not modified
    for (int i = 0; i < 3; i++) assertTrue(MatrixFeatures_DDRM.isIdentical(tensor.getT(i), input.getT(i), 1e-8));
    CommonOps_DDRM.scale(1.0 / CommonOps_DDRM.elementMaxAbs(found2), found2);
    CommonOps_DDRM.scale(1.0 / CommonOps_DDRM.elementMaxAbs(found3), found3);
    Point3D_F64 X = new Point3D_F64(0.1, 0.05, 2);
    // remember the first view is assumed to have a projection matrix of [I|0]
    Point2D_F64 x1 = PerspectiveOps.renderPixel(new Se3_F64(), null, X);
    Point2D_F64 x2 = PerspectiveOps.renderPixel(worldToCam2, K, X);
    Point2D_F64 x3 = PerspectiveOps.renderPixel(worldToCam3, K, X);
    assertEquals(0, MultiViewOps.constraint(found2, x1, x2), 1e-8);
    assertEquals(0, MultiViewOps.constraint(found3, x1, x3), 1e-8);
}

Example 9 with TrifocalTensor

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

the class TestMultiViewOps method createTrifocal_SE.

/**
 * Check the trifocal tensor using its definition
 */
@Test
public void createTrifocal_SE() {
    TrifocalTensor found = MultiViewOps.createTrifocal(worldToCam2, worldToCam3, null);
    SimpleMatrix R2 = SimpleMatrix.wrap(worldToCam2.getR());
    SimpleMatrix R3 = SimpleMatrix.wrap(worldToCam3.getR());
    SimpleMatrix b4 = new SimpleMatrix(3, 1);
    SimpleMatrix a4 = new SimpleMatrix(3, 1);
    b4.set(0, worldToCam3.getX());
    b4.set(1, worldToCam3.getY());
    b4.set(2, worldToCam3.getZ());
    a4.set(0, worldToCam2.getX());
    a4.set(1, worldToCam2.getY());
    a4.set(2, worldToCam2.getZ());
    for (int i = 0; i < 3; i++) {
        SimpleMatrix ai = R2.extractVector(false, i);
        SimpleMatrix bi = R3.extractVector(false, i);
        SimpleMatrix expected = ai.mult(b4.transpose()).minus(a4.mult(bi.transpose()));
        assertTrue(MatrixFeatures_DDRM.isIdentical(expected.getDDRM(), found.getT(i), 1e-8));
    }
}

Example 10 with TrifocalTensor

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

the class TestMultiViewOps method extractEpipoles_threeview.

@Test
public void extractEpipoles_threeview() {
    Point3D_F64 found2 = new Point3D_F64();
    Point3D_F64 found3 = new Point3D_F64();
    TrifocalTensor input = tensor.copy();
    MultiViewOps.extractEpipoles(input, found2, found3);
    // make sure the input was not modified
    for (int i = 0; i < 3; i++) assertTrue(MatrixFeatures_DDRM.isIdentical(tensor.getT(i), input.getT(i), 1e-8));
    Point3D_F64 space = new Point3D_F64();
    // check to see if it is the left-null space of their respective Fundamental matrices
    GeometryMath_F64.multTran(F2, found2, space);
    assertEquals(0, space.norm(), 1e-8);
    GeometryMath_F64.multTran(F3, found3, space);
    assertEquals(0, space.norm(), 1e-8);
}