search

rpng / open_vins

An open source platform for visual-inertial navigation research.
https://docs.openvins.com
GNU General Public License v3.0
2.17k stars 639 forks source link

How to improve extrinsics? Estimated distances are growing, path is not alighed with ground #118

Closed DrWillway closed 3 years ago

DrWillway commented 3 years ago

Hello, I am trying to implement monocular VO with one IMU on ground track robot.

My setup:

Procedures for calibration:

- intrisinc camera: using standart ROS method [http://wiki.ros.org/camera_calibration/Tutorials/MonocularCalibration], while robot is static:

camera_matrix: rows: 3 cols: 3 data: [734.92524, 0. , 670.30661,

  1. , 733.89722, 349.17868,
  2. , 0. , 1. ] distortion_coefficients: rows: 1 cols: 5 data: [-0.007451, 0.000301, 0.002862, -0.000703, 0.000000]

- intrinsici IMU---collected 10h bag file while IMU still, used _kallibrallan and _imuutils packages to compare.

Kalibr_allan values: accelerometer_noise_density =0.116191 accelerometer_random_walk =0.008006
gyroscope_noise_density =0.02718901219936097 gyroscope_random_walk =0.0008663802731459378

imu_utils: type: IMU name: imu_um7 Gyr: unit: " rad/s" avg-axis: gyr_n: 1.5842723154232009e-03 gyr_w: 4.3210082109045327e-05 x-axis: gyr_n: 1.6042342543317045e-03 gyr_w: 5.5463441716470483e-05 y-axis: gyr_n: 1.6956731202492615e-03 gyr_w: 5.0808747112070315e-05 z-axis: gyr_n: 1.4529095716886370e-03 gyr_w: 2.3358057498595162e-05 Acc: unit: " m/s^2" avg-axis: acc_n: 1.3923822406555314e-02 acc_w: 7.1005384351349374e-04 x-axis: acc_n: 1.2029718682399323e-02 acc_w: 6.3233636276683377e-04 y-axis: acc_n: 1.3255809083200933e-02 acc_w: 7.3200228961688098e-04 z-axis: acc_n: 1.6485939454065689e-02 acc_w: 7.6582287815676637e-04

- Extrisic, kalibr auto:


Calibration results
===================
Normalized Residuals
----------------------------
Reprojection error (cam0):     mean 0.319525483609, median 0.232016501052, std: 0.285724778085
Gyroscope error (imu0):        mean 0.0880520011802, median 0.0368061470402, std: 0.120546032514
Accelerometer error (imu0):    mean 0.119330972409, median 0.0560529614742, std: 0.162437537797

Residuals
----------------------------
Reprojection error (cam0) [px]:     mean 0.319525483609, median 0.232016501052, std: 0.285724778085
Gyroscope error (imu0) [rad/s]:     mean 0.0235786278264, median 0.00985597636798, std: 0.0322799027678
Accelerometer error (imu0) [m/s^2]: mean 0.13655623561, median 0.0641441300543, std: 0.18588517495

Transformation (cam0):
-----------------------
T_ci:  (imu0 to cam0): 
[[ 0.14043796 -0.98725888  0.07481366  0.00359312]
 [-0.2876932  -0.11299302 -0.95103375  0.00188212]
 [ 0.94736994  0.11203786 -0.29989618 -0.00210852]
 [ 0.          0.          0.          1.        ]]

T_ic:  (cam0 to imu0): 
[[ 0.14043796 -0.2876932   0.94736994  0.00203442]
 [-0.98725888 -0.11299302  0.11203786  0.00399624]
 [ 0.07481366 -0.95103375 -0.29989618  0.00088881]
 [ 0.          0.          0.          1.        ]]

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

Gravity vector in target coords: [m/s^2]
[ 0.40423973 -9.79821316  0.00565041]

Calibration configuration
=========================

cam0
-----
  Camera model: pinhole
  Focal length: [734.92524, 733.89722]
  Principal point: [670.30661, 349.17868]
  Distortion model: radtan
  Distortion coefficients: [-0.007451, 0.000301, 0.002862, -0.000703]
  Type: aprilgrid
  Tags: 
    Rows: 6
    Cols: 6
    Size: 0.088 [m]
    Spacing 0.0264 [m]

IMU configuration
=================

IMU0:
----------------------------
  Model: calibrated
  Update rate: 97.0
  Accelerometer:
    Noise density: 0.116191 
    Noise density (discrete): 1.14434863685 
    Random walk: 8.006e-05
  Gyroscope:
    Noise density: 0.0271890121994
    Noise density (discrete): 0.267780714923 
    Random walk: 0.000866380273146
  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]

- Extrisic, openvins auto calibration parameter, repeat few times to get different values:

T_C0toI: -0.290,-0.004,-0.957,-0.316, 0.957,-0.001,-0.290,0.026, -0.000,-1.000,0.004,0.033, 0.000,0.000,0.000,1.000

T_C0toI: -0.293,-0.004,-0.956,-0.253, 0.956,-0.003,-0.293,-0.005, -0.002,-1.000,0.005,0.063, 0.000,0.000,0.000,1.000

T_C0toI: -0.291,-0.002,-0.957,-0.395, 0.957,-0.000,-0.291,0.023, 0.000,-1.000,0.002,-0.006, 0.000,0.000,0.000,1.000

- Extrisinc manual: [ 0.956, 0, 0.292, 0.313, 0, 1, 0, 0,
-0.292, 0, 0.956, 0.066, 0.0, 0.0, 0.0, 1.0 ]

For extrinsics I tried to use kalibr / manual calibration. As the robot is heavy we couldn't pick up it and move the robot in all axis, so I just teleoperated it (front-backward, left right rotation) to make the bag file with IMU and HD image stream. IMU is shifted from the camera (around 30 cm in length and 6 cm height).

Configuration file:

`<launch>

    <!-- imu starting thresholds -->
    <arg name="init_window_time"  default="0.75" />
    <arg name="init_imu_thresh"   default="0.5" />

    <!-- saving trajectory path and timing information -->
    <arg name="dosave"      default="false" />
    <arg name="dotime"      default="false" />
    <arg name="path_est"    default="/tmp/traj_estimate.txt" />
    <arg name="path_time"   default="/tmp/traj_timing.txt" />

    <arg name="bag"         default="/home/user/rosbags/square.bag" />
    <arg name="bag_start"   default="0" />

    <!-- MASTER NODE! -->
    <node name="run_subscribe_msckf" pkg="ov_msckf" type="run_subscribe_msckf" output="screen" clear_params="true" required="true">

        <!-- bag topics -->
       <param name="topic_imu"      type="string" value="/imu_um7/data" />
        <param name="topic_camera0"  type="string" value="/usb_cam/image_raw" />

        <!-- world/filter parameters -->
        <param name="use_fej"                type="bool"   value="true" />
        <param name="use_imuavg"             type="bool"   value="true" />
        <param name="use_rk4int"             type="bool"   value="true" />
        <param name="use_stereo"             type="bool"   value="false" />

        <param name="calib_cam_extrinsics"   type="bool"   value="true" />
        <param name="calib_cam_intrinsics"   type="bool"   value="false" />
        <param name="calib_cam_timeoffset"   type="bool"   value="false" />
        <param name="calib_camimu_dt"        type="double" value="-0.34870176141157117" />

        <param name="max_clones"             type="int"    value="25" />
        <param name="max_slam"               type="int"    value="50" />
        <param name="max_slam_in_update"     type="int"    value="25" /> <!-- 25 seems to work well -->
        <param name="max_msckf_in_update"    type="int"    value="999" />
        <param name="max_cameras"            type="int"    value="1" />
        <param name="dt_slam_delay"          type="double" value="3" />
        <param name="init_window_time"       type="double" value="$(arg init_window_time)" />
        <param name="init_imu_thresh"        type="double" value="$(arg init_imu_thresh)" />
        <rosparam param="gravity">[0.0,0.0,0]</rosparam>
        <param name="feat_rep_msckf"         type="string" value="ANCHORED_MSCKF_INVERSE_DEPTH" />
        <param name="feat_rep_slam"          type="string" value="ANCHORED_MSCKF_INVERSE_DEPTH" />
        <param name="feat_rep_aruco"         type="string" value="ANCHORED_MSCKF_INVERSE_DEPTH" />

        <!-- zero velocity update parameters -->
        <param name="try_zupt"               type="bool"   value="false" />
        <param name="zupt_chi2_multipler"    type="int"    value="1" />
        <param name="zupt_max_velocity"      type="double" value="0.25" />
        <param name="zupt_noise_multiplier"  type="double" value="10" />

        <!-- timing statistics recording -->
        <param name="record_timing_information"   type="bool"   value="$(arg dotime)" />
        <param name="record_timing_filepath"      type="string" value="$(arg path_time)" />

        <!-- tracker/extractor properties -->
        <param name="use_klt"            type="bool"   value="true" />
        <param name="num_pts"            type="int"    value="500" />
        <param name="fast_threshold"     type="int"    value="15" />
        <param name="grid_x"             type="int"    value="5" />
        <param name="grid_y"             type="int"    value="5" />
        <param name="min_px_dist"        type="int"    value="1" />
        <param name="knn_ratio"          type="double" value="0.70" />
        <param name="downsample_cameras" type="bool"   value="false" />

        <param name="fi_max_dist"        type="double" value="75" />
        <param name="fi_max_baseline"    type="double" value="500" />
        <param name="fi_max_cond_number" type="double" value="5000" />

        <!-- aruco tag/mapping properties -->
        <param name="use_aruco"        type="bool"   value="false" />
        <param name="num_aruco"        type="int"    value="1024" />
        <param name="downsize_aruco"   type="bool"   value="true" />

        <!-- sensor noise values / update -->
        <param name="up_msckf_sigma_px"            type="double"   value="1" />
        <param name="up_msckf_chi2_multipler"      type="double"   value="1" />
        <param name="up_slam_sigma_px"             type="double"   value="1" />
        <param name="up_slam_chi2_multipler"       type="double"   value="1" />
        <param name="up_aruco_sigma_px"            type="double"   value="1" />
        <param name="up_aruco_chi2_multipler"      type="double"   value="1" />

        <param name="accelerometer_noise_density"  type="double"   value="0.116191" />  
        <param name="accelerometer_random_walk"    type="double"   value="0.008006" />  
        <param name="gyroscope_noise_density"      type="double"   value="0.02718901219936097" /> 
        <param name="gyroscope_random_walk"        type="double"   value="0.0008663802731459378" />  

        <!-- camera intrinsics -->
        <rosparam param="cam0_wh">[1280, 720]</rosparam>
        <param name="cam0_is_fisheye" type="bool" value="false" />
        <rosparam param="cam0_k">[734.92524, 733.89722, 670.30661, 349.17868]</rosparam>
        <rosparam param="cam0_d">[-0.007451, 0.000301, 0.002862, -0.000703]</rosparam>

        <!-- Manual camera extrinsics 
        <rosparam param="T_C0toI">
        [
        0.956, 0, 0.292, 0.313,
        0, 1, 0, 0,  
        -0.292, 0, 0.956, 0.066,
        0.0, 0.0, 0.0, 1.0
        ]
        </rosparam>-->

        <!-- Manual-auto improved camera extrinsics-->
        <rosparam param="T_C0toI">
        [
        -0.291,-0.002,-0.957,-0.395,
        0.957,-0.000,-0.291,0.066,
        0.000,-1.000,0.002,-0.006,
        0.0, 0.0, 0.0, 1.0
        ]
        </rosparam> 

        <!-- Auto camera extrinsics
        <rosparam param="T_C0toI">
        [
        -0.293,-0.004,-0.956,-0.313,
        0.956,-0.00,-0.293,0.005,
        0.0,-1.000,0.004,0.066,
        0.0, 0.0, 0.0, 1.0
        ]
        </rosparam>-->

    </node>

    <!-- play the dataset -->
    <node pkg="rosbag" type="play" name="rosbag" args="-d 1 -s $(arg bag_start) $(arg bag)" required="true"/>

    <!-- record the trajectory if enabled -->
    <group if="$(arg dosave)">
        <node name="recorder_estimate" pkg="ov_eval" type="pose_to_file" output="screen" required="true">
            <param name="topic"      type="str" value="/ov_msckf/poseimu" />
            <param name="topic_type" type="str" value="PoseWithCovarianceStamped" />
            <param name="output"     type="str" value="$(arg path_est)" />
        </node>
    </group>

</launch>`

Issue:

Estimated path is not alighed with ground, distances are growing and getting smaller over time.

Am I missing something? Can you suggest something for improvements?

image

goldbattle commented 3 years ago

It is very challenging to perform calibration if you are unable to excite all axes of the system. If you are going to deploy the system on a planar robot, I recommend you first calibrate it offline and then do not estimate the extrinsic between the camera and IMU online. From what you have said, it seems that you need to detach the sensors from the robotic platform, calibrate them, and re-attach them as one unit. Also the kalibr_allan project noises seem to be way too large, so I recommend you try the default ETH ones, and then also the other IMU utility you have posted. If your IMU is that noisy, I am not sure there is much one can do (but I suspect it isn't that noise just bad calibration calculation).