Eigen matrix product doesn't work correctly in ORB-SLAM3 in release mode on windows

301 Views Asked by At

The whole picture of the problem is as below:

I cloned the ORB-SLAM3 from https://github.com/rexdsp/ORB_SLAM3_Windows. It can be built and works well. Then I tried to evaluate its accuracy with EuRoC MH_01_easy, but found it was at least 10 times worse in release mode than the accuracy reported in the paper. After debugging several days, I found the reason was in the function below:

void EdgeSE3ProjectXYZ::linearizeOplus() {
    g2o::VertexSE3Expmap * vj = static_cast<g2o::VertexSE3Expmap *>(_vertices[1]);
    g2o::SE3Quat T(vj->estimate());
    g2o::VertexSBAPointXYZ* vi = static_cast<g2o::VertexSBAPointXYZ*>(_vertices[0]);
    Eigen::Vector3d xyz = vi->estimate();
    Eigen::Vector3d xyz_trans = T.map(xyz);

    double x = xyz_trans[0];
    double y = xyz_trans[1];
    double z = xyz_trans[2];

    auto projectJac = -pCamera->projectJac(xyz_trans);

    _jacobianOplusXi =  projectJac * T.rotation().toRotationMatrix();
    double* buf = (double*)_jacobianOplusXi.data();

    Eigen::Matrix<double,3,6> SE3deriv;
    SE3deriv << 0.f, z,   -y, 1.f, 0.f, 0.f,
            -z , 0.f, x, 0.f, 1.f, 0.f,
            y ,  -x , 0.f, 0.f, 0.f, 1.f;

    buf = (double*)SE3deriv.data();
    printf("S: %f %f %f %f %f %f\n", buf[0], buf[3], buf[6], buf[9], buf[12], buf[15]);
    printf("S: %f %f %f %f %f %f\n", buf[1], buf[4], buf[7], buf[10], buf[13], buf[16]);
    printf("S: %f %f %f %f %f %f\n", buf[2], buf[5], buf[8], buf[11], buf[14], buf[17]);

    _jacobianOplusXj = projectJac * SE3deriv;
    buf = (double*)_jacobianOplusXj.data();
    printf("j: %f %f %f %f %f %f\n", buf[0], buf[2], buf[4], buf[6], buf[8], buf[10]);
    printf("j: %f %f %f %f %f %f\n", buf[1], buf[3], buf[5], buf[7], buf[9], buf[11]);
}

The function does not work correctly in release mode, as what is shown in the log: (Jac is the content of projectJac printed in other function)

Jac: 6.586051 0.000000 5.789042

Jac: 0.000000 6.566550 -0.414389

S: 0.000000 69.640217 -4.394719 1.000000 0.000000 0.000000

S: -69.640217 0.000000 -61.212726 0.000000 1.000000 0.000000

S: 4.394719 61.212726 0.000000 0.000000 0.000000 1.000000

j: 25.478939 354.888530 0.000000 -0.000000 -0.000000 5.797627

j: -2.092870 -29.150968 0.000000 -0.000000 -0.000000 -0.476224

The whole system works correctly in Debug mode,the accuracy is similar to the results reported in the paper. And the log of the function above is as follows:

Jac: 6.586051 0.000000 5.789042

Jac: 0.000000 6.566550 -0.414389

S: 0.000000 69.640217 -4.394719 1.000000 0.000000 0.000000

S: -69.640217 0.000000 -61.212726 0.000000 1.000000 0.000000

S: 4.394719 61.212726 0.000000 0.000000 0.000000 1.000000

j: -25.441210 -813.017008 28.943840 -6.586051 -0.000000 -5.789042

j: 459.117113 25.365882 401.956448 0.000000 -6.566550 0.414389

Have you met this kind of problem before? Could you tell me the reason of this problem? I guess it is caused by compiler settings. Could anyone tell me how to solve this problem?

Thanks a lot.

Hi, I changed the declaration of projectJac and the whole system works correctly. My changes are as follows:

original:

    auto projectJac = -pCamera->projectJac(xyz_trans);

modification:

    Eigen::Matrix<double, 2, 3> projectJac = -pCamera->projectJac(xyz_trans);

The definition of function projectJac is as follows:

Eigen::Matrix<double, 2, 3> Pinhole::projectJac(const Eigen::Vector3d &v3D) {
    Eigen::Matrix<double, 2, 3> Jac;
    Jac(0, 0) = mvParameters[0] / v3D[2];
    Jac(0, 1) = 0.f;
    Jac(0, 2) = -mvParameters[0] * v3D[0] / (v3D[2] * v3D[2]);
    Jac(1, 0) = 0.f;
    Jac(1, 1) = mvParameters[1] / v3D[2];
    Jac(1, 2) = -mvParameters[1] * v3D[1] / (v3D[2] * v3D[2]);
    printf("v3D: %f %f %f\n", v3D[0], v3D[1], v3D[2]);
    printf("Jac: %f %f %f %f %f %f\n", Jac(0, 0), Jac(0, 1), Jac(0, 2), Jac(1, 0), Jac(1, 1), Jac(1, 2));
    return Jac;
}

Now it is clear the bug is caused by "auto". Could you tell me what is reason for this? How should I changed the settings in Visual Studio 2019 to make "auto" works correctly? Thanks a lot.

0

There are 0 best solutions below