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

Example 81 with Point3D_F64

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

the class TestAssociateStereo2D method constraintX.

/**
 * Makes the observation in the left image > right image along x-axis
 */
@Test
public void constraintX() {
    // zap the rotation so that no adjustment should need to be done
    CommonOps_DDRM.setIdentity(param.rightToLeft.getR());
    Point3D_F64 X = new Point3D_F64(0.02, -0.5, 3);
    SfmTestHelper.renderPointPixel(param, X, leftP, rightP);
    // mangle the x-axis
    leftP.x = rightP.x - 0.25;
    pointsLeft.grow().set(leftP);
    pointsRight.grow().set(rightP);
    descLeft.grow();
    descRight.grow();
    AssociateStereo2D<TupleDesc_F64> alg = new AssociateStereo2D<>(scorer, 0.5, TupleDesc_F64.class);
    alg.setCalibration(param);
    alg.setSource(pointsLeft, descLeft);
    alg.setDestination(pointsRight, descRight);
    alg.associate();
    // at the current tolerance they should still match
    assertEquals(1, alg.getMatches().size);
    // make the tolerance tighter
    alg = new AssociateStereo2D<>(scorer, 0.01, TupleDesc_F64.class);
    alg.setCalibration(param);
    alg.setSource(pointsLeft, descLeft);
    alg.setDestination(pointsRight, descRight);
    alg.associate();
    assertEquals(0, alg.getMatches().size);
}

Example 82 with Point3D_F64

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

the class TestStereoConsistencyCheck method checkRectification.

@Test
public void checkRectification() {
    Se3_F64 leftToRight = new Se3_F64();
    ConvertRotation3D_F64.eulerToMatrix(EulerType.XYZ, 0.01, -0.001, 0.005, leftToRight.getR());
    leftToRight.getT().set(-0.1, 0, 0);
    StereoParameters param = new StereoParameters();
    param.rightToLeft = leftToRight.invert(null);
    param.left = new CameraPinholeRadial(400, 500, 0.1, 160, 120, 320, 240).fsetRadial(0, 0);
    param.right = new CameraPinholeRadial(380, 505, 0.05, 165, 115, 320, 240).fsetRadial(0, 0);
    Point3D_F64 X = new Point3D_F64(0.02, -0.5, 3);
    Point2D_F64 leftP = new Point2D_F64();
    Point2D_F64 rightP = new Point2D_F64();
    SfmTestHelper.renderPointPixel(param, X, leftP, rightP);
    StereoConsistencyCheck alg = new StereoConsistencyCheck(1, 2);
    alg.setCalibration(param);
    alg.checkPixel(leftP, rightP);
    assertEquals(alg.rectLeft.y, alg.rectRight.y, 1e-5);
    assertTrue(alg.rectLeft.x > alg.rectRight.x);
}

Example 83 with Point3D_F64

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

the class CommonStereoMotionNPoint method generateScene.

protected void generateScene(int N, Se3_F64 worldToLeft, boolean planar) {
    if (worldToLeft == null) {
        this.worldToLeft = worldToLeft = new Se3_F64();
        ConvertRotation3D_F64.eulerToMatrix(EulerType.XYZ, 0.1, 1, -0.2, worldToLeft.getR());
        worldToLeft.getT().set(-0.3, 0.4, 1);
    } else {
        this.worldToLeft = worldToLeft;
    }
    // randomly generate points in space
    if (planar) {
        worldPts = createRandomPlane(rand, 3, N);
    } else {
        worldPts = GeoTestingOps.randomPoints_F64(-1, 1, -1, 1, 2, 3, N, rand);
    }
    cameraLeftPts = new ArrayList<>();
    cameraRightPts = new ArrayList<>();
    // transform points into second camera's reference frame
    pointPose = new ArrayList<>();
    for (Point3D_F64 p1 : worldPts) {
        Point3D_F64 leftPt = SePointOps_F64.transform(worldToLeft, p1, null);
        Point3D_F64 rightPt = SePointOps_F64.transform(leftToRight, leftPt, null);
        Point2D_F64 leftObs = new Point2D_F64(leftPt.x / leftPt.z, leftPt.y / leftPt.z);
        Point2D_F64 rightObs = new Point2D_F64(rightPt.x / rightPt.z, rightPt.y / rightPt.z);
        pointPose.add(new Stereo2D3D(leftObs, rightObs, p1));
        cameraLeftPts.add(leftPt);
        cameraRightPts.add(rightPt);
    }
}

Example 84 with Point3D_F64

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

the class TestPnPStereoDistanceReprojectionSq method checkBehind.

public void checkBehind(double Tz, double Pz) {
    // Point location in world frame
    Point3D_F64 X = new Point3D_F64(0.1, -0.04, Pz);
    DMatrixRMaj K_left = PerspectiveOps.calibrationMatrix(param.left, (DMatrixRMaj) null);
    DMatrixRMaj K_right = PerspectiveOps.calibrationMatrix(param.right, (DMatrixRMaj) null);
    // create a noisy observed
    Point2D_F64 obsLeft = PerspectiveOps.renderPixel(worldToLeft, K_left, X);
    Point2D_F64 obsRight = PerspectiveOps.renderPixel(worldToRight, K_right, X);
    // convert to normalized image coordinates
    PerspectiveOps.convertPixelToNorm(K_left, obsLeft, obsLeft);
    PerspectiveOps.convertPixelToNorm(K_right, obsRight, obsRight);
    PnPStereoDistanceReprojectionSq alg = new PnPStereoDistanceReprojectionSq();
    StereoParameters param = new StereoParameters(this.param.left, this.param.right, new Se3_F64());
    param.rightToLeft.getT().set(0.1, 0, Tz);
    alg.setStereoParameters(param);
    alg.setModel(new Se3_F64());
    double found = alg.computeDistance(new Stereo2D3D(obsLeft, obsRight, X));
    assertTrue(Double.MAX_VALUE == found);
}

Example 85 with Point3D_F64

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

the class SfmTestHelper method renderPointPixel.

/**
 * Renders a 3D point in the left and right camera views given the stereo parameters. Lens distortion
 * is taken in account.
 *
 * @param param Stereo parameters
 * @param X Point location in 3D space
 * @param left location in pixels in left camera
 * @param right location in pixels in right camera
 */
public static void renderPointPixel(StereoParameters param, Point3D_F64 X, Point2D_F64 left, Point2D_F64 right) {
    // compute the location of X in the right camera's reference frame
    Point3D_F64 rightX = new Point3D_F64();
    SePointOps_F64.transform(param.getRightToLeft().invert(null), X, rightX);
    // location of object in normalized image coordinates
    Point2D_F64 normLeft = new Point2D_F64(X.x / X.z, X.y / X.z);
    Point2D_F64 normRight = new Point2D_F64(rightX.x / rightX.z, rightX.y / rightX.z);
    // convert into pixel coordinates
    Point2D_F64 pixelLeft = PerspectiveOps.convertNormToPixel(param.left, normLeft.x, normLeft.y, null);
    Point2D_F64 pixelRight = PerspectiveOps.convertNormToPixel(param.right, normRight.x, normRight.y, null);
    // take in account lens distortion
    Point2Transform2_F32 distLeft = LensDistortionOps.narrow(param.left).distort_F32(true, true);
    Point2Transform2_F32 distRight = LensDistortionOps.narrow(param.right).distort_F32(true, true);
    Point2D_F32 lensLeft = new Point2D_F32();
    Point2D_F32 lensRight = new Point2D_F32();
    distLeft.compute((float) pixelLeft.x, (float) pixelLeft.y, lensLeft);
    distRight.compute((float) pixelRight.x, (float) pixelRight.y, lensRight);
    // output solution
    left.set(lensLeft.x, lensLeft.y);
    right.set(lensRight.x, lensRight.y);
}