Examples with Estimate1ofEpipolar boofcv.abst.geo.Estimate1ofEpipolar used on opensource projects

Example 11 with Estimate1ofEpipolar

use of boofcv.abst.geo.Estimate1ofEpipolar in project BoofCV by lessthanoptimal.

the class TestSe3FromEssentialGenerator method simpleTest.

@Test
public void simpleTest() {
    // define motion
    Se3_F64 motion = new Se3_F64();
    motion.getR().set(ConvertRotation3D_F64.eulerToMatrix(EulerType.XYZ, 0.1, -0.05, -0.01, null));
    motion.getT().set(2, -0.1, 0.1);
    // define observations
    List<AssociatedPair> obs = new ArrayList<>();
    for (int i = 0; i < 8; i++) {
        Point3D_F64 p = new Point3D_F64(rand.nextGaussian() * 0.1, rand.nextGaussian() * 0.1, 3 + rand.nextGaussian() * 0.1);
        AssociatedPair o = new AssociatedPair();
        o.p1.x = p.x / p.z;
        o.p1.y = p.y / p.z;
        Point3D_F64 pp = new Point3D_F64();
        SePointOps_F64.transform(motion, p, pp);
        o.p2.x = pp.x / pp.z;
        o.p2.y = pp.y / pp.z;
        obs.add(o);
    }
    // create alg
    Estimate1ofEpipolar essentialAlg = FactoryMultiView.computeFundamental_1(EnumFundamental.LINEAR_8, 0);
    TriangulateTwoViewsCalibrated triangulate = FactoryMultiView.triangulateTwoGeometric();
    Se3FromEssentialGenerator alg = new Se3FromEssentialGenerator(essentialAlg, triangulate);
    Se3_F64 found = new Se3_F64();
    // recompute the motion
    assertTrue(alg.generate(obs, found));
    // account for scale difference
    double scale = found.getT().norm() / motion.getT().norm();
    assertTrue(MatrixFeatures_DDRM.isIdentical(motion.getR(), found.getR(), 1e-6));
    assertEquals(motion.getT().x * scale, found.getT().x, 1e-8);
    assertEquals(motion.getT().y * scale, found.getT().y, 1e-8);
    assertEquals(motion.getT().z * scale, found.getT().z, 1e-8);
}