尝试解决 orbslam3 报错 SO3::exp failed! omega: -nan -nan -nan, real: -nan, img: -nan

问题

运行 orbslam3 有一定概率出现如下错误，在单目和 ros 单目模式中都出现过：

Sophus ensure failed in function 'static Sophus::SO3<Scalar_> Sophus::SO3<Scalar_, Options>::expAndTheta(const Tangent&, Sophus::SO3<Scalar_, Options>::Scalar*) [with Scalar_ = float; int Options = 0; Sophus::SO3<Scalar_, Options>::Tangent = Eigen::Matrix<float, 3, 1>; Sophus::SO3<Scalar_, Options>::Scalar = float]', file '/opt/ros/melodic/include/sophus/so3.hpp', line 566.
SO3::exp failed! omega: -nan -nan -nan, real: -nan, img: -nan

一旦出现该错误，就会导致 orbslam 程序崩溃。

解决尝试

解决方案来源：Solving Sophus -nan error：https://github.com/UZ-SLAMLab/ORB_SLAM3/issues/608

修改 1

即在 Sim3Solver.cc 存在一处可能除 0 的地方，随后会导致 Sophus::SO3f::exp() 函数报错

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();

应该修改为

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();

修改 2

在 ImuTypes.cc 中 GetDeltaRotation() 函数内

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());
}

修改为

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());
}

备注

以上两处修改仅修复了 orbslam3 中两处可能导致 Sophus::SO3f::exp() 函数报错的地方，根据其他开发者反馈，仍有可能继续出现该报错。