Examples with LineParametric3D_F64 georegression.struct.line.LineParametric3D_F64 used on opensource projects

Example 1 with LineParametric3D_F64

use of georegression.struct.line.LineParametric3D_F64 in project BoofCV by lessthanoptimal.

the class TestDistanceSe3SymmetricSq method testIntrinsicParameters.

/**
 * Manually compute the error using a calibration matrix and see if they match
 */
@Test
public void testIntrinsicParameters() {
    // intrinsic camera calibration matrix
    DMatrixRMaj K = new DMatrixRMaj(3, 3, true, 100, 0.01, 200, 0, 150, 200, 0, 0, 1);
    DMatrixRMaj K2 = new DMatrixRMaj(3, 3, true, 105, 0.021, 180, 0, 155, 210, 0, 0, 1);
    DMatrixRMaj K_inv = new DMatrixRMaj(3, 3);
    DMatrixRMaj K2_inv = new DMatrixRMaj(3, 3);
    CommonOps_DDRM.invert(K, K_inv);
    CommonOps_DDRM.invert(K2, K2_inv);
    Se3_F64 keyToCurr = new Se3_F64();
    keyToCurr.getT().set(0.1, -0.1, 0.01);
    Point3D_F64 X = new Point3D_F64(0.02, -0.05, 3);
    AssociatedPair obs = new AssociatedPair();
    AssociatedPair obsP = new AssociatedPair();
    obs.p1.x = X.x / X.z;
    obs.p1.y = X.y / X.z;
    SePointOps_F64.transform(keyToCurr, X, X);
    obs.p2.x = X.x / X.z;
    obs.p2.y = X.y / X.z;
    // translate into pixels
    GeometryMath_F64.mult(K, obs.p1, obsP.p1);
    GeometryMath_F64.mult(K2, obs.p2, obsP.p2);
    // add some noise
    obsP.p1.x += 0.25;
    obsP.p1.y += 0.25;
    obsP.p2.x -= 0.25;
    obsP.p2.y -= 0.25;
    // convert noisy into normalized coordinates
    GeometryMath_F64.mult(K_inv, obsP.p1, obsP.p1);
    GeometryMath_F64.mult(K2_inv, obsP.p2, obsP.p2);
    // triangulate the point's position given noisy data
    LineParametric3D_F64 rayA = new LineParametric3D_F64();
    LineParametric3D_F64 rayB = new LineParametric3D_F64();
    rayA.slope.set(obsP.p1.x, obsP.p1.y, 1);
    rayB.p.set(-0.1, 0.1, -0.01);
    rayB.slope.set(obsP.p2.x, obsP.p2.y, 1);
    ClosestPoint3D_F64.closestPoint(rayA, rayB, X);
    // compute predicted given noisy triangulation
    AssociatedPair ugh = new AssociatedPair();
    ugh.p1.x = X.x / X.z;
    ugh.p1.y = X.y / X.z;
    SePointOps_F64.transform(keyToCurr, X, X);
    ugh.p2.x = X.x / X.z;
    ugh.p2.y = X.y / X.z;
    // convert everything into pixels
    GeometryMath_F64.mult(K, ugh.p1, ugh.p1);
    GeometryMath_F64.mult(K2, ugh.p2, ugh.p2);
    GeometryMath_F64.mult(K, obsP.p1, obsP.p1);
    GeometryMath_F64.mult(K2, obsP.p2, obsP.p2);
    double dx1 = ugh.p1.x - obsP.p1.x;
    double dy1 = ugh.p1.y - obsP.p1.y;
    double dx2 = ugh.p2.x - obsP.p2.x;
    double dy2 = ugh.p2.y - obsP.p2.y;
    double error = dx1 * dx1 + dy1 * dy1 + dx2 * dx2 + dy2 * dy2;
    // convert noisy back into normalized coordinates
    GeometryMath_F64.mult(K_inv, obsP.p1, obsP.p1);
    GeometryMath_F64.mult(K2_inv, obsP.p2, obsP.p2);
    DistanceSe3SymmetricSq alg = new DistanceSe3SymmetricSq(triangulate);
    alg.setIntrinsic(K.get(0, 0), K.get(1, 1), K.get(0, 1), K2.get(0, 0), K2.get(1, 1), K2.get(0, 1));
    alg.setModel(keyToCurr);
    assertEquals(error, alg.computeDistance(obsP), 1e-8);
}

Example 2 with LineParametric3D_F64

use of georegression.struct.line.LineParametric3D_F64 in project BoofCV by lessthanoptimal.

the class TestDistanceSe3SymmetricSq method intrinsicParameters.

/**
 * Manually compute the error using a calibration matrix and see if they match
 */
@Test
void intrinsicParameters() {
    // intrinsic camera calibration matrix
    DMatrixRMaj K1 = new DMatrixRMaj(3, 3, true, 100, 0.01, 200, 0, 150, 200, 0, 0, 1);
    DMatrixRMaj K2 = new DMatrixRMaj(3, 3, true, 105, 0.021, 180, 0, 155, 210, 0, 0, 1);
    DMatrixRMaj K1_inv = new DMatrixRMaj(3, 3);
    DMatrixRMaj K2_inv = new DMatrixRMaj(3, 3);
    CommonOps_DDRM.invert(K1, K1_inv);
    CommonOps_DDRM.invert(K2, K2_inv);
    Se3_F64 keyToCurr = new Se3_F64();
    keyToCurr.getT().setTo(0.1, -0.1, 0.01);
    Point4D_F64 X = new Point4D_F64(0.02, -0.05, 3, 1.0);
    AssociatedPair obs = new AssociatedPair();
    AssociatedPair obsP = new AssociatedPair();
    obs.p1.x = X.x / X.z;
    obs.p1.y = X.y / X.z;
    SePointOps_F64.transform(keyToCurr, X, X);
    obs.p2.x = X.x / X.z;
    obs.p2.y = X.y / X.z;
    // translate into pixels
    GeometryMath_F64.mult(K1, obs.p1, obsP.p1);
    GeometryMath_F64.mult(K2, obs.p2, obsP.p2);
    // add some noise
    obsP.p1.x += 0.25;
    obsP.p1.y += 0.25;
    obsP.p2.x -= 0.25;
    obsP.p2.y -= 0.25;
    // convert noisy into normalized coordinates
    GeometryMath_F64.mult(K1_inv, obsP.p1, obsP.p1);
    GeometryMath_F64.mult(K2_inv, obsP.p2, obsP.p2);
    // triangulate the point's position given noisy data
    LineParametric3D_F64 rayA = new LineParametric3D_F64();
    LineParametric3D_F64 rayB = new LineParametric3D_F64();
    rayA.slope.setTo(obsP.p1.x, obsP.p1.y, 1);
    rayB.p.setTo(-0.1, 0.1, -0.01);
    rayB.slope.setTo(obsP.p2.x, obsP.p2.y, 1);
    ClosestPoint3D_F64.closestPoint(rayA, rayB, X);
    // compute predicted given noisy triangulation
    AssociatedPair ugh = new AssociatedPair();
    ugh.p1.x = X.x / X.z;
    ugh.p1.y = X.y / X.z;
    SePointOps_F64.transform(keyToCurr, X, X);
    ugh.p2.x = X.x / X.z;
    ugh.p2.y = X.y / X.z;
    // convert everything into pixels
    GeometryMath_F64.mult(K1, ugh.p1, ugh.p1);
    GeometryMath_F64.mult(K2, ugh.p2, ugh.p2);
    GeometryMath_F64.mult(K1, obsP.p1, obsP.p1);
    GeometryMath_F64.mult(K2, obsP.p2, obsP.p2);
    double dx1 = ugh.p1.x - obsP.p1.x;
    double dy1 = ugh.p1.y - obsP.p1.y;
    double dx2 = ugh.p2.x - obsP.p2.x;
    double dy2 = ugh.p2.y - obsP.p2.y;
    double error = dx1 * dx1 + dy1 * dy1 + dx2 * dx2 + dy2 * dy2;
    // convert noisy back into normalized coordinates
    GeometryMath_F64.mult(K1_inv, obsP.p1, obsP.p1);
    GeometryMath_F64.mult(K2_inv, obsP.p2, obsP.p2);
    DistanceSe3SymmetricSq alg = new DistanceSe3SymmetricSq(triangulate);
    alg.setIntrinsic(0, PerspectiveOps.matrixToPinhole(K1, 0, 0, null));
    alg.setIntrinsic(1, PerspectiveOps.matrixToPinhole(K2, 0, 0, null));
    alg.setModel(keyToCurr);
    assertEquals(error, alg.distance(obsP), 1e-8);
}

Example 3 with LineParametric3D_F64

use of georegression.struct.line.LineParametric3D_F64 in project BoofCV by lessthanoptimal.

the class SimulatePlanarWorld method render.

public void render() {
    for (int i = 0; i < scene.size(); i++) {
        ImageRect r = scene.get(i);
        r.worldRect();
    }
    LineParametric3D_F64 ray = new LineParametric3D_F64();
    ImageMiscOps.fill(output, 0);
    for (int y = 0; y < output.height; y++) {
        for (int x = 0; x < output.width; x++) {
            if (Float.isNaN(depthMap.unsafe_get(x, y)))
                continue;
            ray.slope.x = pointing[(y * output.width + x) * 3];
            ray.slope.y = pointing[(y * output.width + x) * 3 + 1];
            ray.slope.z = pointing[(y * output.width + x) * 3 + 2];
            renderPixel(ray, x, y);
        }
    }
}

Example 4 with LineParametric3D_F64

use of georegression.struct.line.LineParametric3D_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;
}