尝试解决 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.ccGetDeltaRotation() 函数内

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() 函数报错的地方,根据其他开发者反馈,仍有可能继续出现该报错。

  • 5
    点赞
  • 15
    收藏
    觉得还不错? 一键收藏
  • 0
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值