Examples with ResidualsCodecToMatrix boofcv.abst.geo.optimization.ResidualsCodecToMatrix used on opensource projects

Example 1 with ResidualsCodecToMatrix

use of boofcv.abst.geo.optimization.ResidualsCodecToMatrix 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));
}