ROS2 Initialzation error

[VivekMangeUD]

Hello

I am facing initialization error when testing Openvins on my Laptop Ubuntu 22. Even after moving in random direction and Jerk motions, Its not able to initialize openvins. I have attached a recording for the same. I have also shared the launch file to run the node.
I tried changing the below 3 varaibles in the estimator config file but still same results

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: 10.0

I am also facing issue visualizing the messages on terminal even with verbosity:= DEBUG

ros2 launch ov_msckf ros2_aqua2_subscribe.launch verbosity:=DEBUG
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [run_subscribe_msckf-1]: process started with pid [176172]

Launch File

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, LogInfo, OpaqueFunction
from launch.conditions import IfCondition
from launch.substitutions import LaunchConfiguration, TextSubstitution
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory, get_package_prefix
import os
import sys

launch_args = [
DeclareLaunchArgument(name='namespace', default_value='', description='namespace'),
DeclareLaunchArgument(name='config', default_value='aqua2_sync', description='euroc_mav, tum_vi, rpng_aruco...'),
DeclareLaunchArgument(name='verbosity', default_value='DEBUG', description='ALL, DEBUG, INFO, WARNING, ERROR, SILENT'),
DeclareLaunchArgument(name='use_stereo', default_value='true', description=''),
DeclareLaunchArgument(name='max_cameras', default_value='2', description='')
]

def launch_setup(context):
configs_dir=os.path.join(get_package_share_directory('ov_msckf'),'config')
available_configs = os.listdir(configs_dir)
config = LaunchConfiguration('config').perform(context)
if not config in available_configs:
return[LogInfo(msg='ERROR: unknown config: '{}' - Available configs are: {} - not starting OpenVINS'.format(config,', '.join(available_configs)))]
config_path = os.path.join(get_package_share_directory('ov_msckf'),'config',config,'estimator_config.yaml')
node1 = Node(package = 'ov_msckf',
executable = 'run_subscribe_msckf',
namespace = LaunchConfiguration('namespace'),
parameters =[{'verbosity': LaunchConfiguration('verbosity')},
{'use_stereo': LaunchConfiguration('use_stereo')},
{'max_cameras': LaunchConfiguration('max_cameras')},
{'config_path': config_path}])
return [node1]

def generate_launch_description():
opfunc = OpaqueFunction(function = launch_setup)
ld = LaunchDescription(launch_args)
ld.add_action(opfunc)
return ld

estimator_config

%YAML:1.0 # need to specify the file type at the top!

verbosity: "DEBUG" # ALL, DEBUG, INFO, WARNING, ERROR, SILENT

use_fej: true # if first-estimate Jacobians should be used (enable for good consistency)
integration: "rk4" # discrete, rk4, analytical (if rk4 or analytical used then analytical covariance propagation is used)
use_stereo: true # if we have more than 1 camera, if we should try to track stereo constraints between pairs
max_cameras: 2 # how many cameras we have 1 = mono, 2 = stereo, >2 = binocular (all mono tracking)

calib_cam_extrinsics: true # if the transform between camera and IMU should be optimized R_ItoC, p_CinI
calib_cam_intrinsics: true # if camera intrinsics should be optimized (focal, center, distortion)
calib_cam_timeoffset: true # if timeoffset between camera and IMU should be optimized
calib_imu_intrinsics: false # if imu intrinsics should be calibrated (rotation and skew-scale matrix)
calib_imu_g_sensitivity: false # if gyroscope gravity sensitivity (Tg) should be calibrated

max_clones: 11 # how many clones in the sliding window
max_slam: 50 # number of features in our state vector
max_slam_in_update: 25 # update can be split into sequential updates of batches, how many in a batch
max_msckf_in_update: 40 # how many MSCKF features to use in the update
dt_slam_delay: 1 # delay before initializing (helps with stability from bad initialization...)

gravity_mag: 9.81 # magnitude of gravity in this location

feat_rep_msckf: "GLOBAL_3D"
feat_rep_slam: "ANCHORED_MSCKF_INVERSE_DEPTH"
feat_rep_aruco: "ANCHORED_MSCKF_INVERSE_DEPTH"

try_zupt: false
zupt_chi2_multipler: 0 # set to 0 for only disp-based
zupt_max_velocity: 0.1
zupt_noise_multiplier: 10
zupt_max_disparity: 0.5 # set to 0 for only imu-based
zupt_only_at_beginning: false

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: 10.0 # max disparity to consider the platform stationary (dependent on resolution)
init_max_features: 50 # how many features to track during initialization (saves on computation)

init_dyn_use: false # if dynamic initialization should be used
init_dyn_mle_opt_calib: false # if we should optimize calibration during intialization (not recommended)
init_dyn_mle_max_iter: 50 # how many iterations the MLE refinement should use (zero to skip the MLE)
init_dyn_mle_max_time: 0.05 # how many seconds the MLE should be completed in
init_dyn_mle_max_threads: 6 # how many threads the MLE should use
init_dyn_num_pose: 6 # number of poses to use within our window time (evenly spaced)
init_dyn_min_deg: 10.0 # orientation change needed to try to init

init_dyn_inflation_ori: 10 # what to inflate the recovered q_GtoI covariance by
init_dyn_inflation_vel: 100 # what to inflate the recovered v_IinG covariance by
init_dyn_inflation_bg: 10 # what to inflate the recovered bias_g covariance by
init_dyn_inflation_ba: 100 # what to inflate the recovered bias_a covariance by
init_dyn_min_rec_cond: 1e-12 # reciprocal condition number thresh for info inversion

init_dyn_bias_g: [ 0.0, 0.0, 0.0 ] # initial gyroscope bias guess
init_dyn_bias_a: [ 0.0, 0.0, 0.0 ] # initial accelerometer bias guess

record_timing_information: false # if we want to record timing information of the method
record_timing_filepath: "/tmp/traj_timing.txt" # https://docs.openvins.com/eval-timing.html#eval-ov-timing-flame

save_total_state: false
filepath_est: "/tmp/ov_estimate.txt"
filepath_std: "/tmp/ov_estimate_std.txt"
filepath_gt: "/tmp/ov_groundtruth.txt"

use_klt: true # if true we will use KLT, otherwise use a ORB descriptor + robust matching
num_pts: 200 # number of points (per camera) we will extract and try to track
fast_threshold: 20 # threshold for fast extraction (warning: lower threshs can be expensive)
grid_x: 5 # extraction sub-grid count for horizontal direction (uniform tracking)
grid_y: 5 # extraction sub-grid count for vertical direction (uniform tracking)
min_px_dist: 10 # distance between features (features near each other provide less information)
knn_ratio: 0.70 # descriptor knn threshold for the top two descriptor matches
track_frequency: 21.0 # frequency we will perform feature tracking at (in frames per second / hertz)
downsample_cameras: false # will downsample image in half if true
num_opencv_threads: 4 # -1: auto, 0-1: serial, >1: number of threads
histogram_method: "HISTOGRAM" # NONE, HISTOGRAM, CLAHE

use_aruco: false
num_aruco: 1024
downsize_aruco: true

up_msckf_sigma_px: 1
up_msckf_chi2_multipler: 1
up_slam_sigma_px: 1
up_slam_chi2_multipler: 1
up_aruco_sigma_px: 1
up_aruco_chi2_multipler: 1

use_mask: false

relative_config_imu: "kalibr_imu_chain.yaml"
relative_config_imucam: "kalibr_imucam_chain.yaml"

kalibr_imu_chain

%YAML:1.0

imu0:
T_i_b:
- [1.0, 0.0, 0.0, 0.0]
- [0.0, 1.0, 0.0, 0.0]
- [0.0, 0.0, 1.0, 0.0]
- [0.0, 0.0, 0.0, 1.0]
accelerometer_noise_density: 0.01478092754466938365
accelerometer_random_walk: 50.512610722075978e-05
gyroscope_noise_density: 0.000419407725308617202
gyroscope_random_walk: 10.121726169931657e-06
rostopic: /a13/imu/filtered_data
time_offset: 0.0
update_rate: 50.0
model: "kalibr"
Tw:
- [ 1.0, 0.0, 0.0 ]
- [ 0.0, 1.0, 0.0 ]
- [ 0.0, 0.0, 1.0 ]
R_IMUtoGYRO:
- [ 1.0, 0.0, 0.0 ]
- [ 0.0, 1.0, 0.0 ]
- [ 0.0, 0.0, 1.0 ]
Ta:
- [ 1.0, 0.0, 0.0 ]
- [ 0.0, 1.0, 0.0 ]
- [ 0.0, 0.0, 1.0 ]
R_IMUtoACC:
- [ 1.0, 0.0, 0.0 ]
- [ 0.0, 1.0, 0.0 ]
- [ 0.0, 0.0, 1.0 ]
Tg:
- [ 0.0, 0.0, 0.0 ]
- [ 0.0, 0.0, 0.0 ]
- [ 0.0, 0.0, 0.0 ]

kalibr_imucam_chain

%YAML:1.0

cam0:
T_imu_cam: #rotation from camera to IMU R_CtoI, position of camera in IMU p_CinI
- [0.0027350570249282946, 0.9995034468024393, 0.031390751713102405, -0.00022451075330166953]
- [0.0044611024162799495, -0.03140275216014363, 0.9994968562842006, 0.001158439172347254]
- [0.999986308920428, -0.002593643539857693, -0.004544775559508135, 0.000979827418915682]
- [0.0, 0.0, 0.0, 1.0]
cam_overlaps: [1]
camera_model: pinhole
distortion_coeffs: [-0.32324224995099765, 0.09462260054848305, -0.00044580554405654967, -0.00047623276778878957]
distortion_model: radtan
intrinsics: [436.1545517976996, 436.20480087194466, 322.6650175012526, 233.28224614306288]
resolution: [640, 480]
rostopic: /a13/camera/right/image_raw
timeshift_cam_imu: 1.4990728190294829
cam1:
T_cam_imu:
- [0.001469171404057723, 0.9996135264065523, 0.02776039193603369, 0.09049064226626044]
- [-0.0016035446369905282, -0.02775803122841583, 0.9996133854380499, 0.0036551297174852015]
- [0.9999976350871949, -0.001513118428608684, 0.0015621436036186986, 0.004876166919817045]
- [0.0, 0.0, 0.0, 1.0]
T_cn_cnm1:
- [0.9999926029519827, -0.0036376372322886844, -0.001249654626396775, 0.09072058978617707]
- [0.0036299819161040177, 0.9999749612432031, -0.00607454672745451, 0.002503486528430149]
- [0.00127172033394357, 0.006069965570045485, 0.9999807689377688, 0.0038896121730224546]
- [0.0, 0.0, 0.0, 1.0]
cam_overlaps: [0]
camera_model: pinhole
distortion_coeffs: [-0.3268054095693605, 0.09667096634166004, 0.0004165788885022756, -0.001039686437781763]
distortion_model: radtan
intrinsics: [437.7412328519853, 437.4565377431363, 318.0961775981548, 246.5198957501857]
resolution: [640, 480]
rostopic: /a13/camera/left/image_raw
timeshift_cam_imu: 1.5000136263709618

Screencast.from.05-01-2024.08.16.42.PM.webm

[goldbattle]

You are correct that theses config variables should be tuned. I would first get your console logging working.
Does adding this to your node show anything?

    output='screen',
    emulate_tty=True,

https://answers.ros.org/question/332829/no-stdout-logging-output-in-ros2-using-launch/

[VivekMangeUD]

The snippet didn't work well but I used the snippet from the ROS answers and it worked to get the information on the terminal.

**output='screen',
emulate_tty=True,**

I have shared the window output and also the recordings of the test.
Recording link : https://drive.google.com/file/d/1m4vtrllglC6UgjXBH550nz2rtp1pWb2D/view?usp=sharing

Update Launch file

def launch_setup(context):
configs_dir=os.path.join(get_package_share_directory('ov_msckf'),'config')
available_configs = os.listdir(configs_dir)
config = LaunchConfiguration('config').perform(context)
if not config in available_configs:
return[LogInfo(msg='ERROR: unknown config: '{}' - Available configs are: {} - not starting OpenVINS'.format(config,', '.join(available_configs)))]
config_path = os.path.join(get_package_share_directory('ov_msckf'),'config',config,'estimator_config.yaml')
node1 = Node(package = 'ov_msckf',
executable = 'run_subscribe_msckf',
namespace = LaunchConfiguration('namespace'),
output={
'stdout': 'screen',
'stderr': 'screen'},
parameters =[{'verbosity': LaunchConfiguration('verbosity')},
{'use_stereo': LaunchConfiguration('use_stereo')},
{'max_cameras': LaunchConfiguration('max_cameras')},
{'config_path': config_path}])
return [node1]

ros_openvins_terminal_output_log.txt

Also, Can you help me with some information about the initialization process to better understand how it works and what different motion should I perform to trigger it or what params should I tune to get it triggered.