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

Example 1 with AssociatedTriple

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

the class CommonTrifocalChecks method computeStuffFromPose.

private void computeStuffFromPose() {
    P2 = PerspectiveOps.createCameraMatrix(se2.R, se2.T, K, null);
    P3 = PerspectiveOps.createCameraMatrix(se3.R, se3.T, K, null);
    tensor = MultiViewOps.createTrifocal(P2, P3, null);
    F2 = MultiViewOps.createEssential(se2.getR(), se2.getT());
    F2 = MultiViewOps.createFundamental(F2, K);
    F3 = MultiViewOps.createEssential(se3.getR(), se3.getT());
    F3 = MultiViewOps.createFundamental(F3, K);
    for (int i = 0; i < 20; i++) {
        Point3D_F64 p = new Point3D_F64();
        p.x = rand.nextGaussian() * 0.5;
        p.y = rand.nextGaussian() * 0.5;
        p.z = rand.nextGaussian() * 0.5 + 2;
        worldPts.add(p);
        AssociatedTriple o = new AssociatedTriple();
        o.p1 = PerspectiveOps.renderPixel(new Se3_F64(), K, p);
        o.p2 = PerspectiveOps.renderPixel(se2, K, p);
        o.p3 = PerspectiveOps.renderPixel(se3, K, p);
        AssociatedTriple oS = o.copy();
        oS.p1 = PerspectiveOps.renderPixel(new Se3_F64(), null, p);
        AssociatedTriple oN = new AssociatedTriple();
        oN.p1 = PerspectiveOps.renderPixel(new Se3_F64(), null, p);
        oN.p2 = PerspectiveOps.renderPixel(se2, null, p);
        oN.p3 = PerspectiveOps.renderPixel(se3, null, p);
        observations.add(o);
        observationsSpecial.add(oS);
        observationsNorm.add(oN);
    }
}

Example 2 with AssociatedTriple

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

the class TestTrifocalAlgebraicPoint7 method noisy.

/**
 * Give it noisy inputs and see if it produces a better solution than the non-iterative algorithm.
 */
@Test
public void noisy() {
    List<AssociatedTriple> noisyObs = new ArrayList<>();
    // create a noisy set of observations
    double noiseLevel = 0.25;
    for (AssociatedTriple p : observations) {
        AssociatedTriple n = p.copy();
        n.p1.x += rand.nextGaussian() * noiseLevel;
        n.p1.y += rand.nextGaussian() * noiseLevel;
        n.p2.x += rand.nextGaussian() * noiseLevel;
        n.p2.y += rand.nextGaussian() * noiseLevel;
        n.p3.x += rand.nextGaussian() * noiseLevel;
        n.p3.y += rand.nextGaussian() * noiseLevel;
        noisyObs.add(n);
    }
    UnconstrainedLeastSquares optimizer = FactoryOptimization.leastSquareLevenberg(1e-3);
    TrifocalAlgebraicPoint7 alg = new TrifocalAlgebraicPoint7(optimizer, 300, 1e-20, 1e-20);
    assertTrue(alg.process(noisyObs, found));
    found.normalizeScale();
    // only the induced error is tested since the constraint error has unknown units and is quite large
    checkInducedErrors(found, noisyObs, 2);
}

Example 3 with AssociatedTriple

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

the class TestTrifocalAlgebraicPoint7 method checkInducedErrors.

/**
 * Computes the error using induced homographies.
 */
public void checkInducedErrors(TrifocalTensor tensor, List<AssociatedTriple> observations, double tol) {
    for (AssociatedTriple o : observations) {
        // homogeneous vector for observations in views 2 and 3
        Vector3D_F64 obs2 = new Vector3D_F64(o.p2.x, o.p2.y, 1);
        Vector3D_F64 obs3 = new Vector3D_F64(o.p3.x, o.p3.y, 1);
        // compute lines which pass through the observations
        Vector3D_F64 axisY = new Vector3D_F64(0, 1, 0);
        Vector3D_F64 line2 = new Vector3D_F64();
        Vector3D_F64 line3 = new Vector3D_F64();
        GeometryMath_F64.cross(obs2, axisY, line2);
        GeometryMath_F64.cross(obs3, axisY, line3);
        // compute induced homographies
        DMatrixRMaj H12 = MultiViewOps.inducedHomography12(tensor, line3, null);
        DMatrixRMaj H13 = MultiViewOps.inducedHomography13(tensor, line2, null);
        Point2D_F64 induced2 = new Point2D_F64();
        Point2D_F64 induced3 = new Point2D_F64();
        GeometryMath_F64.mult(H12, o.p1, induced2);
        GeometryMath_F64.mult(H13, o.p1, induced3);
        assertEquals(0, induced2.distance(o.p2), tol);
        assertEquals(0, induced3.distance(o.p3), tol);
    }
}

Example 4 with AssociatedTriple

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

the class TestTrifocalLinearPoint7 method fullTest.

@Test
public void fullTest() {
    TrifocalLinearPoint7 alg = new TrifocalLinearPoint7();
    assertTrue(alg.process(observations, found));
    // validate the solution by using a constraint
    for (AssociatedTriple a : observations) {
        DMatrixRMaj A = MultiViewOps.constraint(found, a.p1, a.p2, a.p3, null);
        assertEquals(0, NormOps_DDRM.normF(A), 1e-7);
    }
}

Example 5 with AssociatedTriple

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

the class ResolveSignAmbiguityPositiveDepth method process.

/**
 * Processes the results and observations to fix the sign
 *
 * @param observations (input) Observations in pixels
 * @param result (input/output) the current solution and modified to have the correct sign on output
 */
public void process(List<AssociatedTriple> observations, MetricCameraTriple result) {
    signChanged = false;
    int best = -1;
    bestInvalid = Integer.MAX_VALUE;
    normalizers.resize(3);
    pixelNorms.resize(3);
    worldToViews.resize(3);
    PinholePtoN_F64 normalize1 = normalizers.get(0);
    PinholePtoN_F64 normalize2 = normalizers.get(1);
    PinholePtoN_F64 normalize3 = normalizers.get(2);
    Point2D_F64 n1 = pixelNorms.get(0);
    Point2D_F64 n2 = pixelNorms.get(1);
    Point2D_F64 n3 = pixelNorms.get(2);
    worldToViews.get(1).setTo(result.view_1_to_2);
    worldToViews.get(2).setTo(result.view_1_to_3);
    normalize1.setK(result.view1);
    normalize2.setK(result.view2);
    normalize3.setK(result.view3);
    for (int trial = 0; trial < 2; trial++) {
        int foundInvalid = 0;
        for (int i = 0; i < observations.size(); i++) {
            AssociatedTriple ap = observations.get(i);
            // Convert from pixels to normalized image coordinates
            normalize1.compute(ap.p1.x, ap.p1.y, n1);
            normalize2.compute(ap.p2.x, ap.p2.y, n2);
            normalize2.compute(ap.p3.x, ap.p3.y, n3);
            // Find point in view-1 reference frame and check constraint
            triangulator.triangulate(pixelNorms.toList(), worldToViews.toList(), pointIn1);
            if (PerspectiveOps.isBehindCamera(pointIn1))
                foundInvalid++;
            // Find in view-2 and check +z constraint
            SePointOps_F64.transform(result.view_1_to_2, pointIn1, Xcam);
            if (PerspectiveOps.isBehindCamera(Xcam))
                foundInvalid++;
            // Find in view-3 and check +z constraint
            SePointOps_F64.transform(result.view_1_to_3, pointIn1, Xcam);
            if (PerspectiveOps.isBehindCamera(Xcam))
                foundInvalid++;
        }
        if (verbose != null)
            verbose.println("trial=" + trial + " invalid=" + foundInvalid + " obs=" + observations.size());
        // flip to test other hypothesis next iteration
        for (int i = 1; i < worldToViews.size(); i++) {
            worldToViews.get(i).T.scale(-1);
        }
        // save best
        if (bestInvalid > foundInvalid) {
            bestInvalid = foundInvalid;
            best = trial;
        }
    }
    if (best == 1) {
        signChanged = true;
        result.view_1_to_2.T.scale(-1);
        result.view_1_to_3.T.scale(-1);
    }
    if (verbose != null)
        verbose.println("best=" + best + " signChanged=" + signChanged);
}