Examples with Se3_F64 georegression.struct.se.Se3_F64 used on opensource projects

Example 46 with Se3_F64

use of georegression.struct.se.Se3_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 47 with Se3_F64

use of georegression.struct.se.Se3_F64 in project BoofCV by lessthanoptimal.

the class TestZhang99ComputeTargetHomography method basicTest.

public void basicTest(boolean removeAFew) {
    // create a grid an apply an arbitrary transform to it
    List<Point2D_F64> layout = GenericCalibrationGrid.standardLayout();
    DMatrixRMaj R = ConvertRotation3D_F64.eulerToMatrix(EulerType.XYZ, 0.02, -0.05, 0.01, null);
    Vector3D_F64 T = new Vector3D_F64(0, 0, -1000);
    Se3_F64 motion = new Se3_F64(R, T);
    CalibrationObservation observations = GenericCalibrationGrid.observations(motion, layout);
    if (removeAFew) {
        for (int i = 0; i < 6; i++) {
            observations.points.remove(5);
        }
    }
    // compute the homography
    Zhang99ComputeTargetHomography alg = new Zhang99ComputeTargetHomography(layout);
    assertTrue(alg.computeHomography(observations));
    DMatrixRMaj H = alg.getHomography();
    // test this homography property: x2 = H*x1
    for (int i = 0; i < observations.size(); i++) {
        int gridIndex = observations.get(i).index;
        Point2D_F64 p = observations.get(i);
        Point2D_F64 a = GeometryMath_F64.mult(H, layout.get(gridIndex), new Point2D_F64());
        double diff = a.distance(p);
        assertEquals(0, diff, 1e-8);
    }
}

Example 48 with Se3_F64

use of georegression.struct.se.Se3_F64 in project BoofCV by lessthanoptimal.

the class ChecksMotionNPoint method planarTest.

/**
 * Test a set of random points in general position
 * @param N number of observed point objects
 */
public void planarTest(int N) {
    Se3_F64 motion = new Se3_F64();
    motion.getR().set(ConvertRotation3D_F64.eulerToMatrix(EulerType.XYZ, 0.05, -0.03, 0.02, null));
    motion.getT().set(0.1, -0.1, 0.01);
    checkMotion(N, motion, true);
}

Example 49 with Se3_F64

use of georegression.struct.se.Se3_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 50 with Se3_F64

use of georegression.struct.se.Se3_F64 in project BoofCV by lessthanoptimal.

the class TestPnPJacobianRodrigues method compareToNumerical.

private void compareToNumerical(double noise) {
    Se3_F64 worldToCamera = new Se3_F64();
    ConvertRotation3D_F64.eulerToMatrix(EulerType.XYZ, 0.1, 1, -0.2, worldToCamera.getR());
    worldToCamera.getT().set(-0.3, 0.4, 1);
    List<Point2D3D> observations = new ArrayList<>();
    for (int i = 0; i < numPoints; i++) {
        Point2D3D p = new Point2D3D();
        p.location.set(rand.nextGaussian() * 0.1, rand.nextGaussian() * 0.2, 3 + rand.nextGaussian());
        p.observation = PerspectiveOps.renderPixel(worldToCamera, null, p.location);
        p.observation.x += rand.nextGaussian() * noise;
        p.observation.y += rand.nextGaussian() * noise;
        observations.add(p);
    }
    PnPJacobianRodrigues alg = new PnPJacobianRodrigues();
    alg.setObservations(observations);
    func.setObservations(observations);
    double[] param = new double[codec.getParamLength()];
    codec.encode(worldToCamera, param);
    // DerivativeChecker.jacobianPrint(func, alg, param, 1e-6);
    assertTrue(DerivativeChecker.jacobian(func, alg, param, 1e-6));
}