search

MIT-SPARK / Kimera-VIO-ROS

ROS wrapper for Kimera-VIO
BSD 2-Clause "Simplified" License
377 stars 157 forks source link

got an identity pose: did you parse your ground-truth correctly? #103

Open zinuok opened 4 years ago

zinuok commented 4 years ago

Description: kimera_vio_ros_euroc node dies, printing error message: Check failed: !backendparams.initial_ground_truthstate.equals(VioNavState()) Requested initialization from Ground-Truth pose but got an identity pose: did you parse your ground-truth correctly?

I tried both V1_01_easy.bag and V1_02_medium.bag (even in both online mode and offline mode) but never succed in any case...

Command:

$ roslaunch kimera_vio_ros kimera_vio_ros_euroc.launch
$ rosbag play --clock V1_02_medium.bag

Console output: ----------- # Log Hz {avg +- std } [min,max] Data Provider [ms] 0
Display [ms] 0
Mesher [ms] 0
VioBackEnd [ms] 0
VioFrontEnd [ms] 0
Visualizer [ms] 0
backend_input_queue Size [#] 0
data_provider_left_frame_queue Size [#] 0
data_provider_right_frame_queue Size [#] 0
display_input_queue Size [#] 0
mesher_backend Size [#] 0
mesher_frontend Size [#] 0
stereo_frontend_input_queue Size [#] 0
visualizer_backend_queue Size [#] 0
visualizer_frontend_queue Size [#] 0
visualizer_mesher_queue Size [#] 0
W0907 20:05:25.083283 22711 DataProviderModule.cpp:103] Waiting for IMU data... W0907 20:05:25.134560 22711 DataProviderModule.cpp:103] Waiting for IMU data... W0907 20:05:25.182603 22711 DataProviderModule.cpp:103] Waiting for IMU data... W0907 20:05:25.232934 22711 DataProviderModule.cpp:103] Waiting for IMU data... W0907 20:05:25.282052 22711 DataProviderModule.cpp:103] Waiting for IMU data... W0907 20:05:25.332911 22711 DataProviderModule.cpp:103] Waiting for IMU data... F0907 20:05:25.353631 22680 VioBackEnd.h:159] Check failed: !backendparams.initial_ground_truthstate.equals(VioNavState()) Requested initialization from Ground-Truth pose but got an identity pose: did you parse your ground-truth correctly? Check failure stack trace: @ 0x7f7e149cf0 google::LogMessage::Fail() @ 0x7f7e14bc28 google::LogMessage::SendToLog() @ 0x7f7e149824 google::LogMessage::Flush() W0907 20:05:25.386818 22711 DataProviderModule.cpp:103] Waiting for IMU data... @ 0x7f7e14c4ac google::LogMessageFatal::~LogMessageFatal() @ 0x7f7cc03d14 VIO::VioBackEnd::initializeFromGt() @ 0x7f7cbdbeb4 VIO::VioBackEnd::spinOnce() @ 0x7f7cbc6210 VIO::VioBackEndModule::spinOnce() @ 0x7f7cbc69e4 VIO::PipelineModule<>::spin() @ 0x7f7d9d8e94 (unknown) @ 0x7f7d766088 start_thread [kimera_vio_ros/kimera_vio_ros_node-1] process has died [pid 22659, exit code -6, cmd /home/nvidia/catkin_ws/devel/lib/kimera_vio_ros/kimera_vio_ros_node --use_lcd=false --vocabulary_path=/home/nvidia/catkin_ws/src/Kimera-VIO/vocabulary/ORBvoc.yml --flagfile=/home/nvidia/catkin_ws/src/Kimera-VIO/params/Euroc/flags/Mesher.flags --flagfile=/home/nvidia/catkin_ws/src/Kimera-VIO/params/Euroc/flags/VioBackEnd.flags --flagfile=/home/nvidia/catkin_ws/src/Kimera-VIO/params/Euroc/flags/RegularVioBackEnd.flags --flagfile=/home/nvidia/catkin_ws/src/Kimera-VIO/params/Euroc/flags/Visualizer3D.flags --logtostderr=1 --colorlogtostderr=1 --log_prefix=1 --v=0 --log_output=false --log_euroc_gt_data=false --output_path=/home/nvidia/catkin_ws/src/Kimera-VIO-ROS/output_logs/ --viz_type=0 --visualize=true left_cam/image_raw:=/cam0/image_raw right_cam/image_raw:=/cam1/image_raw imu:=/imu0 gt_odom:=/vicon/firefly_sbx/firefly_sbx left_cam/camera_info:=/cam0/camera_info right_cam/camera_info:=/cam1/camera_info reinit_flag:=reinit_flag reinit_pose:=reinit_pose odometry:=odometry resiliency:=resiliency imu_bias:=imu_bias optimized_trajectory:=optimized_trajectory pose_graph:=pose_graph mesh:=mesh frontend_stats:=frontend_stats debug_mesh_img/image_raw:=debug_mesh_img/image_raw feature_tracks/image_raw:=feature_tracks/image_raw time_horizon_pointcloud:=time_horizon_pointcloud __name:=kimera_vio_ros_node __log:=/home/nvidia/.ros/log/43872724-f0f8-11ea-a676-48b02d05b9dc/kimera_vio_ros-kimera_vio_ros_node-1.log]. log file: /home/nvidia/.ros/log/43872724-f0f8-11ea-a676-48b02d05b9dc/kimera_vio_ros-kimera_vio_ros_node-1*.log

Please give also the following information:

ToniRV commented 4 years ago

Hi @zinuok, I'll have a look, in the meantime, try to set autoInitialize parameter in BackendParams.yaml to 1, here: https://github.mit.edu/SPARK/Kimera-VIO/blob/fd348b11831f6cdb32c252bbead412ca092a1639/params/Euroc/BackendParams.yaml#L7

FaboNo commented 4 years ago

@zinuok Did you manage to make it working? With autoInitialize set to 1 there is no more Requested initialization from Ground-Truth pose but there is still a problem with the topics because I still have the same messages as you:

I0917 13:42:53.100745  9645 DataProviderModule.cpp:99] No IMU data available. Reason:
I0917 13:42:53.100757  9645 DataProviderModule.cpp:99] No IMU data available. Reason:
I0917 13:42:53.100770  9645 DataProviderModule.cpp:99] No IMU data available. Reason:
I0917 13:42:53.100782  9645 DataProviderModule.cpp:99] No IMU data available. Reason:
I0917 13:42:53.100795  9645 DataProviderModule.cpp:99] No IMU data available. Reason:
I0917 13:42:53.100805  9645 DataProviderModule.cpp:99] No IMU data available. Reason:
I0917 13:42:53.100818  9645 DataProviderModule.cpp:99] No IMU data available. Reason:
I0917 13:42:53.100831  9645 DataProviderModule.cpp:99] No IMU data available. Reason:
I0917 13:42:53.108110  9330 StereoVisionFrontEnd.cpp:128] ------------------- Processing frame k = 2860--------------------
I0917 13:42:53.108171  9330 StereoVisionFrontEnd.cpp:153] Current IMU Preintegration frequency: 270270 Hz. (37 us).
W0917 13:42:53.109020  9645 PipelineModule.h:218] Module: Data Provider - No Input received.
I0917 13:42:53.114650  9330 Tracker.cpp:123] Optical Flow Timing [ms]: 4
I0917 13:42:53.115674  9330 ThreadsafeQueue.h:286] Queue with id: mesher_frontend is getting full, size: 3
I0917 13:42:53.115689  9330 ThreadsafeQueue.h:286] Queue with id: visualizer_frontend_queue is getting full, size: 3
I0917 13:42:53.149528  9645 DataProviderModule.cpp:99] No IMU data available. Reason:
W0917 13:42:53.149567  9645 DataProviderModule.cpp:103] Waiting for IMU data...
I0917 13:42:53.149601  9645 DataProviderModule.cpp:99] No IMU data available. Reason:
I0917 13:42:53.149616  9645 DataProviderModule.cpp:99] No IMU data available. Reason

It looks like that Kimera is not able to subscribe to some topics, namely imu0 but cam1 as well (it is a guess). I am running ROS kinetic. I was believing it might be an issue with ROS but as you run Melodic this is something else.