Examples with UtilVector3D_F64 georegression.geometry.UtilVector3D_F64 used on opensource projects

Example 1 with UtilVector3D_F64

use of georegression.geometry.UtilVector3D_F64 in project BoofCV by lessthanoptimal.

the class Zhang99DecomposeHomography method decompose.

/**
 * Compute the rigid body motion that composes the homography matrix H.  It is assumed
 * that H was computed using {@link Zhang99ComputeTargetHomography}.
 *
 * @param H homography matrix.
 * @return Found camera motion.
 */
public Se3_F64 decompose(DMatrixRMaj H) {
    // step through each calibration grid and compute its parameters
    DMatrixRMaj[] h = SpecializedOps_DDRM.splitIntoVectors(H, true);
    // lambda = 1/norm(inv(K)*h1) or 1/norm(inv(K)*h2)
    // use the average to attempt to reduce error
    CommonOps_DDRM.mult(K_inv, h[0], temp);
    double lambda = NormOps_DDRM.normF(temp);
    CommonOps_DDRM.mult(K_inv, h[1], temp);
    lambda += NormOps_DDRM.normF(temp);
    lambda = 2.0 / lambda;
    // compute the column in the rotation matrix
    CommonOps_DDRM.mult(lambda, K_inv, h[0], r1);
    CommonOps_DDRM.mult(lambda, K_inv, h[1], r2);
    CommonOps_DDRM.mult(lambda, K_inv, h[2], t);
    Vector3D_F64 v1 = UtilVector3D_F64.convert(r1);
    Vector3D_F64 v2 = UtilVector3D_F64.convert(r2);
    Vector3D_F64 v3 = v1.cross(v2);
    UtilVector3D_F64.createMatrix(R, v1, v2, v3);
    Se3_F64 ret = new Se3_F64();
    // the R matrix is probably not a real rotation matrix.  So find
    // the closest real rotation matrix
    ConvertRotation3D_F64.approximateRotationMatrix(R, ret.getR());
    ret.getT().set(t.data[0], t.data[1], t.data[2]);
    return ret;
}

Example 2 with UtilVector3D_F64

use of georegression.geometry.UtilVector3D_F64 in project BoofCV by lessthanoptimal.

the class TestNarrowToWidePtoP_F64 method checkFOVBounds.

/**
 * Request points at the border and see if it has the expected vertical and horizontal FOV
 */
@Test
public void checkFOVBounds() {
    NarrowToWidePtoP_F64 alg = createAlg();
    Point2D_F64 foundA = new Point2D_F64();
    Point2D_F64 foundB = new Point2D_F64();
    Point3D_F64 vA = new Point3D_F64();
    Point3D_F64 vB = new Point3D_F64();
    // Compute the horizontal FOV
    alg.compute(0, 250, foundA);
    alg.compute(500, 250, foundB);
    Point2Transform3_F64 wideToSphere = createModelWide().undistortPtoS_F64();
    wideToSphere.compute(foundA.x, foundA.y, vA);
    wideToSphere.compute(foundB.x, foundB.y, vB);
    double found = UtilVector3D_F64.acute(new Vector3D_F64(vA), new Vector3D_F64(vB));
    double expected = 2.0 * Math.atan(250.0 / 400.0);
    assertEquals(expected, found, 0.01);
    // Compute the vertical FOV
    alg.compute(250, 0, foundA);
    alg.compute(250, 500, foundB);
    wideToSphere.compute(foundA.x, foundA.y, vA);
    wideToSphere.compute(foundB.x, foundB.y, vB);
    found = UtilVector3D_F64.acute(new Vector3D_F64(vA), new Vector3D_F64(vB));
    expected = 2.0 * Math.atan(250.0 / 400.0);
    assertEquals(expected, found, 0.001);
}