Problem running the dynamic initialization

I am working on cable robots and wanted to try the dynamic initialization (due to oscillating motion even in steady state) which was recently added in OpenVINS. The only material available on dynamic initialization with OpenVINS is the youtube video (demonstration). Which is not telling any explanation. I simply tried the tutorial steps on running OpenVINS.

terminal 1. roscore
terminal 2. source devel/setup.bash
terminal 2. roslaunch ov_msckf subscribe.launch config:=SCAMPI  # The SCAMPI project configuration file

And I am getting the following error.

ROS_MASTER_URI=http://localhost:11311

process[run_subscribe_msckf-1]: started with pid [303797]
overriding node verbosity with value from ROS!
Setting printing level to: INFO
overriding node max_cameras with value from ROS!
the node init_dyn_mle_opt_calib of type [b] was not found...
the node init_dyn_mle_max_iter of type [i] was not found...
the node init_dyn_mle_max_threads of type [i] was not found...
the node init_dyn_mle_max_time of type [d] was not found...
the node init_dyn_num_pose of type [i] was not found...
the node init_dyn_min_deg of type [d] was not found...
the node init_dyn_inflation_ori of type [d] was not found...
the node init_dyn_inflation_vel of type [d] was not found...
the node init_dyn_inflation_bg of type [d] was not found...
the node init_dyn_inflation_ba of type [d] was not found...
the node init_dyn_min_rec_cond of type [d] was not found...
the node init_dyn_bias_g of type [St6vectorIdSaIdEE] was not found...
the node init_dyn_bias_a of type [St6vectorIdSaIdEE] was not found...
overriding node max_cameras with value from ROS!
overriding node use_stereo with value from ROS!
the sensor cam1 of type [NSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEE] was not found...
the sensor cam1 of type [St6vectorIdSaIdEE] was not found...
the sensor cam1 of type [St6vectorIdSaIdEE] was not found...
the sensor cam1 of type [St6vectorIiSaIiEE] was not found...
the sensor cam1 of type [N5Eigen6MatrixIdLi4ELi4ELi0ELi4ELi4EEE] was not found...
overriding node record_timing_information with value from ROS!
overriding node record_timing_filepath with value from ROS!
overriding node max_cameras with value from ROS!
the sensor cam1 of type [NSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEE] was not found...
the sensor cam1 of type [St6vectorIdSaIdEE] was not found...
the sensor cam1 of type [St6vectorIdSaIdEE] was not found...
the sensor cam1 of type [St6vectorIiSaIiEE] was not found...
the sensor cam1 of type [N5Eigen6MatrixIdLi4ELi4ELi0ELi4ELi4EEE] was not found...
overriding node use_stereo with value from ROS!
the node track_frequency of type [d] was not found...
the sensor cam1 of type [NSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEE] was not found...
unable to parse all parameters, please fix
terminate called after throwing an instance of 'boost::wrapexcept<boost::lock_error>'
  what():  boost: mutex lock failed in pthread_mutex_lock: Invalid argument
================================================================================REQUIRED process [run_subscribe_msckf-1] has died!
process has died [pid 303797, exit code -6, cmd /home/rohit/workspace/catkin_ws_ov/devel/lib/ov_msckf/run_subscribe_msckf __name:=run_subscribe_msckf __log:=/home/rohit/.ros/log/912210c8-eae0-11ec-bc13-c7b3897d7335/run_subscribe_msckf-1.log].
log file: /home/rohit/.ros/log/912210c8-eae0-11ec-bc13-c7b3897d7335/run_subscribe_msckf-1*.log
Initiating shutdown!
================================================================================
[run_subscribe_msckf-1] killing on exit
shutting down processing monitor...
... shutting down processing monitor complete
done

Any guidance on how to get OpenVINS running on custom data would be really helpful

So I compared the estimator config files euroc dataset and my scampi dataset. And I added the dynamic initialization parameters in scampi config file.

init_window_time: 2.0 # how many seconds to collect initialization information init_imu_thresh: 1.5 # threshold for variance of the accelerometer to detect a "jerk" in motion init_max_disparity: 1.5 # max disparity to consider the platform stationary (dependent on resolution) init_max_features: 25 # how many features to track during initialization (saves on computation)

init_dyn_mle_opt_calib: false
init_dyn_mle_max_iter: 50
init_dyn_mle_max_time: 0.05
init_dyn_mle_max_threads: 6
init_dyn_num_pose: 6
init_dyn_min_deg: 10.0

init_dyn_inflation_ori: 10
init_dyn_inflation_vel: 100
init_dyn_inflation_bg: 10
init_dyn_inflation_ba: 100
init_dyn_min_rec_cond: 1e-12

init_dyn_bias_g: [0.0, 0.0, 0.0]
init_dyn_bias_a: [0.0, 0.0, 0.0]

However the following error is still there.

ROS_MASTER_URI=http://localhost:11311

process[run_subscribe_msckf-1]: started with pid [305739]
overriding node verbosity with value from ROS!
Setting printing level to: INFO
overriding node max_cameras with value from ROS!
overriding node max_cameras with value from ROS!
overriding node use_stereo with value from ROS!
the sensor cam1 of type [NSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEE] was not found...
the sensor cam1 of type [St6vectorIdSaIdEE] was not found...
the sensor cam1 of type [St6vectorIdSaIdEE] was not found...
the sensor cam1 of type [St6vectorIiSaIiEE] was not found...
the sensor cam1 of type [N5Eigen6MatrixIdLi4ELi4ELi0ELi4ELi4EEE] was not found...
overriding node record_timing_information with value from ROS!
overriding node record_timing_filepath with value from ROS!
overriding node max_cameras with value from ROS!
the sensor cam1 of type [NSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEE] was not found...
the sensor cam1 of type [St6vectorIdSaIdEE] was not found...
the sensor cam1 of type [St6vectorIdSaIdEE] was not found...
the sensor cam1 of type [St6vectorIiSaIiEE] was not found...
the sensor cam1 of type [N5Eigen6MatrixIdLi4ELi4ELi0ELi4ELi4EEE] was not found...
overriding node use_stereo with value from ROS!
the node track_frequency of type [d] was not found...
the sensor cam1 of type [NSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEE] was not found...
unable to parse all parameters, please fix
terminate called after throwing an instance of 'boost::wrapexcept<boost::lock_error>'
  what():  boost: mutex lock failed in pthread_mutex_lock: Invalid argument
================================================================================REQUIRED process [run_subscribe_msckf-1] has died!
process has died [pid 305739, exit code -6, cmd /home/rohit/workspace/catkin_ws_ov/devel/lib/ov_msckf/run_subscribe_msckf __name:=run_subscribe_msckf __log:=/home/rohit/.ros/log/912210c8-eae0-11ec-bc13-c7b3897d7335/run_subscribe_msckf-1.log].
log file: /home/rohit/.ros/log/912210c8-eae0-11ec-bc13-c7b3897d7335/run_subscribe_msckf-1*.log
Initiating shutdown!
================================================================================
[run_subscribe_msckf-1] killing on exit
shutting down processing monitor...
... shutting down processing monitor complete
done

So finally I got it working by adding the argument of max_cameras = 1 And also the track_frequency in the estimator config.

However the dynamic initialization is not working properly and the systems is drifting a lot. Maybe due to lack of sufficient excitation. Will try to figure out the problem.

Meanwhile if someone have any advice, please do let me know.

rpng / open_vins

Problem running the dynamic initialization #254