Examples with AssociatedPair boofcv.struct.geo.AssociatedPair used on opensource projects

Example 61 with AssociatedPair

use of boofcv.struct.geo.AssociatedPair 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 62 with AssociatedPair

use of boofcv.struct.geo.AssociatedPair in project BoofCV by lessthanoptimal.

the class PnPInfinitesimalPlanePoseEstimation method estimateTranslation.

/**
 * Estimate's the translation given the previously found rotation
 * @param R Rotation matrix
 * @param T (Output) estimated translation
 */
void estimateTranslation(DMatrixRMaj R, List<AssociatedPair> points, Vector3D_F64 T) {
    final int N = points.size();
    W.reshape(N, 3);
    y.reshape(N, 1);
    Wty.reshape(3, 1);
    DMatrix3x3 Rtmp = new DMatrix3x3();
    ConvertDMatrixStruct.convert(R, Rtmp);
    int indexY = 0, indexW = 0;
    for (int i = 0; i < N; i++) {
        AssociatedPair p = points.get(i);
        // rotate into camera frame
        double u1 = Rtmp.a11 * p.p1.x + Rtmp.a12 * p.p1.y;
        double u2 = Rtmp.a21 * p.p1.x + Rtmp.a22 * p.p1.y;
        double u3 = Rtmp.a31 * p.p1.x + Rtmp.a32 * p.p1.y;
        W.data[indexW++] = 1;
        W.data[indexW++] = 0;
        W.data[indexW++] = -p.p2.x;
        W.data[indexW++] = 0;
        W.data[indexW++] = 1;
        W.data[indexW++] = -p.p2.y;
        y.data[indexY++] = p.p2.x * u3 - u1;
        y.data[indexY++] = p.p2.y * u3 - u2;
    }
    // ======= Compute Pseudo Inverse
    // WW = inv(W^T*W)
    CommonOps_DDRM.multTransA(W, W, WW);
    CommonOps_DDRM.invert(WW);
    // W^T*y
    CommonOps_DDRM.multTransA(W, y, Wty);
    // translation = inv(W^T*W)*W^T*y
    W.reshape(N, 1);
    CommonOps_DDRM.mult(WW, Wty, W);
    T.x = W.data[0];
    T.y = W.data[1];
    T.z = W.data[2];
}

Example 63 with AssociatedPair

use of boofcv.struct.geo.AssociatedPair 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 64 with AssociatedPair

use of boofcv.struct.geo.AssociatedPair in project BoofCV by lessthanoptimal.

the class CheckRefineHomography method perfectInput.

@Test
public void perfectInput() {
    createScene(30, false);
    // use the linear algorithm to compute the homography
    HomographyDirectLinearTransform estimator = new HomographyDirectLinearTransform(true);
    estimator.process(pairs, H);
    ModelFitter<DMatrixRMaj, AssociatedPair> alg = createAlgorithm();
    // give it the perfect matrix and see if it screwed it up
    assertTrue(alg.fitModel(pairs, H, found));
    // normalize so that they are the same
    CommonOps_DDRM.divide(H, H.get(2, 2));
    CommonOps_DDRM.divide(found, found.get(2, 2));
    assertTrue(MatrixFeatures_DDRM.isEquals(H, found, 1e-8));
}

Example 65 with AssociatedPair

use of boofcv.struct.geo.AssociatedPair in project BoofCV by lessthanoptimal.

the class CheckRefineHomography method incorrectInput.

@Test
public void incorrectInput() {
    createScene(30, false);
    // use the linear algorithm to compute the homography
    HomographyDirectLinearTransform estimator = new HomographyDirectLinearTransform(true);
    estimator.process(pairs, H);
    ModelFitter<DMatrixRMaj, AssociatedPair> alg = createAlgorithm();
    // give it the perfect matrix and see if it screwed it up
    DMatrixRMaj Hmod = H.copy();
    Hmod.data[0] += 0.1;
    Hmod.data[5] += 0.1;
    assertTrue(alg.fitModel(pairs, Hmod, found));
    // normalize to allow comparison
    CommonOps_DDRM.divide(H, H.get(2, 2));
    CommonOps_DDRM.divide(Hmod, Hmod.get(2, 2));
    CommonOps_DDRM.divide(found, found.get(2, 2));
    double error0 = 0;
    double error1 = 0;
    // very crude error metric
    for (int i = 0; i < 9; i++) {
        error0 += Math.abs(Hmod.data[i] - H.data[i]);
        error1 += Math.abs(found.data[i] - H.data[i]);
    }
    // System.out.println("error "+error1);
    assertTrue(error1 < error0);
}