Solving Sophus -nan error

[Waterfly222]

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)

[WeilongXia]

I tried to do this, but it can not work for me:(

[Waterfly222]

I found out another issue of the same type after this modification too, I found out it was in the ImuTypes.cc file in the GetDeltaRotation function.

Eigen::Matrix3f Preintegrated::GetDeltaRotation(const Bias &b_)
{
    std::unique_lock<std::mutex> lock(mMutex);
    Eigen::Vector3f dbg;
    dbg << b_.bwx-b.bwx,b_.bwy-b.bwy,b_.bwz-b.bwz;
    return NormalizeRotation(dR * Sophus::SO3f::exp(JRg * dbg).matrix());
}

I've probably done the worst way of solvng it, but it works for me at least now.
I've added this :

Eigen::Matrix3f Preintegrated::GetDeltaRotation(const Bias &b_)
{
    std::unique_lock<std::mutex> lock(mMutex);
    Eigen::Vector3f dbg;
    dbg << b_.bwx-b.bwx,b_.bwy-b.bwy,b_.bwz-b.bwz;
    if(dbg.array().isNaN()[0]){
         dbg = Eigen::Vector3f(0,0,0)
    }
    return NormalizeRotation(dR * Sophus::SO3f::exp(JRg * dbg).matrix());
}

If someone found out a better solution (because there surely are many), please drop it here

[WeilongXia]

I've found the cause of this problem, at least for me, it is because of the confused timestamp of IMU message...Thanks~

[macTracyHuang]

I've found the cause of this problem, at least for me, it is because of the confused timestamp of IMU message...Thanks~

hi, could you describe the reason in detail? what do u mean by confused timestamp? thx

[LarryDong]

I found this error happens when the IMU begin to initialize the dirG.
In LocalMapping.cc, this line:

dirG -= (*itKF)->mPrevKF->GetImuRotation() * (*itKF)->mpImuPreintegrated->GetUpdatedDeltaVelocity();

GetUpdatedDeltaVelocity returns "nan" and then the following line:

Rwg = Sophus::SO3f::exp(vzg).matrix();

reports this "sophus exp" error because vzg become "nan" too.

However, I can't figure out why now.

[LarryDong]

I found this error happens when the IMU begin to initialize the dirG. In LocalMapping.cc, this line:

dirG -= (*itKF)->mPrevKF->GetImuRotation() * (*itKF)->mpImuPreintegrated->GetUpdatedDeltaVelocity();

GetUpdatedDeltaVelocity returns "nan" and then the following line:

Rwg = Sophus::SO3f::exp(vzg).matrix();

reports this "sophus exp" error because vzg become "nan" too.

However, I can't figure out why now.

I made a silly mistake. I forgot to clear the IMU measurement before extracting measurements between two frames. Thus, the preintegration start from the 1st data, which leads to PreintegrateIMU() in tracking.cc gives a "nan" after some frames.
I guess nobody would make the same mistake as me. But I still paste my issue here and suggest others who have the same issue, try to check the IMU raw data and load process.

[KOENSAN]

I've found the cause of this problem, at least for me, it is because of the confused timestamp of IMU message...Thanks~

can you tell me details?

[macTracyHuang]

I've found the cause of this problem, at least for me, it is because of the confused timestamp of IMU message...Thanks~

can you tell me details?

you can check your imu data timestamp order.

[Yupei-Huang]

I found this error happens when the IMU begin to initialize the dirG. In LocalMapping.cc, this line:

dirG -= (*itKF)->mPrevKF->GetImuRotation() * (*itKF)->mpImuPreintegrated->GetUpdatedDeltaVelocity();

GetUpdatedDeltaVelocity returns "nan" and then the following line:

Rwg = Sophus::SO3f::exp(vzg).matrix();

reports this "sophus exp" error because vzg become "nan" too.
However, I can't figure out why now.

I made a silly mistake. I forgot to clear the IMU measurement before extracting measurements between two frames. Thus, the preintegration start from the 1st data, which leads to PreintegrateIMU() in tracking.cc gives a "nan" after some frames. I guess nobody would make the same mistake as me. But I still paste my issue here and suggest others who have the same issue, try to check the IMU raw data and load process.

I had the same problem, thanks for sharing!

[TSoli]

#730 helped with this for me.