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.