Problems with the numerically estimated Jacobian

Hi everybody, I am trying to define a new edge for G2o based on the sba/Stereo_xyz_onlypose but I am using the inverse of the distance from a rgbd image. The problem that I have is when I test it based on the evaluate_jacobian.h tests. I keep having differences between my analytically derivated Jacobian and the numerically generated Jacobian generated by the test:

As you can see below, the numerically estimated Jacobian does not match on several elements from my Jacobian. I tested this and experimentally my Jacobian is working fine. That gives me confidence on my Jacobian derivation and I reviewed it thoughtfully, so I do not think there is something wrong with it. But still the test keeps failing. If you have any ideas on what is going on, it would be really appreciated.

My edge definition:

namespace g2o
{
Vector3 EdgeDepthSE3ProjectXYZOnlyPose::cam_project(const Vector3& transXYZ) const
{
    const float invZ = 1.0f / transXYZ[2];
    Vector3 res;
    res[0] = transXYZ[0] * invZ * fx + cx;
    res[1] = transXYZ[1] * invZ * fy + cy;
    res[2] = invZ;
    return res;
}

/*
.    Jacobian calculation comes from the derivation of the error:
.
.                                                                 [ fx/Pz     0      -fx.Px/Pz^2 ]
.    derror/dTcw = derror/dP . dP/dTcw = - [     0     fy/Pz  -fy.Py/Pz^2 ] . [ -[P^] [I] ]
.                                                                 [     0        0     -1/z^2    ]
.
.                           [ -fx/Pz     0      fx.Px/Pz^2 ]   [  0   Pz -Py  1  0  0 ]
.    derror/dTcw = [     0     -fy/Pz  fy.Py/Pz^2 ] . [ -Pz  0   Px  0  1  0 ]
.                           [     0        0      1/z^2    ]   [  Py -Px  0   0  0  1 ]
.
.                           [  fx.Px.Py/Pz^2    -fx.(1+Px^2/Pz^2)  fx.Py/Pz  -fx/Pz    0    fx.Px/Pz^2 ]
.    derror/dTcw = [ fy.(1+Py^2/Pz^2)   -fy.Py.Px/Pz^2   -fy.Px/Pz     0   -fy/Pz  fy.Py/Pz^2 ]
.                           [     Py/z^2             -Px/z^2          0         0      0        1/z^2  ]
*/
void EdgeDepthSE3ProjectXYZOnlyPose::linearizeOplus()
{
    VertexSE3Expmap* vi = static_cast<VertexSE3Expmap*>(_vertices[0]);
    Vector3 transXYZ = vi->estimate().map(Xw);
    number_t x = transXYZ[0];
    number_t y = transXYZ[1];
    number_t invZ = 1.0 / transXYZ[2];
    number_t invZ_2 = invZ * invZ;

    _jacobianOplusXi(0, 0) = x * y * invZ_2 * fx;
    _jacobianOplusXi(0, 1) = -(1.0 + x * x * invZ_2) * fx;
    _jacobianOplusXi(0, 2) = fx * y * invZ;
    _jacobianOplusXi(0, 3) = -fx * invZ;
    _jacobianOplusXi(0, 4) = 0.0;
    _jacobianOplusXi(0, 5) = x * invZ_2 * fx;

    _jacobianOplusXi(1, 0) = fy * (1.0 + y * y * invZ_2);
    _jacobianOplusXi(1, 1) = -fy * x * y * invZ_2;
    _jacobianOplusXi(1, 2) = -fy * x * invZ;
    _jacobianOplusXi(1, 3) = 0.0;
    _jacobianOplusXi(1, 4) = -fy * invZ;
    _jacobianOplusXi(1, 5) = fy * y * invZ_2;

    _jacobianOplusXi(2, 0) = y * invZ_2;
    _jacobianOplusXi(2, 1) = -x * invZ_2;
    _jacobianOplusXi(2, 2) = 0.0;
    _jacobianOplusXi(2, 3) = 0.0;
    _jacobianOplusXi(2, 4) = 0.0;
    _jacobianOplusXi(2, 5) = invZ_2;
}

} // namespace g2o

The unit test that I am using (it is the same test from this repo but we use the catch framework):

TEST_CASE("Rgbd Pose Only Edges using JacobianWorkspace", "[RGBD-POSE-ONLY-EDGE-JACOBIAN]")
{
    GIVEN("We want verify that the Jacobian behaves correctly")
    {
        CameraParameters params({1280.0f, 720.0f}, {700.0f, 700.0f}, {640.0f, 360.0f});

        g2o::VertexSE3Expmap v1;
        v1.setId(0);

        g2o::EdgeDepthSE3ProjectXYZOnlyPose e;
        e.fx = params.getFocalLength().x();
        e.fy = params.getFocalLength().y();
        e.cx = params.getPrincipalPoint().x();
        e.cy = params.getPrincipalPoint().y();
        e.Xw = Eigen::Vector3d::Random();

        e.setVertex(0, &v1);
        e.setInformation(Eigen::Matrix3d::Identity());

        WHEN("We use our own defined Jacobian")
        {
            g2o::JacobianWorkspace jacobianWorkspace;
            g2o::JacobianWorkspace numericJacobianWorkspace;
            numericJacobianWorkspace.updateSize(&e);
            numericJacobianWorkspace.allocate();

            THEN("Verify the jacobian with the numerically calculated jacobian several times using "
                 "random "
                 "data")
            {
                for (int k = 0; k < 1; ++k)
                {
                    g2o::SE3Quat random(g2o::Quaternion::UnitRandom(), g2o::Vector3::Random());
                    random.normalizeRotation();

                    v1.setEstimate(random);
                    e.setMeasurement(Eigen::Vector3d::Random());

                    testJacobianUnary(e, jacobianWorkspace, numericJacobianWorkspace);
                }
            }
        }
    }
}

The results that I’m getting:

Tests/Unit/G2o/TestJacobian.hpp:47: FAILED:
  CHECK( Approx(n[j]).margin(1e-3).epsilon(1e-5) == a[j] )
with expansion:
  Approx( 0.0 ) == -4.768644081
with message:
  Value: 0

Tests/Unit/G2o/TestJacobian.hpp:47: FAILED:
  CHECK( Approx(n[j]).margin(1e-3).epsilon(1e-5) == a[j] )
with expansion:
  Approx( 700.0000010748 ) == 737.3869441273
with message:
  Value: 1

Tests/Unit/G2o/TestJacobian.hpp:47: FAILED:
  CHECK( Approx(n[j]).margin(1e-3).epsilon(1e-5) == a[j] )
with expansion:
  Approx( 0.0 ) == 0.1431449104
with message:
  Value: 2

Tests/Unit/G2o/TestJacobian.hpp:47: FAILED:
  CHECK( Approx(n[j]).margin(1e-3).epsilon(1e-5) == a[j] )
with expansion:
  Approx( -699.999986864 ) == -700.6082328177
with message:
  Value: 3

Tests/Unit/G2o/TestJacobian.hpp:47: FAILED:
  CHECK( Approx(n[j]).margin(1e-3).epsilon(1e-5) == a[j] )
with expansion:
  Approx( 0.0000284217 ) == 4.768644081
with message:
  Value: 4

Tests/Unit/G2o/TestJacobian.hpp:47: FAILED:
  CHECK( Approx(n[j]).margin(1e-3).epsilon(1e-5) == a[j] )
with expansion:
  Approx( 0.0 ) == 0.0182579011
with message:
  Value: 5

Tests/Unit/G2o/TestJacobian.hpp:47: FAILED:
  CHECK( Approx(n[j]).margin(1e-3).epsilon(1e-5) == a[j] )
with expansion:
  Approx( 0.0 ) == -12.7805307971
with message:
  Value: 15

Tests/Unit/G2o/TestJacobian.hpp:47: FAILED:
  CHECK( Approx(n[j]).margin(1e-3).epsilon(1e-5) == a[j] )
with expansion:
  Approx( 0.0 ) == 100.201437287
with message:
  Value: 16

Tests/Unit/G2o/TestJacobian.hpp:47: FAILED:
  CHECK( Approx(n[j]).margin(1e-3).epsilon(1e-5) == a[j] )
with expansion:
  Approx( 0.0 ) == 0.3836453098
with message:
  Value: 17

RainerKuemmerle / g2o

Problems with the numerically estimated Jacobian #541