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

Example 36 with Point3D_F64

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

the class GenericCalibrationGrid method createObservations.

/**
 * Creates a set of observed points in pixel coordinates given zhang parameters and a calibration
 * grid.
 */
public static List<CalibrationObservation> createObservations(Zhang99AllParam config, List<Point2D_F64> grid) {
    List<CalibrationObservation> ret = new ArrayList<>();
    Point3D_F64 cameraPt = new Point3D_F64();
    Point2D_F64 pixelPt = new Point2D_F64();
    Zhang99IntrinsicParam intrinsic = config.getIntrinsic();
    for (Zhang99AllParam.View v : config.views) {
        CalibrationObservation set = new CalibrationObservation();
        Se3_F64 se = new Se3_F64();
        ConvertRotation3D_F64.rodriguesToMatrix(v.rotation, se.getR());
        se.T = v.T;
        for (int i = 0; i < grid.size(); i++) {
            Point2D_F64 grid2D = grid.get(i);
            Point3D_F64 grid3D = new Point3D_F64(grid2D.x, grid2D.y, 0);
            // Put the point in the camera's reference frame
            SePointOps_F64.transform(se, grid3D, cameraPt);
            // project and distort the point
            intrinsic.project(cameraPt, pixelPt);
            set.add(pixelPt, i);
        }
        ret.add(set);
    }
    return ret;
}

Example 37 with Point3D_F64

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

the class CommonMotionNPoint method generateScene.

protected void generateScene(int N, Se3_F64 motion, boolean planar) {
    this.motion = motion;
    // randomly generate points in space
    if (planar) {
        worldPts = CommonHomographyChecks.createRandomPlane(rand, 3, N);
    } else {
        worldPts = GeoTestingOps.randomPoints_F64(-1, 1, -1, 1, 2, 3, N, rand);
    }
    cameraPts = new ArrayList<>();
    // transform points into second camera's reference frame
    assocPairs = new ArrayList<>();
    pointPose = new ArrayList<>();
    for (Point3D_F64 p1 : worldPts) {
        Point3D_F64 p2 = SePointOps_F64.transform(motion, p1, null);
        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);
        assocPairs.add(pair);
        pointPose.add(new Point2D3D(pair.p2, p1));
        cameraPts.add(p2);
    }
}

Example 38 with Point3D_F64

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

the class CommonP3PSideChecks method pathological1.

/**
 * Check a pathological case where everything is zero
 */
@Test
public void pathological1() {
    Point3D_F64 P1 = new Point3D_F64();
    Point3D_F64 P2 = new Point3D_F64();
    Point3D_F64 P3 = new Point3D_F64();
    Point2D_F64 p1 = new Point2D_F64();
    Point2D_F64 p2 = new Point2D_F64();
    Point2D_F64 p3 = new Point2D_F64();
    double length12 = P1.distance(P2);
    double length23 = P2.distance(P3);
    double length13 = P1.distance(P3);
    List<PointDistance3> solutions = computeSolutions(p1, p2, p3, length23, length13, length12, false);
    assertEquals(0, solutions.size());
}

Example 39 with Point3D_F64

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

the class TestPnPDistanceReprojectionSq method checkErrorSingle.

/**
 * Provide an observation with a known solution and see if it is computed correctly
 */
@Test
public void checkErrorSingle() {
    DMatrixRMaj K = new DMatrixRMaj(3, 3, true, 100, 0.01, 200, 0, 150, 200, 0, 0, 1);
    Se3_F64 worldToCamera = new Se3_F64();
    worldToCamera.getT().set(0.1, -0.1, 0.2);
    // Point location in world frame
    Point3D_F64 X = new Point3D_F64(0.1, -0.04, 2.3);
    double deltaX = 0.1;
    double deltaY = -0.2;
    // create a noisy observed
    Point2D_F64 observed = PerspectiveOps.renderPixel(worldToCamera, K, X);
    observed.x += deltaX;
    observed.y += deltaY;
    // convert to normalized image coordinates
    PerspectiveOps.convertPixelToNorm(K, observed, observed);
    PnPDistanceReprojectionSq alg = new PnPDistanceReprojectionSq(K.get(0, 0), K.get(1, 1), K.get(0, 1));
    alg.setModel(worldToCamera);
    double found = alg.computeDistance(new Point2D3D(observed, X));
    double expected = deltaX * deltaX + deltaY * deltaY;
    assertEquals(expected, found, 1e-8);
}

Example 40 with Point3D_F64

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

the class TestPnPLepetitEPnP method selectControlPoints.

@Test
public void selectControlPoints() {
    List<Point3D_F64> worldPts = GeoTestingOps.randomPoints_F64(-1, 10, -5, 20, 0.1, 0.5, 30, rand);
    FastQueue<Point3D_F64> controlPts = new FastQueue<>(4, Point3D_F64.class, true);
    PnPLepetitEPnP alg = new PnPLepetitEPnP();
    alg.selectWorldControlPoints(worldPts, controlPts);
    // check that each row is unique
    for (int i = 0; i < 4; i++) {
        Point3D_F64 ci = controlPts.get(i);
        for (int j = i + 1; j < 4; j++) {
            Point3D_F64 cj = controlPts.get(j);
            double dx = ci.x - cj.x;
            double dy = ci.y - cj.y;
            double dz = ci.z - cj.z;
            double sum = Math.abs(dx) + Math.abs(dy) + Math.abs(dz);
            assertTrue(sum > 0.00001);
        }
    }
}