Ensemble Visual-Inertial Odometry (EnVIO)

Jae Hyung Jung

Last update: Jan 3, 2023

Related tags

Deep Learning envio

Overview

Ensemble Visual-Inertial Odometry (EnVIO)

Authors : Jae Hyung Jung, Yeongkwon Choe, and Chan Gook Park

1. Overview

This is a ROS package of Ensemble Visual-Inertial Odometry (EnVIO) written in C++. It features a photometric (direct) measurement model and stochastic linearization that are implemented by iterated extended Kalman filter fully built on the matrix Lie group. EnVIO takes time-synced stereo images and IMU readings as input and outputs the current vehicle pose and feature depths at the current camera frame with their estimated uncertainties.

2. Build

This package was tested on Ubuntu 16.04 (ROS Kinetic) with Eigen 3.3.7 for matrix computation and OpenCV 3.3.1 for image processing in C++11.
There are no additional dependencies, we hope this package can be built without any difficulties in different environments.
We use the catkin build system :

cd catkin_ws
catkin_make

3. Run (EuRoC example)

Configuration and launch files are prepared in config/euroc/camchain-imucam-euroc.yaml and launch/nesl_envio_euroc.launch.
The configuration files are output by Kalibr toolbox.
Filter and image processing parameters are set from the launch file.
Please note that our filter implementation requires static state at the beginning to initialize tilt angles, velocity and gyroscope biases. The temporal window for this process can be set by num_init_samples in the launch file.
As default our package outputs est_out.txt that includes estimated states.

roslaunch ensemble_vio nesl_envio_euroc.launch
roslaunch ensemble_vio nesl_envio_rviz.launch
rosbag play rosbag.bag

4. Run your own device

Our implementation assumes that stereo camera is hardware-synced and the spatio-temporal parameters for cameras and IMU are calibrated as it is a critical step in sensor fusion.
You can calibrate your visual-inertial sensor using Kalibr toolbox and place the output file in config.
The input ROS topics and filter parameters are set in launch.
With low cost IMUs as in EuRoC sensor suite, you can use the default parameters of EuRoC example file.

5. Citation

If you feel this work helpful to your academic research, we kindly ask you to cite our paper :

@article{EnVIO_TRO,
  title={Photometric Visual-Inertial Navigation with Uncertainty-Aware Ensembles},
  author={Jung, Jae Hyung and Choe, Yeongkwon and Park, Chan Gook},
  journal={IEEE Transactions on Robotics},
  year={2022},
  publisher={IEEE}
}

6. Acknowledgements

This research was supported in part by Unmanned Vehicle Advanced Research Center funded by the Ministry of Science and ICT, the Republic of Korea and in part by Hyundai NGV Company.

7. License

Our source code is released under GPLv3 license. If there are any issues in our source code please contact to the author ([email protected]).

You might also like...

Neural Ensemble Search for Performant and Calibrated Predictions

Neural Ensemble Search Introduction This repo contains the code accompanying the paper: Neural Ensemble Search for Performant and Calibrated Predictio

26 Dec 12, 2022

Intrusion Detection System using ensemble learning (machine learning)

IDS-ML implementation of an intrusion detection system using ensemble machine learning methods Data set This project is carried out using the UNSW-15

4 Nov 25, 2022

An Ensemble of CNN (Python 3.5.1 Tensorflow 1.3 numpy 1.13)

0 May 6, 2022

zeus is a Python implementation of the Ensemble Slice Sampling method.

zeus is a Python implementation of the Ensemble Slice Sampling method. Fast & Robust Bayesian Inference, Efficient Markov Chain Monte Carlo (MCMC), Bl

197 Dec 4, 2022

Pytorch implementation of SenFormer: Efficient Self-Ensemble Framework for Semantic Segmentation

SenFormer: Efficient Self-Ensemble Framework for Semantic Segmentation Efficient Self-Ensemble Framework for Semantic Segmentation by Walid Bousselham

61 Dec 26, 2022

Ensemble Knowledge Guided Sub-network Search and Fine-tuning for Filter Pruning

Ensemble Knowledge Guided Sub-network Search and Fine-tuning for Filter Pruning This repository is official Tensorflow implementation of paper: Ensemb

12 Oct 18, 2022

improvement of CLIP features over the traditional resnet features on the visual question answering, image captioning, navigation and visual entailment tasks.

CLIP-ViL In our paper "How Much Can CLIP Benefit Vision-and-Language Tasks?", we show the improvement of CLIP features over the traditional resnet fea

310 Dec 28, 2022

Pseudo-Visual Speech Denoising

Pseudo-Visual Speech Denoising This code is for our paper titled: Visual Speech Enhancement Without A Real Visual Stream published at WACV 2021. Autho

94 Oct 22, 2022

Bottleneck Transformers for Visual Recognition

Bottleneck Transformers for Visual Recognition Experiments Model Params (M) Acc (%) ResNet50 baseline (ref) 23.5M 93.62 BoTNet-50 18.8M 95.11% BoTNet-

236 Jan 3, 2023

Comments

Divergence

Hi.

I am trying to run the code on KAIST VIO dataset and no luck yet. I used these following config and launch files

[Config file]


# euroc yaml file

cam0:
  T_cam_imu:
    [0.0403, 0.0263, 0.9988, -0.0364,
    -0.9990, -0.0204, -0.0398, -0.1376,
     0.0194, -0.9994, 0.0271, -0.0188,
     0, 0, 0, 1.0]
  camera_model: pinhole

  distortion_coeffs: [0.006896928127777268, -0.009144207062654397, 0.000254113977103925, 0.0021434982252719545]
  distortion_model: radtan
  intrinsics: [380.9229090195708, 380.29264802262736, 324.68121181846755, 224.6741321466431]
  resolution: [640, 480]
  timeshift_cam_imu: -0.029958533056650416
cam1:
  T_cam_imu:
    [-0.0391, 0.0250, 0.9989, -0.0366,
    -0.9990, -0.0203, -0.0386, -0.1365,
     0.0193, -0.9995, 0.0258, 0.0311,
     0, 0, 0, 1.0]
#  T_cn_cnm1:
#    [0.9999992248836708,   0.00006384241340452582,   0.0012434452955667624,  -0.049960282472300055, 
#    -0.00006225102643531651,   0.9999991790958949,   -0.0012798173093508036,   -0.00005920119010064575,
#    -0.001243525981443161,  0.0012797389115975439,   0.9999984079544582,  -0.000143160033953494487, 
#     0, 0, 0, 1.0]
  camera_model: pinhole
  distortion_coeffs: [0.007044055287844759,  -0.010251485722185347, 0.0006674304399871926, 0.001678899816379666]
  distortion_model: radtan
  intrinsics: [380.95187095303424, 380.3065956074995, 324.0678433553536, 225.9586983198407]
  resolution: [640, 480]
  timeshift_cam_imu: -0.030340187355085417

[launch file]

<launch>
    <!--For the back-end -->
    <arg name="calibration_file" default="$(find ensemble_vio)/config/camchain-imucam-kaistviodataset.yaml"/>

    <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="/imu_data"/>
        <remap from="/left_image" to="/camera/infra1/img"/>
        <remap from="/right_image" to="/camera/infra2/img"/>

        <!-- Vision processing parameters -->
        <param name="nx" value="25" type="int"/>
        <param name="ny" value="15" type="int"/>
        <param name="min_depth" value="0.5" type="double"/>
        <param name="max_depth" value="20" 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="2e-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="600" 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="2.3e-2" type="double"/>
        <param name="Q/attitude" value="1.0e-4" type="double"/>
        <param name="Q/ba" value="2.5e-3" type="double"/>
        <param name="Q/bg" value="7e-5" type="double"/>

        <!-- Estimator parameters -->
        <param name="inverse_depth" value="false" type="bool"/>
        <param name="R_std" value="12" type="double"/>
        <param name="max_lifetime" value="200" type="int"/>
        <param name="thr_stop" value="1e-3" type="double"/>
        <param name="max_diff" value="40" 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="200" type="int"/>
        <param name="uniform_dist" value="15" type="int"/>
        <param name="max_iter" value="10" type="int"/>

    </node>

</launch>

[compressed to raw image file]

#!/usr/bin/env python
# -*- coding: utf-8 -*-

import time
import rospy
import cv2
import numpy as np
import sys
import signal

from sensor_msgs.msg import Image
from sensor_msgs.msg import CompressedImage
from sensor_msgs.msg import Imu
from cv_bridge import CvBridge, CvBridgeError


def signal_handler(signal, frame): # ctrl + c -> exit program
    print('You pressed Ctrl+C!')
    sys.exit(0)
signal.signal(signal.SIGINT, signal_handler)


class converter():
    def __init__(self):
        rospy.init_node('compressed_to_raw', anonymous=True)
        self.comp_sub1 = rospy.Subscriber('/camera/infra1/image_rect_raw/compressed',CompressedImage,self.callback1)
        self.comp_sub2 = rospy.Subscriber('/camera/infra2/image_rect_raw/compressed',CompressedImage,self.callback2)
        self.imu_subscriber = rospy.Subscriber('/mavros/imu/data', Imu, self.imu_callback)
        self.img_pub1 = rospy.Publisher('/camera/infra1/img',Image,queue_size=100)
        self.img_pub2 = rospy.Publisher('/camera/infra2/img',Image,queue_size=100)
        self.imu_publisher = rospy.Publisher('/imu_data', Imu, queue_size=100)
        self.bridge = CvBridge()

    def callback1(self, data):
        try : 
            np_arr = np.fromstring(data.data, np.uint8)
            image_np = cv2.imdecode(np_arr, cv2.IMREAD_COLOR) # OpenCV >= 3.0:
            cv_image=cv2.cvtColor(image_np, cv2.COLOR_BGR2GRAY)
            img=self.bridge.cv2_to_imgmsg(cv_image, "mono8")
            img.header.stamp = data.header.stamp
#            img.header.stamp = rospy.Time.now()
            self.img_pub1.publish(img)
        except CvBridgeError as e:
            pass

    def callback2(self, data):
        try : 
            np_arr = np.fromstring(data.data, np.uint8)
            image_np = cv2.imdecode(np_arr, cv2.IMREAD_COLOR) # OpenCV >= 3.0:
            cv_image=cv2.cvtColor(image_np, cv2.COLOR_BGR2GRAY)
            img=self.bridge.cv2_to_imgmsg(cv_image, "mono8")
            img.header.stamp = data.header.stamp
#            img.header.stamp = rospy.Time.now()
            self.img_pub2.publish(img)
        except CvBridgeError as e:
            pass

#    def callback(self,data):
#        try :
##      self.time = time.time()
#        cvimage=self.bridge.imgmsg_to_cv2(data,"bgr8")

#        cv_image=cv2.cvtColor(cvimage,cv2.COLOR_BGR2GRAY)

#        img=self.bridge.cv2_to_imgmsg(cv_image, "mono8")

#        img.header.stamp = rospy.Time.now()
#        self.img_publisher.publish(img)
#        #print(time.time()-self.time)
#    except CvBridgeError as e:
#        pass

    def imu_callback(self,data):
#        data.header.stamp = rospy.Time.now()
        new_data = data
        new_data.header.stamp = data.header.stamp
        self.imu_publisher.publish(data)

if __name__=='__main__':
    cvt=converter()
    time.sleep(1)
    while 1:
        pass

I am pretty sure the config file (intrinsic and extrinsic) is correct. example video clip with the dataset But with any bag file of the dataset, envio diverges even with static starts.

Can you give me some comments to solve this issue?

Thank you in advance.

opened by engcang 6

envio with D435i

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

opened by EhrazImam 4

rosbag

appreciate your great work！ can you release your rosbag in paper？ and have you test，can your method work in the condition of white wall？ thanks a lot！

opened by dongfangzhou1108 4
Low computation

Res sir will it run on Jetson Nano because I think for a student studying in low income country like India it becomes very expensive to buy ntel nuc platform. Thank you

opened by poudyalbot 2