[KAIST] Problem of running KAIST dataset enabling ZUPT.

[WILBURNHUANG]

Hi, new to SLAM here and really appreciate this great work.

I'm trying to run with KAIST Urban 28. The data was loaded successfully and I followed the suggested setting (disable "skip stop section", Speed 2.00).

I used "pgeneva_ros_kaist.launch" as launch file.

The error is "terminate called after throwing an instance of 'std::out_of_range'
what(): vector::_M_range_check: __n (which is 18446744073709551615) >= this->size() (which is 2001)".

Can anyone give any suggestion?

[bobododosjl]

same with u?also need help

[goldbattle]

Are you able to enable gdb debug mode and see where this is failing.
This likely is due to some of the IMU selection logic, but if you can identify where this seems to fail that would be much appreciated.

[zgxsin]

@WILBURNHUANG @sjlszz try to increase the threads. See #89

[HwangDaeHyun]

I don't think it's an issue in the'zupt' module.

When i run kaist sequence 27 without zupt (speed goes to 0) the same error occurs as below line.

And this is the gdb debug module.

what's the problem?

---

terminate called after throwing an instance of 'std::out_of_range'
  what():  vector::_M_range_check: __n (which is 18446744073709551615) >= this->size() (which is 5)
--Type <RET> for more, q to quit, c to continue without paging--

Thread 1 "run_subscribe_m" received signal SIGABRT, Aborted.
__GI_raise (sig=sig@entry=6) at ../sysdeps/unix/sysv/linux/raise.c:50
50	../sysdeps/unix/sysv/linux/raise.c: No such file or directory.
(gdb) bt
#0  __GI_raise (sig=sig@entry=6) at ../sysdeps/unix/sysv/linux/raise.c:50
#1  0x00007ffff7235859 in __GI_abort () at abort.c:79
#2  0x00007ffff760c951 in ?? () from /lib/x86_64-linux-gnu/libstdc++.so.6
#3  0x00007ffff761847c in ?? () from /lib/x86_64-linux-gnu/libstdc++.so.6
#4  0x00007ffff76184e7 in std::terminate() () from /lib/x86_64-linux-gnu/libstdc++.so.6
#5  0x00007ffff7618799 in __cxa_throw () from /lib/x86_64-linux-gnu/libstdc++.so.6
#6  0x00007ffff760f3eb in ?? () from /lib/x86_64-linux-gnu/libstdc++.so.6
#7  0x00007ffff7efefd4 in std::vector<ov_msckf::Propagator::IMUDATA, std::allocator<ov_msckf::Propagator::IMUDATA> >::_M_range_check (__n=18446744073709551615, this=0x555555b104f0)
    at /home/daehyun/catkin_ws/src/open_vins/ov_msckf/src/state/Propagator.cpp:237
#8  std::vector<ov_msckf::Propagator::IMUDATA, std::allocator<ov_msckf::Propagator::IMUDATA> >::at (
    __n=18446744073709551615, this=0x555555b104f0) at /usr/include/c++/8/bits/stl_vector.h:999
#9  ov_msckf::Propagator::select_imu_readings (
    imu_data=std::vector of length 5, capacity 2048 = {...}, time0=1544582682.4359739, 
    time1=1544582682.7359583)
    at /home/daehyun/catkin_ws/src/open_vins/ov_msckf/src/state/Propagator.cpp:233
#10 0x00007ffff7f063b1 in ov_msckf::Propagator::propagate_and_clone (this=0x555555b104a0, 
    state=0x555555b0afa0, timestamp=timestamp@entry=1544582682.7360213)
    at /home/daehyun/catkin_ws/src/open_vins/ov_msckf/src/state/Propagator.cpp:63
#11 0x00007ffff7f14fb9 in ov_msckf::VioManager::do_feature_propagate_update (this=0x555555b119a0, 
    timestamp=1544582682.7360213)
    at /home/daehyun/catkin_ws/src/open_vins/ov_msckf/src/core/VioManager.cpp:402
#12 0x00007ffff7f1971b in ov_msckf::VioManager::feed_measurement_stereo (this=0x555555b119a0, 
    timestamp=<optimised out>, img0=..., img1=..., cam_id0=<optimised out>, cam_id1=<optimised out>)
    at /home/daehyun/catkin_ws/src/open_vins/ov_msckf/src/core/VioManager.cpp:269
--Type <RET> for more, q to quit, c to continue without paging--
#13 0x0000555555582b52 in callback_stereo (msg0=..., msg1=...)
    at /home/daehyun/catkin_ws/src/open_vins/ov_msckf/src/ros_subscribe_msckf.cpp:207
#14 0x0000555555595c6c in boost::function9<void, boost::shared_ptr<sensor_msgs::Image_<std::allocator<void> > const> const&, boost::shared_ptr<sensor_msgs::Image_<std::allocator<void> > const> const&, boost::shared_ptr<message_filters::NullType const> const&, boost::shared_ptr<message_filters::NullType const> const&, boost::shared_ptr<message_filters::NullType const> const&, boost::shared_ptr<message_filters::NullType const> const&, boost::shared_ptr<message_filters::NullType const> const&, boost::shared_ptr<message_filters::NullType const> const&, boost::shared_ptr<message_filters::NullType const> const&>::operator() (a8=..., a7=..., a6=..., a5=..., a4=..., a3=..., a2=..., a1=..., a0=..., 
    this=<optimised out>) at /usr/include/boost/function/function_template.hpp:677
#15 boost::detail::function::void_function_obj_invoker9<boost::function<void (boost::shared_ptr<sensor_msgs::Image_<std::allocator<void> > const> const&, boost::shared_ptr<sensor_msgs::Image_<std::allocator<void> > const> const&, boost::shared_ptr<message_filters::NullType const> const&, boost::shared_ptr<message_filters::NullType const> const&, boost::shared_ptr<message_filters::NullType const> const&, boost::shared_ptr<message_filters::NullType const> const&, boost::shared_ptr<message_filters::NullType const> const&, boost::shared_ptr<message_filters::NullType const> const&, boost::shared_ptr<message_filters::NullType const> const&)>, void, boost::shared_ptr<sensor_msgs::Image_<std::allocator<void> > const>, boost::shared_ptr<sensor_msgs::Image_<std::allocator<void> > const>, boost::shared_ptr<message_filters::NullType const>, boost::shared_ptr<message_filters::NullType const>, boost::shared_ptr<message_filters::NullType const>, boost::shared_ptr<message_filters::NullType const>, boost::shared_ptr<message_filters::NullType const>, boost::shared_ptr<message_filters::NullType const>, boost::shared_ptr<message_filters::NullType const> >::invoke(boost::detail::function::function_buffer&, boost::shared_ptr<sensor_msgs::Image_<std::allocator<void> > const>, boost::shared_ptr<sensor_msgs::Image_<std::allocator<void> > const>, boost::shared_ptr<message_filters::NullType const>, boost::shared_ptr<message_filters::NullType const>, boost::shared_ptr<message_filters::NullType const>, boost::shared_pt--Type <RET> for more, q to quit, c to continue without paging--
r<message_filters::NullType const>, boost::shared_ptr<message_filters::NullType const>, boost::shared_ptr<message_filters::NullType const>, boost::shared_ptr<message_filters::NullType const>) (
    function_obj_ptr=..., a0=..., a1=..., a2=..., a3=..., a4=..., a5=..., a6=..., a7=..., a8=...)
    at /usr/include/boost/function/function_template.hpp:158
#16 0x00005555555b7130 in boost::function9<void, boost::shared_ptr<sensor_msgs::Image_<std::allocator<void> > const>, boost::shared_ptr<sensor_msgs::Image_<std::allocator<void> > const>, boost::shared_ptr<message_filters::NullType const>, boost::shared_ptr<message_filters::NullType const>, boost::shared_ptr<message_filters::NullType const>, boost::shared_ptr<message_filters::NullType const>, boost::shared_ptr<message_filters::NullType const>, boost::shared_ptr<message_filters::NullType const>, boost::shared_ptr<message_filters::NullType const> >::operator() (a8=..., a7=..., a6=..., a5=..., a4=..., 
    a3=..., a2=..., a1=..., a0=..., this=0x555555b396b8)
    at /usr/include/boost/smart_ptr/detail/shared_count.hpp:474
#17 message_filters::CallbackHelper9T<boost::shared_ptr<sensor_msgs::Image_<std::allocator<void> > const> const&, boost::shared_ptr<sensor_msgs::Image_<std::allocator<void> > const> const&, boost::shared_ptr<message_filters::NullType const> const&, boost::shared_ptr<message_filters::NullType const> const&, boost::shared_ptr<message_filters::NullType const> const&, boost::shared_ptr<message_filters::NullType const> const&, boost::shared_ptr<message_filters::NullType const> const&, boost::shared_ptr<message_filters::NullType const> const&, boost::shared_ptr<message_filters::NullType const> const&>::call (this=0x555555b396b0, nonconst_force_copy=<optimised out>, e0=..., e1=..., e2=..., e3=..., 
    e4=..., e5=..., e6=..., e7=..., e8=...) at /opt/ros/noetic/include/message_filters/signal9.h:128
#18 0x00005555555b0dcc in message_filters::Signal9<sensor_msgs::Image_<std::allocator<void> >, sensor_msgs::Image_<std::allocator<void> >, message_filters::NullType, message_filters::NullType, message_filters::NullType, message_filters::NullType, message_filters::NullType, message_filters::NullType, message_filters::NullType>::call (e8=..., e7=..., e6=..., e5=..., e4=..., e3=..., e2=..., e1=..., 
    e0=..., this=<optimised out>) at /opt/ros/noetic/include/message_filters/signal9.h:301
--Type <RET> for more, q to quit, c to continue without paging--
#19 message_filters::Synchronizer<message_filters::sync_policies::ApproximateTime<sensor_msgs::Image_<std::allocator<void> >, sensor_msgs::Image_<std::allocator<void> >, message_filters::NullType, message_filters::NullType, message_filters::NullType, message_filters::NullType, message_filters::NullType, message_filters::NullType, message_filters::NullType> >::signal (e8=..., e7=..., e6=..., e5=..., 
    e4=..., e3=..., e2=..., e1=..., e0=..., this=<optimised out>)
    at /opt/ros/noetic/include/message_filters/synchronizer.h:332
#20 message_filters::sync_policies::ApproximateTime<sensor_msgs::Image_<std::allocator<void> >, sensor_msgs::Image_<std::allocator<void> >, message_filters::NullType, message_filters::NullType, message_filters::NullType, message_filters::NullType, message_filters::NullType, message_filters::NullType, message_filters::NullType>::publishCandidate (this=0x7fffffffcc70)
    at /opt/ros/noetic/include/message_filters/sync_policies/approximate_time.h:569
#21 0x00005555555b27b8 in message_filters::sync_policies::ApproximateTime<sensor_msgs::Image_<std::allocator<void> >, sensor_msgs::Image_<std::allocator<void> >, message_filters::NullType, message_filters::NullType, message_filters::NullType, message_filters::NullType, message_filters::NullType, message_filters::NullType, message_filters::NullType>::process (this=0x7fffffffcc70)
    at /usr/include/c++/8/bits/stl_bvector.h:882
#22 0x00005555555b6218 in message_filters::sync_policies::ApproximateTime<sensor_msgs::Image_<std::allocator<void> >, sensor_msgs::Image_<std::allocator<void> >, message_filters::NullType, message_filters::NullType, message_filters::NullType, message_filters::NullType, message_filters::NullType, message_filters::NullType, message_filters::NullType>::add<1> (this=0x7fffffffcc70, evt=...)
    at /usr/include/boost/tuple/detail/tuple_basic.hpp:474
#23 0x00005555555aa40e in boost::function1<void, ros::MessageEvent<sensor_msgs::Image_<std::allocator<void> > const> const&>::operator() (a0=..., this=<optimised out>)
    at /usr/include/boost/function/function_template.hpp:677
#24 message_filters::CallbackHelper1T<ros::MessageEvent<sensor_msgs::Image_<std::allocator<void> > co--Type <RET> for more, q to quit, c to continue without paging--
nst> const&, sensor_msgs::Image_<std::allocator<void> > >::call (nonconst_force_copy=false, 
    event=..., this=<optimised out>) at /opt/ros/noetic/include/message_filters/signal1.h:76
#25 message_filters::Signal1<sensor_msgs::Image_<std::allocator<void> > >::call (event=..., 
    this=<optimised out>) at /opt/ros/noetic/include/message_filters/signal1.h:119
#26 message_filters::SimpleFilter<sensor_msgs::Image_<std::allocator<void> > >::signalMessage (
    event=..., this=<optimised out>) at /opt/ros/noetic/include/message_filters/simple_filter.h:136
#27 message_filters::Subscriber<sensor_msgs::Image_<std::allocator<void> > >::cb (
    this=<optimised out>, e=...) at /opt/ros/noetic/include/message_filters/subscriber.h:206
#28 0x00005555555b785c in boost::function1<void, ros::MessageEvent<sensor_msgs::Image_<std::allocator<void> > const> const&>::operator() (a0=..., this=0x555555b38e48)
    at /usr/include/boost/function/function_template.hpp:677
#29 ros::SubscriptionCallbackHelperT<ros::MessageEvent<sensor_msgs::Image_<std::allocator<void> > const> const&, void>::call (this=0x555555b38e40, params=...)
    at /opt/ros/noetic/include/ros/subscription_callback_helper.h:144
#30 0x00007ffff7df5059 in ros::SubscriptionQueue::call() () from /opt/ros/noetic/lib/libroscpp.so
#31 0x00007ffff7da3702 in ros::CallbackQueue::callOneCB(ros::CallbackQueue::TLS*) ()
   from /opt/ros/noetic/lib/libroscpp.so
#32 0x00007ffff7da4c13 in ros::CallbackQueue::callAvailable(ros::WallDuration) ()
   from /opt/ros/noetic/lib/libroscpp.so
#33 0x00007ffff7df7eef in ros::SingleThreadedSpinner::spin(ros::CallbackQueue*) ()
   from /opt/ros/noetic/lib/libroscpp.so
#34 0x00007ffff7de012f in ros::spin() () from /opt/ros/noetic/lib/libroscpp.so
#35 0x0000555555580627 in main (argc=<optimised out>, argv=<optimised out>)
    at /home/daehyun/catkin_ws/src/open_vins/ov_msckf/src/ros_subscribe_msckf.cpp:112

[zgxsin]

The problem seems to happen on this function: select_imu_readings. From the debugging info, you select IMU reading in a 0.3s period. If your IMU measurements is published at 400HZ, you will select 0.3/(1/400)=120 IMU measurements, which might lead to the computation problem. That's my initial thought. Discussion is welcome.

[goldbattle]

It seems that there are not enough IMU measurements to propagate with, thus is crashing.
I just tried the Urban 28 on my machine and it runs fine with 2x speed.
If I run it at 10x speed I got it to crash part way into the dataset (not in a middle of a stop though).
Can you try playing at a slower speed and see if you get the same issue?

[goldbattle]

Can you please see if #117 fixes this issue for you?