Initial commit

.gitignore

@@ -0,0 +1,2 @@
+CMakeLists.txt.user
+*~

CMakeLists.txt

@@ -0,0 +1,71 @@
+cmake_minimum_required(VERSION 2.8)
+project(rvio2)
+
+# Set build type
+IF(NOT CMAKE_BUILD_TYPE)
+#SET(CMAKE_BUILD_TYPE Debug)
+SET(CMAKE_BUILD_TYPE Release)
+ENDIF()
+MESSAGE("Build type: " ${CMAKE_BUILD_TYPE})
+
+# Check C++11 or C++0x support
+include(CheckCXXCompilerFlag)
+CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
+CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
+if(COMPILER_SUPPORTS_CXX11)
+ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
+ add_definitions(-DCOMPILEDWITHC11)
+ message(STATUS "Using flag -std=c++11.")
+elseif(COMPILER_SUPPORTS_CXX0X)
+ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
+ add_definitions(-DCOMPILEDWITHC0X)
+ message(STATUS "Using flag -std=c++0x.")
+else()
+ message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.")
+endif()
+
+# Enable debug flags (if you want to debug in gdb)
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -g3")
+
+# Include libraries
+find_package(catkin REQUIRED COMPONENTS
+ roscpp roslib rosbag tf std_msgs sensor_msgs geometry_msgs nav_msgs cv_bridge
+)
+find_package(Eigen3 REQUIRED)
+find_package(OpenCV 3.0 QUIET)
+
+# Include directories for headers
+include_directories(
+ ${PROJECT_SOURCE_DIR}
+ ${EIGEN3_INCLUDE_DIR}
+ ${catkin_INCLUDE_DIRS}
+)
+
+catkin_package()
+
+add_library(${PROJECT_NAME} SHARED
+ src/rvio2/System.cc
+ src/rvio2/Ransac.cc
+ src/rvio2/Tracker.cc
+ src/rvio2/Updater.cc
+ src/rvio2/Propagator.cc
+ src/rvio2/InputBuffer.cc
+ src/rvio2/Feature.cc
+ src/rvio2/FeatureDetector.cc
+)
+
+add_executable(rvio2_mono src/rvio2_mono.cc)
+target_link_libraries(rvio2_mono
+ ${PROJECT_NAME}
+ ${OpenCV_LIBS}
+ ${EIGEN3_LIBS}
+ ${catkin_LIBRARIES}
+)
+
+add_executable(rvio2_mono_eval src/rvio2_mono_eval.cc)
+target_link_libraries(rvio2_mono_eval
+ ${PROJECT_NAME}
+ ${OpenCV_LIBS}
+ ${EIGEN3_LIBS}
+ ${catkin_LIBRARIES}
+)

README.md

@@ -0,0 +1,61 @@
+# R-VIO2
+
+R-VIO2 is a novel square root information-based robocentric visual-inertial navigation algorithm using a monocular camera and a single IMU for consistent 3D motion tracking. It is developed based on our [robocentric VIO model](https://journals.sagepub.com/doi/pdf/10.1177/0278364919853361), while different with our previous work [R-VIO](https://github.com/rpng/R-VIO), we have derived and used i) our square-root robocentric formulation and ii) QR-based update combined with back substitution to improve the numerical stability and computational efficiency of the estimator. Moreover, the spatiotemporal calibration is performed online to robustify the performance of estimator in the presence of unknown parameter errors. Especially, this implementation can run in two modes: VIO or SLAM, where the former does not estimate any map points during the navigation (our *RA-L2022* paper), while the latter estimates a small set of map points in favor of localization (the frontend developed for our *ICRA2021* paper).
+
+![](rvio2.gif)
+
+If you find this work relevant to or use it for your research, please consider citing the following papers:
+- Zheng Huai and Guoquan Huang, **Square-Root Robocentric Visual-Inertial Odometry with Online Spatiotemporal Calibration**, *IEEE Robotics and Automation Letters (RA-L)*, 2022: (*to appear*).
+```
+@article{huai2022square,
+ title = {Square-root robocentric visual-inertial odometry with online spatiotemporal calibration},
+ author = {Huai, Zheng and Huang, Guoquan},
+ journal = {IEEE Robotics and Automation Letters (RA-L)},
+ year = {2022},
+ publisher = {IEEE}
+}
+```
+- Zheng Huai and Guoquan Huang, **Markov Parallel Tracking and Mapping for Probabilistic SLAM**, *IEEE International Conference on Robotics and Automation (ICRA)*, 2021: [download](https://ieeexplore.ieee.org/abstract/document/9561238).
+```
+@inproceedings{huai2021markov,
+ title = {Markov parallel tracking and mapping for probabilistic SLAM},
+ author = {Huai, Zheng and Huang, Guoquan},
+ booktitle = {IEEE International Conference on Robotics and Automation (ICRA)},
+ pages = {11661--11667},
+ year = {2021}
+}
+```
+
+## 1. Prerequisites
+### ROS
+Download and install instructions can be found at: http:https://wiki.ros.org/kinetic/Installation/Ubuntu.
+### Eigen
+Download and install instructions can be found at: http:https://eigen.tuxfamily.org. **Tested with v3.1.0**.
+### OpenCV
+Download and install instructions can be found at: http:https://opencv.org. **Tested with v3.3.1**.
+
+
+## 2. Build and Run
+First `git clone` the repository and `catkin_make` it. Especially, `rvio2_mono` is used to run with rosbag in real time, while `rvio2_mono_eval` is used for evaluation purpose which preloads the rosbag and reads it as a txt file. A config file and a launch file are required for running R-VIO2 (for example, `rvio2_euroc.yaml` and `euroc.launch` are for [EuRoC](https://projects.asl.ethz.ch/datasets/doku.php?id=kmavvisualinertialdatasets) dataset). The default mode is VIO, while you can switch to SLAM mode by setting the maximum number of SLAM features to nonzero from the config file (see `rvio2_euroc.yaml`). To visualize the outputs, please use `rviz`.
+#### Start ROS:
+ ```
+ Terminal 1: roscore
+ ```
+ ```
+ Terminal 2: rviz (AND OPEN rvio2_rviz.rviz IN THE CONFIG FOLDER)
+ ```
+#### Run `rvio2_mono`:
+ ```
+ Terminal 3: rosbag play --pause V1_01_easy.bag (AND SKIP SOME DATA IF NEEDED)
+ ```
+ ```
+ Terminal 4: roslaunch rvio2 euroc.launch
+ ```
+#### Run `rvio2_mono_eval`:
+ ```
+ Terminal 3: roslaunch rvio2 euroc_eval.launch (PRESET PATH_TO_ROSBAG IN euroc_eval.launch)
+ ```
+Note that this implementation currently requires the sensor platform to start from stationary. Therefore, when testing the `Machine Hall` sequences you should skip the wiggling phase at the beginning. In particular, if you would like to run `rvio2_mono_eval`, the rosbag data to be skipped can be set in the config file (see `rvio2_euroc.yaml`).
+
+## 3. License
+This code is released under [GNU General Public License v3 (GPL-3.0)](https://www.gnu.org/licenses/gpl-3.0.en.html).

config/rvio2_euroc.yaml

@@ -0,0 +1,156 @@
+%YAML:1.0
+
+#--------------------------------------------------------------------------------------------
+# IMU Parameters (cf. EuRoC dataset)
+#--------------------------------------------------------------------------------------------
+
+# IMU data rate
+IMU.dps: 200
+
+# IMU sensor noise
+IMU.sigma_g: 1.6968e-04
+IMU.sigma_wg: 1.9393e-05
+IMU.sigma_a: 2.0000e-3
+IMU.sigma_wa: 3.0000e-3
+
+# Gravity
+IMU.nG: 9.8082
+
+# Threshold of small angle [rad] (<.1deg)
+IMU.nSmallAngle: 0.001745329
+
+#--------------------------------------------------------------------------------------------
+# Camera Parameters (cf. EuRoC dataset)
+#--------------------------------------------------------------------------------------------
+
+# Camera frame rate
+Camera.fps: 20
+
+# Camera image resolution
+Camera.width: 752
+Camera.height: 480
+
+# Is RGB or not
+Camera.RGB: 0
+
+# Is fisheye or not
+Camera.Fisheye: 0
+
+# Camera intrinsics
+Camera.fx: 458.654
+Camera.fy: 457.296
+Camera.cx: 367.215
+Camera.cy: 248.375
+
+Camera.k1: -0.28340811
+Camera.k2: 0.07395907
+Camera.p1: 0.00019359
+Camera.p2: 1.76187114e-05
+
+# Camera image noise (1/f)
+Camera.sigma_px: 0.002180293
+Camera.sigma_py: 0.002186767
+
+# Camera extrinsics [B:IMU,C0:cam0]
+Camera.T_BC0_GT: !!opencv-matrix # Ground truth
+ rows: 4
+ cols: 4
+ dt: d
+ data: [ 0.0148655429818, -0.999880929698, 0.00414029679422, -0.0216401454975,
+ 0.999557249008, 0.0149672133247, 0.025715529948, -0.064676986768,
+ -0.0257744366974, 0.00375618835797, 0.999660727178, 0.00981073058949,
+ 0.0, 0.0, 0.0, 1.0]
+
+Camera.T_BC0: !!opencv-matrix # Approximate
+ rows: 4
+ cols: 4
+ dt: d
+ data: [0.0, -1.0, 0.0, 0.0,
+ 1.0, 0.0, 0.0, 0.0,
+ 0.0, 0.0, 1.0, 0.0,
+ 0.0, 0.0, 0.0, 1.0]
+
+# Time shift from camera to IMU [s] (t_imu=t_cam+offset)
+Camera.nTimeOffset_GT: 0
+Camera.nTimeOffset: 0
+
+#--------------------------------------------------------------------------------------------
+# Initialization Parameters (tunable)
+#--------------------------------------------------------------------------------------------
+
+# Skip the "wiggling" phase [s]
+INI.nTimeskip: 0 # MH_01:40, MH_02:30, MH_03:15, MH_04:15, MH_05:15
+
+# Record the result or not
+INI.RecordOutputs: 0
+
+# Use gravity alignment or not
+INI.EnableAlignment: 1
+
+# Use ground truth for calibration or not
+INI.UseGroundTruthCalib: 1
+
+# Thresholds for moving detection [deg,m]
+INI.nAngleThrd: 0.3
+INI.nLengthThrd: 0.01
+
+#--------------------------------------------------------------------------------------------
+# Tracker Parameters (tunable)
+#--------------------------------------------------------------------------------------------
+
+# Number of features per image
+Tracker.nFeatures: 200
+
+# Min. tracking length
+Tracker.nMinTrackingLength: 3
+
+# Max. tracking length
+Tracker.nMaxTrackingLength: 15
+
+# Quality level of features
+Tracker.nQualLvl: 1e-2
+
+# Min. distance between two features
+Tracker.nMinDist: 15
+
+# Max. number of SLAM features (VIO:0, SLAM:50)
+Tracker.nMaxSlamPoints: 0
+
+# Parallax of good feature [deg] (only for SLAM)
+Tracker.nGoodParallax: 3
+
+# Block size of image grid
+Tracker.nBlockSizeX: 150
+Tracker.nBlockSizeY: 120
+
+# Use image filter or not
+Tracker.EnableFilter: 0
+
+# Use histogram equalizer or not
+Tracker.EnableEqualizer: 1
+
+# Number of iterations (RANSAC)
+Tracker.nRansacIter: 16
+
+# Use Sampson error or not (RANSAC)
+Tracker.UseSampson: 1
+
+# Error thresholds for inlier (RANSAC)
+Tracker.nSampsonErrThrd: 1e-6
+Tracker.nAlgebraicErrThrd: 1e-3
+
+#--------------------------------------------------------------------------------------------
+# Displayer Parameters (tunable)
+#--------------------------------------------------------------------------------------------
+
+# Display feature tracking
+Displayer.ShowTrack: 1
+
+# Display feature redetection
+Displayer.ShowNewer: 0
+
+# Size of point
+Displayer.nLandmarkScale: 0.03
+
+# Publishing rate
+Displayer.nLandmarkPubRate: 5