cartographer问题 imu_tracker.cc:70 Check failed: (orientation_ * gravity_vector_).z() ＞ 0. (nan vs. 0)

汉文修士

已于 2022-12-27 09:47:50 修改

阅读量1k

点赞数 1

分类专栏： cartographer 激光slam 问题文章标签：自动驾驶人工智能

于 2021-06-24 21:19:32 首次发布

本文链接：https://blog.csdn.net/c417469898/article/details/118196467

版权

问题同时被 3 个专栏收录

14 篇文章 0 订阅

订阅专栏

激光slam

9 篇文章 3 订阅

订阅专栏

cartographer

5 篇文章 2 订阅

订阅专栏

本文探讨了在使用自定义imu数据运行Cartographer ROS时遇到的Checkfailed错误，焦点在于(orientation_*gravity_vector_).z()的nan值。通过定位问题，发现imu数据中存在nan，影响了姿态估计。文章提供了关键代码段和解决步骤，包括检查并修复数据质量问题。

摘要由CSDN通过智能技术生成

问题描述

使用自己采集的imu数据，跑cartographer ros 时候发生
Check failed: (orientation_ * gravity_vector_).z() > 0. (nan vs. 0)

问题原因

把错误在cartographer 的imu_tracker.cc:70行代码如下：

CHECK_GT((orientation_ * gravity_vector_).z(), 0.);
CHECK_GT((orientation_ * gravity_vector_).normalized().z(), 0.99);

此处是(orientation_ * gravity_vector_).z()的值没有大于0.，它的实际值为nan所以抛出上述错误。

问题排查与解决

于是分别打印orientation_ 和gravity_vector_看看谁的值为nan，

  if (std::isnan(orientation_.z())) {
    std::cout<<"std::isnan(rotation)"<<orientation_.z()<< std::endl;
  }
  if (std::isnan(gravity_vector_[0]) || std::isnan(gravity_vector_[1]) ||std::isnan(gravity_vector_[2])) {
    std::cout<<"std::isnan(gravity_vector_)"<< gravity_vector_[2]<<std::endl;
  }

发现orientation_.z()的值有为nan的时候，

问题解决

因此可以去确定是imu数据中有nan的存在

header: 
  seq: 4576
  stamp: 
    secs: 1624269353
    nsecs: 608993773
  frame_id: "imu_link"
orientation: 
  x: 0.0
  y: 0.0
  z: 0.0
  w: 1.0
orientation_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
angular_velocity: 
  x: 0.0
  y: 0.0
  z: 0.0
angular_velocity_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
linear_acceleration: 
  x: nan
  y: nan
  z: nan
linear_acceleration_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]