Examples with Point2D3D boofcv.struct.geo.Point2D3D used on opensource projects

Example 16 with Point2D3D

use of boofcv.struct.geo.Point2D3D in project BoofCV by lessthanoptimal.

the class ArtificialStereoScene method init.

public void init(int N, boolean isPixels, boolean planar) {
    this.isPixels = isPixels;
    // define the camera's motion
    motion = new Se3_F64();
    motion.getR().set(ConvertRotation3D_F64.eulerToMatrix(EulerType.XYZ, 0.5, -0.2, 0.15, null));
    motion.getT().set(0.1, -0.2, 0.01);
    // randomly generate points in space
    if (planar)
        worldPoints = createPlanarScene(N);
    else
        worldPoints = GeoTestingOps.randomPoints_F64(-1, 1, -1, 1, 2, 3, N, rand);
    // transform points into second camera's reference frame
    pairs = new ArrayList<>();
    observationCurrent = new ArrayList<>();
    observationPose = new ArrayList<>();
    for (Point3D_F64 p1 : worldPoints) {
        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);
        pairs.add(pair);
        observationCurrent.add(pair.p2);
        observationPose.add(new Point2D3D(pair.p2, p1));
        if (isPixels) {
            PerspectiveOps.convertNormToPixel(K, pair.p1, pair.p1);
            PerspectiveOps.convertNormToPixel(K, pair.p2, pair.p2);
        }
    }
}

Example 17 with Point2D3D

use of boofcv.struct.geo.Point2D3D in project BoofCV by lessthanoptimal.

the class PnPJacobianRodrigues method process.

@Override
public void process(double[] input, DMatrixRMaj J) {
    this.output = J.data;
    // initialize data structures
    rodrigues.setParamVector(input[0], input[1], input[2]);
    rodJacobian.process(input[0], input[1], input[2]);
    worldToCamera.T.x = input[3];
    worldToCamera.T.y = input[4];
    worldToCamera.T.z = input[5];
    ConvertRotation3D_F64.rodriguesToMatrix(rodrigues, worldToCamera.getR());
    // compute the gradient for each observation
    for (int i = 0; i < observations.size(); i++) {
        Point2D3D o = observations.get(i);
        SePointOps_F64.transform(worldToCamera, o.location, cameraPt);
        indexX = 2 * 6 * i;
        indexY = indexX + 6;
        // add gradient from rotation
        addRodriguesJacobian(rodJacobian.Rx, o.location, cameraPt);
        addRodriguesJacobian(rodJacobian.Ry, o.location, cameraPt);
        addRodriguesJacobian(rodJacobian.Rz, o.location, cameraPt);
        // add gradient from translation
        addTranslationJacobian(cameraPt);
    }
}

Example 18 with Point2D3D

use of boofcv.struct.geo.Point2D3D in project BoofCV by lessthanoptimal.

the class FactoryMultiViewRobust method pnpLMedS.

/**
 * Robust solution to PnP problem using {@link LeastMedianOfSquares LMedS}.  Input observations are
 * in normalized image coordinates.
 *
 * <ul>
 *     <li>Input observations are in normalized image coordinates NOT pixels</li>
 *     <li>Error units are pixels squared.</li>
 * </ul>
 *
 * <p>See code for all the details.</p>
 *
 * @param configPnP PnP parameters.  Can't be null.
 * @param configLMedS Parameters for LMedS.  Can't be null.
 * @return Robust Se3_F64 estimator
 */
public static LeastMedianOfSquares<Se3_F64, Point2D3D> pnpLMedS(ConfigPnP configPnP, ConfigLMedS configLMedS) {
    configPnP.checkValidity();
    configLMedS.checkValidity();
    Estimate1ofPnP estimatorPnP = FactoryMultiView.computePnP_1(configPnP.which, configPnP.epnpIterations, configPnP.numResolve);
    DistanceModelMonoPixels<Se3_F64, Point2D3D> distance = new PnPDistanceReprojectionSq();
    distance.setIntrinsic(configPnP.intrinsic.fx, configPnP.intrinsic.fy, configPnP.intrinsic.skew);
    ModelManagerSe3_F64 manager = new ModelManagerSe3_F64();
    EstimatorToGenerator<Se3_F64, Point2D3D> generator = new EstimatorToGenerator<>(estimatorPnP);
    LeastMedianOfSquares<Se3_F64, Point2D3D> lmeds = new LeastMedianOfSquares<>(configLMedS.randSeed, configLMedS.totalCycles, manager, generator, distance);
    lmeds.setErrorFraction(configLMedS.errorFraction);
    return lmeds;
}

Example 19 with Point2D3D

use of boofcv.struct.geo.Point2D3D in project BoofCV by lessthanoptimal.

the class FactoryMultiViewRobust method pnpRansac.

/**
 * Robust solution to PnP problem using {@link Ransac}.  Input observations are in normalized
 * image coordinates.
 *
 * <p>NOTE: Observations are in normalized image coordinates NOT pixels.</p>
 *
 * <p>See code for all the details.</p>
 *
 * @param pnp PnP parameters.  Can't be null.
 * @param ransac Parameters for RANSAC.  Can't be null.
 * @return Robust Se3_F64 estimator
 */
public static Ransac<Se3_F64, Point2D3D> pnpRansac(ConfigPnP pnp, ConfigRansac ransac) {
    pnp.checkValidity();
    ransac.checkValidity();
    Estimate1ofPnP estimatorPnP = FactoryMultiView.computePnP_1(pnp.which, pnp.epnpIterations, pnp.numResolve);
    DistanceModelMonoPixels<Se3_F64, Point2D3D> distance = new PnPDistanceReprojectionSq();
    distance.setIntrinsic(pnp.intrinsic.fx, pnp.intrinsic.fy, pnp.intrinsic.skew);
    ModelManagerSe3_F64 manager = new ModelManagerSe3_F64();
    EstimatorToGenerator<Se3_F64, Point2D3D> generator = new EstimatorToGenerator<>(estimatorPnP);
    // convert from pixels to pixels squared
    double threshold = ransac.inlierThreshold * ransac.inlierThreshold;
    return new Ransac<>(ransac.randSeed, manager, generator, distance, ransac.maxIterations, threshold);
}

Example 20 with Point2D3D

use of boofcv.struct.geo.Point2D3D in project BoofCV by lessthanoptimal.

the class BaseChecksPnP method createObservations.

public List<Point2D3D> createObservations(Se3_F64 worldToCamera, double nominalZ, int total) {
    Se3_F64 cameraToWorld = worldToCamera.invert(null);
    // transform from pixel coordinates to normalized pixel coordinates, which removes lens distortion
    Point2Transform2_F64 pixelToNorm = LensDistortionOps.narrow(intrinsic).undistort_F64(true, false);
    List<Point2D3D> observations = new ArrayList<>();
    Point2D_F64 norm = new Point2D_F64();
    for (int i = 0; i < total; i++) {
        // randomly pixel a point inside the image
        double x = rand.nextDouble() * intrinsic.width;
        double y = rand.nextDouble() * intrinsic.height;
        pixelToNorm.compute(x, y, norm);
        // Randomly pick a depth and compute 3D coordinate
        double Z = rand.nextDouble() + nominalZ;
        double X = norm.x * Z;
        double Y = norm.y * Z;
        // Change the point's reference frame from camera to world
        Point3D_F64 cameraPt = new Point3D_F64(X, Y, Z);
        Point3D_F64 worldPt = new Point3D_F64();
        SePointOps_F64.transform(cameraToWorld, cameraPt, worldPt);
        // Save the perfect noise free observation
        Point2D3D o = new Point2D3D();
        o.getLocation().set(worldPt);
        o.getObservation().set(norm.x, norm.y);
        observations.add(o);
    }
    return observations;
}