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

问题描述

使用自己采集的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]