Examples with PlanePtPixel boofcv.struct.sfm.PlanePtPixel used on opensource projects

Example 1 with PlanePtPixel

use of boofcv.struct.sfm.PlanePtPixel in project BoofCV by lessthanoptimal.

the class GenerateSe2_PlanePtPixel method generate.

@Override
public boolean generate(List<PlanePtPixel> dataSet, Se2_F64 keyToCurr) {
    from.clear();
    to.reset();
    for (int i = 0; i < dataSet.size(); i++) {
        PlanePtPixel p = dataSet.get(i);
        Point2D_F64 planeCurr = to.grow();
        // project current observation onto the plane
        if (planeProjection.normalToPlane(p.normalizedCurr.x, p.normalizedCurr.y, planeCurr)) {
            from.add(p.getPlaneKey());
        } else {
            to.removeTail();
        }
    }
    if (!estimator.process(from, to.toList()))
        return false;
    keyToCurr.set(estimator.getTransformSrcToDst());
    return true;
}

Example 2 with PlanePtPixel

use of boofcv.struct.sfm.PlanePtPixel in project BoofCV by lessthanoptimal.

the class TestDistancePlane2DToPixelSq method perfect.

@Test
public void perfect() {
    pixelToNorm.compute(pixelPtB.x, pixelPtB.y, normPt);
    double error = alg.computeDistance(new PlanePtPixel(planePtA, normPt));
    assertEquals(0, error, 1e-8);
}

Example 3 with PlanePtPixel

use of boofcv.struct.sfm.PlanePtPixel in project BoofCV by lessthanoptimal.

the class TestGenerateSe2_PlanePtPixel method perfect.

@Test
public void perfect() {
    alg.setExtrinsic(planeToCamera);
    CameraPlaneProjection planeProjection = new CameraPlaneProjection();
    planeProjection.setConfiguration(planeToCamera, intrinsic);
    for (int i = 0; i < alg.getMinimumPoints(); i++) {
        PlanePtPixel s = new PlanePtPixel();
        double x = rand.nextDouble() * intrinsic.width;
        double y = rand.nextDouble() * intrinsic.height;
        Point2D_F64 pixelA = new Point2D_F64(x, y);
        Point2D_F64 planePtA = new Point2D_F64();
        planeProjection.pixelToPlane(pixelA.x, pixelA.y, planePtA);
        Point2D_F64 planePtB = new Point2D_F64();
        SePointOps_F64.transform(motion2D, planePtA, planePtB);
        planeProjection.planeToNormalized(planePtB.x, planePtB.y, s.normalizedCurr);
        s.planeKey.set(planePtA);
        observations.add(s);
    }
    Se2_F64 found = new Se2_F64();
    assertTrue(alg.generate(observations, found));
    assertEquals(motion2D.T.x, found.T.x, 1e-8);
    assertEquals(motion2D.T.y, found.T.y, 1e-8);
    assertEquals(motion2D.getYaw(), found.getYaw(), 1e-8);
}

Example 4 with PlanePtPixel

use of boofcv.struct.sfm.PlanePtPixel in project BoofCV by lessthanoptimal.

the class TestDistancePlane2DToPixelSq method noisy.

@Test
public void noisy() {
    pixelToNorm.compute(pixelPtB.x + 2, pixelPtB.y, normPt);
    double error = alg.computeDistance(new PlanePtPixel(planePtA, normPt));
    assertEquals(4, error, 1e-8);
}

Example 5 with PlanePtPixel

use of boofcv.struct.sfm.PlanePtPixel in project BoofCV by lessthanoptimal.

the class VisOdomMonoPlaneInfinity method sortTracksForEstimation.

/**
 * Splits the set of active tracks into on plane and infinity sets.  For each set also perform specific sanity
 * checks to make sure basic constraints are still being meet.  If not then the track will not be considered for
 * motion estimation.
 */
private void sortTracksForEstimation() {
    // reset data structures
    planeSamples.reset();
    farAngles.reset();
    tracksOnPlane.clear();
    tracksFar.clear();
    // list of active tracks
    List<PointTrack> active = tracker.getActiveTracks(null);
    for (PointTrack t : active) {
        VoTrack p = t.getCookie();
        // compute normalized image coordinate
        pixelToNorm.compute(t.x, t.y, n);
        // rotate pointing vector into plane reference frame
        pointing.set(n.x, n.y, 1);
        GeometryMath_F64.mult(cameraToPlane.getR(), pointing, pointing);
        pointing.normalize();
        if (p.onPlane) {
            // see if it still intersects the plane
            if (pointing.y > 0) {
                // create data structure for robust motion estimation
                PlanePtPixel ppp = planeSamples.grow();
                ppp.normalizedCurr.set(n);
                ppp.planeKey.set(p.ground);
                tracksOnPlane.add(t);
            }
        } else {
            // if the point is not on the plane visually and (optionally) if it passes a strict y-axis rotation
            // test, consider using the point for estimating rotation.
            boolean allGood = pointing.y < 0;
            if (strictFar) {
                allGood = isRotationFromAxisY(t, pointing);
            }
            // is it still above the ground plane and only has motion consistent with rotation on ground plane axis
            if (allGood) {
                computeAngleOfRotation(t, pointing);
                tracksFar.add(t);
            }
        }
    }
}