envio with D435i

[EhrazImam]

i am trying to run envio with d435i i am confused with the launch file:

<node pkg="ensemble_vio" type="envio_node" name="envio_node">

    <rosparam command="load" file="$(arg calibration_file)" />

    <!-- Remapping : put your topics -->
    <remap from="/imu" to="/camera/imu"/>
    <remap from="/left_image" to="/camera/infra1/image_rect_raw"/>
    <remap from="/right_image" to="/camera/infra2/image_rect_raw"/>

    <!-- Vision processing parameters -->
    <param name="nx" value="25" type="int"/>
    <param name="ny" value="15" type="int"/>
    <param name="min_depth" value="0.3" type="double"/>
    <param name="max_depth" value="5" type="double"/>
    <param name="min_parallax" value="1" type="double"/>
    <param name="ransac_thr" value="1" type="double"/>

    <!-- Initial std_dev [rad, m, m/s, m/s^2, rad/s] -->
    <param name="P0/attitude" value="0.0175" type="double"/>
    <param name="P0/position" value="3e-2" type="double"/>
    <param name="P0/velocity" value="1e-1" type="double"/>
    <param name="P0/ba" value="0.1962" type="double"/>
    <param name="P0/bg" value="1.0e-3" type="double"/>
    <param name="P0/depth" value="1.5" type="double"/>
    <param name="P0/idepth" value="0.1" type="double"/>
    <param name="num_init_samples" value="500" type="int"/>

    <!-- Process noises [rad/s^(1/2), m/s^(3/2), m/s^(5/2), rad/s^(3/2)]-->
    <param name="Q/velocity" value="2e-3" type="double"/>
    <param name="Q/attitude" value="1e-4" type="double"/>
    <param name="Q/ba" value="2.5e-4" type="double"/>
    <param name="Q/bg" value="1e-7" type="double"/>

    <!-- Estimator parameters -->
    <param name="inverse_depth" value="false" type="bool"/>
    <param name="R_std" value="20" type="double"/>
    <!-- <param name="R_std" value="35" type="double"/> -->
    <param name="max_lifetime" value="300" type="int"/>
    <param name="thr_stop" value="1e-3" type="double"/>
    <param name="max_diff" value="30" type="double"/>
    <param name="N_en" value="100" type="int"/>
    <param name="use_stochastic_gradient" value="true" type="bool"/>
    <param name="thr_weak" value="3" type="double"/>

    <!-- Sparse setting -->
    <param name="thr_num" value="150" type="int"/>
    <param name="uniform_dist" value="30" type="int"/>
    <param name="max_iter" value="10" type="int"/>
<param name="max_itime" value="0.01" type="double"/>

    <!-- Vehicle to IMU z-y-x euler sequence -->
    <param name="roll_imu_vehicle" value="0.0" type="double"/>
    <param name="pitch_imu_vehicle" value="0.0" type="double"/>
    <param name="yaw_imu_vehicle" value="0.0" type="double"/>

</node>

i tried the same calibration file with vins fusion and its working great what thing i need to change in the launch file as i can see the changes will be in :

    <param name="P0/attitude" value="0.0175" type="double"/>
    <param name="P0/position" value="3e-2" type="double"/>
    <param name="P0/velocity" value="1e-1" type="double"/>
    <param name="P0/ba" value="0.1962" type="double"/>
    <param name="P0/bg" value="1.0e-3" type="double"/>
    <param name="P0/depth" value="1.5" type="double"/>
    <param name="P0/idepth" value="0.1" type="double"/>
    <param name="num_init_samples" value="500" type="int"/>

    <!-- Process noises [rad/s^(1/2), m/s^(3/2), m/s^(5/2), rad/s^(3/2)]-->
    <param name="Q/velocity" value="2e-3" type="double"/>
    <param name="Q/attitude" value="1e-4" type="double"/>
    <param name="Q/ba" value="2.5e-4" type="double"/>
    <param name="Q/bg" value="1e-7" type="double"/>

Please help me with this.
Thankyou

[EhrazImam]

The calibration file

Calibration results

Normalized Residuals

Reprojection error (cam0): mean 0.119551441835, median 0.102725395276, std: 0.0807732419199
Reprojection error (cam1): mean 0.113196468523, median 0.0967910664409, std: 0.0764945472412
Gyroscope error (imu0): mean 0.134262449583, median 0.112032458367, std: 0.0983242023043
Accelerometer error (imu0): mean 0.165840889954, median 0.136178941242, std: 0.125839935508

Residuals

Reprojection error (cam0) [px]: mean 0.119551441835, median 0.102725395276, std: 0.0807732419199
Reprojection error (cam1) [px]: mean 0.113196468523, median 0.0967910664409, std: 0.0764945472412
Gyroscope error (imu0) [rad/s]: mean 0.0032251385054, median 0.00269114854119, std: 0.00236186045948
Accelerometer error (imu0) [m/s^2]: mean 0.0355324493651, median 0.0291771910752, std: 0.0269619943417

Transformation (cam0):

T_ci: (imu0 to cam0):
[[ 0.99994202 -0.00774823 -0.00747854 0.00428857]
[ 0.00780169 0.99994403 0.00714635 -0.00504498]
[ 0.00742275 -0.00720428 0.9999465 -0.0116195 ]
[ 0. 0. 0. 1. ]]

T_ic: (cam0 to imu0):
[[ 0.99994202 0.00780169 0.00742275 -0.00416271]
[-0.00774823 0.99994403 -0.00720428 0.00499421]
[-0.00747854 0.00714635 0.9999465 0.01168701]
[ 0. 0. 0. 1. ]]

timeshift cam0 to imu0: [s] (t_imu = t_cam + shift)
0.00183032750045

Transformation (cam1):

T_ci: (imu0 to cam1):
[[ 0.99996399 -0.00739963 -0.00415622 -0.04582789]
[ 0.00743795 0.99992925 0.00928254 -0.00504215]
[ 0.00408724 -0.00931312 0.99994828 -0.01189137]
[ 0. 0. 0. 1. ]]

T_ic: (cam1 to imu0):
[[ 0.99996399 0.00743795 0.00408724 0.04591234]
[-0.00739963 0.99992925 -0.00931312 0.00459194]
[-0.00415622 0.00928254 0.99994828 0.01174708]
[ 0. 0. 0. 1. ]]

timeshift cam1 to imu0: [s] (t_imu = t_cam + shift)
0.00182193844524

Baselines:

Baseline (cam0 to cam1):
[[ 0.99999442 0.0003725 0.00331979 -0.05007598]
[-0.00037958 0.99999765 0.00213349 0.00002923]
[-0.00331899 -0.00213473 0.99999221 -0.00026849]
[ 0. 0. 0. 1. ]]
baseline norm: 0.0500767084192 [m]

Gravity vector in target coords: [m/s^2]
[-0.16964518 -9.80482433 -0.07115613]

Calibration configuration

cam0

Camera model: pinhole
Focal length: [390.855446076306, 391.24083140595525]
Principal point: [319.79565127407614, 236.0557699517314]
Distortion model: radtan
Distortion coefficients: [0.010439254716528169, -0.004799144673588816, -0.0035596590092202935, 0.0005321285962329878]
Type: aprilgrid
Tags:
Rows: 6
Cols: 6
Size: 0.022 [m]
Spacing 0.0066 [m]

cam1

Camera model: pinhole
Focal length: [390.05785968310096, 390.5114409433188]
Principal point: [318.1438071857364, 235.7891127183977]
Distortion model: radtan
Distortion coefficients: [0.012469757654913356, -0.007329293496689123, -0.0035751034229518502, -0.0007262393298711362]
Type: aprilgrid
Tags:
Rows: 6
Cols: 6
Size: 0.022 [m]
Spacing 0.0066 [m]

IMU configuration

IMU0:

Model: calibrated
Update rate: 200.0
Accelerometer:
Noise density: 0.0151502056611
Noise density (discrete): 0.214256263187
Random walk: 0.000446925341059
Gyroscope:
Noise density: 0.00169855183971
Noise density (discrete): 0.0240211504811
Random walk: 2.15139862881e-05
T_i_b
[[ 1. 0. 0. 0.]
[ 0. 1. 0. 0.]
[ 0. 0. 1. 0.]
[ 0. 0. 0. 1.]]
time offset with respect to IMU0: 0.0 [s]

[lastflowers]

Hi. Thanks for having an interest in our work.

What kind of error do you get with the current launch file?
One thing I noticed is that the calibration file with .yaml is not loaded in the launch file.
Also, please note that in the calibration file, the extrinsic parameter between cameras and an IMU should be set as T^{C}_{IMU}.

[EhrazImam]

Sorry i didn't copy the entire code but i have loaded the calibration, what i am facing is drifting in z negative thats because of of launch file parameter can you help me with understanding with launch file as i have already uploaded calibration file and yes i am using T^{C}_{IMU}.

[lastflowers]

Hi, no problem.

The "P0/*" in the launch file is the initial standard deviation in the iterated EKF. For instance, "P0/attitude" is for the initial attitude that is set as 0.0175 rad. And "num_init_samples" is the number of IMU readings that are accumulated to initialize the filter. If you use 100Hz of an IMU, then it gathers IMU readings for 5 seconds and then initializes the filter. Please note that during the initialization sensor should be static.

The "Q/*" are noise densities in a continuous-time. This corresponds to noise parameters in VINS-Fusion.

[wz8530]

Hi, no problem.

The "P0/*" in the launch file is the initial standard deviation in the iterated EKF. For instance, "P0/attitude" is for the initial attitude that is set as 0.0175 rad. And "num_init_samples" is the number of IMU readings that are accumulated to initialize the filter. If you use 100Hz of an IMU, then it gathers IMU readings for 5 seconds and then initializes the filter. Please note that during the initialization sensor should be static.

The "Q/*" are noise densities in a continuous-time. This corresponds to noise parameters in VINS-Fusion.

Hi. I encountered the same z-axis drift issue(static condition) and also modified the launch file according to your suggestion, but it did not work.
The error is as follows:

[ERROR] [1691403544.588481077]: Ignoring transform for child_frame_id "cam0" from authority "unknown_publisher" because of a nan value in the transform (-nan -nan -nan) (-nan -nan -nan -nan)
[ERROR] [1691403544.588572420]: Ignoring transform for child_frame_id "cam0" from authority "unknown_publisher" because of an invalid quaternion in the transform (-nan -nan -nan -nan)
[ERROR] [1691403544.621617204]: Ignoring transform for child_frame_id "cam0" from authority "unknown_publisher" because of a nan value in the transform (-nan -nan -nan) (-nan -nan -nan -nan)
[ERROR] [1691403544.621724371]: Ignoring transform for child_frame_id "cam0" from authority "unknown_publisher" because of an invalid quaternion in the transform (-nan -nan -nan -nan)