Examples with Vector3D_F64 georegression.struct.point.Vector3D_F64 used on opensource projects

Example 16 with Vector3D_F64

use of georegression.struct.point.Vector3D_F64 in project BoofCV by lessthanoptimal.

the class BenchmarkStabilityPnP method evaluateMinimal.

public void evaluateMinimal(double pixelSigma, boolean isPlanar, int numTrials) {
    // make sure each test case has the same random seed
    rand = new Random(234);
    double totalEuler = 0;
    double totalTran = 0;
    int totalFail = 0;
    for (int i = 0; i < numTrials; i++) {
        init(target.getMinimumPoints(), false, isPlanar);
        addPixelNoise(pixelSigma);
        if (!target.process(observationPose, found)) {
            totalFail++;
            continue;
        }
        double[] expectedEuler = ConvertRotation3D_F64.matrixToEuler(motion.getR(), EulerType.XYZ, (double[]) null);
        double[] foundEuler = ConvertRotation3D_F64.matrixToEuler(found.getR(), EulerType.XYZ, (double[]) null);
        Vector3D_F64 expectedTran = motion.getT();
        Vector3D_F64 foundTran = found.getT();
        double errorTran = expectedTran.distance(foundTran);
        double distanceTrue = expectedTran.norm();
        double errorEuler = 0;
        double sum = 0;
        for (int j = 0; j < 3; j++) {
            double e = expectedEuler[j] - foundEuler[j];
            errorEuler += e * e;
            sum += expectedEuler[j];
        }
        errorEuler = 100.0 * Math.sqrt(errorEuler) / Math.sqrt(sum);
        errorTran = 100.0 * (errorTran / distanceTrue);
        // System.out.println(errorTran);
        totalEuler += errorEuler;
        totalTran += errorTran;
    }
    int N = numTrials - totalFail;
    System.out.printf("%20s N = %d failed %.3f%% euler = %3f%% tran = %3f%%\n", name, target.getMinimumPoints(), (totalFail / (double) numTrials), (totalEuler / N), (totalTran / N));
}

Example 17 with Vector3D_F64

use of georegression.struct.point.Vector3D_F64 in project BoofCV by lessthanoptimal.

the class BenchmarkStabilityPnP method evaluateObservationNoise.

public void evaluateObservationNoise(double minPixelError, double maxPixelError, int N, boolean isPlanar) {
    System.out.println("------------------------");
    rand = new Random(234);
    for (int i = 0; i <= N; i++) {
        double mag = (maxPixelError - minPixelError) * i / N + minPixelError;
        init(NUM_POINTS, false, isPlanar);
        addPixelNoise(mag);
        if (!target.process(observationPose, found))
            throw new RuntimeException("Not expected to fail");
        double[] expectedEuler = ConvertRotation3D_F64.matrixToEuler(motion.getR(), EulerType.XYZ, (double[]) null);
        double[] foundEuler = ConvertRotation3D_F64.matrixToEuler(found.getR(), EulerType.XYZ, (double[]) null);
        Vector3D_F64 expectedTran = motion.getT();
        Vector3D_F64 foundTran = found.getT();
        double errorTran = expectedTran.distance(foundTran);
        double errorEuler = 0;
        double sum = 0;
        for (int j = 0; j < 3; j++) {
            double e = expectedEuler[j] - foundEuler[j];
            errorEuler += e * e;
            sum += expectedEuler[j];
        }
        errorEuler = 100 * Math.sqrt(errorEuler) / Math.sqrt(sum);
        System.out.printf("%3d angle %6.2f%% translation %6.2e\n", i, errorEuler, errorTran);
    }
}

Example 18 with Vector3D_F64

use of georegression.struct.point.Vector3D_F64 in project BoofCV by lessthanoptimal.

the class CalibPoseAndPointRodriguesCodec method encode.

@Override
public void encode(CalibratedPoseAndPoint model, double[] param) {
    int paramIndex = 0;
    // first decode the transformation
    for (int i = 0; i < numViews; i++) {
        // don't encode if it is already known
        if (knownView[i])
            continue;
        Se3_F64 se = model.getWorldToCamera(i);
        // otherwise Rodrigues will have issues with the noise
        if (!svd.decompose(se.getR()))
            throw new RuntimeException("SVD failed");
        DMatrixRMaj U = svd.getU(null, false);
        DMatrixRMaj V = svd.getV(null, false);
        CommonOps_DDRM.multTransB(U, V, R);
        // extract Rodrigues coordinates
        ConvertRotation3D_F64.matrixToRodrigues(R, rotation);
        param[paramIndex++] = rotation.unitAxisRotation.x * rotation.theta;
        param[paramIndex++] = rotation.unitAxisRotation.y * rotation.theta;
        param[paramIndex++] = rotation.unitAxisRotation.z * rotation.theta;
        Vector3D_F64 T = se.getT();
        param[paramIndex++] = T.x;
        param[paramIndex++] = T.y;
        param[paramIndex++] = T.z;
    }
    for (int i = 0; i < numPoints; i++) {
        Point3D_F64 p = model.getPoint(i);
        param[paramIndex++] = p.x;
        param[paramIndex++] = p.y;
        param[paramIndex++] = p.z;
    }
}

Example 19 with Vector3D_F64

use of georegression.struct.point.Vector3D_F64 in project BoofCV by lessthanoptimal.

the class DecomposeEssential method extractTransform.

/**
 * There are four possible reconstructions from an essential matrix.  This function will compute different
 * permutations depending on optionA and optionB being true or false.
 */
private void extractTransform(DMatrixRMaj U, DMatrixRMaj V, DMatrixRMaj S, Se3_F64 se, boolean optionA, boolean optionB) {
    DMatrixRMaj R = se.getR();
    Vector3D_F64 T = se.getT();
    // extract rotation
    if (optionA)
        CommonOps_DDRM.mult(U, Rz, temp);
    else
        CommonOps_DDRM.multTransB(U, Rz, temp);
    CommonOps_DDRM.multTransB(temp, V, R);
    // extract screw symmetric translation matrix
    if (optionB)
        CommonOps_DDRM.multTransB(U, Rz, temp);
    else
        CommonOps_DDRM.mult(U, Rz, temp);
    CommonOps_DDRM.mult(temp, S, temp2);
    CommonOps_DDRM.multTransB(temp2, U, temp);
    T.x = temp.get(2, 1);
    T.y = temp.get(0, 2);
    T.z = temp.get(1, 0);
}

Example 20 with Vector3D_F64

use of georegression.struct.point.Vector3D_F64 in project BoofCV by lessthanoptimal.

the class DecomposeHomography method createMirrorSolution.

private void createMirrorSolution(int origIndex, int index) {
    Se3_F64 origSE = solutionsSE.get(origIndex);
    Vector3D_F64 origN = solutionsN.get(origIndex);
    Se3_F64 se = solutionsSE.get(index);
    Vector3D_F64 N = solutionsN.get(index);
    se.getR().set(origSE.getR());
    N.x = -origN.x;
    N.y = -origN.y;
    N.z = -origN.z;
    Vector3D_F64 origT = origSE.getT();
    Vector3D_F64 T = se.getT();
    T.x = -origT.x;
    T.y = -origT.y;
    T.z = -origT.z;
}