Solving Sophus -nan error #608
Description
I got a Sophus error in witch I got nan values in a quaternion on a run that crash it.
I search in the issues here to see if it was a reccurent problem and I found out it was (in #439 and #451 at least), and didn't find any solution here.
So I searched out in the ORB_SLAM3 code for reasons of this issue and found out that it was caused by a division by 0 in the Sim3Solver.cc file in the ComputeSim3 function at the end of the Part 4.
Eigen::Vector3f vec = evec.block<3,1>(1,maxIndex); //extract imaginary part of the quaternion (sin*axis)
// Rotation angle. sin is the norm of the imaginary part, cos is the real part
double ang=atan2(vec.norm(),evec(0,maxIndex));
vec = 2*ang*vec/vec.norm(); //Angle-axis representation. quaternion angle is the half
mR12i = Sophus::SO3f::exp(vec).matrix();
must be replaced by :
Eigen::Vector3f vec = evec.block<3,1>(1,maxIndex); //extract imaginary part of the quaternion (sin*axis)
// Rotation angle. sin is the norm of the imaginary part, cos is the real part
double ang=atan2(vec.norm(),evec(0,maxIndex));
if(vec.norm()!=0){
vec = 2*ang*vec/vec.norm(); //Angle-axis representation. quaternion angle is the half
}
mR12i = Sophus::SO3f::exp(vec).matrix();
It seems the vec vector can be full of 0 some times and with this the error no longer appear (at least for me)