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

Example 1 with UtilPoint3D_F64

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

the class PnPLepetitEPnP method selectWorldControlPoints.

/**
 * Selects control points along the data's axis and the data's centroid.  If the data is determined
 * to be planar then only 3 control points are selected.
 *
 * The data's axis is determined by computing the covariance matrix then performing SVD.  The axis
 * is contained along the
 */
public void selectWorldControlPoints(List<Point3D_F64> worldPts, FastQueue<Point3D_F64> controlWorldPts) {
    UtilPoint3D_F64.mean(worldPts, meanWorldPts);
    // covariance matrix elements, summed up here for speed
    double c11 = 0, c12 = 0, c13 = 0, c22 = 0, c23 = 0, c33 = 0;
    final int N = worldPts.size();
    for (int i = 0; i < N; i++) {
        Point3D_F64 p = worldPts.get(i);
        double dx = p.x - meanWorldPts.x;
        double dy = p.y - meanWorldPts.y;
        double dz = p.z - meanWorldPts.z;
        c11 += dx * dx;
        c12 += dx * dy;
        c13 += dx * dz;
        c22 += dy * dy;
        c23 += dy * dz;
        c33 += dz * dz;
    }
    c11 /= N;
    c12 /= N;
    c13 /= N;
    c22 /= N;
    c23 /= N;
    c33 /= N;
    DMatrixRMaj covar = new DMatrixRMaj(3, 3, true, c11, c12, c13, c12, c22, c23, c13, c23, c33);
    // find the data's orientation and check to see if it is planar
    svd.decompose(covar);
    double[] singularValues = svd.getSingularValues();
    DMatrixRMaj V = svd.getV(null, false);
    SingularOps_DDRM.descendingOrder(null, false, singularValues, 3, V, false);
    // planar check
    if (singularValues[0] < singularValues[2] * 1e13) {
        numControl = 4;
    } else {
        numControl = 3;
    }
    // put control points along the data's major axises
    controlWorldPts.reset();
    for (int i = 0; i < numControl - 1; i++) {
        double m = Math.sqrt(singularValues[1]) * magicNumber;
        double vx = V.unsafe_get(0, i) * m;
        double vy = V.unsafe_get(1, i) * m;
        double vz = V.unsafe_get(2, i) * m;
        controlWorldPts.grow().set(meanWorldPts.x + vx, meanWorldPts.y + vy, meanWorldPts.z + vz);
    }
    // set a control point to be the centroid
    controlWorldPts.grow().set(meanWorldPts.x, meanWorldPts.y, meanWorldPts.z);
}

Example 2 with UtilPoint3D_F64

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

the class PnPLepetitEPnP method matchScale.

/**
 * Examines the distance each point is from the centroid to determine the scaling difference
 * between world control points and the null points.
 */
protected double matchScale(List<Point3D_F64> nullPts, FastQueue<Point3D_F64> controlWorldPts) {
    Point3D_F64 meanNull = UtilPoint3D_F64.mean(nullPts, numControl, null);
    Point3D_F64 meanWorld = UtilPoint3D_F64.mean(controlWorldPts.toList(), numControl, null);
    // compute the ratio of distance between world and null points from the centroid
    double top = 0;
    double bottom = 0;
    for (int i = 0; i < numControl; i++) {
        Point3D_F64 wi = controlWorldPts.get(i);
        Point3D_F64 ni = nullPts.get(i);
        double dwx = wi.x - meanWorld.x;
        double dwy = wi.y - meanWorld.y;
        double dwz = wi.z - meanWorld.z;
        double dnx = ni.x - meanNull.x;
        double dny = ni.y - meanNull.y;
        double dnz = ni.z - meanNull.z;
        double n2 = dnx * dnx + dny * dny + dnz * dnz;
        double w2 = dwx * dwx + dwy * dwy + dwz * dwz;
        top += w2;
        bottom += n2;
    }
    // compute beta
    return Math.sqrt(top / bottom);
}