Code for "Photometric Visual-Inertial Navigation with Uncertainty-Aware Ensembles" in TRO 2022

Related tags

Artificial Intelligence ros slam
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.

Video Label

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-dev 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

Video Label

4. Hand-held dataset with textureless wall

  • We recorded stereo images & IMU readings, and high precision position reference by using MYNTEYE S1030 and Leica TS-16, respectively in BLDG. 301 at our campus, SNU. It features the textureless white wall during the course.

ex_screenshot

  • Trajectory information is summarized as below

    Sequence num. Length [m] Duration [sec] White wall ?
    snu301_00 89 105 X
    snu301_01 117 143 O
    snu301_02 147 182 O
    snu301_03 159 199 O

  • You can download our dataset here. (6.2 GB)

  • Please use nesl_envio_mynt.launch to test the dataset :

    roslaunch ensemble_vio nesl_envio_mynt.launch
roslaunch ensemble_vio nesl_envio_rviz.launch
rosbag play snu301_xx.bag

Video Label

5. 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.

6. 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}
}

7. 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.

8. 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...
The code for C programming 2021, Department of Computer Science, National Taiwan University.

C2021 .c for sousce code, .in for input file, and .out for correct output. The numbers are the problem indices in the judge system. "make number" to m

Pangfeng Liu 6 Jan 10, 2022
YOLO v5 ONNX Runtime C++ inference code.
YOLO v5 ONNX Runtime C++ inference code.

yolov5-onnxruntime C++ YOLO v5 ONNX Runtime inference code for object detection. Dependecies: OpenCV 4.5+ ONNXRuntime 1.7+ OS: Windows 10 or Ubuntu 20

null 99 Dec 30, 2022
Code generation for automatic differentiation with GPU support.

Code generation for automatic differentiation with GPU support.

Eric Heiden 41 Dec 5, 2022
The code implemented in ROS projects a point cloud obtained by a Velodyne VLP16 3D-Lidar sensor on an image from an RGB camera.
The code implemented in ROS projects a point cloud obtained by a Velodyne VLP16 3D-Lidar sensor on an image from an RGB camera.

PointCloud on Image The code implemented in ROS projects a point cloud obtained by a Velodyne VLP16 3D-Lidar sensor on an image from an RGB camera. Th

Edison Velasco Sánchez 5 Aug 12, 2022
C++ Live Toolkit are tools subset used to perform on-the-fly compilation and running of cpp code

C++ Live Toolkit CLT (C++ Live Toolkit) is subset of tools that are very light in size, and maintained to help programmers in compiling and executing

MondeO 1 Jan 4, 2022
An x64 binary executing code that's not inside of it.

Remote Machine Code Fetch & Exec in other words, another self rewriting binary.. boy I just love doing these. Description The idea behind this one is

x0reaxeax 2 Nov 19, 2022
Compile and run/debug C or C++ code easily

run-c Compile and run/debug C or C++ code easily. Installation and Updating Install & Update Script To install or update run-c, you should run the ins

null 1 Dec 8, 2021
A system to flag anomalous source code expressions by learning typical expressions from training data
A system to flag anomalous source code expressions by learning typical expressions from training data

A friendly request: Thanks for visiting control-flag GitHub repository! If you find control-flag useful, we would appreciate a note from you (to niran

Intel Labs 1.2k Dec 30, 2022
Mirror of compiler project code. Not for SVC purpose.

Compiler-proj Project progress is updated here. Progress 2021/11/28: Started! Set up Makefile and finished basic scanner. 2021/10/24: Repo created. Ac

Yuheng 0 Dec 23, 2021
Comments
  • Divergence

    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

    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

    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

    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
Owner
Jae Hyung Jung
Jae Hyung Jung
GitHub https://ieeexplore.ieee.org/document/9686364
The repository contains our dataset and C++ implementation of the CVPR 2022 paper, Geometric Structure Preserving Warp for Natural Image Stitching.

Geometric Structure Preserving Warp for Natural Image Stitching This repository contains our dataset and C++ implementation of the CVPR 2022 paper, Ge

null 21 Dec 22, 2022
This is a code repository for pytorch c++ (or libtorch) tutorial.

LibtorchTutorials English version 环境 win10 visual sutdio 2017 或者Qt4.11.0 Libtorch 1.7 Opencv4.5 配置 libtorch+Visual Studio和libtorch+QT分别记录libtorch在VS和Q

null 464 Jan 9, 2023
Official PyTorch Code of GrooMeD-NMS: Grouped Mathematically Differentiable NMS for Monocular 3D Object Detection (CVPR 2021)

GrooMeD-NMS: Grouped Mathematically Differentiable NMS for Monocular 3D Object Detection GrooMeD-NMS: Grouped Mathematically Differentiable NMS for Mo

Abhinav Kumar 76 Jan 2, 2023
Source code to the 1995 DOS roguelike game Alphaman

Alphaman Source and Files Source code and related files for the 1995 DOS roguelike game Alphaman by Jeffrey R. Olson. Jeff can be reached via email at

Jamie 36 Dec 6, 2022
Code and Data for our CVPR 2021 paper "Structured Scene Memory for Vision-Language Navigation"

SSM-VLN Code and Data for our CVPR 2021 paper "Structured Scene Memory for Vision-Language Navigation". Environment Installation Download Room-to-Room

hanqing 35 Dec 3, 2022
This is the code of our paper An Efficient Training Approach for Very Large Scale Face Recognition or F²C for simplicity.

Fast Face Classification (F²C) This is the code of our paper An Efficient Training Approach for Very Large Scale Face Recognition or F²C for simplicit

null 33 Jun 27, 2021
HIPIFY: Convert CUDA to Portable C++ Code

Tools to translate CUDA source code into portable HIP C++ automatically

ROCm Developer Tools 206 Dec 31, 2022
This code accompanies the paper "Human-Level Performance in No-Press Diplomacy via Equilibrium Search".

Diplomacy SearchBot This code accompanies the paper "Human-Level Performance in No-Press Diplomacy via Equilibrium Search". A very brief orientation:

Facebook Research 34 Dec 20, 2022
Code shown in the lectures

Reference Material for Getting Started with CP (an NPTEL course) This repository contains the code shown in the lectures for the NPTEL course on Getti

null 87 Dec 16, 2022
Provide sample code of efficient operator implementation based on the Cambrian Machine Learning Unit (MLU) .

Cambricon CNNL-Example CNNL-Example 提供基于寒武纪机器学习单元（Machine Learning Unit，MLU）开发高性能算子、C 接口封装的示例代码。 依赖条件 操作系统： 目前只支持 Ubuntu 16.04 x86_64 寒武纪 MLU SDK： 编译和

Cambricon Technologies 1 Mar 7, 2022