Kimera-VIO-ROS
Kimera-VIO-ROS copied to clipboard
Kimera-VIO node crashes if loop closure detection is enabled
Description: When running Kimera-VIO-ROS in real-time using a Mynt Eye S camera as data source, the Kimera node crashes at startup if the loop closure detector is enabled. If the LCD is disabled, the crash does not happen.
Command:
roslaunch kimera_vio_ros kimera_ros_mymynt.launch use_lcd:=true
Console output:
[ INFO] [1579263518.245934011]: Starting KimeraVIO wrapper for online
[ INFO] [1579263518.261440349]: >>>>>>> Started data subscribers <<<<<<<<
I0117 14:18:38.291894 11760 RegularVioBackEnd.cpp:113] Using Regular VIO backend.
I0117 14:18:38.292043 11760 LoopClosureDetector.cpp:98] LoopClosureDetector:: Loading vocabulary from /home/martino/catkin_ws/src/Kimera-VIO/vocabulary/ORBvoc.yml
F0117 14:18:38.414885 11881 RosOnlineDataProvider.cpp:156] Check failed: imu_single_callback_ Did you forget to register the IMU callback?
*** Check failure stack trace: ***
@ 0x7f5d24e4643d google::LogMessage::Fail()
@ 0x7f5d24e48253 google::LogMessage::SendToLog()
@ 0x7f5d24e45fcb google::LogMessage::Flush()
@ 0x7f5d24e48c3e google::LogMessageFatal::~LogMessageFatal()
@ 0x47d06f VIO::RosOnlineDataProvider::callbackIMU()
@ 0x48553b boost::detail::function::void_function_obj_invoker1<>::invoke()
@ 0x49e2ee ros::SubscriptionCallbackHelperT<>::call()
@ 0x7f5d242b0d8d ros::SubscriptionQueue::call()
@ 0x7f5d24256838 ros::CallbackQueue::callOneCB()
@ 0x7f5d24257074 ros::CallbackQueue::callOne()
@ 0x7f5d242b4265 ros::AsyncSpinnerImpl::threadFunc()
@ 0x7f5d1bb295d5 (unknown)
@ 0x7f5d234e86ba start_thread
@ 0x7f5d218a141d clone
Full output (click to expand)
$ roslaunch kimera_vio_ros kimera_ros_mymynt.launch use_lcd:=true
WARNING: Package name "KimeraRPGO" does not follow the naming conventions. It should start with a lower case letter and only contain lower case letters, digits, underscores, and dashes.
... logging to /home/martino/.ros/log/f6f3aedc-3919-11ea-8226-a4c3f096aa6e/roslaunch-martino-ThinkPad-T480-11743.log
Checking log directory for disk usage. This may take awhile.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.
WARNING: Package name "KimeraRPGO" does not follow the naming conventions. It should start with a lower case letter and only contain lower case letters, digits, underscores, and dashes.
started roslaunch server http://martino-ThinkPad-T480:37317/
SUMMARY
========
PARAMETERS
* /kimera_vio_ros/kimera_vio_ros/base_link_frame_id: base_link
* /kimera_vio_ros/kimera_vio_ros/left_cam_frame_id: left_cam
* /kimera_vio_ros/kimera_vio_ros/map_frame_id: map
* /kimera_vio_ros/kimera_vio_ros/mono_ransac_threshold: 30
* /kimera_vio_ros/kimera_vio_ros/online_run: True
* /kimera_vio_ros/kimera_vio_ros/position_det_threshold: 0.3
* /kimera_vio_ros/kimera_vio_ros/right_cam_frame_id: right_cam
* /kimera_vio_ros/kimera_vio_ros/stereo_ransac_threshold: 20
* /kimera_vio_ros/kimera_vio_ros/use_lcd: True
* /kimera_vio_ros/kimera_vio_ros/velocity_det_threshold: 0.1
* /kimera_vio_ros/kimera_vio_ros/world_frame_id: world
* /kimera_vio_ros/posegraph_viewer/frame_id: world
* /rosdistro: kinetic
* /rosversion: 1.12.14
* /use_sim_time: True
NODES
/kimera_vio_ros/
kimera_vio_ros (kimera_vio_ros/kimera_vio_ros)
posegraph_viewer (pose_graph_tools/visualizer_node)
ROS_MASTER_URI=http://localhost:11311
WARNING: Package name "KimeraRPGO" does not follow the naming conventions. It should start with a lower case letter and only contain lower case letters, digits, underscores, and dashes.
process[kimera_vio_ros/kimera_vio_ros-1]: started with pid [11760]
process[kimera_vio_ros/posegraph_viewer-2]: started with pid [11761]
[ INFO] [1579263518.118055797]: Initializing pose graph visualizer
[ERROR] [1579263518.118760149]: Did not load frame id
I0117 14:18:38.180212 11760 CameraParams.cpp:31] Parsing camera parameters for: left_cam
I0117 14:18:38.180420 11760 CameraParams.cpp:31] Parsing camera parameters for: right_cam
I0117 14:18:38.180681 11760 DataProviderInterface.cpp:96] Running dataset between frame 50 and frame 10000
[ INFO] [1579263518.181391194]: >>>>>>> Initializing Kimera-VIO-ROS <<<<<<<
I0117 14:18:38.181444 11760 RosDataProvider.cpp:879] ================================================================================
- Left camera info:
I0117 14:18:38.181465 11760 CameraParams.cpp:175] ------------ Camera ID: left_cam -------------
intrinsics_: 365.791000 , 365.396000 , 395.442000 , 251.729000 ,
I0117 14:18:38.181473 11760 CameraParams.cpp:178] body_Pose_cam_:
|0.000849008, -0.999944, -0.0105228|
|0.999938, 0.000731755, 0.0111416|
|-0.0111333, -0.0105316, 0.999883|
[0.00180156, -0.0425878, 0.0265878]';
gtsam calibration:
.K[
365.791 0 395.442;
0 365.396 251.729;
0 0 1
]
gtsam calibration:
.k[-0.022520039; -0.008515824; 0.011157318; -0.006467303];
I0117 14:18:38.181605 11760 CameraParams.cpp:183] frame_rate_: 0.05
image_size_: width= 752 height= 480
camera_matrix_:
[365.791, 0, 395.442;
0, 365.396, 251.729;
0, 0, 1]
distortion_model_: equidistant
distortion_coeff_:
[-0.022520039, -0.008515824, 0.011157318, -0.006467303]
R_rectify_:
[]
undistRect_map_y_ too large to display (only created in StereoFrame)
P_:
[]
I0117 14:18:38.181645 11760 RosDataProvider.cpp:881] ================================================================================
- Right camera info:
I0117 14:18:38.181658 11760 CameraParams.cpp:175] ------------ Camera ID: right_cam -------------
intrinsics_: 366.845000 , 366.261000 , 390.195000 , 256.602000 ,
I0117 14:18:38.181664 11760 CameraParams.cpp:178] body_Pose_cam_:
|-0.00102832, -0.999971, -0.00756387|
|0.999947, -0.00110545, 0.0102007|
|-0.0102088, -0.00755298, 0.999919|
[0.0021576, 0.0765324, 0.0256731]';
gtsam calibration:
.K[
366.845 0 390.195;
0 366.261 256.602;
0 0 1
]
gtsam calibration:
.k[-0.018005025; -0.013159767; 0.014297214; -0.007286674];
I0117 14:18:38.181732 11760 CameraParams.cpp:183] frame_rate_: 0.05
image_size_: width= 752 height= 480
camera_matrix_:
[366.845, 0, 390.195;
0, 366.261, 256.602;
0, 0, 1]
distortion_model_: equidistant
distortion_coeff_:
[-0.018005025, -0.013159767, 0.014297214, -0.007286674]
R_rectify_:
[]
undistRect_map_y_ too large to display (only created in StereoFrame)
P_:
[]
I0117 14:18:38.181762 11760 RosDataProvider.cpp:883] ================================================================================
- IMU info:
I0117 14:18:38.181772 11760 ImuFrontEndParams.cpp:89] ------------ ImuParams::print -------------
gyroscope_noise_density: 0.0125
gyroscope_random_walk: 2.3e-05
accelerometer_noise_density: 0.0169
accelerometer_random_walk: 0.0005
imu_integration_sigma: 1e-08
imu_time_shift: 0
n_gravity: 00000
00000
-9.81
I0117 14:18:38.181795 11760 RosDataProvider.cpp:885] ================================================================================
- IMU params:
I0117 14:18:38.181802 11760 ImuFrontEndParams.cpp:89] ------------ ImuParams::print -------------
gyroscope_noise_density: 0.0125
gyroscope_random_walk: 2.3e-05
accelerometer_noise_density: 0.0169
accelerometer_random_walk: 0.0005
imu_integration_sigma: 1e-08
imu_time_shift: 0
n_gravity: 00000
00000
-9.81
I0117 14:18:38.181819 11760 RosDataProvider.cpp:887] ================================================================================
- Backend params
I0117 14:18:38.181828 11760 VioBackEndParams.cpp:154] $$$$$$$$$$$$$$$$$$$$$ VIO PARAMETERS $$$$$$$$$$$$$$$$$$$$$
** INITIALIZATION parameters **
autoInitialize_: 1
initial_ground_truth_state_:
I0117 14:18:38.181839 11760 DataProviderInterface-definitions.cpp:35] --- ---
pose:
R:
[
1 0 0;
0 1 0;
0 0 1
]
I0117 14:18:38.181881 11760 DataProviderInterface-definitions.cpp:37]
velocity:
0 0 0
[0, 0, 0]';
imuBias:
acc = [0, 0, 0]' gyro = [0, 0, 0]'
I0117 14:18:38.181913 11760 VioBackEndParams.cpp:159] roundOnAutoInitialize_: 0
initialPositionSigma: 1e-05
initialRollPitchSigma: 0.174533
initialYawSigma: 0.00174533
initialVelocitySigma: 0.001
initialAccBiasSigma: 0.1
initialGyroBiasSigma: 0.01
** VISION parameters **
linearizationMode_: 0 HESSIAN, IMPLICIT_SCHUR, JACOBIAN_Q, JACOBIAN_SVD
degeneracyMode_: 1 IGNORE_DEGENERACY, ZERO_ON_DEGENERACY, HANDLE_INFINITY
rankTolerance_: 1
landmarkDistanceThreshold_: 15
outlierRejection_: 8
retriangulationThreshold_: 0.001
addBetweenStereoFactors_: 1
betweenRotationPrecision_: 0
betweenTranslationPrecision_: 100
** OPTIMIZATION parameters **
relinearizeThreshold_: 0.01
relinearizeSkip_: 1
zeroVelocitySigma_: 0.001
noMotionPositionSigma_: 0.001
noMotionRotationSigma_: 0.0001
constantVelSigma_: 0.01
numOptimize_: 1
horizon_: 5
useDogLeg_: 0
I0117 14:18:38.181957 11760 RosDataProvider.cpp:889] ================================================================================
I0117 14:18:38.181964 11760 LoopClosureDetectorParams.cpp:113] $$$$$$$$$$$$$$$$$$$$$ LCD PARAMETERS $$$$$$$$$$$$$$$$$$$$$
image_width_: 752
image_height_: 480
focal_length_: 1
principle_point_: [0, 0]
use_nss_: 1
alpha_: 0.001
min_temporal_matches_: 1
dist_local_: 1
max_db_results_: 50
max_db_results_: 50
min_nss_factor_: 0.05
min_matches_per_group_: 1
max_intragroup_gap_: 3
max_distance_between_groups_: 3
max_distance_between_queries_: 2
geom_check_: 0
min_correspondences_: 12
max_ransac_iterations_mono_: 500
ransac_probability_mono_: 0.995
ransac_threshold_mono_: 1e-05
ransac_randomize_mono_: 1
ransac_inlier_threshold_mono_: 0.01
pose_recovery_option_: 0
max_ransac_iterations_stereo_: 500
ransac_probability_stereo_: 0.995
ransac_threshold_stereo_: 0.3
ransac_randomize_stereo_: 1
ransac_inlier_threshold_stereo_: 0.3
use_mono_rot_:0
lowe_ratio_: 0.2
matcher_type_:3
nfeatures_: 1000
scale_factor_: 1.2
nlevels_: 8
edge_threshold_: 31
first_level_: 0
WTA_K_: 2
score_type_: 0
patch_sze_: 31
fast_threshold_: 20
pgo_rot_threshold_: 0.005
pgo_trans_threshold_: 0.05
I0117 14:18:38.182006 11760 RosDataProvider.cpp:891] ================================================================================
[ INFO] [1579263518.245934011]: Starting KimeraVIO wrapper for online
[ INFO] [1579263518.261440349]: >>>>>>> Started data subscribers <<<<<<<<
I0117 14:18:38.291894 11760 RegularVioBackEnd.cpp:113] Using Regular VIO backend.
I0117 14:18:38.292043 11760 LoopClosureDetector.cpp:98] LoopClosureDetector:: Loading vocabulary from /home/martino/catkin_ws/src/Kimera-VIO/vocabulary/ORBvoc.yml
F0117 14:18:38.414885 11881 RosOnlineDataProvider.cpp:156] Check failed: imu_single_callback_ Did you forget to register the IMU callback?
*** Check failure stack trace: ***
@ 0x7f5d24e4643d google::LogMessage::Fail()
@ 0x7f5d24e48253 google::LogMessage::SendToLog()
@ 0x7f5d24e45fcb google::LogMessage::Flush()
@ 0x7f5d24e48c3e google::LogMessageFatal::~LogMessageFatal()
@ 0x47d06f VIO::RosOnlineDataProvider::callbackIMU()
@ 0x48553b boost::detail::function::void_function_obj_invoker1<>::invoke()
@ 0x49e2ee ros::SubscriptionCallbackHelperT<>::call()
@ 0x7f5d242b0d8d ros::SubscriptionQueue::call()
@ 0x7f5d24256838 ros::CallbackQueue::callOneCB()
@ 0x7f5d24257074 ros::CallbackQueue::callOne()
@ 0x7f5d242b4265 ros::AsyncSpinnerImpl::threadFunc()
@ 0x7f5d1bb295d5 (unknown)
@ 0x7f5d234e86ba start_thread
@ 0x7f5d218a141d clone
[kimera_vio_ros/kimera_vio_ros-1] process has died [pid 11760, exit code -6, cmd /home/martino/catkin_ws/devel/lib/kimera_vio_ros/kimera_vio_ros --left_cam_params_path=/home/martino/catkin_ws/src/Kimera-VIO-ROS/param/MyMynt/LeftCameraParams.yaml --right_cam_params_path=/home/martino/catkin_ws/src/Kimera-VIO-ROS/param/MyMynt/RightCameraParams.yaml --imu_params_path=/home/martino/catkin_ws/src/Kimera-VIO-ROS/param/MyMynt/ImuParams.yaml --backend_params_path=/home/martino/catkin_ws/src/Kimera-VIO-ROS/param/MyMynt/regularVioParameters.yaml --frontend_params_path=/home/martino/catkin_ws/src/Kimera-VIO-ROS/param/MyMynt/trackerParameters.yaml --use_lcd=true --lcd_params_path=/home/martino/catkin_ws/src/Kimera-VIO-ROS/param/MyMynt/LCDParameters.yaml --vocabulary_path=/home/martino/catkin_ws/src/Kimera-VIO/vocabulary/ORBvoc.yml --flagfile=/home/martino/catkin_ws/src/Kimera-VIO-ROS/param/MyMynt/flags/Mesher.flags --flagfile=/home/martino/catkin_ws/src/Kimera-VIO-ROS/param/MyMynt/flags/VioBackEnd.flags --flagfile=/home/martino/catkin_ws/src/Kimera-VIO-ROS/param/MyMynt/flags/RegularVioBackEnd.flags --flagfile=/home/martino/catkin_ws/src/Kimera-VIO-ROS/param/MyMynt/flags/Visualizer3D.flags --logtostderr=1 --colorlogtostderr=1 --log_prefix=1 --v=0 --backend_type=1 --log_output=false --output_path=/home/martino/catkin_ws/src/Kimera-VIO-ROS/output_logs/ --visualize=false --parallel_run=True left_cam:=/mynteye/left/image_raw right_cam:=/mynteye/right/image_raw imu:=/mynteye/imu/data_raw 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:=debug_mesh_img time_horizon_pointcloud:=time_horizon_pointcloud __name:=kimera_vio_ros __log:=/home/martino/.ros/log/f6f3aedc-3919-11ea-8226-a4c3f096aa6e/kimera_vio_ros-kimera_vio_ros-1.log].
log file: /home/martino/.ros/log/f6f3aedc-3919-11ea-8226-a4c3f096aa6e/kimera_vio_ros-kimera_vio_ros-1*.log
Additional files: Please attach all the files needed to reproduce the error.
- kimera_ros_mymynt.launch (identical to the example MyntEye launch file except for the name of the parameter folder)
Please give also the following information:
- SparkVio branch, tag or commit used: bceed7203f824c1bcffc0c195a186d13c5ea2b22
- GTSAM version used: 4.0.0 (342f30d148fae84c92ff71705c9e50e0a3683bda)
- OpenGV version used: 306a54e6c6b94e2048f820cdf77ef5281d4b48ad
- OpenCV version used: type
opencv_version
: 3.4.2 - Operating system and version (e.g. Ubuntu 16.04 or Windows 10): Ubuntu 16.04
- Did you change the source code? (yes / no): no
It seems like setting use_lcd=True
skips the registration of the IMU callback.
@marcusabate any idea?
Hi, @marcusabate @ToniRV , I know this is an older issue but currently experiencing this one due to me using older versions of Kimera VIO/ROS due to the issues persistent here: https://github.com/MIT-SPARK/Kimera-VIO-ROS/issues/78 I'm guessing this issue was solved with the heavy rewrites of some of the components but was this issue discovered? Can I solve it somehow on my end on this older version,
Hi @ToniRV @marcusabate @m-pilia, did any of you solved this issue? A very similar things happens when running the default example as:
roslaunch kimera_vio_ros kimera_vio_ros_euroc.launch rosbag_path:="/home/c-morsingher/Downloads/V1_01_easy.bag" use_lcd:=true
It works fine for a while, but then it crashes with the following error:
[kimera_vio_ros/kimera_vio_ros_node-2] process has died [pid 1285605, exit code -9, cmd /home/c-morsingher/catkin_ws/devel/lib/kimera_vio_ros/kimera_vio_ros_node --use_lcd=true --vocabulary_path=/home/c-morsingher/catkin_ws/src/Kimera-VIO/vocabulary/ORBvoc.yml --flagfile=/home/c-morsingher/catkin_ws/src/Kimera-VIO/params/Euroc/flags/Mesher.flags --flagfile=/home/c-morsingher/catkin_ws/src/Kimera-VIO/params/Euroc/flags/VioBackend.flags --flagfile=/home/c-morsingher/catkin_ws/src/Kimera-VIO/params/Euroc/flags/RegularVioBackend.flags --flagfile=/home/c-morsingher/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/c-morsingher/catkin_ws/src/Kimera-VIO-ROS/output_logs/ --viz_type=0 --visualize=true 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/c-morsingher/.ros/log/738a43aa-d4d4-11eb-9477-fdf8646b1d04/kimera_vio_ros-kimera_vio_ros_node-2.log]. log file: /home/c-morsingher/.ros/log/738a43aa-d4d4-11eb-9477-fdf8646b1d04/kimera_vio_ros-kimera_vio_ros_node-2*.log
If I run the same command without the use_lcd:=true
flag, everything works just fine. Any ideas?
Hi,
I also encounter this problem and it appears to be something wrong with the opengv or eigen. Has anybody found the solution?