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

Example 91 with Point3D_F64

use of georegression.struct.point.Point3D_F64 in project MAVSlam by ecmnet.

the class MAVOdomPixelDepthPnP method performSecondPass.

private boolean performSecondPass(List<PointTrack> active, List<Point2D3D> obs) {
    Se3_F64 keyToCurr = motionEstimator.getModelParameters();
    Point3D_F64 cameraPt = new Point3D_F64();
    Point2D_F64 predicted = new Point2D_F64();
    // predict where each track should be given the just estimated motion
    List<PointTrack> all = tracker.getAllTracks(null);
    for (PointTrack t : all) {
        Point2D3D p = t.getCookie();
        SePointOps_F64.transform(keyToCurr, p.location, cameraPt);
        normToPixel.compute(cameraPt.x / cameraPt.z, cameraPt.y / cameraPt.z, predicted);
        tracker.setHint(predicted.x, predicted.y, t);
    }
    // redo tracking with the additional information
    tracker.performSecondPass();
    active.clear();
    obs.clear();
    tracker.getActiveTracks(active);
    for (PointTrack t : active) {
        Point2D3D p = t.getCookie();
        pixelToNorm.compute(t.x, t.y, p.observation);
        obs.add(p);
    }
    return motionEstimator.process(obs);
}

Example 92 with Point3D_F64

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

the class BenchmarkStabilityFundamental method createObservations.

public void createObservations() {
    observations = new ArrayList<>();
    for (Point3D_F64 p1 : scene) {
        Point3D_F64 p2 = SePointOps_F64.transform(motion, p1, null);
        if (p1.z < 0 || p2.z < 0)
            continue;
        AssociatedPair pair = new AssociatedPair();
        pair.p1.set(p1.x / p1.z, p1.y / p1.z);
        pair.p2.set(p2.x / p2.z, p2.y / p2.z);
        observations.add(pair);
        // convert to pixels
        GeometryMath_F64.mult(K, pair.p1, pair.p1);
        GeometryMath_F64.mult(K, pair.p2, pair.p2);
        // add noise
        pair.p1.x += rand.nextGaussian() * sigmaPixels;
        pair.p1.y += rand.nextGaussian() * sigmaPixels;
        pair.p2.x += rand.nextGaussian() * sigmaPixels;
        pair.p2.y += rand.nextGaussian() * sigmaPixels;
        // if needed, convert back into normalized image coordinates
        if (!isPixels) {
            GeometryMath_F64.mult(K_inv, pair.p1, pair.p1);
            GeometryMath_F64.mult(K_inv, pair.p2, pair.p2);
        }
    }
}

Example 93 with Point3D_F64

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

the class PinholeRadialToEquirectangular_F64 method setPinhole.

/**
 * Specifies the pinhole camera
 * @param pinhole intrinsic parameters of pinhole camera
 */
public void setPinhole(CameraPinholeRadial pinhole) {
    this.pinhole = pinhole;
    declareVectors(pinhole.width, pinhole.height);
    // computing the 3D ray through each pixel in the pinhole camera at it's canonical
    // location
    Point2Transform2_F64 pixelToNormalized = new LensDistortionRadialTangential(pinhole).undistort_F64(true, false);
    Point2D_F64 norm = new Point2D_F64();
    for (int pixelY = 0; pixelY < pinhole.height; pixelY++) {
        for (int pixelX = 0; pixelX < pinhole.width; pixelX++) {
            pixelToNormalized.compute(pixelX, pixelY, norm);
            Point3D_F64 v = vectors[pixelY * pinhole.width + pixelX];
            v.set(norm.x, norm.y, 1);
        }
    }
}

Example 94 with Point3D_F64

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

the class PinholeToEquirectangular_F64 method setPinhole.

/**
 * Specifies the pinhole camera
 * @param pinhole intrinsic parameters of pinhole camera
 */
public void setPinhole(CameraPinhole pinhole) {
    this.pinhole = pinhole;
    declareVectors(pinhole.width, pinhole.height);
    // computing the 3D ray through each pixel in the pinhole camera at it's canonical
    // location
    PinholePtoN_F64 pixelToNormalized = new PinholePtoN_F64();
    pixelToNormalized.set(pinhole.fx, pinhole.fy, pinhole.skew, pinhole.cx, pinhole.cy);
    Point2D_F64 norm = new Point2D_F64();
    for (int pixelY = 0; pixelY < pinhole.height; pixelY++) {
        for (int pixelX = 0; pixelX < pinhole.width; pixelX++) {
            pixelToNormalized.compute(pixelX, pixelY, norm);
            Point3D_F64 v = vectors[pixelY * pinhole.width + pixelX];
            v.set(norm.x, norm.y, 1);
        }
    }
}

Example 95 with Point3D_F64

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

the class ArtificialStereoScene method createPlanarScene.

private List<Point3D_F64> createPlanarScene(int N) {
    List<Point3D_F64> ret = new ArrayList<>();
    for (int i = 0; i < N; i++) {
        double x = (rand.nextDouble() - 0.5) * 2;
        double y = (rand.nextDouble() - 0.5) * 2;
        ret.add(new Point3D_F64(x, y, 3));
    }
    return ret;
}