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

Example 51 with Se3_F64

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

the class TestPnPLepetitEPnP method standardTest.

private void standardTest(final int numIterations) {
    ChecksMotionNPoint test = new ChecksMotionNPoint() {

        @Override
        public Se3_F64 compute(List<AssociatedPair> obs, List<Point3D_F64> locations) {
            PnPLepetitEPnP alg = new PnPLepetitEPnP();
            List<Point2D_F64> currObs = new ArrayList<>();
            for (AssociatedPair a : obs) {
                currObs.add(a.p2);
            }
            Se3_F64 solution = new Se3_F64();
            alg.setNumIterations(numIterations);
            alg.process(locations, currObs, solution);
            return solution;
        }
    };
    for (int i = 0; i < 20; i++) {
        // the minimal case is not tested here since its too unstable
        for (int N = 5; N < 10; N++) {
            test.testNoMotion(N);
            test.standardTest(N);
            test.planarTest(N - 1);
        }
    // System.out.println();
    }
}

Example 52 with Se3_F64

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

the class TestPnPResidualReprojection method basicTest.

@Test
public void basicTest() {
    Se3_F64 motion = new Se3_F64();
    ConvertRotation3D_F64.eulerToMatrix(EulerType.XYZ, 0.1, 1, -0.2, motion.getR());
    motion.getT().set(-0.3, 0.4, 1);
    Point3D_F64 world = new Point3D_F64(0.5, -0.5, 3);
    Point2D_F64 obs = new Point2D_F64();
    Point3D_F64 temp = SePointOps_F64.transform(motion, world, null);
    obs.x = temp.x / temp.z;
    obs.y = temp.y / temp.z;
    PnPResidualReprojection alg = new PnPResidualReprojection();
    // compute errors with perfect model
    double[] error = new double[alg.getN()];
    alg.setModel(motion);
    int index = alg.computeResiduals(new Point2D3D(obs, world), error, 0);
    assertEquals(alg.getN(), index);
    assertEquals(0, error[0], 1e-8);
    assertEquals(0, error[1], 1e-8);
    // compute errors with an incorrect model
    motion.getR().set(2, 1, 2);
    alg.setModel(motion);
    index = alg.computeResiduals(new Point2D3D(obs, world), error, 0);
    assertEquals(alg.getN(), index);
    assertTrue(Math.abs(error[0]) > 1e-8);
    assertTrue(Math.abs(error[1]) > 1e-8);
}

Example 53 with Se3_F64

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

the class CommonTriangulationChecks method createScene.

public void createScene() {
    worldPoint = new Point3D_F64(0.1, -0.2, 4);
    motionWorldToCamera = new ArrayList<>();
    obsPts = new ArrayList<>();
    essential = new ArrayList<>();
    Point3D_F64 cameraPoint = new Point3D_F64();
    for (int i = 0; i < N; i++) {
        // random motion from world to frame 'i'
        Se3_F64 tranWtoI = new Se3_F64();
        if (i > 0) {
            tranWtoI.getR().set(ConvertRotation3D_F64.eulerToMatrix(EulerType.XYZ, rand.nextGaussian() * 0.01, rand.nextGaussian() * 0.05, rand.nextGaussian() * 0.1, null));
            tranWtoI.getT().set(0.2 + rand.nextGaussian() * 0.1, rand.nextGaussian() * 0.1, rand.nextGaussian() * 0.01);
        }
        DMatrixRMaj E = MultiViewOps.createEssential(tranWtoI.getR(), tranWtoI.getT());
        SePointOps_F64.transform(tranWtoI, worldPoint, cameraPoint);
        Point2D_F64 o = new Point2D_F64(cameraPoint.x / cameraPoint.z, cameraPoint.y / cameraPoint.z);
        obsPts.add(o);
        motionWorldToCamera.add(tranWtoI);
        essential.add(E);
    }
}

Example 54 with Se3_F64

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

the class TestPixelDepthLinear method depth2View.

@Test
public void depth2View() {
    // define the camera's motion
    Se3_F64 motion1 = new Se3_F64();
    motion1.getR().set(ConvertRotation3D_F64.eulerToMatrix(EulerType.XYZ, 0.05, -0.03, 0.02, null));
    motion1.getT().set(0.1, -0.1, 0.01);
    // compute the point's location in each camera's view
    Point3D_F64 A = new Point3D_F64(2, 3, 2);
    Point3D_F64 B = SePointOps_F64.transform(motion1, A, null);
    // projected points
    Point2D_F64 x1 = new Point2D_F64(A.x / A.z, A.y / A.z);
    Point2D_F64 x2 = new Point2D_F64(B.x / B.z, B.y / B.z);
    PixelDepthLinear alg = new PixelDepthLinear();
    double depth = alg.depth2View(x1, x2, motion1);
    // see if the origin point was recomputed
    assertEquals(x1.x * depth, A.x, 1e-8);
    assertEquals(x1.y * depth, A.y, 1e-8);
    assertEquals(depth, A.z, 1e-8);
}

Example 55 with Se3_F64

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

the class TestPixelDepthLinear method depthNView.

@Test
public void depthNView() {
    // define the camera's motion
    Se3_F64 motion1 = new Se3_F64();
    motion1.getR().set(ConvertRotation3D_F64.eulerToMatrix(EulerType.XYZ, 0.05, -0.03, 0.02, null));
    motion1.getT().set(0.1, -0.1, 0.01);
    Se3_F64 motion2 = new Se3_F64();
    motion2.getR().set(ConvertRotation3D_F64.eulerToMatrix(EulerType.XYZ, -0.15, -0.3, 0.08, null));
    motion2.getT().set(-0.2, -0.15, 0.2);
    // compute the point's location in each camera's view
    Point3D_F64 A = new Point3D_F64(2, 3, 2);
    Point3D_F64 B = SePointOps_F64.transform(motion1, A, null);
    Point3D_F64 C = SePointOps_F64.transform(motion2, A, null);
    // projected points
    Point2D_F64 x1 = new Point2D_F64(A.x / A.z, A.y / A.z);
    Point2D_F64 x2 = new Point2D_F64(B.x / B.z, B.y / B.z);
    Point2D_F64 x3 = new Point2D_F64(C.x / C.z, C.y / C.z);
    // setup data structures
    List<Se3_F64> listMotion = new ArrayList<>();
    List<Point2D_F64> listPoint = new ArrayList<>();
    listMotion.add(motion1);
    listMotion.add(motion2);
    listPoint.add(x1);
    listPoint.add(x2);
    listPoint.add(x3);
    PixelDepthLinear alg = new PixelDepthLinear();
    double depth = alg.depthNView(listPoint, listMotion);
    // see if the origin point was recomputed
    assertEquals(x1.x * depth, A.x, 1e-8);
    assertEquals(x1.y * depth, A.y, 1e-8);
    assertEquals(depth, A.z, 1e-8);
}