Examples with Se3_F64 georegression.struct.se.Se3_F64 used on opensource projects

Example 56 with Se3_F64

use of georegression.struct.se.Se3_F64 in project BoofCV by lessthanoptimal.

the class CommonTrifocalChecks method computeStuffFromPose.

private void computeStuffFromPose() {
    P2 = PerspectiveOps.createCameraMatrix(se2.R, se2.T, K, null);
    P3 = PerspectiveOps.createCameraMatrix(se3.R, se3.T, K, null);
    tensor = MultiViewOps.createTrifocal(P2, P3, null);
    F2 = MultiViewOps.createEssential(se2.getR(), se2.getT());
    F2 = MultiViewOps.createFundamental(F2, K);
    F3 = MultiViewOps.createEssential(se3.getR(), se3.getT());
    F3 = MultiViewOps.createFundamental(F3, K);
    for (int i = 0; i < 20; i++) {
        Point3D_F64 p = new Point3D_F64();
        p.x = rand.nextGaussian() * 0.5;
        p.y = rand.nextGaussian() * 0.5;
        p.z = rand.nextGaussian() * 0.5 + 2;
        worldPts.add(p);
        AssociatedTriple o = new AssociatedTriple();
        o.p1 = PerspectiveOps.renderPixel(new Se3_F64(), K, p);
        o.p2 = PerspectiveOps.renderPixel(se2, K, p);
        o.p3 = PerspectiveOps.renderPixel(se3, K, p);
        AssociatedTriple oS = o.copy();
        oS.p1 = PerspectiveOps.renderPixel(new Se3_F64(), null, p);
        AssociatedTriple oN = new AssociatedTriple();
        oN.p1 = PerspectiveOps.renderPixel(new Se3_F64(), null, p);
        oN.p2 = PerspectiveOps.renderPixel(se2, null, p);
        oN.p3 = PerspectiveOps.renderPixel(se3, null, p);
        observations.add(o);
        observationsSpecial.add(oS);
        observationsNorm.add(oN);
    }
}

Example 57 with Se3_F64

use of georegression.struct.se.Se3_F64 in project BoofCV by lessthanoptimal.

the class CommonHomographyChecks method createScene.

public void createScene(int numPoints, boolean isPixels) {
    // define the camera's motion
    motion = new Se3_F64();
    motion.getR().set(ConvertRotation3D_F64.eulerToMatrix(EulerType.XYZ, 0.05, -0.03, 0.02, null));
    motion.getT().set(0.1, -0.1, 0.01);
    // randomly generate points in space
    pts = createRandomPlane(rand, d, numPoints);
    // transform points into second camera's reference frame
    pairs = new ArrayList<>();
    for (Point3D_F64 p1 : pts) {
        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);
        if (isPixels) {
            GeometryMath_F64.mult(K, pair.p1, pair.p1);
            GeometryMath_F64.mult(K, pair.p2, pair.p2);
        }
    }
}

Example 58 with Se3_F64

use of georegression.struct.se.Se3_F64 in project BoofCV by lessthanoptimal.

the class TestDistanceSe3SymmetricSq method testIntrinsicParameters.

/**
 * Manually compute the error using a calibration matrix and see if they match
 */
@Test
public void testIntrinsicParameters() {
    // intrinsic camera calibration matrix
    DMatrixRMaj K = new DMatrixRMaj(3, 3, true, 100, 0.01, 200, 0, 150, 200, 0, 0, 1);
    DMatrixRMaj K2 = new DMatrixRMaj(3, 3, true, 105, 0.021, 180, 0, 155, 210, 0, 0, 1);
    DMatrixRMaj K_inv = new DMatrixRMaj(3, 3);
    DMatrixRMaj K2_inv = new DMatrixRMaj(3, 3);
    CommonOps_DDRM.invert(K, K_inv);
    CommonOps_DDRM.invert(K2, K2_inv);
    Se3_F64 keyToCurr = new Se3_F64();
    keyToCurr.getT().set(0.1, -0.1, 0.01);
    Point3D_F64 X = new Point3D_F64(0.02, -0.05, 3);
    AssociatedPair obs = new AssociatedPair();
    AssociatedPair obsP = new AssociatedPair();
    obs.p1.x = X.x / X.z;
    obs.p1.y = X.y / X.z;
    SePointOps_F64.transform(keyToCurr, X, X);
    obs.p2.x = X.x / X.z;
    obs.p2.y = X.y / X.z;
    // translate into pixels
    GeometryMath_F64.mult(K, obs.p1, obsP.p1);
    GeometryMath_F64.mult(K2, obs.p2, obsP.p2);
    // add some noise
    obsP.p1.x += 0.25;
    obsP.p1.y += 0.25;
    obsP.p2.x -= 0.25;
    obsP.p2.y -= 0.25;
    // convert noisy into normalized coordinates
    GeometryMath_F64.mult(K_inv, obsP.p1, obsP.p1);
    GeometryMath_F64.mult(K2_inv, obsP.p2, obsP.p2);
    // triangulate the point's position given noisy data
    LineParametric3D_F64 rayA = new LineParametric3D_F64();
    LineParametric3D_F64 rayB = new LineParametric3D_F64();
    rayA.slope.set(obsP.p1.x, obsP.p1.y, 1);
    rayB.p.set(-0.1, 0.1, -0.01);
    rayB.slope.set(obsP.p2.x, obsP.p2.y, 1);
    ClosestPoint3D_F64.closestPoint(rayA, rayB, X);
    // compute predicted given noisy triangulation
    AssociatedPair ugh = new AssociatedPair();
    ugh.p1.x = X.x / X.z;
    ugh.p1.y = X.y / X.z;
    SePointOps_F64.transform(keyToCurr, X, X);
    ugh.p2.x = X.x / X.z;
    ugh.p2.y = X.y / X.z;
    // convert everything into pixels
    GeometryMath_F64.mult(K, ugh.p1, ugh.p1);
    GeometryMath_F64.mult(K2, ugh.p2, ugh.p2);
    GeometryMath_F64.mult(K, obsP.p1, obsP.p1);
    GeometryMath_F64.mult(K2, obsP.p2, obsP.p2);
    double dx1 = ugh.p1.x - obsP.p1.x;
    double dy1 = ugh.p1.y - obsP.p1.y;
    double dx2 = ugh.p2.x - obsP.p2.x;
    double dy2 = ugh.p2.y - obsP.p2.y;
    double error = dx1 * dx1 + dy1 * dy1 + dx2 * dx2 + dy2 * dy2;
    // convert noisy back into normalized coordinates
    GeometryMath_F64.mult(K_inv, obsP.p1, obsP.p1);
    GeometryMath_F64.mult(K2_inv, obsP.p2, obsP.p2);
    DistanceSe3SymmetricSq alg = new DistanceSe3SymmetricSq(triangulate);
    alg.setIntrinsic(K.get(0, 0), K.get(1, 1), K.get(0, 1), K2.get(0, 0), K2.get(1, 1), K2.get(0, 1));
    alg.setModel(keyToCurr);
    assertEquals(error, alg.computeDistance(obsP), 1e-8);
}

Example 59 with Se3_F64

use of georegression.struct.se.Se3_F64 in project BoofCV by lessthanoptimal.

the class TestDistanceSe3SymmetricSq method testBehindCamera.

@Test
public void testBehindCamera() {
    Se3_F64 keyToCurr = new Se3_F64();
    keyToCurr.getR().set(ConvertRotation3D_F64.eulerToMatrix(EulerType.XYZ, 0.05, -0.03, 0.02, null));
    keyToCurr.getT().set(0.1, -0.1, 0.01);
    Point3D_F64 X = new Point3D_F64(0.1, -0.05, -3);
    AssociatedPair obs = new AssociatedPair();
    obs.p1.x = X.x / X.z;
    obs.p1.y = X.y / X.z;
    SePointOps_F64.transform(keyToCurr, X, X);
    obs.p2.x = X.x / X.z;
    obs.p2.y = X.y / X.z;
    alg.setModel(keyToCurr);
    assertTrue(Double.MAX_VALUE == alg.computeDistance(obs));
}

Example 60 with Se3_F64

use of georegression.struct.se.Se3_F64 in project BoofCV by lessthanoptimal.

the class TestDistanceTranGivenRotSq method testPerfect.

@Test
public void testPerfect() {
    Se3_F64 keyToCurr = new Se3_F64();
    keyToCurr.getR().set(ConvertRotation3D_F64.eulerToMatrix(EulerType.XYZ, 0.05, -0.03, 0.02, null));
    keyToCurr.getT().set(0.1, -0.1, 0.01);
    Point3D_F64 X = new Point3D_F64(0.1, -0.05, 3);
    Point2D3D obs = new Point2D3D();
    obs.location = X.copy();
    SePointOps_F64.transform(keyToCurr, X, X);
    obs.observation.x = X.x / X.z;
    obs.observation.y = X.y / X.z;
    alg.setRotation(keyToCurr.getR());
    alg.setModel(keyToCurr.getT());
    assertEquals(0, alg.computeDistance(obs), 1e-8);
}