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

Example 26 with Point3D_F64

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

the class BenchmarkStabilityFundamental method createScenePlane.

public void createScenePlane() {
    scene = new ArrayList<>();
    for (int i = 0; i < totalPoints; i++) {
        Point3D_F64 p = new Point3D_F64();
        p.x = (rand.nextDouble() - 0.5) * 2 * sceneRadius;
        p.y = (rand.nextDouble() - 0.5) * 2 * sceneRadius;
        p.z = sceneCenterZ;
        scene.add(p);
    }
}

Example 27 with Point3D_F64

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

the class MultiViewOps method decomposeCameraMatrix.

/**
 * <p>
 * Decomposes a camera matrix P=A*[R|T], where A is an upper triangular camera calibration
 * matrix, R is a rotation matrix, and T is a translation vector.
 *
 * <ul>
 * <li> NOTE: There are multiple valid solutions to this problem and only one solution is returned.
 * <li> NOTE: The camera center will be on the plane at infinity.
 * </ul>
 * </p>
 *
 * @param P Input: Camera matrix, 3 by 4
 * @param K Output: Camera calibration matrix, 3 by 3.
 * @param pose Output: The rotation and translation.
 */
public static void decomposeCameraMatrix(DMatrixRMaj P, DMatrixRMaj K, Se3_F64 pose) {
    DMatrixRMaj KR = new DMatrixRMaj(3, 3);
    CommonOps_DDRM.extract(P, 0, 3, 0, 3, KR, 0, 0);
    QRDecomposition<DMatrixRMaj> qr = DecompositionFactory_DDRM.qr(3, 3);
    if (!CommonOps_DDRM.invert(KR))
        throw new RuntimeException("Inverse failed!  Bad input?");
    if (!qr.decompose(KR))
        throw new RuntimeException("QR decomposition failed!  Bad input?");
    DMatrixRMaj U = qr.getQ(null, false);
    DMatrixRMaj B = qr.getR(null, false);
    if (!CommonOps_DDRM.invert(U, pose.getR()))
        throw new RuntimeException("Inverse failed!  Bad input?");
    Point3D_F64 KT = new Point3D_F64(P.get(0, 3), P.get(1, 3), P.get(2, 3));
    GeometryMath_F64.mult(B, KT, pose.getT());
    if (!CommonOps_DDRM.invert(B, K))
        throw new RuntimeException("Inverse failed!  Bad input?");
    CommonOps_DDRM.scale(1.0 / K.get(2, 2), K);
}

Example 28 with Point3D_F64

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

the class MultiViewOps method extractCameraMatrices.

/**
 * <p>
 * Extract the camera matrices up to a common projective transform.
 * </p>
 *
 * <p>
 * NOTE: The camera matrix for the first view is assumed to be P1 = [I|0].
 * </p>
 *
 * @param tensor Trifocal tensor.  Not modified.
 * @param P2 Output: 3x4 camera matrix for views 1 to 2. Modified.
 * @param P3 Output: 3x4 camera matrix for views 1 to 3. Modified.
 */
public static void extractCameraMatrices(TrifocalTensor tensor, DMatrixRMaj P2, DMatrixRMaj P3) {
    // extract the epipoles
    Point3D_F64 e2 = new Point3D_F64();
    Point3D_F64 e3 = new Point3D_F64();
    extractEpipoles(tensor, e2, e3);
    // storage for intermediate results
    Point3D_F64 temp0 = new Point3D_F64();
    Point3D_F64 column = new Point3D_F64();
    // temp1 = [e3*e3^T -I]
    DMatrixRMaj temp1 = new DMatrixRMaj(3, 3);
    for (int i = 0; i < 3; i++) {
        for (int j = 0; j < 3; j++) {
            temp1.set(i, j, e3.getIndex(i) * e3.getIndex(j));
        }
        temp1.set(i, i, temp1.get(i, i) - 1);
    }
    // compute the camera matrices one column at a time
    for (int i = 0; i < 3; i++) {
        DMatrixRMaj T = tensor.getT(i);
        GeometryMath_F64.mult(T, e3, column);
        P2.set(0, i, column.x);
        P2.set(1, i, column.y);
        P2.set(2, i, column.z);
        P2.set(i, 3, e2.getIndex(i));
        GeometryMath_F64.multTran(T, e2, temp0);
        GeometryMath_F64.mult(temp1, temp0, column);
        P3.set(0, i, column.x);
        P3.set(1, i, column.y);
        P3.set(2, i, column.z);
        P3.set(i, 3, e3.getIndex(i));
    }
}

Example 29 with Point3D_F64

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

the class MultiViewOps method extractFundamental.

/**
 * <p>
 * Extract the fundamental matrices between views 1 + 2 and views 1 + 3.  The returned Fundamental
 * matrices will have the following properties: x<sub>i</sub><sup>T</sup>*Fi*x<sub>1</sub> = 0, where i is view 2 or 3.
 * </p>
 *
 * <p>
 * NOTE: The first camera is assumed to have the camera matrix of P1 = [I|0].  Thus observations in pixels for
 * the first camera will not meet the epipolar constraint when applied to the returned fundamental matrices.
 * </p>
 *
 * @param tensor Trifocal tensor.  Not modified.
 * @param F2 Output: Fundamental matrix for views 1 and 2. Modified.
 * @param F3 Output: Fundamental matrix for views 1 and 3. Modified.
 */
public static void extractFundamental(TrifocalTensor tensor, DMatrixRMaj F2, DMatrixRMaj F3) {
    // extract the epipoles
    Point3D_F64 e2 = new Point3D_F64();
    Point3D_F64 e3 = new Point3D_F64();
    extractEpipoles(tensor, e2, e3);
    // storage for intermediate results
    Point3D_F64 temp0 = new Point3D_F64();
    Point3D_F64 column = new Point3D_F64();
    // compute the Fundamental matrices one column at a time
    for (int i = 0; i < 3; i++) {
        DMatrixRMaj T = tensor.getT(i);
        GeometryMath_F64.mult(T, e3, temp0);
        GeometryMath_F64.cross(e2, temp0, column);
        F2.set(0, i, column.x);
        F2.set(1, i, column.y);
        F2.set(2, i, column.z);
        GeometryMath_F64.multTran(T, e2, temp0);
        GeometryMath_F64.cross(e3, temp0, column);
        F3.set(0, i, column.x);
        F3.set(1, i, column.y);
        F3.set(2, i, column.z);
    }
}

Example 30 with Point3D_F64

use of georegression.struct.point.Point3D_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;
    }
}