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

Example 96 with Se3_F64

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

the class TestComputeObservationAcuteAngle method simpleWithRotation.

@Test
public void simpleWithRotation() {
    ComputeObservationAcuteAngle alg = new ComputeObservationAcuteAngle();
    Se3_F64 fromAtoB = new Se3_F64();
    fromAtoB.getT().set(-2, 0, 0);
    ConvertRotation3D_F64.eulerToMatrix(EulerType.XYZ, 0, -Math.PI / 4.0, 0, fromAtoB.getR());
    Point2D_F64 a = new Point2D_F64(0, 0);
    Point2D_F64 b = new Point2D_F64(0, 0);
    alg.setFromAtoB(fromAtoB);
    assertEquals(Math.PI / 4.0, alg.computeAcuteAngle(a, b), 1e-8);
}

Example 97 with Se3_F64

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

the class TestComputeObservationAcuteAngle method simpleNoRotation.

@Test
public void simpleNoRotation() {
    ComputeObservationAcuteAngle alg = new ComputeObservationAcuteAngle();
    Se3_F64 fromAtoB = new Se3_F64();
    fromAtoB.getT().set(-2, 0, 0);
    Point2D_F64 a = new Point2D_F64(0, 0);
    Point2D_F64 b = new Point2D_F64(0, 0);
    alg.setFromAtoB(fromAtoB);
    assertEquals(0, alg.computeAcuteAngle(a, b), 1e-8);
    b.set(-1, 0);
    assertEquals(Math.PI / 4.0, alg.computeAcuteAngle(a, b), 1e-8);
}

Example 98 with Se3_F64

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

the class TestCreateSyntheticOverheadViewPL method checkRender.

@Test
public void checkRender() {
    // Easier to make up a plane in this direction
    Se3_F64 cameraToPlane = new Se3_F64();
    ConvertRotation3D_F64.eulerToMatrix(EulerType.XYZ, UtilAngle.degreeToRadian(0), 0, 0, cameraToPlane.getR());
    cameraToPlane.getT().set(0, -5, 0);
    Se3_F64 planeToCamera = cameraToPlane.invert(null);
    CreateSyntheticOverheadViewPL<GrayF32> alg = new CreateSyntheticOverheadViewPL<>(InterpolationType.BILINEAR, 3, GrayF32.class);
    alg.configure(param, planeToCamera, centerX, centerY, cellSize, overheadW, overheadH);
    Planar<GrayF32> input = new Planar<>(GrayF32.class, width, height, 3);
    for (int i = 0; i < 3; i++) ImageMiscOps.fill(input.getBand(i), 10 + i);
    Planar<GrayF32> output = new Planar<>(GrayF32.class, overheadW, overheadH, 3);
    alg.process(input, output);
    for (int i = 0; i < 3; i++) {
        GrayF32 o = output.getBand(i);
        // check parts that shouldn't be in view
        assertEquals(0, o.get(0, 300), 1e-8);
        assertEquals(0, o.get(5, 0), 1e-8);
        assertEquals(0, o.get(5, 599), 1e-8);
        // check areas that should be in view
        assertEquals(10 + i, o.get(499, 300), 1e-8);
    }
}

Example 99 with Se3_F64

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

the class TestCreateSyntheticOverheadViewS method checkRender.

@Test
public void checkRender() {
    // Easier to make up a plane in this direction
    Se3_F64 cameraToPlane = new Se3_F64();
    ConvertRotation3D_F64.eulerToMatrix(EulerType.XYZ, UtilAngle.degreeToRadian(0), 0, 0, cameraToPlane.getR());
    cameraToPlane.getT().set(0, -5, 0);
    Se3_F64 planeToCamera = cameraToPlane.invert(null);
    InterpolatePixelS<GrayF32> interp = FactoryInterpolation.bilinearPixelS(GrayF32.class, BorderType.EXTENDED);
    CreateSyntheticOverheadViewS<GrayF32> alg = new CreateSyntheticOverheadViewS<>(interp);
    alg.configure(param, planeToCamera, centerX, centerY, cellSize, overheadW, overheadH);
    GrayF32 input = new GrayF32(width, height);
    ImageMiscOps.fill(input, 10);
    GrayF32 output = new GrayF32(overheadW, overheadH);
    alg.process(input, output);
    // check parts that shouldn't be in view
    assertEquals(0, output.get(0, 300), 1e-8);
    assertEquals(0, output.get(5, 0), 1e-8);
    assertEquals(0, output.get(5, 599), 1e-8);
    // check areas that should be in view
    assertEquals(10, output.get(499, 300), 1e-8);
}

Example 100 with Se3_F64

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

the class CheckVisualOdometryMonoPlaneSim method motionCheck.

public void motionCheck(double angleRate, double forwardRate) {
    // Easier to make up a plane in this direction
    Se3_F64 cameraToPlane = new Se3_F64();
    ConvertRotation3D_F64.eulerToMatrix(EulerType.XYZ, UtilAngle.degreeToRadian(cameraAngle), 0.1, 0.0, cameraToPlane.getR());
    cameraToPlane.getT().set(0, -2, 0);
    Se3_F64 planeToCamera = cameraToPlane.invert(null);
    Se3_F64 worldToCurr = new Se3_F64();
    Se3_F64 worldToCamera = new Se3_F64();
    algorithm.reset();
    algorithm.setCalibration(new MonoPlaneParameters(param, planeToCamera));
    for (int i = 0; i < 10; i++) {
        // System.out.println("-------- Real rotY = "+angleRate*i);
        // move forward
        worldToCurr.getT().z = -i * forwardRate;
        ConvertRotation3D_F64.rotY(angleRate * i, worldToCurr.getR());
        worldToCurr.concat(planeToCamera, worldToCamera);
        // render the images
        setIntrinsic(param);
        left.setTo(render(worldToCamera));
        // process the images
        assertTrue(algorithm.process(left));
        // Compare to truth.  Only go for a crude approximation
        Se3_F64 foundWorldToCamera = algorithm.getCameraToWorld().invert(null);
        Se3_F64 foundWorldToCurr = foundWorldToCamera.concat(cameraToPlane, null);
        // worldToCurr.getT().print();
        // foundWorldToCurr.getT().print();
        // worldToCurr.getR().print();
        // foundWorldToCurr.getR().print();
        assertTrue(MatrixFeatures_DDRM.isIdentical(foundWorldToCurr.getR(), worldToCurr.getR(), 0.1));
        assertTrue(foundWorldToCurr.getT().distance(worldToCurr.getT()) < tolerance);
    }
}