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

Example 51 with Point4D_F64

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

the class CompatibleProjectiveHomography method fitCameraPoints.

/**
 * Solves for the transform H using one view and 2 or more points. The two camera matrices associated with the
 * same view constrain 11 out of the 15 DOF. Each points provides another 3 linear constraints.
 *
 * <p>H(v) = pinv(P)*P' + hv<sup>T</sup></p>
 * <p>where 'h' is null-space of P. 'v' is 4-vector and is unknown, solved with linear equation</p>
 *
 * <p>NOTE: While 2 is the minimum number of views during testing it seemed to require 4 points with perfect data.
 * The math was double checked and no fundamental flaw could be found in the test. More investigation is needed.
 * There is a large difference in scales that could be contributing to this problem.</p>
 *
 * <p>NOTE: Produces poor results when the scene is planar</p>
 *
 * @param camera1 Camera matrix
 * @param camera2 Camera matrix of the same view but another projective space
 * @param points1 Observations from camera1
 * @param points2 Observations from camera2
 * @param H (Output) 4x4 homography relating the views. P1*H = P2, X1 = H*X2
 * @return true if successful
 */
public boolean fitCameraPoints(DMatrixRMaj camera1, DMatrixRMaj camera2, List<Point4D_F64> points1, List<Point4D_F64> points2, DMatrixRMaj H) {
    if (points1.size() != points2.size())
        throw new IllegalArgumentException("Lists must be the same size");
    if (points1.size() < 2)
        throw new IllegalArgumentException("A minimum of two points are required");
    // yes the SVD is computed twice. This can be optimized later, right now it isn't worth it. not a bottle neck
    if (!solvePInv.setA(camera1))
        return false;
    if (!nullspace.process(camera1, 1, h))
        return false;
    // PinvP = pinv(P)*P'
    solvePInv.solve(camera2, PinvP);
    final int size = points1.size();
    A.reshape(size * 3, 4);
    B.reshape(size * 3, 1);
    for (int i = 0, idxA = 0, idxB = 0; i < size; i++) {
        Point4D_F64 p1 = points1.get(i);
        Point4D_F64 p2 = points2.get(i);
        // a = P+P'X'
        GeometryMath_F64.mult(PinvP, p2, a);
        // double a_sum = a.w;
        // double h_sum = h.data[3];
        // double x_sum = p1.w;
        // Use sum instead of a[4], ... so that points at infinity can be handled
        double a_sum = a.x + a.y + a.z + a.w;
        double h_sum = h.data[0] + h.data[1] + h.data[2] + h.data[3];
        double x_sum = p1.x + p1.y + p1.z + p1.w;
        for (int k = 0; k < 3; k++) {
            // b[k] = h[k]*X[4] - h[4]*X[k]
            double b_k = h.data[k] * x_sum - h_sum * p1.getIdx(k);
            // c[k] = X[k]*a[4] - X[4]*a[k]
            double c_k = p1.getIdx(k) * a_sum - x_sum * a.getIdx(k);
            // b*X'^T*v = c
            A.data[idxA++] = b_k * p2.x;
            A.data[idxA++] = b_k * p2.y;
            A.data[idxA++] = b_k * p2.z;
            A.data[idxA++] = b_k * p2.w;
            B.data[idxB++] = c_k;
        }
    }
    // Solve for v
    if (!solver.setA(A))
        return false;
    H.reshape(4, 1);
    solver.solve(B, H);
    // copy v into 'a'
    a.setTo(H.data[0], H.data[1], H.data[2], H.data[3]);
    // H = P+P' + h*v^T
    H.reshape(4, 4);
    for (int i = 0; i < 4; i++) {
        for (int j = 0; j < 4; j++) {
            H.data[i * 4 + j] = PinvP.get(i, j) + h.data[i] * a.getIdx(j);
        }
    }
    return true;
}

Example 52 with Point4D_F64

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

the class CompatibleProjectiveHomography method fitPoints.

/**
 * Finds the homography H by by minimizing algebriac error. Modified version of algorithm described in [1].
 * See [2] for implementation details. Solution is found by finding the null space. A minimum of 5 points are
 * required to solve the 15 degrees of freedom in H.
 *
 * <p>X1 = H*X2</p>
 *
 * <p>NOTE: Fails when all points lie exactly on the same plane</p>
 *
 * @param points1 Set of points from view A but in projective 1. Recommended that they have f-norm of 1
 * @param points2 Set of points from view A but in projective 2. Recommended that they have f-norm of 1
 * @param H (Output) 4x4 homography relating the views. P1*H = P2, X1 = H*X2
 * @return true if successful or false if it fails
 */
public boolean fitPoints(List<Point4D_F64> points1, List<Point4D_F64> points2, DMatrixRMaj H) {
    if (points1.size() != points2.size())
        throw new IllegalArgumentException("Must have the same number in each list");
    if (points1.size() < 5)
        throw new IllegalArgumentException("At least 5 points required");
    final int size = points1.size();
    A.reshape(size * 3, 16);
    for (int i = 0; i < size; i++) {
        Point4D_F64 a = points1.get(i);
        Point4D_F64 b = points2.get(i);
        double alpha = -(a.x + a.y + a.z + a.w);
        for (int j = 0; j < 3; j++) {
            int idx = 16 * (3 * i + j);
            double va = a.getIdx(j);
            for (int k = 0; k < 4; k++) {
                A.data[idx++] = va * b.x;
                A.data[idx++] = va * b.y;
                A.data[idx++] = va * b.z;
                A.data[idx++] = va * b.w;
            }
        }
        for (int j = 0; j < 3; j++) {
            int idx = 16 * (3 * i + j) + 4 * j;
            A.data[idx] += b.x * alpha;
            A.data[idx + 1] += b.y * alpha;
            A.data[idx + 2] += b.z * alpha;
            A.data[idx + 3] += b.w * alpha;
        }
    }
    if (!nullspace.process(A, 1, H))
        return false;
    H.reshape(4, 4);
    return true;
}

Example 53 with Point4D_F64

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

the class PRnPDirectLinearTransform method process.

/**
 * Computes projective camera matrix.
 *
 * @param worldPts points in homogenous 3D coordinates in world frame. Might be modified.
 * @param observed pixel coordinates of points. not modified
 * @param solutionModel (Output) 3x4 camera matrix
 * @return true if succesfull
 */
public boolean process(List<Point4D_F64> worldPts, List<Point2D_F64> observed, DMatrixRMaj solutionModel) {
    if (worldPts.size() != observed.size())
        throw new IllegalArgumentException("Number of 3D and 2D points must match");
    if (worldPts.size() < 5)
        throw new IllegalArgumentException("A minimum of 4 points are required");
    LowLevelMultiViewOps.computeNormalization(observed, N1);
    // if configured to do so normalize 3D points to have a F-norm of 1
    if (normalize3D) {
        for (int i = 0; i < worldPts.size(); i++) {
            worldPts.get(i).normalize();
        }
    }
    final int N = worldPts.size();
    A.reshape(3 * N, 12);
    for (int i = 0; i < N; i++) {
        Point2D_F64 pixel = observed.get(i);
        Point4D_F64 X = worldPts.get(i);
        // apply normalization to pixels
        double x = (pixel.x - N1.meanX) / N1.stdX;
        double y = (pixel.y - N1.meanY) / N1.stdY;
        // only need to fill in non-zero elements. zeros are already zero
        // @formatter:off
        int idx = i * 3 * 12;
        A.data[idx + 4] = -X.x;
        A.data[idx + 5] = -X.y;
        A.data[idx + 6] = -X.z;
        A.data[idx + 7] = -X.w;
        A.data[idx + 8] = y * X.x;
        A.data[idx + 9] = y * X.y;
        A.data[idx + 10] = y * X.z;
        A.data[idx + 11] = y * X.w;
        idx += 12;
        A.data[idx] = X.x;
        A.data[idx + 1] = X.y;
        A.data[idx + 2] = X.z;
        A.data[idx + 3] = X.w;
        A.data[idx + 8] = -x * X.x;
        A.data[idx + 9] = -x * X.y;
        A.data[idx + 10] = -x * X.z;
        A.data[idx + 11] = -x * X.w;
        idx += 12;
        A.data[idx] = -y * X.x;
        A.data[idx + 1] = -y * X.y;
        A.data[idx + 2] = -y * X.z;
        A.data[idx + 3] = -y * X.w;
        A.data[idx + 4] = x * X.x;
        A.data[idx + 5] = x * X.y;
        A.data[idx + 6] = x * X.z;
        A.data[idx + 7] = x * X.w;
    // @formatter:on
    }
    // Find the null-space, which will the camera matrix in distorted pixels
    if (!solverNullspace.process(A, 1, ns))
        return false;
    // P' = N*P
    ns.reshape(3, 4);
    // Remove the normalization
    // P = inv(N)*P'
    N1.remove(ns, solutionModel);
    return true;
}

Example 54 with Point4D_F64

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

the class PoseFromPairLinear6 method setupHomogenousA.

private void setupHomogenousA(List<AssociatedPair> observations, List<Point4D_F64> locations) {
    A.reshape(2 * observations.size(), 12, false);
    for (int i = 0; i < observations.size(); i++) {
        AssociatedPair p = observations.get(i);
        Point4D_F64 loc = locations.get(i);
        Point2D_F64 pt1 = p.p1;
        Point2D_F64 pt2 = p.p2;
        // normalize the points
        int w = i * 2;
        double alpha = loc.w / loc.z;
        A.set(w, 4, -pt1.x);
        A.set(w, 5, -pt1.y);
        A.set(w, 6, -1);
        A.set(w, 8, pt2.y * pt1.x);
        A.set(w, 9, pt2.y * pt1.y);
        A.set(w, 10, pt2.y);
        A.set(w, 3, 0);
        A.set(w, 7, -alpha);
        A.set(w, 11, alpha * pt2.y);
        w++;
        A.set(w, 0, pt1.x);
        A.set(w, 1, pt1.y);
        A.set(w, 2, 1);
        A.set(w, 8, -pt2.x * pt1.x);
        A.set(w, 9, -pt2.x * pt1.y);
        A.set(w, 10, -pt2.x);
        A.set(w, 3, alpha);
        A.set(w, 7, 0);
        A.set(w, 11, -alpha * pt2.x);
    }
}

Example 55 with Point4D_F64

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

the class TestProjectiveInitializeAllCommon method checkReconstruction.

/**
 * Check reconstruction by seeing if it's consistent with the input observations
 */
private void checkReconstruction(ProjectiveInitializeAllCommon alg, MockLookupSimilarImagesRealistic db, DogArray_I32 seedConnIdx, double reprojectionTol) {
    final SceneStructureProjective structure = alg.getStructure();
    // Sanity check the number of each type of structure
    assertEquals(seedConnIdx.size + 1, structure.views.size);
    List<String> viewIds = BoofMiscOps.collectList(db.views, v -> v.id);
    int dbIndexSeed = viewIds.indexOf(alg.getPairwiseGraphViewByStructureIndex(0).id);
    // Check results for consistency. Can't do a direct comparision to ground truth since a different
    // but equivalent projective frame would have been estimated.
    Point4D_F64 X = new Point4D_F64();
    Point2D_F64 found = new Point2D_F64();
    DogArray_I32 inlierToSeed = alg.inlierIndexes.get(0);
    for (int i = 0; i < inlierToSeed.size; i++) {
        int seedFeatureIdx = inlierToSeed.get(i);
        int truthFeatIdx = db.observationToFeatureIdx(dbIndexSeed, seedFeatureIdx);
        int structureIdx = alg.seedToStructure.get(seedFeatureIdx);
        // only features that have structure should be in this list
        assertTrue(structureIdx >= 0);
        // Get the estimated point in 3D
        structure.points.get(structureIdx).get(X);
        // Project the point to the camera using found projection matrices
        for (int viewIdx = 0; viewIdx < structure.views.size; viewIdx++) {
            int viewDbIdx = viewIds.indexOf(alg.getPairwiseGraphViewByStructureIndex(viewIdx).id);
            // Project this feature to the camera
            DMatrixRMaj P = structure.views.get(viewIdx).worldToView;
            PerspectiveOps.renderPixel(P, X, found);
            // undo the offset
            found.x += db.intrinsic.cx;
            found.y += db.intrinsic.cy;
            // Lookup the expected pixel location
            // The seed feature ID and the ground truth feature ID are the same
            Point2D_F64 expected = db.featureToObservation(viewDbIdx, truthFeatIdx).pixel;
            assertEquals(0.0, expected.distance(found), reprojectionTol);
        }
    }
}