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

Example 61 with Se3_F64

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

the class TestDistanceTranGivenRotSq method testNoisy.

@Test
public void testNoisy() {
    Se3_F64 keyToCurr = new Se3_F64();
    keyToCurr.getR().set(ConvertRotation3D_F64.eulerToMatrix(EulerType.XYZ, 0.05, -0.03, 0.02, null));
    keyToCurr.getT().set(0.1, -0.1, 0.01);
    Point3D_F64 X = new Point3D_F64(0.1, -0.05, 3);
    Point2D3D obs = new Point2D3D();
    obs.location = X.copy();
    SePointOps_F64.transform(keyToCurr, X, X);
    obs.observation.x = X.x / X.z + 1;
    obs.observation.y = X.y / X.z + 1;
    alg.setRotation(keyToCurr.getR());
    alg.setModel(keyToCurr.getT());
    assertTrue(alg.computeDistance(obs) > 1e-8);
}

Example 62 with Se3_F64

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

the class TestPnPRodriguesCodec method decode_encode.

@Test
public void decode_encode() {
    double[] param = new double[] { 0.1, -0.3, 4, 1, 2, 3 };
    PnPRodriguesCodec alg = new PnPRodriguesCodec();
    double[] found = new double[6];
    Se3_F64 storage = new Se3_F64();
    Se3_F64 storage2 = new Se3_F64();
    alg.decode(param, storage);
    alg.encode(storage, found);
    alg.decode(found, storage2);
    // multiple parameterization can represent the same model, so test using the model
    assertTrue(storage.T.isIdentical(storage2.T, 1e-8));
    assertTrue(MatrixFeatures_DDRM.isIdentical(storage.R, storage2.R, 1e-8));
}

Example 63 with Se3_F64

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

the class TestPoseRodriguesCodec method encode_decode.

@Test
public void encode_decode() {
    double[] orig = new double[] { .1, .2, .3, 4, 5, 6 };
    double[] found = new double[6];
    Se3_F64 encoded = new Se3_F64();
    PnPRodriguesCodec codec = new PnPRodriguesCodec();
    assertEquals(6, codec.getParamLength());
    codec.decode(orig, encoded);
    codec.encode(encoded, found);
    for (int i = 0; i < 6; i++) {
        assertEquals(orig[i], found[i], 1e-6);
    }
}

Example 64 with Se3_F64

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

the class TestPositionFromPairLinear2 method compute.

@Override
public Se3_F64 compute(List<AssociatedPair> obs, List<Point3D_F64> locations) {
    List<Point2D_F64> l = new ArrayList<>();
    for (AssociatedPair p : obs) l.add(p.p2);
    assertTrue(alg.process(motion.getR(), locations, l));
    Se3_F64 found = new Se3_F64();
    found.R.set(motion.R);
    found.T.set(alg.getT());
    return found;
}

Example 65 with Se3_F64

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

the class TestRectifyCalibrated method compareWithKnown.

/**
 * Compare results from rectified transform and a set of camera which are already rectified.
 */
@Test
public void compareWithKnown() {
    DMatrixRMaj K = new DMatrixRMaj(3, 3, true, 300, 0, 200, 0, 400, 205, 0, 0, 1);
    // transforms are world to camera, but I'm thinking camera to world, which is why invert
    Se3_F64 poseR1 = createPose(0, 0, 0, 0.1, 0, 0.1).invert(null);
    Se3_F64 poseR2 = createPose(0, 0, 0, 1, 0, 0.1).invert(null);
    // only rotate around the y-axis so that the rectified coordinate system will have to be
    // the same as the global
    Se3_F64 poseA1 = createPose(0, 0.05, 0, 0.1, 0, 0.1).invert(null);
    Se3_F64 poseA2 = createPose(0, -0.1, 0, 1, 0, 0.1).invert(null);
    RectifyCalibrated alg = new RectifyCalibrated();
    alg.process(K, poseA1, K, poseA2);
    // original camera matrix
    DMatrixRMaj foundP1 = PerspectiveOps.createCameraMatrix(poseA1.getR(), poseA1.getT(), K, null);
    DMatrixRMaj foundP2 = PerspectiveOps.createCameraMatrix(poseA2.getR(), poseA2.getT(), K, null);
    // apply rectification transform
    DMatrixRMaj temp = new DMatrixRMaj(3, 4);
    CommonOps_DDRM.mult(alg.getRect1(), foundP1, temp);
    foundP1.set(temp);
    CommonOps_DDRM.mult(alg.getRect2(), foundP2, temp);
    foundP2.set(temp);
    CommonOps_DDRM.scale(0.1 / Math.abs(foundP1.get(2, 3)), foundP1);
    Point3D_F64 X = new Point3D_F64(0, 0, 3);
    // compare results, both should match because of rotation only being around y-axis
    assertEquals(PerspectiveOps.renderPixel(poseR1, K, X).x, PerspectiveOps.renderPixel(foundP1, X).x, 1e-5);
    assertEquals(PerspectiveOps.renderPixel(poseR1, K, X).y, PerspectiveOps.renderPixel(foundP1, X).y, 1e-5);
    assertEquals(PerspectiveOps.renderPixel(poseR2, K, X).x, PerspectiveOps.renderPixel(foundP2, X).x, 1e-5);
    assertEquals(PerspectiveOps.renderPixel(poseR2, K, X).y, PerspectiveOps.renderPixel(foundP2, X).y, 1e-5);
}