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

Example 96 with Point3D_F64

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

the class ArtificialStereoScene method init.

public void init(int N, boolean isPixels, boolean planar) {
    this.isPixels = isPixels;
    // define the camera's motion
    motion = new Se3_F64();
    motion.getR().set(ConvertRotation3D_F64.eulerToMatrix(EulerType.XYZ, 0.5, -0.2, 0.15, null));
    motion.getT().set(0.1, -0.2, 0.01);
    // randomly generate points in space
    if (planar)
        worldPoints = createPlanarScene(N);
    else
        worldPoints = GeoTestingOps.randomPoints_F64(-1, 1, -1, 1, 2, 3, N, rand);
    // transform points into second camera's reference frame
    pairs = new ArrayList<>();
    observationCurrent = new ArrayList<>();
    observationPose = new ArrayList<>();
    for (Point3D_F64 p1 : worldPoints) {
        Point3D_F64 p2 = SePointOps_F64.transform(motion, p1, null);
        AssociatedPair pair = new AssociatedPair();
        pair.p1.set(p1.x / p1.z, p1.y / p1.z);
        pair.p2.set(p2.x / p2.z, p2.y / p2.z);
        pairs.add(pair);
        observationCurrent.add(pair.p2);
        observationPose.add(new Point2D3D(pair.p2, p1));
        if (isPixels) {
            PerspectiveOps.convertNormToPixel(K, pair.p1, pair.p1);
            PerspectiveOps.convertNormToPixel(K, pair.p2, pair.p2);
        }
    }
}

Example 97 with Point3D_F64

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

the class PnPLepetitEPnP method refine.

/**
 * Given the set of betas it computes a new set of control points and adjust sthe scale
 * using the {@llin #matchScale} function.
 */
private void refine(double[] betas) {
    for (int i = 0; i < numControl; i++) {
        double x = 0, y = 0, z = 0;
        for (int j = 0; j < numControl; j++) {
            Point3D_F64 p = nullPts[j].get(i);
            x += betas[j] * p.x;
            y += betas[j] * p.y;
            z += betas[j] * p.z;
        }
        tempPts0.get(i).set(x, y, z);
    }
    double adj = matchScale(tempPts0, controlWorldPts);
    adj = adjustBetaSign(adj, tempPts0);
    for (int i = 0; i < 4; i++) {
        betas[i] *= adj;
    }
}

Example 98 with Point3D_F64

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

the class PnPLepetitEPnP method adjustBetaSign.

/**
 * Use the positive depth constraint to determine the sign of beta
 */
private double adjustBetaSign(double beta, List<Point3D_F64> nullPts) {
    if (beta == 0)
        return 0;
    int N = alphas.numRows;
    int positiveCount = 0;
    for (int i = 0; i < N; i++) {
        double z = 0;
        for (int j = 0; j < numControl; j++) {
            Point3D_F64 c = nullPts.get(j);
            z += alphas.get(i, j) * c.z;
        }
        if (z > 0)
            positiveCount++;
    }
    if (positiveCount < N / 2)
        beta *= -1;
    return beta;
}

Example 99 with Point3D_F64

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

the class PoseFromPairLinear6 method setupA.

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

Example 100 with Point3D_F64

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

the class GeneralTestRefineTriangulate method incorrectInput.

@Test
public void incorrectInput() {
    createScene();
    Point3D_F64 initial = worldPoint.copy();
    initial.x += 0.01;
    initial.y += 0.1;
    initial.z += -0.05;
    Point3D_F64 found = new Point3D_F64();
    triangulate(obsPts, motionWorldToCamera, essential, initial, found);
    double error2 = worldPoint.distance(initial);
    double error = worldPoint.distance(found);
    assertTrue(error < error2 * 0.5);
}