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

Example 41 with Point3D_F64

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

the class TestPnPLepetitEPnP method estimateCase1.

/**
 * Create two sets of points with a known scale difference.
 */
@Test
public void estimateCase1() {
    PnPLepetitEPnP alg = new PnPLepetitEPnP();
    // skip the adjust step
    alg.numControl = 4;
    alg.alphas = new DMatrixRMaj(0, 0);
    alg.nullPts[0] = GeoTestingOps.randomPoints_F64(5, 10, -1, 2, -5, 20, 4, rand);
    double beta = 10;
    for (int i = 0; i < alg.numControl; i++) {
        Point3D_F64 p = alg.nullPts[0].get(i);
        Point3D_F64 c = alg.controlWorldPts.grow();
        c.x = p.x * beta;
        c.y = p.y * beta;
        c.z = p.z * beta;
    }
    double[] betas = new double[] { 1, 2, 3, 4 };
    alg.estimateCase1(betas);
    assertEquals(beta, betas[0], 1e-8);
    assertEquals(0, betas[1], 1e-8);
    assertEquals(0, betas[2], 1e-8);
    assertEquals(0, betas[3], 1e-8);
}

Example 42 with Point3D_F64

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

the class TestPnPLepetitEPnP method selectWorldControlPoints_planar.

@Test
public void selectWorldControlPoints_planar() {
    List<Point3D_F64> worldPts = CommonHomographyChecks.createRandomPlane(rand, 3, 30);
    FastQueue<Point3D_F64> controlPts = new FastQueue<>(4, Point3D_F64.class, true);
    PnPLepetitEPnP alg = new PnPLepetitEPnP();
    alg.selectWorldControlPoints(worldPts, controlPts);
    assertEquals(3, alg.numControl);
    // check that each row is unique
    for (int i = 0; i < 3; i++) {
        Point3D_F64 ci = controlPts.get(i);
        for (int j = i + 1; j < 3; j++) {
            Point3D_F64 cj = controlPts.get(j);
            double dx = ci.x - cj.x;
            double dy = ci.y - cj.y;
            double dz = ci.z - cj.z;
            double sum = Math.abs(dx) + Math.abs(dy) + Math.abs(dz);
            assertTrue(sum > 0.00001);
        }
    }
}

Example 43 with Point3D_F64

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

the class TestPnPLepetitEPnP method extractNullPoints.

@Test
public void extractNullPoints() {
    // create a singular matrix
    SimpleMatrix M = SimpleMatrix.wrap(RandomMatrices_DDRM.singular(12, 40, rand, 12, 11, 10, 9, 8, 7, 6, 5, 4, 3, 2, 0));
    PnPLepetitEPnP alg = new PnPLepetitEPnP();
    alg.numControl = 4;
    alg.extractNullPoints(M.getDDRM());
    // see if the first set of null points is the null space pf M*M
    List<Point3D_F64> l = alg.nullPts[0];
    SimpleMatrix v = new SimpleMatrix(12, 1);
    for (int i = 0; i < 4; i++) {
        Point3D_F64 p = l.get(i);
        v.set(3 * i + 0, p.x);
        v.set(3 * i + 1, p.y);
        v.set(3 * i + 2, p.z);
    }
    SimpleMatrix MM = M.mult(M.transpose());
    SimpleMatrix x = MM.mult(v);
    double mag = x.normF();
    assertEquals(0, mag, 1e-8);
}

Example 44 with Point3D_F64

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

the class TestPnPResidualReprojection method basicTest.

@Test
public void basicTest() {
    Se3_F64 motion = new Se3_F64();
    ConvertRotation3D_F64.eulerToMatrix(EulerType.XYZ, 0.1, 1, -0.2, motion.getR());
    motion.getT().set(-0.3, 0.4, 1);
    Point3D_F64 world = new Point3D_F64(0.5, -0.5, 3);
    Point2D_F64 obs = new Point2D_F64();
    Point3D_F64 temp = SePointOps_F64.transform(motion, world, null);
    obs.x = temp.x / temp.z;
    obs.y = temp.y / temp.z;
    PnPResidualReprojection alg = new PnPResidualReprojection();
    // compute errors with perfect model
    double[] error = new double[alg.getN()];
    alg.setModel(motion);
    int index = alg.computeResiduals(new Point2D3D(obs, world), error, 0);
    assertEquals(alg.getN(), index);
    assertEquals(0, error[0], 1e-8);
    assertEquals(0, error[1], 1e-8);
    // compute errors with an incorrect model
    motion.getR().set(2, 1, 2);
    alg.setModel(motion);
    index = alg.computeResiduals(new Point2D3D(obs, world), error, 0);
    assertEquals(alg.getN(), index);
    assertTrue(Math.abs(error[0]) > 1e-8);
    assertTrue(Math.abs(error[1]) > 1e-8);
}

Example 45 with Point3D_F64

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

the class CommonTriangulationChecks method createScene.

public void createScene() {
    worldPoint = new Point3D_F64(0.1, -0.2, 4);
    motionWorldToCamera = new ArrayList<>();
    obsPts = new ArrayList<>();
    essential = new ArrayList<>();
    Point3D_F64 cameraPoint = new Point3D_F64();
    for (int i = 0; i < N; i++) {
        // random motion from world to frame 'i'
        Se3_F64 tranWtoI = new Se3_F64();
        if (i > 0) {
            tranWtoI.getR().set(ConvertRotation3D_F64.eulerToMatrix(EulerType.XYZ, rand.nextGaussian() * 0.01, rand.nextGaussian() * 0.05, rand.nextGaussian() * 0.1, null));
            tranWtoI.getT().set(0.2 + rand.nextGaussian() * 0.1, rand.nextGaussian() * 0.1, rand.nextGaussian() * 0.01);
        }
        DMatrixRMaj E = MultiViewOps.createEssential(tranWtoI.getR(), tranWtoI.getT());
        SePointOps_F64.transform(tranWtoI, worldPoint, cameraPoint);
        Point2D_F64 o = new Point2D_F64(cameraPoint.x / cameraPoint.z, cameraPoint.y / cameraPoint.z);
        obsPts.add(o);
        motionWorldToCamera.add(tranWtoI);
        essential.add(E);
    }
}