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

Example 91 with Se3_F64

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

the class TestPnPStereoDistanceReprojectionSq method checkBehind.

public void checkBehind(double Tz, double Pz) {
    // Point location in world frame
    Point3D_F64 X = new Point3D_F64(0.1, -0.04, Pz);
    DMatrixRMaj K_left = PerspectiveOps.calibrationMatrix(param.left, (DMatrixRMaj) null);
    DMatrixRMaj K_right = PerspectiveOps.calibrationMatrix(param.right, (DMatrixRMaj) null);
    // create a noisy observed
    Point2D_F64 obsLeft = PerspectiveOps.renderPixel(worldToLeft, K_left, X);
    Point2D_F64 obsRight = PerspectiveOps.renderPixel(worldToRight, K_right, X);
    // convert to normalized image coordinates
    PerspectiveOps.convertPixelToNorm(K_left, obsLeft, obsLeft);
    PerspectiveOps.convertPixelToNorm(K_right, obsRight, obsRight);
    PnPStereoDistanceReprojectionSq alg = new PnPStereoDistanceReprojectionSq();
    StereoParameters param = new StereoParameters(this.param.left, this.param.right, new Se3_F64());
    param.rightToLeft.getT().set(0.1, 0, Tz);
    alg.setStereoParameters(param);
    alg.setModel(new Se3_F64());
    double found = alg.computeDistance(new Stereo2D3D(obsLeft, obsRight, X));
    assertTrue(Double.MAX_VALUE == found);
}

Example 92 with Se3_F64

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

the class TestPnPStereoJacobianRodrigues method compareToNumerical.

private void compareToNumerical(double noise) {
    Se3_F64 worldToLeft = new Se3_F64();
    ConvertRotation3D_F64.eulerToMatrix(EulerType.XYZ, 0.1, 1, -0.2, worldToLeft.getR());
    worldToLeft.getT().set(-0.3, 0.4, 1);
    generateScene(numPoints, worldToLeft, false);
    addNoise(noise);
    PnPStereoJacobianRodrigues alg = new PnPStereoJacobianRodrigues();
    alg.setLeftToRight(leftToRight);
    alg.setObservations(pointPose);
    StereoPose storage = new StereoPose(new Se3_F64(), leftToRight);
    ResidualsCodecToMatrix<StereoPose, Stereo2D3D> func = new ResidualsCodecToMatrix<>(codec, new PnPStereoResidualReprojection(), storage);
    func.setObservations(pointPose);
    StereoPose pose = new StereoPose(worldToLeft, leftToRight);
    double[] param = new double[codec.getParamLength()];
    codec.encode(pose, param);
    // DerivativeChecker.jacobianPrint(func,alg,param,1e-6);
    assertTrue(DerivativeChecker.jacobian(func, alg, param, 1e-6));
}

Example 93 with Se3_F64

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

the class TestPnPStereoRefineRodrigues method noisy.

/**
 * Noisy input.  Should generate something close to the actual solution
 */
@Test
public void noisy() {
    generateScene(30, null, false);
    PnPStereoRefineRodrigues alg = new PnPStereoRefineRodrigues(1e-12, 200);
    alg.setLeftToRight(leftToRight);
    Se3_F64 input = worldToLeft.copy();
    // noise up the initial guess
    DMatrixRMaj R = ConvertRotation3D_F64.eulerToMatrix(EulerType.XYZ, 0.1, -0.04, -0.2, null);
    CommonOps_DDRM.mult(R, input.getR().copy(), input.getR());
    input.T.x += 0.2;
    input.T.x -= 0.05;
    input.T.z += 0.03;
    Se3_F64 found = new Se3_F64();
    assertTrue(alg.fitModel(pointPose, input, found));
    assertTrue(alg.minimizer.getFunctionValue() < 1e-12);
    assertTrue(MatrixFeatures_DDRM.isIdentical(worldToLeft.getR(), found.getR(), 1e-8));
    assertTrue(found.getT().isIdentical(worldToLeft.getT(), 1e-8));
}

Example 94 with Se3_F64

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

the class TestStereoProcessingBase method createStereoParam.

public static StereoParameters createStereoParam(int width, int height) {
    StereoParameters ret = new StereoParameters();
    ret.setRightToLeft(new Se3_F64());
    ret.getRightToLeft().getT().set(-0.2, 0.001, -0.012);
    ConvertRotation3D_F64.eulerToMatrix(EulerType.XYZ, 0.001, -0.01, 0.0023, ret.getRightToLeft().getR());
    ret.left = new CameraPinholeRadial().fsetK(300, 320, 0, width / 2, height / 2, width, height).fsetRadial(0.1, 1e-4);
    ret.right = new CameraPinholeRadial().fsetK(290, 310, 0, width / 2 + 2, height / 2 - 6, width, height).fsetRadial(0.05, -2e-4);
    return ret;
}

Example 95 with Se3_F64

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

the class TestStereoSparse3D method checkGeometry.

/**
 * Provide perfect image processing and validate the geometry
 */
@Test
public void checkGeometry() {
    Dummy disparity = new Dummy();
    StereoSparse3D alg = new StereoSparse3D(disparity, GrayF32.class);
    alg.setCalibration(param);
    Point3D_F64 X = new Point3D_F64(0.2, -0.34, 3);
    // original pixel coordinates
    Point2D_F64 x1 = PerspectiveOps.renderPixel(new Se3_F64(), K1, X);
    Point2D_F64 x2 = PerspectiveOps.renderPixel(param.rightToLeft.invert(null), K2, X);
    Point2Transform2_F64 pixelToRect1 = RectifyImageOps.transformPixelToRect(param.left, alg.rect1);
    Point2Transform2_F64 pixelToRect2 = RectifyImageOps.transformPixelToRect(param.right, alg.rect2);
    // rectified coordinates
    Point2D_F64 r1 = new Point2D_F64();
    Point2D_F64 r2 = new Point2D_F64();
    pixelToRect1.compute(x1.x, x1.y, r1);
    pixelToRect2.compute(x2.x, x2.y, r2);
    // compute the true disparity
    disparity.d = r1.x - r2.x;
    assertTrue(alg.process(x1.x, x1.y));
    double x = alg.getX() / alg.getW();
    double y = alg.getY() / alg.getW();
    double z = alg.getZ() / alg.getW();
    assertEquals(X.x, x, 1e-8);
    assertEquals(X.y, y, 1e-8);
    assertEquals(X.z, z, 1e-8);
}