Examples with Rodrigues_F64 georegression.struct.so.Rodrigues_F64 used on opensource projects

Example 11 with Rodrigues_F64

use of georegression.struct.so.Rodrigues_F64 in project BoofCV by lessthanoptimal.

the class BoofTesting method assertEqualsToScale.

/**
 * Checks to see if the two transforms are equal up to a scale factor. Rotation is tested using a single angle
 * and translation using magnitude. NOTE: Scale ambiguity also includes sign ambiguity for translation.
 *
 * @param expected the expected transform
 * @param found the found transform
 * @param tolAngle radians
 * @param tolT fractional error tolernace
 */
public static void assertEqualsToScale(Se3_F64 expected, Se3_F64 found, double tolAngle, double tolT) {
    var R = new DMatrixRMaj(3, 3);
    CommonOps_DDRM.multTransA(expected.R, found.R, R);
    Rodrigues_F64 rod = ConvertRotation3D_F64.matrixToRodrigues(R, null);
    assertEquals(0.0, rod.theta, tolAngle);
    double normFound = found.T.norm();
    double normExpected = expected.T.norm();
    if (normFound == 0.0 || normExpected == 0.0) {
        assertEquals(0.0, normFound, tolT);
        return;
    }
    int largestIdx = -1;
    double largestMag = 0;
    for (int i = 0; i < 3; i++) {
        double mag = Math.abs(expected.T.getIdx(i));
        if (mag > largestMag) {
            largestMag = mag;
            largestIdx = i;
        }
    }
    if (Math.signum(expected.T.getIdx(largestIdx)) != Math.signum(found.T.getIdx(largestIdx)))
        normFound *= -1;
    // they will have the same scale and a norm of 1
    double dx = expected.T.x / normExpected - found.T.x / normFound;
    double dy = expected.T.y / normExpected - found.T.y / normFound;
    double dz = expected.T.z / normExpected - found.T.z / normFound;
    double r = Math.sqrt(dx * dx + dy * dy + dz * dz);
    assertEquals(0.0, r, tolT, "E=" + expected.T + " F=" + found.T);
}

Example 12 with Rodrigues_F64

use of georegression.struct.so.Rodrigues_F64 in project BoofCV by lessthanoptimal.

the class BoofTesting method assertEqualsToScaleS.

/**
 * Checks to see if the two transforms are equal up to a scale factor. Rotation is tested using a single angle
 * and translation using magnitude. NOTE: The direction of the translation is NOT considered part of the
 * sign ambiguity in this came.
 *
 * @param expected the expected transform
 * @param found the found transform
 * @param tolAngle radians
 * @param tolT fractional error tolernace
 */
public static void assertEqualsToScaleS(Se3_F64 expected, Se3_F64 found, double tolAngle, double tolT) {
    var R = new DMatrixRMaj(3, 3);
    CommonOps_DDRM.multTransA(expected.R, found.R, R);
    Rodrigues_F64 rod = ConvertRotation3D_F64.matrixToRodrigues(R, null);
    assertEquals(0.0, rod.theta, tolAngle);
    double normFound = found.T.norm();
    double normExpected = expected.T.norm();
    if (normExpected == 0.0 || normFound == 0.0) {
        assertEquals(0.0, normFound, tolT);
        return;
    }
    // Normalize so that both have a norm of 1
    double dx = expected.T.x / normExpected - found.T.x / normFound;
    double dy = expected.T.y / normExpected - found.T.y / normFound;
    double dz = expected.T.z / normExpected - found.T.z / normFound;
    double r = Math.sqrt(dx * dx + dy * dy + dz * dz);
    assertEquals(0.0, r, tolT, "E=" + expected.T + " F=" + found.T);
}