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

Example 21 with Se3_F64

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

the class TestCalibPoseAndPointRodiguesCodec method checkZeroAngle.

/**
 * A pathological situation for Rodigues coordinates is zero degrees
 */
@Test
public void checkZeroAngle() {
    int numViews = 1;
    int numPoints = 0;
    // Set a world transform of zero degrees
    CalibratedPoseAndPoint model = new CalibratedPoseAndPoint();
    model.configure(numViews, numPoints);
    model.getWorldToCamera(0).getT().set(1, 2, 3);
    // encode the model
    CalibPoseAndPointRodriguesCodec codec = new CalibPoseAndPointRodriguesCodec();
    codec.configure(numViews, numPoints, numViews, new boolean[] { false });
    double[] param = new double[codec.getParamLength()];
    codec.encode(model, param);
    // decode the model
    CalibratedPoseAndPoint found = new CalibratedPoseAndPoint();
    found.configure(numViews, numPoints);
    codec.decode(param, found);
    // compare results
    for (int i = 0; i < numViews; i++) {
        Se3_F64 o = model.getWorldToCamera(i);
        Se3_F64 f = found.getWorldToCamera(i);
        assertEquals(0, o.getT().distance(f.getT()), 1e-8);
        assertTrue(MatrixFeatures_DDRM.isIdentical(o.getR(), f.getR(), 1e-8));
    }
}

Example 22 with Se3_F64

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

the class TestPnPRefineRodrigues method noisy.

@Test
public void noisy() {
    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);
    generateScene(50, motion, false);
    PnPRefineRodrigues alg = new PnPRefineRodrigues(1e-20, 500);
    Se3_F64 n = motion.copy();
    n.getT().setX(0);
    assertTrue(alg.fitModel(pointPose, n, found));
    assertEquals(motion.getT().getX(), found.getX(), 1e-5);
    assertEquals(motion.getT().getY(), found.getY(), 1e-5);
    assertEquals(motion.getT().getZ(), found.getZ(), 1e-5);
}

Example 23 with Se3_F64

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

the class VisOdomDualTrackPnP method refineMotionEstimate.

/**
 * Non-linear refinement of motion estimate
 */
private void refineMotionEstimate() {
    // use observations from the inlier set
    List<Stereo2D3D> data = new ArrayList<>();
    int N = matcher.getMatchSet().size();
    for (int i = 0; i < N; i++) {
        int index = matcher.getInputIndex(i);
        PointTrack l = candidates.get(index);
        LeftTrackInfo info = l.getCookie();
        PointTrack r = info.right;
        Stereo2D3D stereo = info.location;
        // compute normalized image coordinate for track in left and right image
        leftImageToNorm.compute(l.x, l.y, info.location.leftObs);
        rightImageToNorm.compute(r.x, r.y, info.location.rightObs);
        data.add(stereo);
    }
    // refine the motion estimate using non-linear optimization
    Se3_F64 keyToCurr = currToKey.invert(null);
    Se3_F64 found = new Se3_F64();
    if (modelRefiner.fitModel(data, keyToCurr, found)) {
        found.invert(currToKey);
    }
}

Example 24 with Se3_F64

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

the class VisOdomDualTrackPnP method changePoseToReference.

/**
 * Updates the relative position of all points so that the current frame is the reference frame.  Mathematically
 * this is not needed, but should help keep numbers from getting too large.
 */
private void changePoseToReference() {
    Se3_F64 keyToCurr = currToKey.invert(null);
    List<PointTrack> all = trackerLeft.getAllTracks(null);
    for (PointTrack t : all) {
        LeftTrackInfo p = t.getCookie();
        SePointOps_F64.transform(keyToCurr, p.location.location, p.location.location);
    }
    concatMotion();
}

Example 25 with Se3_F64

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

the class VisOdomDualTrackPnP method estimateMotion.

/**
 * Given the set of active tracks, estimate the cameras motion robustly
 * @return
 */
private boolean estimateMotion() {
    // organize the data
    List<Stereo2D3D> data = new ArrayList<>();
    for (PointTrack l : candidates) {
        LeftTrackInfo info = l.getCookie();
        PointTrack r = info.right;
        Stereo2D3D stereo = info.location;
        // compute normalized image coordinate for track in left and right image
        leftImageToNorm.compute(l.x, l.y, info.location.leftObs);
        rightImageToNorm.compute(r.x, r.y, info.location.rightObs);
        data.add(stereo);
    }
    // Robustly estimate left camera motion
    if (!matcher.process(data))
        return false;
    Se3_F64 keyToCurr = matcher.getModelParameters();
    keyToCurr.invert(currToKey);
    // mark tracks that are in the inlier set
    int N = matcher.getMatchSet().size();
    for (int i = 0; i < N; i++) {
        int index = matcher.getInputIndex(i);
        LeftTrackInfo info = candidates.get(index).getCookie();
        info.lastInlier = tick;
    }
    return true;
}