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

Example 86 with Point3D_F64

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

the class TestStereoProcessingBase method compute3D.

@Test
public void compute3D() {
    // point being viewed
    Point3D_F64 X = new Point3D_F64(-0.01, 0.1, 3);
    StereoParameters param = createStereoParam(width, height);
    DMatrixRMaj K = PerspectiveOps.calibrationMatrix(param.left, (DMatrixRMaj) null);
    // compute the view in pixels of the point in the left and right cameras
    Point2D_F64 lensLeft = new Point2D_F64();
    Point2D_F64 lensRight = new Point2D_F64();
    SfmTestHelper.renderPointPixel(param, X, lensLeft, lensRight);
    StereoProcessingBase<GrayU8> alg = new StereoProcessingBase<>(GrayU8.class);
    alg.setCalibration(param);
    // Rectify the points
    Point2Transform2_F64 rectLeft = RectifyImageOps.transformPixelToRect(param.left, alg.getRect1());
    Point2Transform2_F64 rectRight = RectifyImageOps.transformPixelToRect(param.right, alg.getRect2());
    Point2D_F64 l = new Point2D_F64();
    Point2D_F64 r = new Point2D_F64();
    rectLeft.compute(lensLeft.x, lensLeft.y, l);
    rectRight.compute(lensRight.x, lensRight.y, r);
    // make sure I rectified it correctly
    assertEquals(l.y, r.y, 1);
    // find point in homogeneous coordinates
    Point3D_F64 found = new Point3D_F64();
    alg.computeHomo3D(l.x, l.y, found);
    // disparity between the two images
    double disparity = l.x - r.x;
    found.x /= disparity;
    found.y /= disparity;
    found.z /= disparity;
    assertTrue(found.isIdentical(X, 0.01));
}

Example 87 with Point3D_F64

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

the class TestStereoProcessingBase method checkRectification.

/**
 * Center a point in the left and right images.  Search for the point and see if after rectification
 * the point can be found on the same row in both images.
 */
@Test
public void checkRectification() {
    // point being viewed
    Point3D_F64 X = new Point3D_F64(-0.01, 0.1, 3);
    StereoParameters param = createStereoParam(width, height);
    // create input images by rendering the point in both
    GrayU8 left = new GrayU8(width, height);
    GrayU8 right = new GrayU8(width, height);
    // compute the view in pixels of the point in the left and right cameras
    Point2D_F64 lensLeft = new Point2D_F64();
    Point2D_F64 lensRight = new Point2D_F64();
    SfmTestHelper.renderPointPixel(param, X, lensLeft, lensRight);
    // render the pixel in the image
    left.set((int) lensLeft.x, (int) lensLeft.y, 200);
    right.set((int) lensRight.x, (int) lensRight.y, 200);
    // test the algorithm
    StereoProcessingBase<GrayU8> alg = new StereoProcessingBase<>(GrayU8.class);
    alg.setCalibration(param);
    alg.setImages(left, right);
    alg.initialize();
    // Test tolerances are set to one pixel due to discretization errors in the image
    // sanity check test
    assertFalse(Math.abs(lensLeft.y - lensRight.y) <= 1);
    // check properties of a rectified image and stereo pairs
    Point2D_F64 foundLeft = centroid(alg.getImageLeftRect());
    Point2D_F64 foundRight = centroid(alg.getImageRightRect());
    assertTrue(Math.abs(foundLeft.y - foundRight.y) <= 1);
    assertTrue(foundRight.x < foundLeft.x);
}

Example 88 with Point3D_F64

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

the class TestStereoSparse3D method checkGeometry.

/**
 * Provide perfect image processing and validate the geometry
 */
@Test
public void checkGeometry() {
    Dummy disparity = new Dummy();
    StereoSparse3D alg = new StereoSparse3D(disparity, GrayF32.class);
    alg.setCalibration(param);
    Point3D_F64 X = new Point3D_F64(0.2, -0.34, 3);
    // original pixel coordinates
    Point2D_F64 x1 = PerspectiveOps.renderPixel(new Se3_F64(), K1, X);
    Point2D_F64 x2 = PerspectiveOps.renderPixel(param.rightToLeft.invert(null), K2, X);
    Point2Transform2_F64 pixelToRect1 = RectifyImageOps.transformPixelToRect(param.left, alg.rect1);
    Point2Transform2_F64 pixelToRect2 = RectifyImageOps.transformPixelToRect(param.right, alg.rect2);
    // rectified coordinates
    Point2D_F64 r1 = new Point2D_F64();
    Point2D_F64 r2 = new Point2D_F64();
    pixelToRect1.compute(x1.x, x1.y, r1);
    pixelToRect2.compute(x2.x, x2.y, r2);
    // compute the true disparity
    disparity.d = r1.x - r2.x;
    assertTrue(alg.process(x1.x, x1.y));
    double x = alg.getX() / alg.getW();
    double y = alg.getY() / alg.getW();
    double z = alg.getZ() / alg.getW();
    assertEquals(X.x, x, 1e-8);
    assertEquals(X.y, y, 1e-8);
    assertEquals(X.z, z, 1e-8);
}

Example 89 with Point3D_F64

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

the class SimulatePlanarWorld method computePixel.

/**
 * Project a point which lies on the 2D planar polygon's surface onto the rendered image
 */
public void computePixel(int which, double x, double y, Point2D_F64 output) {
    ImageRect r = scene.get(which);
    Point3D_F64 p3 = new Point3D_F64(x, -y, 0);
    SePointOps_F64.transform(r.rectToWorld, p3, p3);
    // unit sphere
    p3.scale(1.0 / p3.norm());
    sphereToPixel.compute(p3.x, p3.y, p3.z, output);
}

Example 90 with Point3D_F64

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

the class VfhFeatureDetector method process.

@Override
public void process(MAVDepthVisualOdometry<GrayU8, GrayU16> odometry, GrayU16 depth, GrayU8 gray) {
    Point2D_F64 xy;
    Point3D_F64 p;
    AccessPointTracks3D points = (AccessPointTracks3D) odometry;
    nearestPoints.clear();
    current_pos.set(model.state.l_x, model.state.l_y, model.state.l_z, model.state.h);
    // current.set(odometry.getCameraToWorld());
    getAttitudeToState(model, current);
    boolean first = true;
    // int i = 0; {
    for (int i = 0; i < points.getAllTracks().size(); i++) {
        if (points.isInlier(i)) {
            // xy is the observation
            xy = points.getAllTracks().get(i);
            // p is the obstacle location in body-frame
            p = odometry.getTrackLocation(i);
            SePointOps_F64.transform(current, p, rel_ned);
            MSP3DUtils.toNED(rel_ned);
            if (// model.raw.di > min_altitude &&
            p.z < max_distance && p.y < min_altitude && p.y > -min_altitude) {
                if (first) {
                    debug1 = (float) (rel_ned.x);
                    debug2 = (float) current.T.z;
                    debug3 = (float) current_pos.x;
                    msg_debug_vect v = new msg_debug_vect(2, 1);
                    v.x = debug1;
                    v.y = debug2;
                    v.z = debug3;
                    control.sendMAVLinkMessage(v);
                    first = false;
                }
                map.update(rel_ned, current_pos);
                nearestPoints.add(xy);
            // break;
            }
        }
    }
}