Added new InputBuffer and FeatureDetector

Author: huaizheng

CMakeLists.txt

@@ -23,11 +23,8 @@ else()
   message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.")
 endif()
 
-# Enable compile optimizations
-set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -fsee -fomit-frame-pointer -fno-signed-zeros -fno-math-errno -funroll-loops")
-
 # Enable debug flags (if you want to debug in gdb)
-set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g3 -Wall")
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -g3")
 
 # Find catkin (the ROS build system)
 # Add this for finding ros dependencies
@@ -68,9 +65,8 @@ add_library(${PROJECT_NAME} SHARED
   src/rvio/Tracker.cc
   src/rvio/PreIntegrator.cc
   src/rvio/Ransac.cc
-  src/rvio/ImuBuffer.cc
-  src/rvio/CornerCluster.cc
-  src/rvio/CornerDetector.cc
+  src/rvio/InputBuffer.cc
+  src/rvio/FeatureDetector.cc
 )
 
 target_link_libraries(${PROJECT_NAME}

README.md

@@ -1,12 +1,12 @@
 # R-VIO
 
-R-VIO is an efficient, lightweight, **robocentric visual-inertial odometry** algorithm for consistent 3D motion tracking using only a monocular camera and a single IMU. Different from the standard world-centric algorithms which directly estimate absolute motion of the mobile platform with respect to a fixed, gravity-aligned, global frame of reference, R-VIO i) estimates relative motion of higher accuracy with respect to a moving, local frame (for example, IMU frame), and ii) incrementally updates global pose (orientation and position) through composition. This code is developed with the robocentric sliding-window filtering-based VIO framework that was originally proposed in our *IROS2018* paper and further extended in our recent *IJRR* paper:
+R-VIO is an efficient, lightweight, **robocentric** visual-inertial navigation algorithm for consistent 3D motion tracking using only a monocular camera and a single IMU. Different from the standard world-centric algorithms which directly estimate absolute motion of the mobile platform with respect to a fixed, gravity-aligned, global frame of reference, R-VIO i) estimates relative motion of higher accuracy with respect to a moving, local frame (the IMU frame here), and ii) incrementally updates global pose (orientation and position) through a composition step. This code is developed with the robocentric sliding-window filtering-based VIO framework that was originally proposed in our *IROS2018* paper and further extended in our recent *IJRR* paper:
 
 - Zheng Huai and Guoquan Huang, **Robocentric visual-inertial odometry**, *The International Journal of Robotics Research (IJRR)*, July 2019: [download](https://journals.sagepub.com/doi/10.1177/0278364919853361).
 
 ```
 @article{huai2019robocentric,
-  title     = {Robocentric visual--inertial odometry},
+  title     = {Robocentric visual-inertial odometry},
   author    = {Huai, Zheng and Huang, Guoquan},
   journal   = {The International Journal of Robotics Research},
   publisher = {SAGE Publications Sage UK: London, England},
@@ -36,7 +36,7 @@ IJRR video (Our 9.8km **Urban Driving** dataset): [YouTube](https://www.youtube.
 
 ## 1. Prerequisites
 
-We have tested the code under Ubuntu **16.04** and ROS **Kinetic**.
+We have tested this code under Ubuntu **16.04** and ROS **Kinetic**.
 
 ### ROS
 Download and install instructions can be found at: http://wiki.ros.org/kinetic/Installation/Ubuntu.
@@ -63,7 +63,10 @@ First, `git clone` the repository and `catkin_make` it. Then, to run `rvio` with
   ```
   Terminal 4: rosbag play --pause V1_01_easy.bag /cam0/image_raw:=/camera/image_raw /imu0:=/imu
   ```
-You can also run R-VIO with your own sensor (data) by creating a config file `rvio_NAME_OF_YOUR_DATA.yaml` in *config* folder and the corresponding launch file `NAME_OF_YOUR_DATA.launch` in *launch* folder referring to the above EuRoC example.
+
+Note that when testing the `Machine Hall` sequences, you should skip the data in the first few seconds (e.g., 40s for `MH_01_easy`) which are used for initializing the map for SLAM-based algorithms.
+
+You can also run R-VIO with your own sensors (data) by creating a config file `rvio_NAME_OF_YOUR_DATA.yaml` in *config* folder and the corresponding launch file `NAME_OF_YOUR_DATA.launch` in *launch* folder, referring to our EuRoC example.
 
 ## 3. License
 

config/rvio_euroc.yaml

@@ -7,9 +7,6 @@
 # IMU data rate
 IMU.dps: 200
 
-# Threshold of small angle [rad] (<.1deg)
-IMU.nSmallAngle: 0.001745329
-
 # IMU sensor noise
 IMU.sigma_g: 1.6968e-04
 IMU.sigma_wg: 1.9393e-05
@@ -19,6 +16,9 @@ IMU.sigma_wa: 3.0000e-3
 # Gravity
 IMU.nG: 9.8082
 
+# Threshold of small angle [rad] (<.1deg)
+IMU.nSmallAngle: 0.001745329
+
 #--------------------------------------------------------------------------------------------
 # Camera Parameters (fixed).
 #--------------------------------------------------------------------------------------------
@@ -72,19 +72,20 @@ Camera.nTimeOffset: 0
 Tracker.nFeatures: 200
 
 # Max. tracking length
-Tracker.nMaxTrackingLength: 15 # 10 # 5
+Tracker.nMaxTrackingLength: 15
 
 # Min. tracking length
 Tracker.nMinTrackingLength: 3
 
 # Min. distance between features
-Tracker.nMinDist: 10
+Tracker.nMinDist: 15
 
 # Quality level of features
 Tracker.nQualLvl: 0.01
 
-# Size of chess grid
-Tracker.nGridSize: 100
+# Block size of image chess grid
+Tracker.nBlockSizeX: 150
+Tracker.nBlockSizeY: 120
 
 # Use histogram equalizer or not
 Tracker.EnableEqualizer: 1
@@ -93,14 +94,14 @@ Tracker.EnableEqualizer: 1
 Tracker.UseSampson: 1
 
 # Error threshold for inlier (RANSAC)
-Tracker.nInlierThrd: 1e-4
+Tracker.nInlierThrd: 1e-5
 
 #--------------------------------------------------------------------------------------------
 # Initialization Parameters (tunable).
 #--------------------------------------------------------------------------------------------
 
 # Thresholds for moving detection [rad,m]
-INI.nThresholdAngle: 0.005
+INI.nThresholdAngle: 0.005 # 0.01 (for MH_*)
 INI.nThresholdDispl: 0.01
 
 # Use gravity alignment or not