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

Example 26 with Vector3D_F64

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

the class PoseFromPairLinear6 method massageResults.

/**
 * <p>
 * Since the linear solution is probably not an exact rotation matrix, this code finds the best
 * approximation.
 * </p>
 *
 * See page 280 of [1]
 */
private void massageResults() {
    DMatrixRMaj R = motion.getR();
    Vector3D_F64 T = motion.getT();
    if (!svd.decompose(R))
        throw new RuntimeException("SVD Failed");
    CommonOps_DDRM.multTransB(svd.getU(null, false), svd.getV(null, false), R);
    // determinant should be +1
    double det = CommonOps_DDRM.det(R);
    if (det < 0)
        CommonOps_DDRM.scale(-1, R);
    // compute the determinant of the singular matrix
    double b = 1.0;
    double[] s = svd.getSingularValues();
    for (int i = 0; i < svd.numberOfSingularValues(); i++) {
        b *= s[i];
    }
    b = Math.signum(det) / Math.pow(b, 1.0 / 3.0);
    GeometryMath_F64.scale(T, b);
}

Example 27 with Vector3D_F64

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

the class RectifyCalibrated method selectAxises.

/**
 * Selects axises of new coordinate system
 */
private void selectAxises(SimpleMatrix R, SimpleMatrix c1, SimpleMatrix c2) {
    // --------- Compute the new x-axis
    v1.set(c2.get(0) - c1.get(0), c2.get(1) - c1.get(1), c2.get(2) - c1.get(2));
    v1.normalize();
    // --------- Compute the new y-axis
    // cross product of old z axis and new x axis
    // According to the paper [1] this choice is arbitrary, however it is not.  By selecting
    // the original axis the similarity with the first view is maximized.  The other extreme
    // would be to make it perpendicular, resulting in an unusable rectification.
    // extract old z-axis from rotation matrix
    Vector3D_F64 oldZ = new Vector3D_F64(R.get(2, 0), R.get(2, 1), R.get(2, 2));
    GeometryMath_F64.cross(oldZ, v1, v2);
    v2.normalize();
    // ---------- Compute the new z-axis
    // simply the process product of the first two
    GeometryMath_F64.cross(v1, v2, v3);
    v3.normalize();
}

Example 28 with Vector3D_F64

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

the class RectifyFundamental method computeHZero.

/**
 * H0 = H*M
 * P=[M|m] from canonical camera
 */
private SimpleMatrix computeHZero(DMatrixRMaj F, Point3D_F64 e2, SimpleMatrix H) {
    Vector3D_F64 v = new Vector3D_F64(.1, 0.5, .2);
    // need to make sure M is not singular for this technique to work
    SimpleMatrix P = SimpleMatrix.wrap(MultiViewOps.canonicalCamera(F, e2, v, 1));
    SimpleMatrix M = P.extractMatrix(0, 3, 0, 3);
    return H.mult(M);
}

Example 29 with Vector3D_F64

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

the class TriangulateGeometric method triangulate.

/**
 * <p>
 * Given two observations of the same point from two views and a known motion between the
 * two views, triangulate the point's 3D position in camera 'a' reference frame.
 * </p>
 *
 * @param a Observation from camera view 'a' in normalized coordinates. Not modified.
 * @param b Observation from camera view 'b' in normalized coordinates. Not modified.
 * @param fromAtoB Transformation from camera view 'a' to 'b'  Not modified.
 * @param foundInA (Output) Found 3D position of the point in reference frame 'a'.  Modified.
 */
public void triangulate(Point2D_F64 a, Point2D_F64 b, Se3_F64 fromAtoB, Point3D_F64 foundInA) {
    // set camera B's principle point
    Vector3D_F64 t = fromAtoB.getT();
    rayB.p.set(-t.x, -t.y, -t.z);
    // rotate observation in B into camera A's view
    GeometryMath_F64.multTran(fromAtoB.getR(), rayB.p, rayB.p);
    GeometryMath_F64.multTran(fromAtoB.getR(), b, rayB.slope);
    rayA.slope.set(a.x, a.y, 1);
    ClosestPoint3D_F64.closestPoint(rayA, rayB, foundInA);
}

Example 30 with Vector3D_F64

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

the class StereoParameters method print.

public void print() {
    double[] euler = ConvertRotation3D_F64.matrixToEuler(rightToLeft.getR(), EulerType.XYZ, (double[]) null);
    Vector3D_F64 t = rightToLeft.getT();
    System.out.println();
    System.out.println("Left Camera");
    left.print();
    System.out.println();
    System.out.println("Right Camera");
    right.print();
    System.out.println("Right to Left");
    System.out.printf("  Euler XYZ   [ %8.3f , %8.3f , %8.3f ]\n", euler[0], euler[1], euler[2]);
    System.out.printf("  Translation [ %8.3f , %8.3f , %8.3f ]\n", t.x, t.y, t.z);
}