Examples with PlaneNormal3D_F64 georegression.struct.plane.PlaneNormal3D_F64 used on opensource projects

Example 1 with PlaneNormal3D_F64

use of georegression.struct.plane.PlaneNormal3D_F64 in project BoofCV by lessthanoptimal.

the class CommonStructure method simulate.

public void simulate(int numViews, int numFeatures, boolean planar) {
    worldToViews = new ArrayList<>();
    projections = new ArrayList<>();
    observations = new ArrayList<>();
    // Randomly generate structure in front of the cameras
    if (planar) {
        PlaneNormal3D_F64 plane = new PlaneNormal3D_F64(0, 0, 2, 0.1, -0.05, 1);
        plane.n.normalize();
        features3D = UtilPoint3D_F64.random(plane, 0.5, numFeatures, rand);
    } else {
        features3D = UtilPoint3D_F64.random(new Point3D_F64(0, 0, 2), -0.5, 0.5, numFeatures, rand);
    }
    DMatrixRMaj K = PerspectiveOps.pinholeToMatrix(pinhole, (DMatrixRMaj) null);
    // Generate views the adjust all 6-DOF but and distinctive while pointing at the points
    for (int i = 0; i < numViews; i++) {
        Se3_F64 worldToView = new Se3_F64();
        worldToView.T.x = -1 + 0.1 * i;
        worldToView.T.y = rand.nextGaussian() * 0.05;
        worldToView.T.z = -0.5 + 0.05 * i + rand.nextGaussian() * 0.01;
        double rotX = rand.nextGaussian() * 0.05;
        double rotY = rand.nextGaussian() * 0.05;
        ConvertRotation3D_F64.eulerToMatrix(EulerType.XYZ, rotX, rotY, 0, worldToView.R);
        DMatrixRMaj P = new DMatrixRMaj(3, 4);
        PerspectiveOps.createCameraMatrix(worldToView.R, worldToView.T, K, P);
        worldToViews.add(worldToView);
        projections.add(P);
    }
    // generate observations
    WorldToCameraToPixel w2p = new WorldToCameraToPixel();
    for (int i = 0; i < numViews; i++) {
        List<Point2D_F64> viewObs = new ArrayList<>();
        w2p.configure(pinhole, worldToViews.get(i));
        for (int j = 0; j < numFeatures; j++) {
            viewObs.add(w2p.transform(features3D.get(j)));
        }
        observations.add(viewObs);
    }
}

Example 2 with PlaneNormal3D_F64

use of georegression.struct.plane.PlaneNormal3D_F64 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 = LensDistortionFactory.narrow(intrinsic).undistort_F64(true, false);
    List<Point2D3D> observations = new ArrayList<>();
    Point2D_F64 norm = new Point2D_F64();
    PlaneNormal3D_F64 plane3D = new PlaneNormal3D_F64();
    plane3D.n.setTo(0, 0, 1);
    GeometryMath_F64.mult(worldToCamera.R, plane3D.n, plane3D.n);
    plane3D.p.setTo(worldToCamera.T.x, worldToCamera.T.y, worldToCamera.T.z);
    LineParametric3D_F64 ray = new LineParametric3D_F64();
    Point3D_F64 cameraPt = new Point3D_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);
        if (worldPointsZZero) {
            // find the world plane in the image
            ray.slope.setTo(norm.x, norm.y, 1);
            Intersection3D_F64.intersection(plane3D, ray, cameraPt);
        } else {
            cameraPt.z = rand.nextDouble() + nominalZ;
            cameraPt.x = norm.x * cameraPt.z;
            cameraPt.y = norm.y * cameraPt.z;
        }
        // Change the point's reference frame from camera to world
        Point3D_F64 worldPt = new Point3D_F64();
        SePointOps_F64.transform(cameraToWorld, cameraPt, worldPt);
        if (worldPointsZZero) {
            if (Math.abs(worldPt.z) > 1e-8)
                throw new RuntimeException("Bug");
            // make it exactly zero
            worldPt.z = 0;
        }
        // Save the perfect noise free observation
        Point2D3D o = new Point2D3D();
        o.getLocation().setTo(worldPt);
        o.getObservation().setTo(norm.x, norm.y);
        observations.add(o);
    }
    return observations;
}