Skip to content

Commit e4fb543

Browse files
huaizhenghuaizheng
committed
Uploaded source files
1 parent ef502b7 commit e4fb543

26 files changed

+3787
-0
lines changed

‎.gitignore

+3
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,3 @@
1+
CMakeLists.txt.user
2+
*~
3+

‎CMakeLists.txt

+93
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,93 @@
1+
cmake_minimum_required(VERSION 2.8)
2+
project(rvio)
3+
4+
# Set our build type
5+
IF(NOT CMAKE_BUILD_TYPE)
6+
SET(CMAKE_BUILD_TYPE Release)
7+
ENDIF()
8+
MESSAGE("Build type: " ${CMAKE_BUILD_TYPE})
9+
10+
# Check C++11 or C++0x support
11+
include(CheckCXXCompilerFlag)
12+
CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
13+
CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
14+
if(COMPILER_SUPPORTS_CXX11)
15+
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
16+
add_definitions(-DCOMPILEDWITHC11)
17+
message(STATUS "Using flag -std=c++11.")
18+
elseif(COMPILER_SUPPORTS_CXX0X)
19+
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
20+
add_definitions(-DCOMPILEDWITHC0X)
21+
message(STATUS "Using flag -std=c++0x.")
22+
else()
23+
message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.")
24+
endif()
25+
26+
# Enable compile optimizations
27+
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -fsee -fomit-frame-pointer -fno-signed-zeros -fno-math-errno -funroll-loops")
28+
29+
# Enable debug flags (use if you want to debug in gdb)
30+
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g3 -Wall")
31+
32+
# Include our cmake files
33+
LIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules/)
34+
35+
# Find catkin (the ROS build system)
36+
# Add this for finding ros dependencies
37+
find_package(catkin REQUIRED COMPONENTS
38+
roscpp tf sensor_msgs geometry_msgs nav_msgs cv_bridge eigen_conversions
39+
)
40+
41+
# Describe catkin project
42+
catkin_package(CATKIN_DEPENDS
43+
roscpp tf std_msgs sensor_msgs geometry_msgs nav_msgs cv_bridge eigen_conversions
44+
INCLUDE_DIRS src
45+
LIBRARIES ${PROJECT_NAME}
46+
)
47+
48+
# Include libraries
49+
find_package(Eigen3 3.1.0 REQUIRED)
50+
find_package(OpenCV 3.0 QUIET)
51+
if(NOT OpenCV_FOUND)
52+
find_package(OpenCV 2.4.3 QUIET)
53+
if(NOT OpenCV_FOUND)
54+
message(FATAL_ERROR "OpenCV > 2.4.3 not found.")
55+
endif()
56+
endif()
57+
58+
# Include directory for headers
59+
include_directories(
60+
${PROJECT_SOURCE_DIR}
61+
${PROJECT_SOURCE_DIR}/include
62+
${EIGEN3_INCLUDE_DIR}
63+
${catkin_INCLUDE_DIRS}
64+
)
65+
66+
##################################################
67+
# Make binary for the main library
68+
##################################################
69+
add_library(${PROJECT_NAME} SHARED
70+
src/rvio/System.cc
71+
src/rvio/Updater.cc
72+
src/rvio/Tracker.cc
73+
src/rvio/CornerDetector.cc
74+
src/rvio/CornerCluster.cc
75+
src/rvio/Ransac.cc
76+
src/rvio/PreIntegrator.cc
77+
src/rvio/SensorDatabase.cc
78+
)
79+
80+
target_link_libraries(${PROJECT_NAME}
81+
${OpenCV_LIBS}
82+
${EIGEN3_LIBS}
83+
)
84+
85+
##################################################
86+
# Make binary for main method ROS
87+
##################################################
88+
add_executable(rvio_mono src/rvio_mono.cc)
89+
target_link_libraries(rvio_mono
90+
${PROJECT_NAME}
91+
${OpenCV_LIBS}
92+
${catkin_LIBRARIES}
93+
)

‎README.md

+51
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,51 @@
1+
# R-VIO
2+
3+
R-VIO is an efficient, lightweight, **robocentric visual-inertial odometry** algorithm for consistent 3D motion tracking using only a monocular camera and a 6-axis IMU. Different from standard world-centric VINS algorithms which directly estimate absolute motion of the sensing platform with respect to a fixed, gravity-aligned global frame of reference {G}, R-VIO estimates the relative motion of high accuracy with respect to a moving local frame {R} (for example, IMU frame) and updates the global pose (orientation and position) estimate through composition. This algorithm is developed with the robocentric sliding-window filtering-based VIO framework that we originally proposed in our IROS 2018 paper and further extended in our recent *IJRR* submission:
4+
5+
- Zheng Huai and Guoquan Huang, **Robocentric visual-inertial odometry**, *International Conference on Intelligent Robots and Systems (IROS)*, Madrid, Spain, Oct 1-5, 2018.
6+
7+
- Zheng Huai and Guoquan Huang, **Robocentric visual-inertial odometry**, *The International Journal of Robotics Research*, Nov 2018. [cond. accepted]
8+
9+
- Zheng Huai and Guoquan Huang, **Robocentric visual-inertial odometry**, arXiv:1805.04031, May 2018: https://arxiv.org/abs/1805.04031
10+
11+
- For the implementation details, please also refer to our companion technical report: http://udel.edu/~ghuang/papers/tr_rvio_ijrr.pdf.
12+
13+
## 1. Prerequisites
14+
15+
We have tested the code under Ubuntu **16.04** and ROS **Kinetic**.
16+
17+
### ROS
18+
Download and install instructions can be found at: http://wiki.ros.org/kinetic/Installation/Ubuntu.
19+
20+
Additional ROS packages needed: tf, sensor_msgs, geometry_msgs, nav_msgs, cv_bridge, eigen_conversions.
21+
22+
### Eigen
23+
Download and install instructions can be found at: http://eigen.tuxfamily.org. **Required at least 3.1.0**.
24+
25+
### OpenCV
26+
Download and install instructions can be found at: http://opencv.org. **Required at leat 2.4.3**. **Tested with 2.4.11 and 3.3.1**.
27+
28+
## 2. Build and Run
29+
First, `git clone` the repository and `catkin_make` it. Then, to run `rvio` with single camera/IMU inputs from the ROS topics `/camera/image_raw` and `/imu`, a config file in *config* folder and the corresponding launch file in *launch* folder (for example, `rvio_euroc.yaml` and `euroc.launch` are for EuRoC dataset) are needed, and to visualize the outputs of R-VIO please use `rviz` with the settings file `rvio_rviz.rviz` in *config* folder.
30+
```
31+
Terminal 1: roscore
32+
```
33+
```
34+
Terminal 2: rviz (AND OPEN rvio_rviz.rviz IN THE CONFIG FOLDER)
35+
```
36+
```
37+
Terminal 3: roslaunch rvio euroc.launch
38+
```
39+
```
40+
Terminal 4: rosbag play --pause V1_01_easy.bag /cam0/image_raw:=/camera/image_raw /imu0:=/imu
41+
```
42+
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.
43+
44+
## 3. License
45+
46+
The source code is released under [GPLv3](https://www.gnu.org/licenses/gpl-3.0.en.html) license.
47+
48+
We are still working on improving the code reliability. For any technical issue, please contact Zheng Huai: <[email protected]>.
49+
50+
For commercial inquiries, please contact Guoquan (Paul) Huang: <[email protected]>.
51+

‎cmake_modules/FindEigen3.cmake

+87
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,87 @@
1+
# - Try to find Eigen3 lib
2+
#
3+
# This module supports requiring a minimum version, e.g. you can do
4+
# find_package(Eigen3 3.1.2)
5+
# to require version 3.1.2 or newer of Eigen3.
6+
#
7+
# Once done this will define
8+
#
9+
# EIGEN3_FOUND - system has eigen lib with correct version
10+
# EIGEN3_INCLUDE_DIR - the eigen include directory
11+
# EIGEN3_VERSION - eigen version
12+
13+
# Copyright (c) 2006, 2007 Montel Laurent, <[email protected]>
14+
# Copyright (c) 2008, 2009 Gael Guennebaud, <[email protected]>
15+
# Copyright (c) 2009 Benoit Jacob <[email protected]>
16+
# Redistribution and use is allowed according to the terms of the 2-clause BSD license.
17+
18+
if(NOT Eigen3_FIND_VERSION)
19+
if(NOT Eigen3_FIND_VERSION_MAJOR)
20+
set(Eigen3_FIND_VERSION_MAJOR 2)
21+
endif(NOT Eigen3_FIND_VERSION_MAJOR)
22+
if(NOT Eigen3_FIND_VERSION_MINOR)
23+
set(Eigen3_FIND_VERSION_MINOR 91)
24+
endif(NOT Eigen3_FIND_VERSION_MINOR)
25+
if(NOT Eigen3_FIND_VERSION_PATCH)
26+
set(Eigen3_FIND_VERSION_PATCH 0)
27+
endif(NOT Eigen3_FIND_VERSION_PATCH)
28+
29+
set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}")
30+
endif(NOT Eigen3_FIND_VERSION)
31+
32+
macro(_eigen3_check_version)
33+
file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header)
34+
35+
string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}")
36+
set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}")
37+
string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}")
38+
set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}")
39+
string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}")
40+
set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}")
41+
42+
set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION})
43+
if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})
44+
set(EIGEN3_VERSION_OK FALSE)
45+
else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})
46+
set(EIGEN3_VERSION_OK TRUE)
47+
endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})
48+
49+
if(NOT EIGEN3_VERSION_OK)
50+
51+
message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, "
52+
"but at least version ${Eigen3_FIND_VERSION} is required")
53+
endif(NOT EIGEN3_VERSION_OK)
54+
endmacro(_eigen3_check_version)
55+
56+
if (EIGEN3_INCLUDE_DIR)
57+
58+
# in cache already
59+
_eigen3_check_version()
60+
set(EIGEN3_FOUND ${EIGEN3_VERSION_OK})
61+
62+
else (EIGEN3_INCLUDE_DIR)
63+
64+
# specific additional paths for some OS
65+
if (WIN32)
66+
set(EIGEN_ADDITIONAL_SEARCH_PATHS ${EIGEN_ADDITIONAL_SEARCH_PATHS} "C:/Program Files/Eigen/include" "C:/Program Files (x86)/Eigen/include")
67+
endif(WIN32)
68+
69+
find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library
70+
PATHS
71+
${CMAKE_INSTALL_PREFIX}/include
72+
${EIGEN_ADDITIONAL_SEARCH_PATHS}
73+
${KDE4_INCLUDE_DIR}
74+
PATH_SUFFIXES eigen3 eigen
75+
)
76+
77+
if(EIGEN3_INCLUDE_DIR)
78+
_eigen3_check_version()
79+
endif(EIGEN3_INCLUDE_DIR)
80+
81+
include(FindPackageHandleStandardArgs)
82+
find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK)
83+
84+
mark_as_advanced(EIGEN3_INCLUDE_DIR)
85+
86+
endif(EIGEN3_INCLUDE_DIR)
87+

‎config/rvio_euroc.yaml

+108
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,108 @@
1+
%YAML:1.0
2+
3+
#--------------------------------------------------------------------------------------------
4+
# IMU Parameters.
5+
#--------------------------------------------------------------------------------------------
6+
7+
# IMU data rate
8+
IMU.dps: 200
9+
10+
# Threshold of small angle (<1/2deg)
11+
IMU.nSmallAngle: 0.008726646
12+
13+
# IMU sensor noise
14+
IMU.sigma_g: 1.6968e-04
15+
IMU.sigma_wg: 1.9393e-05
16+
IMU.sigma_a: 2.0000e-3
17+
IMU.sigma_wa: 3.0000e-3
18+
19+
# Gravity
20+
IMU.nG: 9.8082
21+
22+
#--------------------------------------------------------------------------------------------
23+
# Camera Parameters.
24+
#--------------------------------------------------------------------------------------------
25+
26+
# Camera frame rate
27+
Camera.fps: 20
28+
29+
# Is RGB or not
30+
Camera.RGB: 0
31+
32+
# Is fisheye or not
33+
Camera.Fisheye: 0
34+
35+
# Camera image resolution
36+
Camera.width: 752
37+
Camera.height: 480
38+
39+
# Camera intrinsics
40+
Camera.fx: 458.654
41+
Camera.fy: 457.296
42+
Camera.cx: 367.215
43+
Camera.cy: 248.375
44+
45+
Camera.k1: -0.28340811
46+
Camera.k2: 0.07395907
47+
Camera.p1: 0.00019359
48+
Camera.p2: 1.76187114e-05
49+
50+
# Camera image noise (1/f)
51+
Camera.sigma_px: 0.002180293
52+
Camera.sigma_py: 0.002186767
53+
54+
# Camera extrinsics [B:IMU,C0:cam0]
55+
Camera.T_BC0: !!opencv-matrix
56+
rows: 4
57+
cols: 4
58+
dt: d
59+
data: [0.0148655429818, -0.999880929698, 0.00414029679422, -0.0216401454975,
60+
0.999557249008, 0.0149672133247, 0.025715529948, -0.064676986768,
61+
-0.0257744366974, 0.00375618835797, 0.999660727178, 0.00981073058949,
62+
0.0, 0.0, 0.0, 1.0]
63+
64+
# Timeshift camera to IMU [s] (t_imu=t_cam+shift)
65+
Camera:nTimeOffset: 0
66+
67+
#--------------------------------------------------------------------------------------------
68+
# Tracker Parameters (tunable).
69+
#--------------------------------------------------------------------------------------------
70+
71+
# Number of features per image
72+
Tracker.nFeatures: 200
73+
74+
# Max. tracking length
75+
Tracker.nTrackingLength: 15
76+
77+
# Use image equalizer or not
78+
Tracker.EnableEqualizer: 1
79+
80+
# Use Sampson error or not (RANSAC)
81+
Tracker.UseSampson: 1
82+
83+
# Threshold for inlier (RANSAC)
84+
Tracker.nSampsonThrd: 1e-4
85+
86+
#--------------------------------------------------------------------------------------------
87+
# Initialization Parameters (tunable).
88+
#--------------------------------------------------------------------------------------------
89+
90+
# Use initial alignment or not
91+
INI.EnableAlignment: 1
92+
93+
# Time length of initialization [s]
94+
INI.nTimeLength: 1
95+
96+
# Thresholds for moving detection [rad,m]
97+
INI.nThresholdAngle: 0.005
98+
INI.nThresholdDispl: 0.005
99+
100+
#--------------------------------------------------------------------------------------------
101+
# Display Parameters (tunable).
102+
#--------------------------------------------------------------------------------------------
103+
104+
# Point size
105+
Landmark.nScale: 0.025
106+
107+
# Publishing rate
108+
Landmark.nPubRate: 5

0 commit comments

Comments
 (0)