Examples with StereoPose boofcv.struct.sfm.StereoPose used on opensource projects

Example 1 with StereoPose

use of boofcv.struct.sfm.StereoPose 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 2 with StereoPose

use of boofcv.struct.sfm.StereoPose in project BoofCV by lessthanoptimal.

the class TestPnPStereoResidualReprojection method basicTest.

@Test
public void basicTest() {
    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(10, worldToLeft, false);
    PnPStereoResidualReprojection alg = new PnPStereoResidualReprojection();
    alg.setModel(new StereoPose(worldToLeft, leftToRight));
    // compute errors with perfect model
    double[] error = new double[alg.getN()];
    int index = alg.computeResiduals(pointPose.get(0), error, 0);
    assertEquals(alg.getN(), index);
    assertEquals(0, error[0], 1e-8);
    assertEquals(0, error[1], 1e-8);
    // compute errors with an incorrect model
    worldToLeft.getR().set(2, 1, 2);
    alg.setModel(new StereoPose(worldToLeft, leftToRight));
    index = alg.computeResiduals(pointPose.get(0), error, 0);
    assertEquals(alg.getN(), index);
    assertTrue(Math.abs(error[0]) > 1e-8);
    assertTrue(Math.abs(error[1]) > 1e-8);
}

Example 3 with StereoPose

use of boofcv.struct.sfm.StereoPose in project BoofCV by lessthanoptimal.

the class TestPnPStereoResidualReprojection method compareToReprojection.

@Test
public void compareToReprojection() {
    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(10, worldToLeft, false);
    // make the input model incorrect
    worldToLeft.getR().set(2, 1, 2);
    // compute the error in normalized image coordinates per element
    PnPStereoResidualReprojection alg = new PnPStereoResidualReprojection();
    alg.setModel(new StereoPose(worldToLeft, leftToRight));
    // compute errors with perfect model
    double[] error = new double[alg.getN()];
    alg.computeResiduals(pointPose.get(0), error, 0);
    double found = 0;
    for (double e : error) {
        found += e * e;
    }
    PnPStereoDistanceReprojectionSq validation = new PnPStereoDistanceReprojectionSq();
    StereoParameters param = new StereoParameters();
    param.rightToLeft = this.param.rightToLeft;
    // intrinsic parameters are configured to be identical to normalized image coordinates
    param.left = new CameraPinholeRadial(1, 1, 0, 0, 0, 0, 0).fsetRadial(0, 0);
    param.right = new CameraPinholeRadial(1, 1, 0, 0, 0, 0, 0).fsetRadial(0, 0);
    validation.setStereoParameters(param);
    validation.setModel(worldToLeft);
    double expected = validation.computeDistance(pointPose.get(0));
    assertEquals(expected, found, 1e-8);
}