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

Example 71 with Point3D_F64

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

the class PnPLepetitEPnP method extractNullPoints.

/**
 * Computes M'*M and finds the null space.  The 4 eigenvectors with the smallest eigenvalues are found
 * and the null points extracted from them.
 */
protected void extractNullPoints(DMatrixRMaj M) {
    // compute MM and find its null space
    MM.reshape(M.numRows, M.numRows, false);
    CommonOps_DDRM.multTransB(M, M, MM);
    if (!solverNull.process(MM, numControl, nullspace))
        throw new IllegalArgumentException("SVD failed?!?!");
    // extract null points from the null space
    for (int i = 0; i < numControl; i++) {
        int column = numControl - i - 1;
        for (int j = 0; j < numControl; j++) {
            Point3D_F64 p = nullPts[i].get(j);
            p.x = nullspace.get(j * 3 + 0, column);
            p.y = nullspace.get(j * 3 + 1, column);
            p.z = nullspace.get(j * 3 + 2, column);
        }
    }
}

Example 72 with Point3D_F64

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

Example 73 with Point3D_F64

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

the class PnPLepetitEPnP method computeBarycentricCoordinates.

/**
 * <p>
 * Given the control points it computes the 4 weights for each camera point.  This is done by
 * solving the following linear equation: C*&alpha;=X. where C is the control point matrix,
 * &alpha; is the 4 by n matrix containing the solution, and X is the camera point matrix.
 * N is the number of points.
 * </p>
 * <p>
 * C = [ controlPts' ; ones(1,4) ]<br>
 * X = [ cameraPts' ; ones(1,N) ]
 * </p>
 */
protected void computeBarycentricCoordinates(FastQueue<Point3D_F64> controlWorldPts, DMatrixRMaj alphas, List<Point3D_F64> worldPts) {
    alphas.reshape(worldPts.size(), numControl, false);
    v_temp.reshape(3, 1);
    A_temp.reshape(3, numControl - 1);
    for (int i = 0; i < numControl - 1; i++) {
        Point3D_F64 c = controlWorldPts.get(i);
        A_temp.set(0, i, c.x - meanWorldPts.x);
        A_temp.set(1, i, c.y - meanWorldPts.y);
        A_temp.set(2, i, c.z - meanWorldPts.z);
    }
    // invert the matrix
    solverPinv.setA(A_temp);
    A_temp.reshape(A_temp.numCols, A_temp.numRows);
    solverPinv.invert(A_temp);
    w_temp.reshape(numControl - 1, 1);
    for (int i = 0; i < worldPts.size(); i++) {
        Point3D_F64 p = worldPts.get(i);
        v_temp.data[0] = p.x - meanWorldPts.x;
        v_temp.data[1] = p.y - meanWorldPts.y;
        v_temp.data[2] = p.z - meanWorldPts.z;
        MatrixVectorMult_DDRM.mult(A_temp, v_temp, w_temp);
        int rowIndex = alphas.numCols * i;
        for (int j = 0; j < numControl - 1; j++) alphas.data[rowIndex++] = w_temp.data[j];
        if (numControl == 4)
            alphas.data[rowIndex] = 1 - w_temp.data[0] - w_temp.data[1] - w_temp.data[2];
        else
            alphas.data[rowIndex] = 1 - w_temp.data[0] - w_temp.data[1];
    }
}

Example 74 with Point3D_F64

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

the class PositionFromPairLinear2 method process.

/**
 * Computes the translation given two or more feature observations and the known rotation
 *
 * @param R Rotation matrix. World to view.
 * @param worldPts Location of features in world coordinates.
 * @param observed Observations of point in current view.  Normalized coordinates.
 * @return true if it succeeded.
 */
public boolean process(DMatrixRMaj R, List<Point3D_F64> worldPts, List<Point2D_F64> observed) {
    if (worldPts.size() != observed.size())
        throw new IllegalArgumentException("Number of worldPts and observed must be the same");
    if (worldPts.size() < 2)
        throw new IllegalArgumentException("A minimum of two points are required");
    int N = worldPts.size();
    A.reshape(3 * N, 3);
    b.reshape(A.numRows, 1);
    for (int i = 0; i < N; i++) {
        Point3D_F64 X = worldPts.get(i);
        Point2D_F64 o = observed.get(i);
        int indexA = i * 3 * 3;
        int indexB = i * 3;
        A.data[indexA + 1] = -1;
        A.data[indexA + 2] = o.y;
        A.data[indexA + 3] = 1;
        A.data[indexA + 5] = -o.x;
        A.data[indexA + 6] = -o.y;
        A.data[indexA + 7] = o.x;
        GeometryMath_F64.mult(R, X, RX);
        b.data[indexB++] = 1 * RX.y - o.y * RX.z;
        b.data[indexB++] = -1 * RX.x + o.x * RX.z;
        b.data[indexB] = o.y * RX.x - o.x * RX.y;
    }
    if (!solver.setA(A))
        return false;
    solver.solve(b, x);
    T.x = x.data[0];
    T.y = x.data[1];
    T.z = x.data[2];
    return true;
}

Example 75 with Point3D_F64

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

the class UtilLepetitEPnP method computeCameraControl.

/**
 * Computes the camera control points as weighted sum of null points.
 *
 * @param beta Beta values which describe the weights of null points
 * @param nullPts Null points that the camera point is a weighted sum of
 * @param cameraPts The output
 */
public static void computeCameraControl(double[] beta, List<Point3D_F64>[] nullPts, FastQueue<Point3D_F64> cameraPts, int numControl) {
    cameraPts.reset();
    for (int i = 0; i < numControl; i++) {
        cameraPts.grow().set(0, 0, 0);
    }
    for (int i = 0; i < numControl; i++) {
        double b = beta[i];
        for (int j = 0; j < numControl; j++) {
            Point3D_F64 s = cameraPts.get(j);
            Point3D_F64 p = nullPts[i].get(j);
            s.x += b * p.x;
            s.y += b * p.y;
            s.z += b * p.z;
        }
    }
}