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

Example 66 with Point2D_F64

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

the class AssociateStereo2D method setSource.

/**
 * Converts location into rectified coordinates and saved a reference to the description.
 */
@Override
public void setSource(FastQueue<Point2D_F64> location, FastQueue<Desc> descriptions) {
    locationLeft.reset();
    for (int i = 0; i < location.size; i++) {
        Point2D_F64 orig = location.get(i);
        Point2D_F64 rectified = locationLeft.grow();
        leftImageToRect.compute(orig.x, orig.y, rectified);
    }
    this.descriptionsLeft = descriptions;
}

Example 67 with Point2D_F64

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

the class AssociateStereo2D method associate.

@Override
public void associate() {
    matches.reset();
    unassociatedSrc.reset();
    for (int i = 0; i < locationLeft.size; i++) {
        Point2D_F64 left = locationLeft.get(i);
        Desc descLeft = descriptionsLeft.get(i);
        int bestIndex = -1;
        double bestScore = scoreThreshold;
        for (int j = 0; j < locationRight.size; j++) {
            Point2D_F64 right = locationRight.get(j);
            if (checkRectified(left, right)) {
                double dist = scorer.score(descLeft, descriptionsRight.get(j));
                if (dist < bestScore) {
                    bestScore = dist;
                    bestIndex = j;
                }
            }
        }
        if (bestIndex >= 0) {
            matches.grow().setAssociation(i, bestIndex, bestScore);
        } else {
            unassociatedSrc.push(i);
        }
    }
}

Example 68 with Point2D_F64

use of georegression.struct.point.Point2D_F64 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 69 with Point2D_F64

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

the class FactoryMotion2D method createMotion2D.

/**
 * Estimates the 2D motion of an image using different models.
 *
 * @param ransacIterations Number of RANSAC iterations
 * @param inlierThreshold Threshold which defines an inlier.
 * @param outlierPrune If a feature is an outlier for this many turns in a row it is dropped. Try 2
 * @param absoluteMinimumTracks New features will be respawned if the number of inliers drop below this number.
 * @param respawnTrackFraction If the fraction of current inliers to the original number of inliers drops below
 *                             this fraction then new features are spawned.  Try 0.3
 * @param respawnCoverageFraction If the area covered drops by this fraction then spawn more features.  Try 0.8
 * @param refineEstimate Should it refine the model estimate using all inliers.
 * @param tracker Point feature tracker.
 * @param motionModel Instance of the model model used. Affine2D_F64 or Homography2D_F64
 * @param <I> Image input type.
 * @param <IT> Model model
 * @return  ImageMotion2D
 */
public static <I extends ImageBase<I>, IT extends InvertibleTransform> ImageMotion2D<I, IT> createMotion2D(int ransacIterations, double inlierThreshold, int outlierPrune, int absoluteMinimumTracks, double respawnTrackFraction, double respawnCoverageFraction, boolean refineEstimate, PointTracker<I> tracker, IT motionModel) {
    ModelManager<IT> manager;
    ModelGenerator<IT, AssociatedPair> fitter;
    DistanceFromModel<IT, AssociatedPair> distance;
    ModelFitter<IT, AssociatedPair> modelRefiner = null;
    if (motionModel instanceof Homography2D_F64) {
        GenerateHomographyLinear mf = new GenerateHomographyLinear(true);
        manager = (ModelManager) new ModelManagerHomography2D_F64();
        fitter = (ModelGenerator) mf;
        if (refineEstimate)
            modelRefiner = (ModelFitter) mf;
        distance = (DistanceFromModel) new DistanceHomographySq();
    } else if (motionModel instanceof Affine2D_F64) {
        manager = (ModelManager) new ModelManagerAffine2D_F64();
        GenerateAffine2D mf = new GenerateAffine2D();
        fitter = (ModelGenerator) mf;
        if (refineEstimate)
            modelRefiner = (ModelFitter) mf;
        distance = (DistanceFromModel) new DistanceAffine2DSq();
    } else if (motionModel instanceof Se2_F64) {
        manager = (ModelManager) new ModelManagerSe2_F64();
        MotionTransformPoint<Se2_F64, Point2D_F64> alg = new MotionSe2PointSVD_F64();
        GenerateSe2_AssociatedPair mf = new GenerateSe2_AssociatedPair(alg);
        fitter = (ModelGenerator) mf;
        distance = (DistanceFromModel) new DistanceSe2Sq();
    // no refine, already optimal
    } else {
        throw new RuntimeException("Unknown model type: " + motionModel.getClass().getSimpleName());
    }
    ModelMatcher<IT, AssociatedPair> modelMatcher = new Ransac(123123, manager, fitter, distance, ransacIterations, inlierThreshold);
    ImageMotionPointTrackerKey<I, IT> lowlevel = new ImageMotionPointTrackerKey<>(tracker, modelMatcher, modelRefiner, motionModel, outlierPrune);
    ImageMotionPtkSmartRespawn<I, IT> smartRespawn = new ImageMotionPtkSmartRespawn<>(lowlevel, absoluteMinimumTracks, respawnTrackFraction, respawnCoverageFraction);
    return new WrapImageMotionPtkSmartRespawn<>(smartRespawn);
}

Example 70 with Point2D_F64

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

the class MonoOverhead_to_MonocularPlaneVisualOdometry method computeTracks.

private void computeTracks() {
    if (computed)
        return;
    if (!(alg.getMotion2D() instanceof AccessPointTracks))
        return;
    AccessPointTracks accessPlane = (AccessPointTracks) alg.getMotion2D();
    List<Point2D_F64> tracksPlane = accessPlane.getAllTracks();
    OverheadView<T> map = alg.getOverhead();
    points3D.reset();
    pixels.reset();
    for (Point2D_F64 worldPt : tracksPlane) {
        // 2D to 3D
        Point3D_F64 p = points3D.grow();
        p.z = worldPt.x * map.cellSize - map.centerX;
        p.x = -(worldPt.y * map.cellSize - map.centerY);
        p.y = 0;
        // 3D world to camera
        SePointOps_F64.transform(planeToCamera, p, p);
        // normalized image coordinates
        normToPixel.compute(p.x / p.z, p.y / p.z, pixels.grow());
    }
    computed = true;
}