diff --git a/.gitignore b/.gitignore
new file mode 100644
index 00000000..d1122334
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1,12 @@
+build
+.idea
+*.pyc
+estimator/*json
+tmp.*
+test_cfg/
+bin/
+lib/
+!thirdparty
+src/src/
+src/Makefile
+back_results/
diff --git a/CMakeLists.txt b/CMakeLists.txt
new file mode 100644
index 00000000..cefec4cf
--- /dev/null
+++ b/CMakeLists.txt
@@ -0,0 +1,79 @@
+cmake_minimum_required(VERSION 3.5)
+project(feh)
+
+option(BUILD_ROSNODE "wrap the system in a rosnode" OFF)
+
+# set(CMAKE_CC_COMPILER "/usr/bin/gcc-7")
+# set(CMAKE_CXX_COMPILER "/usr/bin/g++-7")
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -std=c++17 -Wno-narrowing -Wno-register -g -fPIC")
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -mtune=native -march=native")
+set(CMAKE_BUILD_TYPE "Release")
+add_definitions(-DNDEBUG)
+
+set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_SOURCE_DIR}/bin)
+set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${CMAKE_SOURCE_DIR}/lib)
+set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${CMAKE_SOURCE_DIR}/lib)
+
+# add_definitions(-DEIGEN_DEFAULT_TO_ROW_MAJOR)
+add_definitions(-DEIGEN_INITIALIZE_MATRICES_BY_ZERO)
+
+# comment out the following line to disable NaN checks on Jacobians
+find_package(OpenCV REQUIRED)
+
+link_directories(
+ ${PROJECT_SOURCE_DIR}/lib
+ ${PROJECT_SOURCE_DIR}/thirdparty/gflags/lib
+ ${PROJECT_SOURCE_DIR}/thirdparty/glog/lib
+ ${PROJECT_SOURCE_DIR}/thirdparty/Pangolin/lib
+ ${PROJECT_SOURCE_DIR}/thirdparty/jsoncpp/lib
+ # ${PROJECT_SOURCE_DIR}/thirdparty/gperftools/lib
+ /usr/local/lib
+ # /opt/ros/kinetic/lib/x86_64-linux-gnu
+ # /usr/lib
+ # /usr/lib/x86_64-linux-gnu
+)
+
+include_directories(
+ ${PROJECT_SOURCE_DIR}/common
+ ${PROJECT_SOURCE_DIR}/src
+ ${PROJECT_SOURCE_DIR}/thirdparty/gflags/include
+ ${PROJECT_SOURCE_DIR}/thirdparty/glog/include
+ ${PROJECT_SOURCE_DIR}/thirdparty/Pangolin/include
+ ${PROJECT_SOURCE_DIR}/thirdparty/jsoncpp/include
+ ${PROJECT_SOURCE_DIR}/thirdparty/eigen3/include/eigen3
+ ${PROJECT_SOURCE_DIR}/thirdparty/pybind11/include
+ ${JSONCPP_INCLUDE_DIRS}
+ /usr/local/include/opencv2
+ # /opt/ros/kinetic/include/opencv-3.3.1-dev
+ # ${PROJECT_SOURCE_DIR}/thirdparty/libigl/include
+ # ${PROJECT_SOURCE_DIR}/thirdparty/gperftools/include
+)
+
+
+
+enable_testing()
+add_subdirectory(thirdparty/googletest)
+add_subdirectory(thirdparty/gflags)
+add_subdirectory(thirdparty/abseil-cpp)
+# add_subdirectory(thirdparty/xfeatures2d)
+
+# feh
+add_subdirectory(common)
+add_subdirectory(src)
+if (BUILD_ROSNODE)
+ add_subdirectory(node)
+endif(BUILD_ROSNODE)
+# add_subdirectory(experimental)
+
+########################################
+# PYTHON BINDING
+########################################
+# NOTE: to build with a specific python version
+# cmake -DPYTHON_EXECUTABLE=path/to/python ..
+# By default, the python binding generated is only compatible with
+# your default python interpreter, which you can check by typing
+# "which python" in your terminal.
+set(PYBIND11_CPP_STANDARD -std=c++17)
+add_subdirectory(thirdparty/pybind11)
+pybind11_add_module(pyxivo MODULE pybind11/pyxivo.cpp)
+target_link_libraries(pyxivo PRIVATE common estimator application)
diff --git a/LICENSE b/LICENSE
new file mode 100644
index 00000000..dcd450ba
--- /dev/null
+++ b/LICENSE
@@ -0,0 +1,31 @@
+Academic Software License
+
+XIVO
+
+No Commercial Use
+
+This License governs use of the accompanying Software, and your use of the Software constitutes acceptance of this license.
+
+You may use this Software for any non-commercial purpose, subject to the restrictions in this license. Uses which are non-commercial include teaching, academic research, and personal experimentation.
+
+You may not use or distribute this Software or any derivative works in any form for any commercial purpose. Examples of commercial purposes would be running business operations, licensing, leasing, or selling the Software, or distributing the Software for use with commercial products.
+
+You may modify this Software and distribute the modified Software for non-commercial purposes; however, you may not grant rights to the Software or derivative works that are broader than those provided by this License. For example, you may not distribute modifications of the Software under terms that would permit commercial use, or under terms that purport to require the Software or derivative works to be sublicensed to others.
+
+You agree:
+
+Not remove any copyright or other notices from the Software.
+
+That if you distribute the Software in source or object form, you will include a verbatim copy of this license.
+
+That if you distribute derivative works of the Software in source code form you do so only under a license that includes all of the provisions of this License, and if you distribute derivative works of the Software solely in object form you do so only under a license that complies with this License.
+
+That if you have modified the Software or created derivative works, and distribute such modifications or derivative works, you will cause the modified files to carry prominent notices so that recipients know that they are not receiving the original Software. Such notices must state: (i) that you have changed the Software; and (ii) the date of any changes.
+
+THAT THIS PRODUCT IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS PRODUCT, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. YOU MUST PASS THIS LIMITATION OF LIABILITY ON WHENEVER YOU DISTRIBUTE THE SOFTWARE OR DERIVATIVE WORKS.
+
+That if you sue anyone over patents that you think may apply to the Software or anyone's use of the Software, your license to the Software ends automatically.
+
+That your rights under the License end automatically if you breach it in any way.
+
+UCLA reserves all rights not expressly granted to you in this license.
\ No newline at end of file
diff --git a/README.md b/README.md
new file mode 100644
index 00000000..73ca955c
--- /dev/null
+++ b/README.md
@@ -0,0 +1,215 @@
+# X Inertial-aided Visual Odometry
+
+I started writing this software as a hobby project in winter 2018. The motivation behind this is in part that I want to have a (hopefully) minimal implementation of EKF-based Visual-Inertial Odometry and to play with some new C++ language features (C++ 14/17) and coding techniques (templates, design patterns, etc.). The project was paused for months due to job hunting, paper submission, defense and vacation, etc. Fortunately, I was able to spend more time on it before I leave UCLA.
+
+This is still work in progress, and under no circumstances, it can be considered as done. However, with readability and maintainability in mind when writing this piece of software, I believe it still has some value for people who want to dive into the world of VIO. Besides, the performance of the system in terms of ATE (Absolute Trajectory Error) and RPE (Relative Pose Error) is comparable to other open-source VIO systems (see the performance section).
+
+
+A detailed technical report of the implementation will follow. Stay tuned.
+
+Have fun!
+
+## Install
+
+To install the state estiamtor and dependencies, execute the `build.sh` script in the root directory of the project.
+
+## Usage
+
+The current implementation supports two executation modes: Either run on a folder of image sequences and a text file of inertial measurements (like what provided by the EuRoC and TUM-VI datasets), or a rosbag (TUM-VI also provides rosbags).
+
+### Dataset
+
+Assume the environment variable `$TUMVIROOT` has been set to the root directory of your [TUMVI](https://vision.in.tum.de/data/datasets/visual-inertial-dataset) dataset. For example,
+
+```
+export TUMVIROOT=/home/Data/tumvi/exported/euroc/512_16
+```
+
+where on my machine `/home/Data/tumvi/exported/euroc/512_16` hosts folders of data, such as `dataset-room1_512_16`, `dataset-corridor1_512_16`, etc.
+
+### Run
+
+In the project root directory, you can run the estimator with configuration specified by the `-cfg cfg/vio.json` option on images captured by camera 0 (`-cam_id 0`) of 6-th room sequence (`-seq room6`) of the TUMVI dataset (`-dataset tumvi`) which resides in `$TUMVIROOT` (option `-root $TUMVIROOT`) as follows:
+
+```
+bin/st_vio -cfg cfg/vio.json -root $TUMVIROOT -dataset tumvi -seq room6 -cam_id 0 -out out_state
+```
+
+where estimated states are saved to the output file `out_state`.
+
+For detailed usage of the application, see the flags defined at the begining of the application source file `src/app/singlethread_vio.cpp`.
+
+
+### Evaluation
+
+
+
+We provide a python script `scripts/run_and_eval_pyxivo.py` to run the estimator on a specified TUM-VI sequence and benchmark the performance in terms ATE (Absolute Trajectory Error) and RPE (Relative Pose Error). To use it, execute the following in the project root directory:
+
+```
+python scripts/run_and_eval_pyxivo.py -root $TUMVIROOT -seq room6 -stdout -out_dir tmp -use_viewer
+```
+the `-seq` and `-root` options are the same as explained above. If the `-stdout` option is on, the script will print out the benchmarked performance to the terminal; `-out_dir` specifies the directory to save state estimates; `-use_viewer` option will turn on a 3D visualization. For detailed usage about the script, see the options defined at the begining of the script.
+
+### Log to file for debugging
+
+We use [glog](https://github.com/google/glog) for system logging. If you want to log debug information to a file for inspection later,
+
+```
+GLOG_log_dir=log GLOG_v=0 bin/st_vio -cfg cfg/vio.json -root $TUMVIROOT
+```
+
+Note, the directory `log`, which is where the logs are kept, should be created first (simply `mkdir log`).
+
+By default, log is suppressed by setting `add_definitions(-DGOOGLE_STRIP_LOG=1)` in `src/CMakeLists.txt`. To enable log, simply comment out that line and re-build.
+
+For more details on how to use glog, see [the tutorial here](http://rpg.ifi.uzh.ch/docs/glog.html).
+
+
+
+## Rosbag mode
+
+### Build
+
+By default the ROS wrapper of the system will not be built, to build the ROS support, you need to turn on the `BUILD_ROSNODE` option when generating the makefile: In `build` directory of the project root, do the following:
+
+```
+cmake .. -DBUILD_ROSNODE=ON
+```
+
+followed by `make` to build with ROS support.
+
+### Run
+
+1. In the project root directory, `source build/devel/setup.zsh`. If another shell, such as bash/sh, is used, please source the corresponding shell script (`setup.bash`/`setup.sh`) respectively.
+2. `roslaunch node/launch/xivo.launch` to launch the ros node, which subscribes to `/imu0` and `/cam0/image_raw` topics.
+3. In antoher terminal, playback rosbags from the TUM-VI dataset, e.g., `rosbag play PATH/TO/YOUR/ROSBAG` .
+
+
+
+
+## Python binding
+
+A simple python binding is provided by wrapping some public interfaces of the estimator via [`pybind11`](https://github.com/pybind/pybind11). Check out `pybind11/pyxivo.cpp` for the available interfaces in python. With pybind11, it is relatively easy if you want to expose more interfaces of the C++ implementation to python.
+
+An example of using the python binding is available in `scripts/pyxivo.py`, which demonstrates estimator creation, data loading, and visualization in python.
+
+To run the demo, simply execute:
+
+```
+python scripts/pyxivo.py -root $TUMVIROOT -seq room6 -cam_id 0
+```
+
+in the project root directory. The command line options are more or less same as the C++ executable. For detailed usage, you can look at the options defined at the begining of the script `scripts/pyxivo.py`. Note you might need to install some python dependencies:
+
+```
+pip install -r requirements.txt
+```
+
+executed in the project root directory.
+
+## Performance
+
+The benchmark performance of this software on TUM-VI dataset is comparable to other open-source VIO systems (slightly worse than optimization-based OKVIS and VINS-Mono and on par with EKF-based ROVIO). Also, our system runs at more than 100 Hz on a desktop PC with a Core i7 6th gen CPU at very low CPU consumption rate. The runtime can be further improved by utilizing CPU cache and memory better. The following table shows the performance on 6 indoor sequences where ground-truth poses are available. The numbers for OKVIS, VINS-Mono and ROVIO are taken from the TUM-VI benchmark paper. Ours XIVO is obtained by using the aforementioned evaluation script.
+
+
+| Sequence | length | OKVIS | VINS-Mono | ROVIO | Ours-XIVO |
+|:--- | :--- | :---: | :---: | :---: | :---: |
+|room1 | 156m | **0.06m** | 0.07m | 0.16m | 0.22m |
+|room2 | 142m | 0.11m | **0.07m** | 0.33m | 0.08m |
+|room3 | 135m | **0.7m** | 0.11m | 0.15m | 0.11m |
+|room4 | 68m | **0.03m** | 0.04m | 0.09m | 0.08m |
+|room5 | 131m | **0.07m** | 0.20m | 0.12m | 0.11m |
+|room6 | 67m | **0.04m** | 0.08m | 0.05m | 0.12m |
+
+*Table 1. RMSE ATE* in meters. OKVIS and VINS-Mono are optimization-based, whereas ROVIO and Ours-XIVO are EKF-based.
+
+
+| Sequence | OKVIS | VINS-Mono | ROVIO | Ours-XIVO |
+|:--- | :---: | :---: | :---: | :---: |
+|room1 | **0.013**m/**0.43**o | 0.015m/0.44o | 0.029m/0.53o | 0.031m/0.59o |
+|room2 | **0.015**m/**0.62**o | 0.017m/0.63o | 0.030m/0.67o | 0.023m/0.75o |
+|room3 | **0.012**m/0.64o | 0.023m/**0.63**o | 0.027m/0.66o | 0.027m/0.73o |
+|room4 | **0.012**m/0.57o | 0.015m/**0.41**o | 0.022m/0.61o | 0.023m/0.62o |
+|room5 | **0.012**m/**0.47**o | 0.026m/**0.47**o | 0.031m/0.60o | 0.023m/0.65o |
+|room6| **0.012**m/0.49o | 0.014m/**0.44**o | 0.019m/0.50o | 0.031m/0.53o |
+
+*Table 2. RMSE RPE* in meters (translation) and degrees (rotation).
+
+
+---
+
+## Reference
+
+If you find this software useful and use it in your work, please cite:
+
+```
+@misc{feiS19xivo,
+ author = {Xiaohan Fei, and Stefano Soatto},
+ title = {XIVO: X Inertial-aided Visual Odometry},
+ howpublished = "\url{https://github.com/feixh/xivo}",
+}
+```
+
+
\ No newline at end of file
diff --git a/build.sh b/build.sh
new file mode 100755
index 00000000..ba6c064e
--- /dev/null
+++ b/build.sh
@@ -0,0 +1,58 @@
+#!/bin/sh
+PROJECT_DIR=$(pwd)
+echo $PROJECT_DIR
+
+cd $PROJECT_DIR/thirdparty/googletest
+mkdir build
+cd build
+cmake .. -DCMAKE_INSTALL_PREFIX=..
+make install -j
+
+cd $PROJECT_DIR/thirdparty/gflags
+mkdir build
+cd build
+cmake .. -DCMAKE_INSTALL_PREFIX=..
+make install -j
+
+cd $PROJECT_DIR/thirdparty/glog
+mkdir build
+cd build
+cmake .. -DCMAKE_INSTALL_PREFIX=..
+make install -j
+
+cd $PROJECT_DIR/thirdparty/eigen3
+mkdir build
+cd build
+cmake .. -DCMAKE_INSTALL_PREFIX=..
+make install -j
+
+cd $PROJECT_DIR/thirdparty/Sophus
+mkdir build
+cd build
+cmake .. -DCMAKE_INSTALL_PREFIX=..
+make install -j
+
+cd $PROJECT_DIR/thirdparty/Pangolin
+mkdir build
+cd build
+cmake .. -DCMAKE_INSTALL_PREFIX=..
+make install -j
+
+cd $PROJECT_DIR/thirdparty/jsoncpp
+mkdir build
+cd build
+cmake .. -DCMAKE_INSTALL_PREFIX=.. -DBUILD_SHARED_LIBS=TRUE
+make install -j
+
+# to build gperftools, need to install autoconf and libtool first
+# sudo apt-get install autoconf libtool
+cd $PROJECT_DIR/thirdparty/gperftools
+./autogen.sh
+./configure --prefix=$PROJECT_DIR/thirdparty/gperftools
+make install
+
+
+mkdir ${PROJECT_DIR}/build
+cd ${PROJECT_DIR}/build
+cmake ..
+make -j
diff --git a/cfg/atan.json b/cfg/atan.json
new file mode 100644
index 00000000..c094d8b5
--- /dev/null
+++ b/cfg/atan.json
@@ -0,0 +1,10 @@
+{
+"model": "atan",
+"rows": 320,
+"cols": 480,
+"fx": 0.425881,
+"fy": 0.639873,
+"cx": 0.504153,
+"cy": 0.525719,
+"w": 0.857591
+}
diff --git a/cfg/equidistant.json b/cfg/equidistant.json
new file mode 100644
index 00000000..7d8ed7a6
--- /dev/null
+++ b/cfg/equidistant.json
@@ -0,0 +1,13 @@
+{
+"model": "equidistant",
+"rows": 512,
+"cols": 512,
+"fx": 190.97847715128717,
+"fy": 190.9733070521226,
+"cx": 254.93170605935475,
+"cy": 256.8974428996504,
+"k0123": [0.0034823894022493434, 0.0007150348452162257, -0.0020532361418706202, 0.00020293673591811182],
+"max_iter": 15,
+"comment": "512-cam0"
+}
+
diff --git a/cfg/estimator.json b/cfg/estimator.json
new file mode 100644
index 00000000..1bca2572
--- /dev/null
+++ b/cfg/estimator.json
@@ -0,0 +1,271 @@
+{
+ // verbose
+ "simulation": false,
+ "print_timing": false,
+ "use_canvas": true,
+ "async_run": false, // turn this off in benchmarking
+
+ // algorithmic-level knobs
+ "integration_method": "PrinceDormand", // "PrinceDormand", "RK4", //, Fehlberg
+ "use_OOS": false, // update with Out-Of-State features
+ "use_depth_opt": true, // depth optimization
+ "use_MH_gating": true,
+ "use_1pt_RANSAC": false,
+ "use_compression": true, // measurement compression
+ "triangulate_pre_subfilter": true,
+ "compression_trigger_ratio": 1.5,
+ "max_group_lifetime": 30,
+
+ "PrinceDormand": {
+ "control_stepsize": false,
+ "tolerance": 1e-15,
+ "attempts": 12,
+ "min_scale_factor": 0.125,
+ "max_scale_factor": 4.0,
+ "stepsize": 0.002
+ },
+
+ "RK4": {
+ "stepsize": 0.002
+ },
+
+ // memory
+ "memory": {
+ "max_features": 256,
+ "max_groups": 256 // max groups should increase as allowed group lifetime increases
+ },
+
+
+ // gravity constant
+ "gravity": [0, 0, -9.8],
+ // Initial State
+ "X" : {
+ "W" : [0, 0, 0],
+ "T" : [0, 0, 0],
+ "V" : [0, 0, 0],
+ "bg" : [0, 0, 0],
+ "ba" : [0, 0, 0],
+ "Wbc" : [[-0.99952504, 0.00750192, -0.02989013],
+ [0.02961534, -0.03439736, -0.99896935],
+ [-0.00852233, -0.99938008, 0.03415885]],
+ // Wbc in tangent space: [-0.0418, -2.172, 2.248]
+ "Tbc": [0.04557484, -0.0711618, -0.04468125],
+ "Wg" : [0, 0, 0],
+ "td" : 0
+ },
+
+ "P" : {
+ "W" : 0.0001,
+ "T" : 0.001,
+ "V" : 0.5,
+ "bg" : 0.0001,
+ "ba" : 0.001,
+ "Wbc" : 0.0001,
+ "Tbc" : 0.001,
+ "Wg" : 0.01,
+ "td" : 1e-5, // 1ms
+ "Cg" : 1e-5,
+ "Ca" : 1e-5,
+ "FC" : 50,
+ "distortion": 1e-3
+ },
+
+ "Qmodel" : {
+ "W" : 0.000,
+ "T" : 0.0,
+ "V" : 0.0,
+ "wb" : 0.0000,
+ "ab" : 0.000,
+ "Wbc" : 0.000,
+ "Tbc" : 0.0,
+ "Wg" : 0.000
+ },
+
+ // // exact densities provided by TUM-VI dataset
+ // // https://vision.in.tum.de/data/datasets/visual-inertial-dataset
+ // "Qimu": {
+ // "gyro": 8.0e-5,
+ // "gyro_bias": 2.2e-6,
+ // "accel":1.4e-3,
+ // "accel_bias": 8.6e-5
+ // },
+
+ // // inflated by sqrt(3) times
+ // "Qimu": {
+ // "gyro": 14.0e-5,
+ // "gyro_bias": 3.8e-6,
+ // "accel": 2.4e-3,
+ // "accel_bias": 14.9e-5
+ // },
+
+ // // doubled
+ // "Qimu": {
+ // "gyro": 16.0e-5,
+ // "gyro_bias": 4.4e-6,
+ // "accel":2.8e-3,
+ // "accel_bias": 17.2e-5
+ // },
+
+ // inflated by 3 times
+ "Qimu": {
+ "gyro": 24.0e-5,
+ "gyro_bias": 6.6e-6,
+ "accel": 4.2e-3,
+ "accel_bias": 25.8e-5
+ },
+
+
+ // initial std on feature state
+ "initial_z": 2.5, // meter
+ "initial_std_x": 3.5, // pixel
+ "initial_std_y": 3.5, // pixel
+ "initial_std_z": 1.0, // meter
+
+ // std of visuale measurement, in pixels
+ "visual_meas_std": 1.5,
+ "outlier_thresh": 1.1,
+ "oos_meas_std": 3.5,
+ "max_depth": 5.0,
+ "min_depth": 0.05,
+
+
+ // depth sub-filter setting
+ "subfilter": {
+ "visual_meas_std": 3.5,
+ "ready_steps": 2,
+ "MH_thresh": 8.991
+ },
+
+ // pre-subfilter triangulation options
+ "triangulation": {
+ "method": 1,
+ "zmin": 0.05,
+ "zmax": 5.0
+ },
+
+ "depth_opt": {
+ "two_view": false,
+ "max_iters": 5,
+ "eps": 1e-3,
+ "damping": 1e-3,
+ "max_res_norm": 2.0 // maximal norm of per observation residuals
+ },
+
+ "feature_P0_damping": 10.0, // 10.0 seems most appropriate
+
+ "imu_calib": {
+ "Cas": [1, 1, 1],
+ "Car": [[1, 0, 0],
+ [0, 1, 0],
+ [0, 0, 1]],
+ "Cgs": [1, 1, 1],
+ "Cgr": [[1, 0, 0],
+ [0, 1, 0],
+ [0, 0, 1]]
+ },
+ "gravity_init_counter": 20,
+
+ "camera_cfg": {
+ "model": "equidistant",
+ "rows": 512,
+ "cols": 512,
+ "fx": 190.97847715128717,
+ "fy": 190.9733070521226,
+ "cx": 254.93170605935475,
+ "cy": 256.8974428996504,
+ "k0123": [0.0034823894022493434, 0.0007150348452162257, -0.0020532361418706202, 0.00020293673591811182],
+ "max_iter": 15,
+ "comment": "512-cam0"
+ },
+
+ "min_inliers": 5, // minimum number of inlier measurements
+
+ "MH_thresh": 5.991, // 8.991
+ "MH_adjust_factor": 1.15,
+
+ "1pt_RANSAC_thresh": 1.5,
+ "1pt_RANSAC_prob": 0.95,
+ "1pt_RANSAC_Chi2": 5.89,
+
+ "tracker_cfg": {
+ "use_prediction": true,
+ "mask_size": 15,
+ "margin": 8, // image boundary to mask out
+ "num_features_min": 90,
+ "num_features_max": 120,
+ "max_pixel_displacement": 64,
+ "normalize": false,
+
+ "KLT": {
+ "win_size": 15,
+ "max_level": 5,
+ "max_iter": 30,
+ "eps": 0.01
+ },
+
+ "extract_descriptor": false,
+ "descriptor_distance_thresh": -1, // -1 to disable descriptor check
+ "default_descriptor": "BRIEF",
+
+ // "detector": "BRISK",
+ "detector": "FAST",
+ // "detector": "ORB",
+ // "detector": "AGAST",
+ // "detector": "GFTT",
+
+ "FAST": {
+ // https://docs.opencv.org/3.4/df/d74/classcv_1_1FastFeatureDetector.html
+ "threshold": 20,
+ "nonmaxSuppression": true
+ },
+
+ "BRISK": {
+ // https://docs.opencv.org/3.4/de/dbf/classcv_1_1BRISK.html
+ "thresh": 25,
+ "octaves": 3,
+ "patternScale": 1.0
+ },
+
+ "ORB": {
+ // https://docs.opencv.org/3.3.0/db/d95/classcv_1_1ORB.html
+ "nfeatures": 500,
+ "scaleFactor": 1.2,
+ "nlevels": 4,
+ "edgeThreshold": 31,
+ "firstLevel": 0,
+ "WTA_K": 2,
+ "patchSize": 31,
+ "fastThreshold": 20
+ },
+
+ "AGAST": {
+ // https://docs.opencv.org/3.4/d7/d19/classcv_1_1AgastFeatureDetector.html
+ "threshold": 10,
+ "nonmaxSuppression": true
+ },
+
+ "GFTT": {
+ // Good Feature To Track
+ // https://docs.opencv.org/3.3.0/df/d21/classcv_1_1GFTTDetector.html
+ "maxCorners": 1000,
+ "qualityLevel": 0.01,
+ "minDistance": 1.0,
+ "blockSize": 3,
+ "useHarrisDetector": false,
+ "k": 0.04
+ },
+
+ "BRIEF": {
+ "bytes": 64,
+ "use_orientation": false
+ },
+
+ "FREAK": {
+ "orientationNormalized": true,
+ "scaleNormalized": true,
+ "patternScale": 22.0,
+ "nOctaves": 4
+ }
+ }
+
+}
diff --git a/cfg/estimator_latex.json b/cfg/estimator_latex.json
new file mode 100644
index 00000000..e2c56900
--- /dev/null
+++ b/cfg/estimator_latex.json
@@ -0,0 +1,122 @@
+{
+ // This particular configuration is used to generate the results in
+ // the latex manuscript.
+ "simulation": false,
+ "update_method": "ekf",
+ "gravity": [0, 0, -9.8],
+ // Initial State
+ "X" : {
+ "W" : [0, 0, 0],
+ "T" : [0, 0, 0],
+ "V" : [0, 0, 0],
+ "bg" : [0, 0, 0],
+ "ba" : [0, 0, 0],
+ "Wbc" : [[-0.99952504, 0.00750192, -0.02989013],
+ [0.02961534, -0.03439736, -0.99896935],
+ [-0.00852233, -0.99938008, 0.03415885]],
+ "Tbc": [0.04557484, -0.0711618, -0.04468125],
+ "Wg" : [0, 0]
+ },
+
+ "P" : {
+ "W" : 0.0001,
+ "T" : 0.001,
+ "V" : 0.5,
+ "bg" : 0.0001,
+ "ba" : 0.001,
+ "Wbc" : 0.0001,
+ "Tbc" : 0.001,
+ "Wg" : 0.01
+ },
+
+ "Qmodel" : {
+ "W" : 0.000,
+ "T" : 0.0,
+ "V" : 0.0,
+ "wb" : 0.0000,
+ "ab" : 0.000,
+ "Wbc" : 0.000,
+ "Tbc" : 0.0,
+ "Wg" : 0.000
+ },
+
+ "Qimu": {
+ "gyro": 1e-3,
+ "gyro_bias": 1e-4,
+ "accel":1e-2,
+ "accel_bias": 1e-1
+ },
+
+ // initial std on feature state
+ "initial_z": 2.5, // meter
+ "initial_std_x": 3.5, // pixel
+ "initial_std_y": 3.5, // pixel
+ "initial_std_z": 1.0, // meter
+
+ // std of visuale measurement, in pixels
+ "visual_meas_std": 1.5,
+ "subfilter_visual_meas_std": 3.5,
+ "max_depth": 5.0,
+ "min_depth": 0.05,
+ "max_depth_init_steps": 5,
+ "depth_opt_iters": 5,
+ "depth_opt_eps": 0.001,
+ "depth_opt_damping": 1e-3,
+ "inn_thresh": 64,
+ "MH_thresh": 6,
+
+ "imu_calib": {
+ "Cas": [1, 1, 1],
+ "Car": [[1, 0, 0],
+ [0, 1, 0],
+ [0, 0, 1]],
+ "Cgs": [1, 1, 1],
+ "Cgr": [[1, 0, 0],
+ [0, 1, 0],
+ [0, 0, 1]]
+ },
+ "imu_init_counter": 20,
+ "RK4": true,
+
+ "camera_cfg": {
+ "model": "equidistant",
+ "rows": 512,
+ "cols": 512,
+ "fx": 190.97847715128717,
+ "fy": 190.9733070521226,
+ "cx": 254.93170605935475,
+ "cy": 256.8974428996504,
+ "k0123": [0.0034823894022493434, 0.0007150348452162257, -0.0020532361418706202, 0.00020293673591811182],
+ "max_iter": 15,
+ "comment": "512-cam0"
+ },
+
+ "tracker_cfg": {
+ "mask_size": 15,
+ "num_features_min": 90,
+ "num_features_max": 120,
+ "normalize": true,
+
+ "OpticalFlow": {
+ "win_size": 15,
+ "max_level": 5,
+ "max_iter": 15,
+ "eps": 1e-4,
+ "threshold": 20
+ },
+
+ // somehow, only BRISK and FAST work well with the generic FeatureDetector interface (Version 2.4.9)
+ "DetectorType": "FAST",
+
+ "FAST": {
+ "threshold": 20,
+ "nonmaxSuppression": true
+ },
+
+ "GridAdapter": {
+ "grid_rows": 4,
+ "grid_cols": 4
+ }
+ }
+
+}
diff --git a/cfg/estimator_scenenet.json b/cfg/estimator_scenenet.json
new file mode 100644
index 00000000..5efe4954
--- /dev/null
+++ b/cfg/estimator_scenenet.json
@@ -0,0 +1,154 @@
+{
+ // verbose
+ "simulation": false,
+ "print_timer": false,
+
+ // algorithmic-level knobs
+ "integration_method": "PrinceDormand", // "PrinceDormand", "RK4", //, Fehlberg
+ "use_OOS": false, // update with Out-Of-State features
+ "use_depth_opt": true, // depth optimization
+ "use_MH_gating": true,
+ "use_1pt_RANSAC": false,
+ "use_compression": true, // measurement compression
+ "compression_trigger_ratio": 1.5,
+
+ "PrinceDormand": {
+ "control_stepsize": false,
+ "tolerance": 1e-15,
+ "attempts": 12,
+ "min_scale_factor": 0.125,
+ "max_scale_factor": 4.0,
+ "stepsize": 0.002
+ },
+
+ "RK4": {
+ "stepsize": 0.002
+ },
+
+ // memory
+ "max_features": 256,
+ "max_groups": 256, // max groups should increase as allowed group lifetime increases
+
+ "gravity": [0, 0, -9.8],
+ "timing": false,
+
+ // Initial State
+ "X" : {
+ "W" : [0, 0, 0],
+ "T" : [0, 0, 0],
+ "V" : [0, 0, 0],
+ "bg" : [0, 0, 0],
+ "ba" : [0, 0, 0],
+ "Wbc" : [[1, 0, 0],
+ [0, 1, 0],
+ [0, 0, 1]],
+ "Tbc": [0, 0, 0],
+ "Wg" : [0, 0]
+ },
+
+ "P" : {
+ "W" : 0.001,
+ "T" : 0.01,
+ "V" : 0.5,
+ "bg" : 0.000,
+ "ba" : 0.00,
+ "Wbc" : 0.00001,
+ "Tbc" : 0.0001,
+ "Wg" : 0.01
+ },
+
+ "Qmodel" : {
+ "W" : 0.000,
+ "T" : 0.0,
+ "V" : 0.0,
+ "wb" : 0.0000,
+ "ab" : 0.000,
+ "Wbc" : 0.000,
+ "Tbc" : 0.0,
+ "Wg" : 0.000
+ },
+
+ // // exact densities provided by TUM-VI dataset
+ // // https://vision.in.tum.de/data/datasets/visual-inertial-dataset
+ "Qimu": {
+ "gyro": 8.0e-5,
+ "gyro_bias": 2.2e-6,
+ "accel":1.4e-3,
+ "accel_bias": 8.6e-5
+ },
+
+ // initial std on feature state
+ "initial_z": 5.5, // meter
+ "initial_std_x": 0.5, // pixel
+ "initial_std_y": 0.5, // pixel
+ "initial_std_z": 2.0, // meter
+
+ // std of visuale measurement, in pixels
+ "visual_meas_std": 1.5,
+ "subfilter_visual_meas_std": 1.5,
+ "max_depth": 10.0,
+ "min_depth": 0.05,
+ "max_depth_init_steps": 5,
+
+ "use_depth_opt": true,
+ "depth_opt_iters": 5,
+ "depth_opt_eps": 0.001,
+ "depth_opt_damping": 1e-3,
+
+ "feature_P0_damping": 10.0,
+ "inn_thresh": 64,
+ "MH_thresh": 8.991,
+
+ "imu_calib": {
+ "Cas": [1, 1, 1],
+ "Car": [[1, 0, 0],
+ [0, 1, 0],
+ [0, 0, 1]],
+ "Cgs": [1, 1, 1],
+ "Cgr": [[1, 0, 0],
+ [0, 1, 0],
+ [0, 0, 1]]
+ },
+ "imu_init_counter": 5,
+
+ "camera_cfg": {
+ "model": "pinhole",
+ "rows": 240,
+ "cols": 320,
+ "fx": 277.12812921,
+ "fy": 289.70562748,
+ "cx": 160,
+ "cy": 120,
+ "comment": "pinhole"
+ },
+
+ "tracker_cfg": {
+ "use_prediction": true,
+ "mask_size": 15,
+ "num_features_min": 60,
+ "num_features_max": 90,
+ "normalize": false,
+
+ "OpticalFlow": {
+ "win_size": 7,
+ "max_level": 5,
+ "max_iter": 15,
+ "eps": 1e-4,
+ "threshold": 20
+ },
+
+ // somehow, only BRISK and FAST work well with the generic FeatureDetector interface (Version 2.4.9)
+ "DetectorType": "FAST",
+
+ "FAST": {
+ "threshold": 20,
+ "nonmaxSuppression": true
+ },
+
+ "GridAdapter": {
+ "grid_rows": 4,
+ "grid_cols": 4
+ }
+ }
+
+}
diff --git a/cfg/estimator_sim.json b/cfg/estimator_sim.json
new file mode 100644
index 00000000..c4473710
--- /dev/null
+++ b/cfg/estimator_sim.json
@@ -0,0 +1,77 @@
+{
+ "simulation": true,
+ "gravity": [0, 0, -9.8],
+ // Initial State
+ "X" : {
+ "W" : [0, 0, 0],
+ "T" : [0, 0, 0],
+ "V" : [0, 0, 0],
+ "bg" : [0, 0, 0],
+ "ba" : [0, 0, 0],
+ "Wbc" : [[-0.99952504, 0.00750192, -0.02989013],
+ [0.02961534, -0.03439736, -0.99896935],
+ [-0.00852233, -0.99938008, 0.03415885]],
+ "Tbc": [0.04557484, -0.0711618, -0.04468125],
+ "Wg" : [0, 0]
+ },
+
+ "P" : {
+ "W" : 0.0000,
+ "T" : 0.000,
+ "V" : 0.0,
+ "bg" : 0.005,
+ "ba" : 0.05,
+ "Wbc" : 0.005,
+ "Tbc" : 0.05,
+ "Wg" : 0.0
+ },
+
+ "Qmodel" : {
+ "W" : 0.000,
+ "T" : 0.0,
+ "V" : 0.0,
+ "wb" : 0.0000,
+ "ab" : 0.000,
+ "Wbc" : 0.000,
+ "Tbc" : 0.0,
+ "Wg" : 0.000
+ },
+
+ "Qimu": {
+ "gyro": 3e-3,
+ "accel":3e-2
+ },
+
+ // initial std on feature state
+ "initial_z": 2.0, // meter
+ "initial_std_x": 0.1, // pixel
+ "initial_std_y": 0.1, // pixel
+ "initial_std_z": 1.0, // meter
+
+ // std of visuale measurement, in pixels
+ "visual_meas_std": 1.5,
+ "subfilter_visual_meas_std": 1.5,
+ "max_depth": 5.0,
+ "min_depth": 0.05,
+ "max_depth_init_steps": 2,
+ "depth_opt_iters": 10,
+ "depth_opt_eps": 0.001,
+ "depth_opt_damping": 1e-4,
+ "inn_thresh": 20,
+
+
+ "imu_calib": {
+ "Cas": [1, 1, 1],
+ "Car": [[1, 0, 0],
+ [0, 1, 0],
+ [0, 0, 1]],
+ "Cgs": [1, 1, 1],
+ "Cgr": [[1, 0, 0],
+ [0, 1, 0],
+ [0, 0, 1]]
+ },
+ "imu_init_counter": 10,
+
+ "camera_cfg": "../cfg/equidistant.json", // atan
+ "tracker_cfg": "../cfg/tracker.json"
+}
diff --git a/cfg/lyapunov_baseline.json b/cfg/lyapunov_baseline.json
new file mode 100644
index 00000000..da685893
--- /dev/null
+++ b/cfg/lyapunov_baseline.json
@@ -0,0 +1,156 @@
+{
+ // verbose
+ "simulation": false,
+ "print_timer": false,
+
+ // algorithmic-level knobs
+ "update_method": "ekf",
+ "integration_method": "RK4", // RK4, ForwardEuler
+ "RK4_stepsize": 0.002,
+ "use_OOS": false, // update with Out-Of-State features
+ "use_depth_opt": true, // depth optimization
+ "use_MH_gating": true,
+ "use_1pt_RANSAC": false,
+ "use_compression": true, // measurement compression
+ "compression_trigger_ratio": 1.5,
+ "use_discrete_covariance_propagation": true,
+
+ // memory
+ "max_features": 256,
+ "max_groups": 256, // max groups should increase as allowed group lifetime increases
+ "max_group_lifetime": 120,
+
+ // gravity constant
+ "gravity": [0, 0, -9.8],
+ // Initial State
+ "X" : {
+ "W" : [0, 0, 0],
+ "T" : [0, 0, 0],
+ "V" : [0, 0, 0],
+ "bg" : [0, 0, 0],
+ "ba" : [0, 0, 0],
+ "Wbc" : [[-0.99952504, 0.00750192, -0.02989013],
+ [0.02961534, -0.03439736, -0.99896935],
+ [-0.00852233, -0.99938008, 0.03415885]],
+ "Tbc": [0.04557484, -0.0711618, -0.04468125],
+ "Wg" : [0, 0, 0]
+ },
+
+ "P" : {
+ "W" : 0.0001,
+ "T" : 0.001,
+ "V" : 0.5,
+ "bg" : 0.0001,
+ "ba" : 0.001,
+ "Wbc" : 0.00001,
+ "Tbc" : 0.001,
+ "Wg" : 0.01
+ },
+
+ "Qmodel" : {
+ "W" : 0.000,
+ "T" : 0.0,
+ "V" : 0.0,
+ "wb" : 0.0000,
+ "ab" : 0.000,
+ "Wbc" : 0.000,
+ "Tbc" : 0.0,
+ "Wg" : 0.000
+ },
+
+ // original block without numerical integration of Lyapunov
+ "Qimu": {
+ "gyro": 0.5e-3,
+ "gyro_bias": 1e-4,
+ "accel":1e-2,
+ "accel_bias": 1e-1
+ },
+
+ // initial std on feature state
+ "initial_z": 2.5, // meter
+ "initial_std_x": 0.5, // pixel
+ "initial_std_y": 0.5, // pixel
+ "initial_std_z": 2.0, // meter
+
+ // std of visuale measurement, in pixels
+ "visual_meas_std": 1.5,
+ "oos_meas_std": 3.5,
+ "subfilter_visual_meas_std": 3.5,
+ "max_depth": 5.0,
+ "min_depth": 0.05,
+
+
+ // depth sub-filter setting
+ "subfilter_ready_steps": 5,
+ "subfilter_MH_thresh": 8.991,
+
+ "depth_opt_iters": 5,
+ "depth_opt_eps": 0.001,
+ "depth_opt_damping": 1e-3,
+
+ "feature_P0_damping": 1000.0,
+
+ "imu_calib": {
+ "Cas": [1, 1, 1],
+ "Car": [[1, 0, 0],
+ [0, 1, 0],
+ [0, 0, 1]],
+ "Cgs": [1, 1, 1],
+ "Cgr": [[1, 0, 0],
+ [0, 1, 0],
+ [0, 0, 1]]
+ },
+ "imu_init_counter": 20,
+
+ "camera_cfg": {
+ "model": "equidistant",
+ "rows": 512,
+ "cols": 512,
+ "fx": 190.97847715128717,
+ "fy": 190.9733070521226,
+ "cx": 254.93170605935475,
+ "cy": 256.8974428996504,
+ "k0123": [0.0034823894022493434, 0.0007150348452162257, -0.0020532361418706202, 0.00020293673591811182],
+ "max_iter": 25,
+ "comment": "512-cam0"
+ },
+
+ "min_inliers": 5, // minimum number of inlier measurements
+
+ "MH_thresh": 8.991, // 8.991
+ "MH_adjust_factor": 1.5,
+
+ "1pt_RANSAC_thresh": 1.5,
+ "1pt_RANSAC_prob": 0.95,
+ "1pt_RANSAC_Chi2": 5.89,
+
+ "tracker_cfg": {
+ "use_prediction": true,
+ "mask_size": 15,
+ "num_features_min": 60,
+ "num_features_max": 90,
+ "normalize": true,
+
+ "OpticalFlow": {
+ "win_size": 15,
+ "max_level": 5,
+ "max_iter": 15,
+ "eps": 1e-4,
+ "threshold": 20
+ },
+
+ // somehow, only BRISK and FAST work well with the generic FeatureDetector interface (Version 2.4.9)
+ "DetectorType": "FAST",
+
+ "FAST": {
+ "threshold": 20,
+ "nonmaxSuppression": true
+ },
+
+ "GridAdapter": {
+ "grid_rows": 4,
+ "grid_cols": 4
+ }
+ }
+
+}
diff --git a/cfg/pointgrey_xsens.json b/cfg/pointgrey_xsens.json
new file mode 100644
index 00000000..5131dc03
--- /dev/null
+++ b/cfg/pointgrey_xsens.json
@@ -0,0 +1,228 @@
+{
+ // verbose
+ "simulation": false,
+ "print_timer": false,
+ "use_canvas": true,
+
+ // algorithmic-level knobs
+ "integration_method": "PrinceDormand", // "PrinceDormand", "RK4", //, Fehlberg
+ "use_OOS": false, // update with Out-Of-State features
+ "use_depth_opt": true, // depth optimization
+ "use_MH_gating": true,
+ "use_1pt_RANSAC": false,
+ "use_compression": true, // measurement compression
+ "compression_trigger_ratio": 1.5,
+ "max_group_lifetime": 6,
+
+ "PrinceDormand": {
+ "control_stepsize": false,
+ "tolerance": 1e-15,
+ "attempts": 12,
+ "min_scale_factor": 0.125,
+ "max_scale_factor": 4.0,
+ "stepsize": 0.002
+ },
+
+ "RK4": {
+ "stepsize": 0.002
+ },
+
+ // memory
+ "memory": {
+ "max_features": 256,
+ "max_groups": 256 // max groups should increase as allowed group lifetime increases
+ },
+
+
+ // gravity constant
+ "gravity": [0, 0, -9.8],
+ // Initial State
+ "X" : {
+ "W" : [0, 0, 0],
+ "T" : [0, 0, 0],
+ "V" : [0, 0, 0],
+
+ "bg" : [-0.05163, 0.0108186, 0.0188156],
+ "ba" : [-0.00342173, -0.00861755, 0.0218768],
+ "Wbc" : [[4.01e-02, -4.24e-02, 9.98e-01],
+ [-9.99e-01, -2.05e-03, 4.00e-02],
+ [3.50e-04, -9.99e-01, -4.25e-02]],
+ "Tbc": [0.0737, -0.00977, -0.1088],
+ "Wg" : [0, 0, 0]
+ },
+
+ "P" : {
+ "W" : 0.0001,
+ "T" : 0.001,
+ "V" : 0.5,
+ "bg" : 0.001,
+ "ba" : 0.01,
+ "Wbc" : 0.001,
+ "Tbc" : 0.01,
+ "Wg" : 0.01
+ },
+
+ "Qmodel" : {
+ "W" : 0.000,
+ "T" : 0.0,
+ "V" : 0.0,
+ "wb" : 0.0000,
+ "ab" : 0.000,
+ "Wbc" : 0.000,
+ "Tbc" : 0.0,
+ "Wg" : 0.000
+ },
+
+ "Qimu": {
+ "gyro": 24.0e-4,
+ "gyro_bias": 6.6e-5,
+ "accel": 4.2e-2,
+ "accel_bias": 25.8e-4
+ },
+
+
+ // initial std on feature state
+ "initial_z": 3.5, // meter
+ "initial_std_x": 1.5, // pixel
+ "initial_std_y": 1.5, // pixel
+ "initial_std_z": 2.0, // meter
+
+ // std of visuale measurement, in pixels
+ "visual_meas_std": 3.5,
+ "subfilter_visual_meas_std": 3.5,
+ "oos_meas_std": 3.5,
+ "max_depth": 8.0,
+ "min_depth": 0.05,
+
+
+ // depth sub-filter setting
+ "subfilter": {
+ "ready_steps": 5,
+ "MH_thresh": 8.991
+ },
+
+ "depth_opt": {
+ "max_iters": 5,
+ "eps": 1e-5,
+ "damping_factor": 1e-3,
+ "adjust_factor": 5.0,
+ "max_res_norm": 5.0 // maximal norm of per observation residuals
+ },
+
+ "feature_P0_damping": 10.0,
+
+ "imu_calib": {
+ "Cas": [1.00056, 1.00062, 1.00136],
+ "Car": [[1, -0.000914275, -0.000414882],
+ [0, 1, -0.0000350183],
+ [0, 0, 1]],
+ "Cgs": [0.999823, 1.00412, 0.987355],
+ "Cgr": [[1, -0.00224194, 0.0120628],
+ [0.00306442, 1, -0.000998032],
+ [0.00285738, -0.0154485, 1]]
+ },
+ "imu_init_counter": 20,
+
+ "camera_cfg": {
+ "model": "atan",
+ "rows": 500,
+ "cols": 960,
+ "fx": 0.561859,
+ "fy": 1.081848,
+ "cx": 0.491896,
+ "cy": 0.5151548,
+ "w": 0.709402,
+ "comment": "intrinsics converted from relative to absolute values"
+ },
+
+ "min_inliers": 5, // minimum number of inlier measurements
+
+ "MH_thresh": 5.991, // 8.991
+ "MH_adjust_factor": 1.5,
+
+ "1pt_RANSAC_thresh": 1.5,
+ "1pt_RANSAC_prob": 0.95,
+ "1pt_RANSAC_Chi2": 5.89,
+
+ "tracker_cfg": {
+ "use_prediction": true,
+ "mask_size": 15,
+ "margin": 16, // image boundary to mask out
+ "num_features_min": 60,
+ "num_features_max": 90,
+ "max_pixel_displacement": 128,
+ "normalize": false,
+
+ "KLT": {
+ "win_size": 15,
+ "max_level": 5,
+ "max_iter": 15,
+ "eps": 1e-2
+ },
+
+ "extract_descriptor": false,
+ "descriptor_distance_thresh": -1, // -1 to disable descriptor check
+ "default_descriptor": "BRIEF",
+
+ // "detector": "BRISK",
+ "detector": "FAST",
+ // "detector": "ORB",
+ // "detector": "AGAST",
+ // "detector": "GFTT",
+
+ "FAST": {
+ // https://docs.opencv.org/3.4/df/d74/classcv_1_1FastFeatureDetector.html
+ "threshold": 5,
+ "nonmaxSuppression": true
+ },
+
+ "BRISK": {
+ // https://docs.opencv.org/3.4/de/dbf/classcv_1_1BRISK.html
+ "thresh": 25,
+ "octaves": 3,
+ "patternScale": 1.0
+ },
+
+ "ORB": {
+ // https://docs.opencv.org/3.3.0/db/d95/classcv_1_1ORB.html
+ "nfeatures": 500,
+ "scaleFactor": 1.2,
+ "nlevels": 4,
+ "edgeThreshold": 31,
+ "firstLevel": 0,
+ "WTA_K": 2,
+ "patchSize": 31,
+ "fastThreshold": 20
+ },
+
+ "AGAST": {
+ // https://docs.opencv.org/3.4/d7/d19/classcv_1_1AgastFeatureDetector.html
+ "threshold": 10,
+ "nonmaxSuppression": true
+ },
+
+ "GFTT": {
+ // Good Feature To Track
+ // https://docs.opencv.org/3.3.0/df/d21/classcv_1_1GFTTDetector.html
+ "maxCorners": 1000,
+ "qualityLevel": 0.01,
+ "minDistance": 1.0,
+ "blockSize": 3,
+ "useHarrisDetector": false,
+ "k": 0.04
+ },
+
+ "BRIEF": {
+ "bytes": 64,
+ "use_orientation": false
+ },
+
+ "FREAK": {
+ "orientationNormalized": true,
+ "scaleNormalized": true,
+ "patternScale": 22.0,
+ "nOctaves": 4
+ }
+ }
+
+}
diff --git a/cfg/pointgrey_xsens_viewer.json b/cfg/pointgrey_xsens_viewer.json
new file mode 100644
index 00000000..b1f63b58
--- /dev/null
+++ b/cfg/pointgrey_xsens_viewer.json
@@ -0,0 +1,28 @@
+{
+ "window": {
+ "height": 1000,
+ "width": 960
+ },
+
+ "viewport": {
+ "height": 500,
+ "width": 960,
+ "fx": 800,
+ "fy": 800,
+ "cx": 480,
+ "cy": 250,
+ "znear": 0.1,
+ "zfar": 10
+ },
+
+ "bg_color": {
+ "r": 0.0,
+ "g": 0.0,
+ "b": 0.0,
+ "a": 1.0
+ },
+ "grid_size": 20,
+ "draw_trace_as_dots": false
+
+}
+
diff --git a/cfg/simulation.json b/cfg/simulation.json
new file mode 100644
index 00000000..3d5d6d73
--- /dev/null
+++ b/cfg/simulation.json
@@ -0,0 +1,8 @@
+{
+ "simulator_cfg": "../cfg/simulator.json",
+ "estimator_cfg": "../cfg/estimator_sim.json",
+
+ "add_noise_to_init_state": true,
+ "enable_update": true,
+ "visualize": false
+}
diff --git a/cfg/simulator.json b/cfg/simulator.json
new file mode 100644
index 00000000..f16b2e74
--- /dev/null
+++ b/cfg/simulator.json
@@ -0,0 +1,60 @@
+{
+ // sensor setups
+ // total simulation time, in seconds
+ "total_time": 10,
+ "imu_hz": 200,
+ "camera_hz": 30,
+ "num_pts_to_track": 120,
+ "min_num_pts_to_track": 90,
+ "motion_type": 2,
+ "init_static_period": 1,
+
+ "fixed_seed": true,
+ "seed": 0,
+
+ // measurement noise
+ "gyro_noise_std": 1e-3,
+ "accel_noise_std": 1e-2,
+ "tracking_noise_std": 0.1, // in pixels
+ "max_track_lifetime": 10,
+
+ // initial standard deviation added to states
+ "init_noise_std": {
+ "W": 0.0,
+ "T": 0.0,
+ "V": 0.0,
+ "bg": 0.001,
+ "ba": 0.01,
+ "Wbc": 0.001,
+ "Tbc": 0.01,
+ "Wg": 0.0
+ },
+
+ "X" : {
+ "W" : [0, 0, 0],
+ "T" : [0, 0, 0],
+ "V" : [0, 0, 0],
+ "wb" : [0, 0, 0],
+ "ab" : [0, 0, 0],
+ "Wbc" : [3.1319, 0.00905, 0.00800],
+ "Tbc": [-0.0162997, -0.00375004, -0.0383651],
+ "Wg" : [0, 0],
+ "to" : 0.0
+ },
+
+ // camera latency
+ "delay_camera_time": 0.00,
+
+ // gravity
+ "gravity_magnitude": 9.8,
+
+ // camera calibration
+ // "camera_cfg": "../cfg/atan.json",
+ "camera_cfg": "../cfg/equidistant.json",
+
+ "z_near": 0.1,
+ "z_far": 5.0,
+ "visualize_tracks": false,
+ "track_drop_probability": 0.005,
+ "mask_size": 15.0
+}
diff --git a/cfg/tuning.json b/cfg/tuning.json
new file mode 100644
index 00000000..adeb3d6c
--- /dev/null
+++ b/cfg/tuning.json
@@ -0,0 +1,155 @@
+{
+ // verbose
+ "simulation": false,
+ "print_timer": false,
+
+ // algorithmic-level knobs
+ "update_method": "ekf",
+ "integration_method": "RK4", // RK4, ForwardEuler
+ "RK4_stepsize": -1, //0.002,
+ "use_OOS": false, // update with Out-Of-State features
+ "use_depth_opt": true, // depth optimization
+ "use_MH_gating": true,
+ "use_1pt_RANSAC": false,
+ "use_compression": true, // measurement compression
+ "compression_trigger_ratio": 1.5,
+
+ // memory
+ "max_features": 256,
+ "max_groups": 256, // max groups should increase as allowed group lifetime increases
+ "max_group_lifetime": 120,
+
+ // gravity constant
+ "gravity": [0, 0, -9.8],
+ // Initial State
+ "X" : {
+ "W" : [0, 0, 0],
+ "T" : [0, 0, 0],
+ "V" : [0, 0, 0],
+ "bg" : [0, 0, 0],
+ "ba" : [0, 0, 0],
+ "Wbc" : [[-0.99952504, 0.00750192, -0.02989013],
+ [0.02961534, -0.03439736, -0.99896935],
+ [-0.00852233, -0.99938008, 0.03415885]],
+ "Tbc": [0.04557484, -0.0711618, -0.04468125],
+ "Wg" : [0, 0, 0]
+ },
+
+ "P" : {
+ "W" : 0.0001,
+ "T" : 0.001,
+ "V" : 0.5,
+ "bg" : 0.001,
+ "ba" : 0.001,
+ "Wbc" : 0.00001,
+ "Tbc" : 0.001,
+ "Wg" : 0.01
+ },
+
+ "Qmodel" : {
+ "W" : 0.000,
+ "T" : 0.0,
+ "V" : 0.0,
+ "wb" : 0.0000,
+ "ab" : 0.000,
+ "Wbc" : 0.000,
+ "Tbc" : 0.0,
+ "Wg" : 0.000
+ },
+
+ // original block without numerical integration of Lyapunov
+ "Qimu": {
+ "gyro": 0.5e-3,
+ "gyro_bias": 1e-4,
+ "accel":1e-2,
+ "accel_bias": 1e-1
+ },
+
+ // initial std on feature state
+ "initial_z": 2.5, // meter
+ "initial_std_x": 0.5, // pixel
+ "initial_std_y": 0.5, // pixel
+ "initial_std_z": 2.0, // meter
+
+ // std of visuale measurement, in pixels
+ "visual_meas_std": 1.5,
+ "oos_meas_std": 3.5,
+ "subfilter_visual_meas_std": 3.5,
+ "max_depth": 5.0,
+ "min_depth": 0.05,
+
+
+ // depth sub-filter setting
+ "subfilter_ready_steps": 5,
+ "subfilter_MH_thresh": 8.991,
+
+ "depth_opt_iters": 5,
+ "depth_opt_eps": 0.001,
+ "depth_opt_damping": 1e-3,
+
+ "feature_P0_damping": 1000.0,
+
+ "imu_calib": {
+ "Cas": [1, 1, 1],
+ "Car": [[1, 0, 0],
+ [0, 1, 0],
+ [0, 0, 1]],
+ "Cgs": [1, 1, 1],
+ "Cgr": [[1, 0, 0],
+ [0, 1, 0],
+ [0, 0, 1]]
+ },
+ "imu_init_counter": 20,
+
+ "camera_cfg": {
+ "model": "equidistant",
+ "rows": 512,
+ "cols": 512,
+ "fx": 190.97847715128717,
+ "fy": 190.9733070521226,
+ "cx": 254.93170605935475,
+ "cy": 256.8974428996504,
+ "k0123": [0.0034823894022493434, 0.0007150348452162257, -0.0020532361418706202, 0.00020293673591811182],
+ "max_iter": 25,
+ "comment": "512-cam0"
+ },
+
+ "min_inliers": 5, // minimum number of inlier measurements
+
+ "MH_thresh": 8.991, // 8.991
+ "MH_adjust_factor": 1.5,
+
+ "1pt_RANSAC_thresh": 1.5,
+ "1pt_RANSAC_prob": 0.95,
+ "1pt_RANSAC_Chi2": 5.89,
+
+ "tracker_cfg": {
+ "use_prediction": true,
+ "mask_size": 15,
+ "num_features_min": 60,
+ "num_features_max": 90,
+ "normalize": true,
+
+ "OpticalFlow": {
+ "win_size": 15,
+ "max_level": 5,
+ "max_iter": 15,
+ "eps": 1e-4,
+ "threshold": 20
+ },
+
+ // somehow, only BRISK and FAST work well with the generic FeatureDetector interface (Version 2.4.9)
+ "DetectorType": "FAST",
+
+ "FAST": {
+ "threshold": 20,
+ "nonmaxSuppression": true
+ },
+
+ "GridAdapter": {
+ "grid_rows": 4,
+ "grid_cols": 4
+ }
+ }
+
+}
diff --git a/cfg/viewer.json b/cfg/viewer.json
new file mode 100644
index 00000000..723aa6b2
--- /dev/null
+++ b/cfg/viewer.json
@@ -0,0 +1,27 @@
+{
+ "window": {
+ "height": 512,
+ "width": 1024
+ },
+
+ "viewport": {
+ "width": 512,
+ "height": 512,
+ "fx": 800,
+ "fy": 800,
+ "cx": 256,
+ "cy": 256,
+ "znear": 0.1,
+ "zfar": 10
+ },
+
+ "bg_color": {
+ "r": 0.0,
+ "g": 0.0,
+ "b": 0.0,
+ "a": 1.0
+ },
+ "grid_size": 20,
+ "draw_trace_as_dots": false
+
+}
diff --git a/cfg/viewer_scenenet.json b/cfg/viewer_scenenet.json
new file mode 100644
index 00000000..ea5d42c3
--- /dev/null
+++ b/cfg/viewer_scenenet.json
@@ -0,0 +1,27 @@
+{
+ "window": {
+ "height": 240,
+ "width": 640
+ },
+
+ "viewport": {
+ "width": 320,
+ "height": 240,
+ "fx": 400,
+ "fy": 400,
+ "cx": 160,
+ "cy": 120,
+ "znear": 0.1,
+ "zfar": 10
+ },
+
+ "bg_color": {
+ "r": 0.0,
+ "g": 0.0,
+ "b": 0.0,
+ "a": 1.0
+ },
+ "grid_size": 20
+
+}
+
diff --git a/cfg/vio.json b/cfg/vio.json
new file mode 100644
index 00000000..92d15ad1
--- /dev/null
+++ b/cfg/vio.json
@@ -0,0 +1,14 @@
+{
+ "estimator_cfg": "cfg/estimator.json",
+ "viewer_cfg": "cfg/viewer.json",
+
+ "visualize": true,
+ "wait_time": 1,
+ "verbose": true,
+
+
+ "evaluation_cfg": {
+ "ignore_seconds": 0, // seconds
+ "RPE_interval": 1.0 // seconds
+ }
+}
diff --git a/common/CMakeLists.txt b/common/CMakeLists.txt
new file mode 100644
index 00000000..b983d3d7
--- /dev/null
+++ b/common/CMakeLists.txt
@@ -0,0 +1,19 @@
+cmake_minimum_required(VERSION 3.5)
+project(common)
+
+# # uncomment to overwrite target destinations
+# set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/bin)
+# set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/lib)
+
+add_library(common STATIC utils.cpp)
+target_link_libraries(common absl::str_format pthread)
+
+# add_executable(example_process example_process.cpp)
+# target_link_libraries(example_process glog pthread)
+#
+# option(BUILD_COMMON_TESTS True)
+# if (${BUILD_COMMON_TESTS})
+# add_executable(test_rodrigues test/test_rodrigues.cpp)
+# target_link_libraries(test_rodrigues gtest gtest_main)
+# # add_executable(test_se3 test_se3.cpp)
+# endif(${BUILD_COMMON_TESTS})
diff --git a/common/ProducerConsumerQueue.h b/common/ProducerConsumerQueue.h
new file mode 100644
index 00000000..87933a2c
--- /dev/null
+++ b/common/ProducerConsumerQueue.h
@@ -0,0 +1,189 @@
+/*
+ * Copyright 2012-present Facebook, Inc.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+// @author Bo Hu (bhu@fb.com)
+// @author Jordan DeLong (delong.j@fb.com)
+//
+// Doc:
+// https://github.com/facebook/folly/blob/master/folly/docs/ProducerConsumerQueue.md
+// References on memory_order_relaxed and more:
+// https://bartoszmilewski.com/2008/12/01/c-atomics-and-memory-ordering/
+
+#pragma once
+
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+
+// #include
+
+namespace folly {
+
+constexpr size_t hardware_destructive_interference_size = 64;
+
+/*
+ * ProducerConsumerQueue is a one producer and one consumer queue
+ * without locks.
+ */
+template struct ProducerConsumerQueue {
+ typedef T value_type;
+
+ ProducerConsumerQueue(const ProducerConsumerQueue &) = delete;
+ ProducerConsumerQueue &operator=(const ProducerConsumerQueue &) = delete;
+
+ // size must be >= 2.
+ //
+ // Also, note that the number of usable slots in the queue at any
+ // given time is actually (size-1), so if you start with an empty queue,
+ // isFull() will return true after size-1 insertions.
+ explicit ProducerConsumerQueue(uint32_t size)
+ : size_(size), records_(static_cast(std::malloc(sizeof(T) * size))),
+ readIndex_(0), writeIndex_(0) {
+ assert(size >= 2);
+ if (!records_) {
+ throw std::bad_alloc();
+ }
+ }
+
+ ~ProducerConsumerQueue() {
+ // We need to destruct anything that may still exist in our queue.
+ // (No real synchronization needed at destructor time: only one
+ // thread can be doing this.)
+ if (!std::is_trivially_destructible::value) {
+ size_t readIndex = readIndex_;
+ size_t endIndex = writeIndex_;
+ while (readIndex != endIndex) {
+ records_[readIndex].~T();
+ if (++readIndex == size_) {
+ readIndex = 0;
+ }
+ }
+ }
+
+ std::free(records_);
+ }
+
+ template bool write(Args &&... recordArgs) {
+ auto const currentWrite = writeIndex_.load(std::memory_order_relaxed);
+ auto nextRecord = currentWrite + 1;
+ if (nextRecord == size_) {
+ nextRecord = 0;
+ }
+ if (nextRecord != readIndex_.load(std::memory_order_acquire)) {
+ new (&records_[currentWrite]) T(std::forward(recordArgs)...);
+ writeIndex_.store(nextRecord, std::memory_order_release);
+ return true;
+ }
+
+ // queue is full
+ return false;
+ }
+
+ // move (or copy) the value at the front of the queue to given variable
+ bool read(T &record) {
+ auto const currentRead = readIndex_.load(std::memory_order_relaxed);
+ if (currentRead == writeIndex_.load(std::memory_order_acquire)) {
+ // queue is empty
+ return false;
+ }
+
+ auto nextRecord = currentRead + 1;
+ if (nextRecord == size_) {
+ nextRecord = 0;
+ }
+ record = std::move(records_[currentRead]);
+ records_[currentRead].~T();
+ readIndex_.store(nextRecord, std::memory_order_release);
+ return true;
+ }
+
+ // pointer to the value at the front of the queue (for use in-place) or
+ // nullptr if empty.
+ T *frontPtr() {
+ auto const currentRead = readIndex_.load(std::memory_order_relaxed);
+ if (currentRead == writeIndex_.load(std::memory_order_acquire)) {
+ // queue is empty
+ return nullptr;
+ }
+ return &records_[currentRead];
+ }
+
+ // queue must not be empty
+ void popFront() {
+ auto const currentRead = readIndex_.load(std::memory_order_relaxed);
+ assert(currentRead != writeIndex_.load(std::memory_order_acquire));
+
+ auto nextRecord = currentRead + 1;
+ if (nextRecord == size_) {
+ nextRecord = 0;
+ }
+ records_[currentRead].~T();
+ readIndex_.store(nextRecord, std::memory_order_release);
+ }
+
+ bool isEmpty() const {
+ return readIndex_.load(std::memory_order_acquire) ==
+ writeIndex_.load(std::memory_order_acquire);
+ }
+
+ bool isFull() const {
+ auto nextRecord = writeIndex_.load(std::memory_order_acquire) + 1;
+ if (nextRecord == size_) {
+ nextRecord = 0;
+ }
+ if (nextRecord != readIndex_.load(std::memory_order_acquire)) {
+ return false;
+ }
+ // queue is full
+ return true;
+ }
+
+ // * If called by consumer, then true size may be more (because producer may
+ // be adding items concurrently).
+ // * If called by producer, then true size may be less (because consumer may
+ // be removing items concurrently).
+ // * It is undefined to call this from any other thread.
+ size_t sizeGuess() const {
+ int ret = writeIndex_.load(std::memory_order_acquire) -
+ readIndex_.load(std::memory_order_acquire);
+ if (ret < 0) {
+ ret += size_;
+ }
+ return ret;
+ }
+
+ // maximum number of items in the queue.
+ size_t capacity() const { return size_ - 1; }
+
+private:
+ using AtomicIndex = std::atomic;
+
+ char pad0_[hardware_destructive_interference_size];
+ const uint32_t size_;
+ T *const records_;
+
+ alignas(hardware_destructive_interference_size) AtomicIndex readIndex_;
+ alignas(hardware_destructive_interference_size) AtomicIndex writeIndex_;
+
+ char pad1_[hardware_destructive_interference_size - sizeof(AtomicIndex)];
+};
+
+} // namespace folly
diff --git a/common/alias.h b/common/alias.h
new file mode 100644
index 00000000..acd65e77
--- /dev/null
+++ b/common/alias.h
@@ -0,0 +1,70 @@
+#pragma once
+
+#include "rodrigues.h"
+#include "se3.h"
+
+namespace feh {
+
+using ftype = double;
+
+using Mat3 = Eigen::Matrix;
+using Vec3 = Eigen::Matrix;
+using Mat4 = Eigen::Matrix;
+using Vec4 = Eigen::Matrix;
+using Mat2 = Eigen::Matrix;
+using Vec2 = Eigen::Matrix;
+using Vec6 = Eigen::Matrix;
+using Vec9 = Eigen::Matrix;
+using Mat23 = Eigen::Matrix;
+using Mat32 = Eigen::Matrix;
+using Mat34 = Eigen::Matrix;
+using Mat93 = Eigen::Matrix;
+using Mat39 = Eigen::Matrix;
+using MatX = Eigen::Matrix;
+using VecX = Eigen::Matrix;
+
+using Mat3f = Eigen::Matrix;
+using Mat4f = Eigen::Matrix;
+using Mat34f = Eigen::Matrix;
+using Mat23f = Eigen::Matrix;
+using Mat24f = Eigen::Matrix;
+using MatXf = Eigen::Matrix;
+using MatX3f = Eigen::Matrix;
+
+using Mat3d = Eigen::Matrix;
+using Mat4d = Eigen::Matrix;
+using Mat34d = Eigen::Matrix;
+using Mat23d = Eigen::Matrix;
+using Mat24d = Eigen::Matrix;
+using MatXd = Eigen::Matrix;
+using MatX3d = Eigen::Matrix;
+
+using Vec2f = Eigen::Matrix;
+using Vec3f = Eigen::Matrix;
+using Vec4f = Eigen::Matrix;
+using VecXf = Eigen::Matrix;
+
+using Vec2d = Eigen::Matrix;
+using Vec3d = Eigen::Matrix;
+using Vec4d = Eigen::Matrix;
+using VecXd = Eigen::Matrix;
+
+using Vec2i = Eigen::Matrix;
+using Vec3i = Eigen::Matrix;
+using Vec4i = Eigen::Matrix;
+using VecXi = Eigen::Matrix;
+using MatXi = Eigen::Matrix;
+using MatX3i = Eigen::Matrix;
+
+using SE3 = SE3Type;
+using SO3 = SO3Type;
+
+using SE3f = SE3Type;
+using SO3f = SO3Type;
+
+using SE3d = SE3Type;
+using SO3d = SO3Type;
+
+static const ftype eps = 1e-4f;
+
+} // namespace feh
diff --git a/common/atan.h b/common/atan.h
new file mode 100644
index 00000000..4cf15a91
--- /dev/null
+++ b/common/atan.h
@@ -0,0 +1,142 @@
+#pragma once
+#include "camera.h"
+
+namespace feh {
+
+template class ATANCamera : public BaseCamera> {
+public:
+ using MyBase = BaseCamera>;
+ static constexpr int DIM = 5; // size of intrinsic parameters
+
+ ATANCamera(int rows, int cols, T fx, T fy, T cx, T cy, T w)
+ : BaseCamera>{rows, cols, fx, fy, cx, cy}, w_(w),
+ invw_(1.0 / w), w2_(2.0 * std::tan(w * 0.5)) {}
+
+ template
+ Eigen::Matrix Project(
+ const Eigen::MatrixBase &xc,
+ Eigen::Matrix *jac = nullptr,
+ Eigen::Matrix *jacc = nullptr) const {
+ EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived, 2, 1);
+ using f_t = typename Derived::Scalar;
+
+ Eigen::Matrix xp;
+
+ f_t R = xc.norm();
+ f_t f{1};
+ bool singular = (R < 0.0001 || w_ == 0);
+
+ if (!singular) {
+ f = invw_ * std::atan(w2_ * R) / R;
+ }
+
+ // Project through distortion model
+ xp(0) = fx_ * f * xc(0) + cx_;
+ xp(1) = fy_ * f * xc(1) + cy_;
+
+ if (jac != nullptr) {
+ auto &J{*jac};
+ // compute jacobians
+ if (singular) {
+ J(0, 0) = fx_;
+ J(1, 1) = fy_;
+ } else {
+ // FIXME: optimize computation
+ f_t df_dx, df_dy, df_dR;
+ f_t a = w2_ * R;
+ df_dR = invw_ * (1. / (1 + a * a) * a - std::atan(a)) / R / R;
+ df_dx = df_dR * xc(0) / R;
+ df_dy = df_dR * xc(1) / R;
+
+ J << fx_ * f + fx_ * xc(0) * df_dx, fx_ * xc(0) * df_dy,
+ fy_ * xc(1) * df_dx, fy_ * f + fy_ * xc(1) * df_dy;
+ }
+ }
+
+ if (jacc != nullptr) {
+ auto &J{*jacc};
+ J.setZero(2, DIM); // d[x, y]_d[fx, fy, cx, cy, w]
+ if (singular) {
+ J << xc(0), 0, 1, 0, 0, 0, xc(1), 0, 1, 0;
+ } else {
+ J(0, 0) = f * xc(0);
+ J(0, 2) = 1;
+ J(1, 1) = f * xc(1);
+ J(1, 3) = 1;
+ // f = inv(w) * atan(w2 * R) / R
+ // R is constant w.r.t. w
+ f_t df_dinvw = std::atan(w2_ * R) / R;
+ f_t dinvw_dw = -invw_ * invw_;
+
+ f_t df_datanw2R = invw_ / R;
+ // datan(x)_dx = 1 / (1 + x * x)
+ f_t datanw2R_dw2R = 1 / (1 + (w2_ * R) * (w2_ * R));
+ f_t dw2R_dw2 = R; // w2R = w2 * R
+ // Recall: w2 = 2 * tan(w * 0.5)
+ // dw2_dw = 2.0 / cos^2(w*0.5) * 0.5 = 1.0 / cos^2(2 * 0.5);
+ f_t dw2_dw = 1 / std::cos(w_ * 0.5);
+ dw2_dw *= dw2_dw;
+ f_t df_dw = df_dinvw * dinvw_dw +
+ df_datanw2R * datanw2R_dw2R * dw2R_dw2 * dw2_dw;
+ J(0, 4) = fx_ * xc(0) * df_dw;
+ J(1, 4) = fy_ * xc(1) * df_dw;
+ }
+ }
+ return xp;
+ }
+
+ template
+ Eigen::Matrix UnProject(
+ const Eigen::MatrixBase &xp,
+ Eigen::Matrix *jac = nullptr,
+ Eigen::Matrix *jacc = nullptr) const {
+ EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived, 2, 1);
+
+ using f_t = typename Derived::Scalar;
+ Eigen::Matrix xc;
+
+ Eigen::Matrix tmp((xp(0) - cx_) / fx_, (xp(1) - cy_) / fy_);
+ f_t R = tmp.norm();
+ f_t RR{w_ == 0 ? R : std::tan(R * w_) / w2_};
+ f_t f{R > 0.01 ? RR / R : 1.0};
+ xc = f * tmp;
+
+ if (jac != nullptr) {
+ if (f == 1) {
+ (*jac)(0, 0) = 1.0 / fx_;
+ (*jac)(1, 1) = 1.0 / fy_;
+ } else {
+ f_t df_dR;
+ f_t a = std::tan(R * w_);
+ df_dR = 1.0 / w2_ * (((1 + a * a) * w_ * R - a) / R / R);
+
+ f_t df_dx, df_dy;
+ df_dx = df_dR * tmp(0) / R / fx_;
+ df_dy = df_dR * tmp(1) / R / fy_;
+
+ (*jac) << tmp(0) * df_dx + f / fx_, tmp(0) * df_dy, tmp(1) * df_dx,
+ tmp(1) * df_dy + f / fy_;
+ }
+ }
+ return xc;
+ }
+
+ void Print(std::ostream &out) const {
+ out << "ATAN Camera" << std::endl
+ << "[rows, cols]=" << rows_ << "," << cols_ << "]" << std::endl
+ << "[fx, fy, cx, cy, w]=[" << fx_ << "," << fy_ << "," << cx_ << ","
+ << cy_ << "," << w_ << "]" << std::endl;
+ }
+
+protected:
+ using MyBase::rows_;
+ using MyBase::cols_;
+ using MyBase::fx_;
+ using MyBase::fy_;
+ using MyBase::cx_;
+ using MyBase::cy_;
+
+ T w_, invw_, w2_;
+};
+
+} // namespace feh
diff --git a/common/camera.h b/common/camera.h
new file mode 100644
index 00000000..f7db0e1f
--- /dev/null
+++ b/common/camera.h
@@ -0,0 +1,77 @@
+#pragma once
+
+#include "Eigen/Dense"
+
+namespace feh {
+
+template class BaseCamera {
+public:
+ // constructor
+ BaseCamera(int rows, int cols, T fx, T fy, T cx, T cy)
+ : rows_{rows}, cols_{cols}, fx_{fx}, fy_{fy}, cx_{cx}, cy_{cy} {}
+
+ // copy constructor & assignment
+ BaseCamera &operator=(const BaseCamera &) = default;
+ BaseCamera(const BaseCamera &) = default;
+
+ // move constructor & assignment
+ BaseCamera &operator=(BaseCamera &&) = default;
+ BaseCamera(BaseCamera &&) = default;
+
+ // project a point xc in camera coordinates to pixel coordinates.
+ // xc: camera coordinates of a point.
+ // jac: jacobian matrix of xp w.r.t. xc, i.e., dxp/dxc
+ // jacc: jacobian matrix of xp w.r.t. camera intrinsics, for online
+ // calibration
+ template
+ Eigen::Matrix Project(
+ const Eigen::MatrixBase &xc,
+ Eigen::Matrix *jac = nullptr,
+ Eigen::Matrix *jacc = nullptr) const {
+ return static_cast(this)->Project(xc, jac);
+ }
+
+ // un-project a point xp in pixel coordinates to the camera coordinates.
+ // xp: pixel coordinates of a point.
+ // jac: jacobian matrix of xc w.r.t. xp, i.e., dxc/dxp
+ // jacc: jacobian matrix of xc w.r.t. camera intrinsics, for online
+ // calibration
+ template
+ Eigen::Matrix UnProject(
+ const Eigen::MatrixBase &xp,
+ Eigen::Matrix *jac = nullptr,
+ Eigen::Matrix *jacc = nullptr) const {
+ return static_cast(this)->UnProject(xp, jac);
+ }
+
+ // print intrinsics of the camer amodel
+ void Print(std::ostream &out) const { static_cast(this)->Print(out); }
+
+ // handy accessors
+ int rows() const { return rows_; }
+ int cols() const { return cols_; }
+ T cx() const { return cx_; }
+ T cy() const { return cy_; }
+ T fx() const { return fx_; }
+ T fy() const { return fy_; }
+
+protected:
+ int rows_, cols_;
+ T fx_, fy_, cx_, cy_;
+};
+
+// check whether a given point is out of view.
+template bool OutOfView(T x, T y, const Cam &cam) {
+ float margin(0.01f);
+ return x < cam.cols() * margin || x >= cam.cols() * (1 - margin) ||
+ y < cam.rows() * margin || y >= cam.rows() * (1 - margin);
+}
+
+// Eigen version of the above function.
+template
+bool OutOfView(const Eigen::MatrixBase &xp, const Cam &cam) {
+ EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived, 2, 1);
+ return OutOfView(xp(0), xp(1), cam);
+}
+
+} // feh
diff --git a/common/component.h b/common/component.h
new file mode 100644
index 00000000..8dd47020
--- /dev/null
+++ b/common/component.h
@@ -0,0 +1,52 @@
+// For readability and maintainability, it's better to adopt
+// a modular design approach, where componets like groups and features,
+// and even IMUs and cameras whose intrinsic parameters can be estimated,
+// maintain their own local nominal state.
+// The estimator maintains the overall error state,
+// and performs state propagation and measurement update.
+// When needed, the estimator pulls local nominal states from each module
+// to compute Jacobians, and updates the local nominal states
+// by injecting the error state back to the nominal state internal to each
+// module.
+// We abstract as the notion of ''Componet'' all those modules whose
+// internal parameters can be estimated.
+//
+// We adopt the CRTP idiom to ensure a minimal set of local state
+// operations are implemented.
+// This is more like to impose design requirements rather than to
+// enable (static) polymorphism.
+//
+// Author: Xiaohan Fei (feixh@cs.ucla.edu)
+#pragma once
+
+namespace feh {
+
+// helper function to check whehter the given "State" type has a "Tangent" type
+// defined.
+// referene:
+// https://en.cppreference.com/w/cpp/types/void_t
+// modified according the has_type_member function
+template > struct has_tangent : std::false_type {};
+
+template
+struct has_tangent> : std::true_type {};
+
+// CRTP pattern for static polymorphism
+// Derived: Derived class to enforce the implementation of UpdateState
+// S: the nominal state for the local parameters.
+// Error: the error state for the local parameters.
+template struct Component {
+ std::enable_if_t::value, void>
+ UpdateState(const typename State::Tangent &dX) {
+ static_cast(this)->UpdateState(dX);
+ }
+
+ std::enable_if_t::value, void>
+ UpdateState(const State &dX) {
+ static_cast(this)->UpdateState(dX);
+ }
+};
+
+// example usage: see src/imu.h
+
+} // namespace feh
diff --git a/common/equidist.h b/common/equidist.h
new file mode 100644
index 00000000..5d6f44ff
--- /dev/null
+++ b/common/equidist.h
@@ -0,0 +1,175 @@
+#pragma once
+#include "camera.h"
+
+namespace feh {
+
+template
+class EquidistantCamera : public BaseCamera> {
+public:
+ using MyBase = BaseCamera>;
+ static constexpr int DIM = 8; // size of intrinsic parameters
+
+ EquidistantCamera(int rows, int cols, T fx, T fy, T cx, T cy, T k0, T k1,
+ T k2, T k3, int max_iter = 15)
+ : BaseCamera>{rows, cols, fx, fy, cx, cy},
+ k0_{k0}, k1_{k1}, k2_{k2}, k3_{k3}, max_iter_{max_iter} {}
+
+ template
+ Eigen::Matrix Project(
+ const Eigen::MatrixBase &xc,
+ Eigen::Matrix *jac = nullptr,
+ Eigen::Matrix *jacc = nullptr) const {
+ EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived, 2, 1);
+ using f_t = typename Derived::Scalar;
+ Eigen::Matrix xp;
+
+ f_t xy_norm2 = xc.squaredNorm();
+ f_t xy_norm = sqrt(xy_norm2);
+ f_t xyz_norm2 = xy_norm2 + 1;
+
+ f_t th = std::atan2(xy_norm, 1.0);
+
+ f_t phi = std::atan2(xc[1], xc[0]);
+
+ f_t th2 = th * th;
+ f_t th3 = th2 * th;
+ f_t th4 = th3 * th;
+ f_t th5 = th3 * th2;
+ f_t th6 = th5 * th;
+ f_t th7 = th5 * th2;
+ f_t th8 = th7 * th;
+ f_t th9 = th7 * th2;
+ f_t r = th + k0_ * th3 + k1_ * th5 + k2_ * th7 + k3_ * th9;
+
+ f_t cos_phi = std::cos(phi);
+ f_t sin_phi = std::sin(phi);
+
+ f_t u = fx_ * r * cos_phi + cx_;
+ f_t v = fy_ * r * sin_phi + cy_;
+
+ // fill in xp
+ xp[0] = u;
+ xp[1] = v;
+
+ if (jac != nullptr) {
+ f_t dphi_dx = -xc[1] / xy_norm2;
+ f_t dphi_dy = xc[0] / xy_norm2;
+
+ f_t dth_dx = xc[0] / xyz_norm2 / xy_norm;
+ f_t dth_dy = xc[1] / xyz_norm2 / xy_norm;
+
+ f_t dr_dth =
+ 1 + k0_ * 3 * th2 + k1_ * 5 * th4 + k2_ * 7 * th6 + k3_ * 9 * th8;
+
+ f_t du_dx = fx_ * cos_phi * dr_dth * dth_dx - fx_ * r * sin_phi * dphi_dx;
+ f_t du_dy = fx_ * cos_phi * dr_dth * dth_dy - fx_ * r * sin_phi * dphi_dy;
+
+ f_t dv_dx = fy_ * sin_phi * dr_dth * dth_dx + fy_ * r * cos_phi * dphi_dx;
+ f_t dv_dy = fy_ * sin_phi * dr_dth * dth_dy + fy_ * r * cos_phi * dphi_dy;
+
+ // fill in jacobians
+ (*jac) << du_dx, du_dy, dv_dx, dv_dy;
+ }
+
+ if (jacc != nullptr) {
+ auto &J{*jacc};
+ J.setZero(2, 8); // d[x,y]_[fx. fy, cx, cy, k0, k1, k2, k3]
+
+ J(0, 0) = r * cos_phi; // dx_dfx
+ J(0, 2) = 1; // dx_dcx
+ J(1, 1) = r * sin_phi; // dy_dfy
+ J(1, 3) = 1; // dy_dcy
+
+ Eigen::Matrix dr_dk{th3, th5, th7,
+ th9}; // dr_d[k0, k1, k2, k3]
+ J.template block<1, 4>(0, 4) = fx_ * cos_phi * dr_dk;
+ J.template block<1, 4>(1, 4) = fy_ * sin_phi * dr_dk;
+ }
+ return xp;
+ }
+
+ template
+ Eigen::Matrix UnProject(
+ const Eigen::MatrixBase &xp,
+ Eigen::Matrix *jac = nullptr,
+ Eigen::Matrix *jacc = nullptr) const {
+ EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived, 2, 1);
+ using f_t = typename Derived::Scalar;
+ using Vec2 = Eigen::Matrix;
+ Vec2 xc;
+
+ f_t xn = xp[0] - cx_;
+ f_t yn = xp[1] - cy_;
+
+ f_t b(fx_ * yn), a(fy_ * xn);
+ f_t phi = std::atan2(b, a);
+ f_t cos_phi = std::cos(phi);
+ f_t sin_phi = std::sin(phi);
+
+ f_t rth = xn / (fx_ * cos_phi);
+
+ f_t th = rth;
+ // solve th:
+ // th + k0*th**3 + k1*th**5 + k2*th**7 + k3*th**9 = rth
+
+ f_t th2, th3, th4, th6, x0, x1;
+ for (int i = 0; i < max_iter_; i++) {
+ // f = (th + k0*th**3 + k1*th**5 + k2*th**7 + k3*th**9 - rth)^2
+ th2 = th * th;
+ th3 = th2 * th;
+ th4 = th2 * th2;
+ th6 = th4 * th2;
+ x0 = k0_ * th3 + k1_ * th4 * th + k2_ * th6 * th + k3_ * th6 * th3 - rth +
+ th;
+ x1 = 3 * k0_ * th2 + 5 * k1_ * th4 + 7 * k2_ * th6 + 9 * k3_ * th6 * th2 +
+ 1;
+ f_t d = 2 * x0 * x1;
+ f_t d2 = 4 * th * x0 * (3 * k0_ + 10 * k1_ * th2 + 21 * k2_ * th4 +
+ 36 * k3_ * th6) +
+ 2 * x1 * x1;
+ f_t delta = d / d2;
+ th -= delta;
+ }
+ f_t tan_th = std::tan(th);
+ xc[0] = tan_th * cos_phi;
+ xc[1] = tan_th * sin_phi;
+
+ if (jac != nullptr) {
+ f_t a2b2 = a * a + b * b;
+ Vec2 dphi_dxy(-b / a2b2 * fy_, a / a2b2 * fx_);
+ Vec2 dcosphi_dxy(-sin_phi * dphi_dxy);
+ Vec2 dsinphi_dxy(cos_phi * dphi_dxy);
+ Vec2 drth_dxy(cos_phi - xn * dcosphi_dxy[0], -xn * dcosphi_dxy[1]);
+ drth_dxy /= (fx_ * cos_phi * cos_phi);
+
+ f_t cos_th(cos(th));
+ Vec2 dtanth_dxy(drth_dxy / x1 / (cos_th * cos_th));
+ Vec2 doutx_dxy(cos_phi * dtanth_dxy + tan_th * dcosphi_dxy);
+ Vec2 douty_dxy(sin_phi * dtanth_dxy + tan_th * dsinphi_dxy);
+ (*jac) << doutx_dxy[0], doutx_dxy[1], douty_dxy[0], douty_dxy[1];
+ }
+ return xc;
+ }
+
+ void Print(std::ostream &out) const {
+ out << "Equidistant Camera" << std::endl
+ << "[rows, cols]=" << rows_ << "," << cols_ << "]" << std::endl
+ << "[fx, fy, cx, cy]=[" << fx_ << "," << fy_ << "," << cx_ << "," << cy_
+ << "]" << std::endl
+ << "[k0, k1, k2, k3]=[" << k0_ << "," << k1_ << "," << k2_ << "," << k3_
+ << "]" << std::endl;
+ }
+
+protected:
+ using MyBase::rows_;
+ using MyBase::cols_;
+ using MyBase::fx_;
+ using MyBase::fy_;
+ using MyBase::cx_;
+ using MyBase::cy_;
+
+ T k0_, k1_, k2_, k3_;
+ int max_iter_;
+};
+
+} // namespace feh
diff --git a/common/example_process.cpp b/common/example_process.cpp
new file mode 100644
index 00000000..c37e5e4f
--- /dev/null
+++ b/common/example_process.cpp
@@ -0,0 +1,47 @@
+#include "process.h"
+#include
+
+using namespace feh;
+
+struct ExampleMessage {
+ virtual ~ExampleMessage() {}
+};
+
+struct PrintMessage : public ExampleMessage {
+ PrintMessage(int value) : value_{value} {}
+ int value_;
+};
+
+class ExampleProcess : public Process {
+public:
+ using MessageT = ExampleMessage;
+ ExampleProcess(uint32_t size = 1000) : Process{size}, accumulator_{0} {}
+
+ // Decalre process-specific member variables.
+ int accumulator_;
+
+protected:
+ void Handle(ExampleMessage *message) {
+ // Define the process-specific handle function.
+ if (typeid(*message) == typeid(PrintMessage)) {
+ accumulator_ += dynamic_cast(message)->value_;
+ delete message;
+ }
+ }
+};
+
+int main() {
+ ExampleProcess proc{2000};
+ proc.Start();
+ int accumulator{0};
+ for (int i = 0; i < 2000; ++i) {
+ usleep(10); // sleep for the process to catch up
+ proc.Enqueue(new PrintMessage(i));
+ accumulator += i;
+ }
+ proc.Enqueue(nullptr);
+ std::cout << "process.accumulator=" << proc.accumulator_ << std::endl;
+ std::cout << "main thread accumulator=" << accumulator << std::endl;
+ CHECK_EQ(proc.accumulator_, accumulator)
+ << "main thread and process accumulated different values";
+}
diff --git a/common/pinhole.h b/common/pinhole.h
new file mode 100644
index 00000000..ebdb7f73
--- /dev/null
+++ b/common/pinhole.h
@@ -0,0 +1,69 @@
+#pragma once
+#include "camera.h"
+
+namespace feh {
+
+template
+class PinholeCamera : public BaseCamera> {
+public:
+ using MyBase = BaseCamera>;
+ static constexpr int DIM = 4; // size of intrinsic parameters
+
+ PinholeCamera(int rows, int cols, T fx, T fy, T cx, T cy)
+ : BaseCamera>{rows, cols, fx, fy, cx, cy} {}
+
+ template
+ Eigen::Matrix Project(
+ const Eigen::MatrixBase &xc,
+ Eigen::Matrix *jac = nullptr,
+ Eigen::Matrix *jacc = nullptr) const {
+ EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived, 2, 1);
+ using f_t = typename Derived::Scalar;
+ Eigen::Matrix xp{fx_ * xc(0) + cx_, fy_ * xc(1) + cy_};
+
+ if (jac != nullptr) {
+ // fill in jacobians
+ (*jac) << fx_, 0, 0, fy_;
+ }
+
+ if (jacc != nullptr) {
+ auto &J{*jacc};
+ J.setZero(2, 4); // d[x, y]_d[fx, fy, cx, cy]
+ J << xc(0), 0, 1, 0, 0, xc(1), 0, 1;
+ }
+ return xp;
+ }
+
+ template
+ Eigen::Matrix UnProject(
+ const Eigen::MatrixBase &xp,
+ Eigen::Matrix *jac = nullptr,
+ Eigen::Matrix *jacc = nullptr) const {
+ EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived, 2, 1);
+ using f_t = typename Derived::Scalar;
+ using Vec2 = Eigen::Matrix;
+ Vec2 xc{(xp(0) - cx_) / fx_, (xp(1) - cy_) / fy_};
+
+ if (jac != nullptr) {
+ (*jac) << 1 / fx_, 0, 0, 1 / fy_;
+ }
+ return xc;
+ }
+
+ void Print(std::ostream &out) const {
+ out << "Pinhole Camera" << std::endl
+ << "[rows, cols]=" << rows_ << "," << cols_ << "]" << std::endl
+ << "[fx, fy, cx, cy]=[" << fx_ << "," << fy_ << "," << cx_ << "," << cy_
+ << "]" << std::endl;
+ }
+
+protected:
+ using MyBase::rows_;
+ using MyBase::cols_;
+ using MyBase::fx_;
+ using MyBase::fy_;
+ using MyBase::cx_;
+ using MyBase::cy_;
+};
+
+} // namespace feh
diff --git a/common/process.h b/common/process.h
new file mode 100644
index 00000000..bda6242b
--- /dev/null
+++ b/common/process.h
@@ -0,0 +1,91 @@
+#pragma once
+// stl
+#include
+#include
+#include
+// 3rdparty
+#include "ProducerConsumerQueue.h"
+#include "glog/logging.h"
+
+// own
+#include "utils.h"
+
+namespace feh {
+
+// my little process wrapper of
+// the single producer single consumer queue
+// from facebook's folly library
+template class Process {
+public:
+ // struct Block : public MessageT {
+ // Block() : ready_{false} {}
+ // std::atomic ready_;
+ // };
+
+ Process(uint32_t size = 1000) : worker_{nullptr}, queue_{size} {}
+
+ Process(const Process &) = delete;
+ Process &operator=(const Process &) = delete;
+
+ virtual void Wait() {
+ while (!queue_.isEmpty()) {
+ usleep(1);
+ }
+ }
+
+ virtual ~Process() {
+ if (worker_) {
+ worker_->join();
+ delete worker_;
+ }
+ LOG(INFO) << "process stopped";
+ }
+
+ void Start() {
+ worker_ = new std::thread([this]() {
+ for (;;) {
+ std::unique_ptr message;
+ while (!queue_.read(message)) {
+ continue;
+ }
+ if (!this->Handle(message.get())) {
+ LOG(FATAL) << "cannot handle unknown message type";
+ }
+ }
+ });
+ }
+
+ void Enqueue(std::unique_ptr message) {
+ // DLOG(INFO) << "enqueueing message ..." << std::endl;
+ while (!queue_.write(std::move(message))) {
+ continue;
+ }
+ // DLOG(INFO) << "message euqueued" << std::endl;
+ }
+
+protected:
+ // Message handler. Return true if the message is known to this process and
+ // successfully processed; otherwise return false.
+ // This is like the "chain of responsibility" design pattern, where the
+ // message
+ // will be passed to a child class only if the parent class cannot handle it.
+ virtual bool Handle(MessageT *message) {
+ // LOG(INFO) << "Template process; implement actual handle function here";
+ // NOTE: Handle function is in charge of destroying the message if needed.
+ // if (auto msg = dynamic_cast(message)) {
+ // msg->ready_ = true;
+ // delete message;
+ // return true;
+ // }
+ return false;
+ }
+
+private:
+ own worker_;
+ // folly::ProducerConsumerQueue> queue_; // the queue owns the
+ // pointers and need to delete them after use
+ folly::ProducerConsumerQueue>
+ queue_; // the queue owns the pointers and need to delete them after use
+};
+
+} // namespace feh
diff --git a/common/radtan.h b/common/radtan.h
new file mode 100644
index 00000000..672019d6
--- /dev/null
+++ b/common/radtan.h
@@ -0,0 +1,134 @@
+#pragma once
+#include "camera.h"
+
+namespace feh {
+
+template
+class RadialTangentialCamera : public BaseCamera> {
+public:
+ using MyBase = BaseCamera>;
+ static constexpr int DIM = 9; // size of intrinsic parameters
+
+ RadialTangentialCamera(int rows, int cols, T fx, T fy, T cx, T cy, T p1, T p2,
+ T k1, T k2, T k3, int max_iter = 15)
+ : BaseCamera>{rows, cols, fx, fy, cx, cy},
+ p1_{p1}, p2_{p2}, k1_{k1}, k2_{k2}, k3_{k3}, max_iter_{max_iter} {}
+
+ template
+ Eigen::Matrix Project(
+ const Eigen::MatrixBase &xc,
+ Eigen::Matrix *jac = nullptr,
+ Eigen::Matrix *jacc = nullptr) const {
+
+ EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived, 2, 1);
+
+ using f_t = typename Derived::Scalar;
+ // intermediate quantities
+ f_t x2 = xc(0) * xc(0);
+ f_t y2 = xc(1) * xc(1);
+ f_t xy = xc(0) * xc(1);
+ f_t r2{x2 + y2};
+ f_t r4{r2 * r2};
+ f_t r6{r2 * r4};
+ // compute the correction for radial distortion
+ f_t kr{1.0 + k1_ * r2 + k2_ * r4 + k3_ * r6};
+ // compute the correction for tangential distortion
+ Eigen::Matrix kt{2.0 * p1_ * xy + p2_ * (r2 + 2.0 * x2),
+ 2.0 * p2_ * xy + p1_ * (r2 + 2.0 * y2)};
+ // apply un-distortion
+ Eigen::Matrix xk{kr * xc + kt};
+ // convert to pixels
+ Eigen::Matrix xp{xk(0) * fx_ + cx_, xk(1) * fy_ + cy_};
+
+ // optionally compute the jacobian
+ if (jac != nullptr) {
+ // fill in jacobians
+ // dxp_dxc = dxp_dxk * dxk_dxc
+ // dxk_dxc = dxk_dkr * dkr_dxc + diag([kr, kr]) + dxk_dkt * dkt_dxc
+ Eigen::Matrix dxp_dxk;
+ dxp_dxk << fx_, 0, 0, fy_;
+ Eigen::Matrix dxk_dkr{xc};
+
+ // dkr_dx = k1_ * 2 * r * dr_dx + k2_ * 4.0 * r3 * dr_dx + k3_ * 6.0
+ // * r5 * dr_dx;
+ // dkr_dy = k1_ * 2 * r * dr_dy + k2_ * 4.0 * r3 * dr_dy + k3_ * 6.0
+ // * r5 * dr_dy;
+ // Since dr_dx = x/r and dr_dy = y / r
+ // dkr_dx = k1_ * 2 * x + k2_ * 4.0 * r2 * x + k3_ * 6.0 * r4 * x;
+ // = (k1_ * 2 + k2_ * 4 * r2 + k3_ * 6 * r4) * x;
+ f_t dkr_dxc_coeff = k1_ * 2 + k2_ * 4 * r2 + k3_ * 6 * r4;
+ Eigen::Matrix dkr_dxc{dkr_dxc_coeff, dkr_dxc_coeff};
+
+ // kt = [2.0 * p1_ * xy + p2_ * (r2 + 2.0 * x2),
+ // 2.0 * p2_ * xy + p1_ * (r2 + 2.0 * y2)]
+ Eigen::Matrix dkt_dxc;
+ dkt_dxc << 2.0 * p1_ * xc(1) + p2_ * (2 * xc(0) + 4.0 * xc(0)),
+ 2.0 * p1_ * xc(0) + p2_ * (2 * xc(1)),
+ 2.0 * p2_ * xc(1) + p1_ * (2.0 * xc(0)),
+ 2.0 * p2_ * xc(0) + p1_ * (2 * xc(1) + 4 * xc(1));
+
+ Eigen::Matrix dxk_dxc{dxk_dkr * dkr_dxc + dkt_dxc};
+ dxk_dxc(0, 0) += kr;
+ dxk_dxc(1, 1) += kr;
+ *jac = dxp_dxk * dxk_dxc;
+ }
+
+ if (jacc != nullptr) {
+ auto &J{*jacc};
+ J.setZero(2, DIM); // d[x, y]_d[fx, fy, cx, cy, p1, p2, k1, k2, k3]
+ J(0, 0) = xk(0);
+ J(0, 2) = 1;
+ J(1, 1) = xk(1);
+ J(1, 3) = 1;
+ Eigen::Matrix dxk_dp; // d[x, y]_d[p1, p2]
+ dxk_dp << 2.0 * xy, r2 + 2.0 * x2, r2 + 2.0 * y2, 2.0 * xy;
+
+ Eigen::Matrix dkr_dk{r2, r4, r6}; // dkr_d[k1, k2, k3]
+ Eigen::Matrix dxk_dk{xc * dkr_dk};
+ // dxk_dk << xc(0) * dkr_dk,
+ // xc(1) * dkr_dk;
+
+ J.template block<1, 5>(0, 4) << fx_ * dxk_dp.row(0), fx_ * dxk_dk.row(0);
+ J.template block<1, 5>(1, 4) << fy_ * dxk_dp.row(1), fy_ * dxk_dk.row(1);
+ }
+ return xp;
+ }
+
+ template
+ Eigen::Matrix UnProject(
+ const Eigen::MatrixBase &xp,
+ Eigen::Matrix *jac = nullptr,
+ Eigen::Matrix *jacc = nullptr) const {
+ EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived, 2, 1);
+ using f_t = typename Derived::Scalar;
+ using Vec2 = Eigen::Matrix;
+ Vec2 xc{(xp(0) - cx_) / fx_, (xp(1) - cy_) / fy_};
+
+ if (jac != nullptr) {
+ (*jac) << 1 / fx_, 0, 0, 1 / fy_;
+ }
+ return xc;
+ }
+
+ void Print(std::ostream &out) const {
+ out << "RadialTangential Camera" << std::endl
+ << "[rows, cols]=" << rows_ << "," << cols_ << "]" << std::endl
+ << "[fx, fy, cx, cy]=[" << fx_ << "," << fy_ << "," << cx_ << "," << cy_
+ << "]"
+ << "[p1, p2]=[" << p1_ << "," << p2_ << "]"
+ << "[k0, k1, k2]=[" << k1_ << "," << k2_ << "," << k3_ << "]"
+ << std::endl;
+ }
+
+protected:
+ using MyBase::rows_;
+ using MyBase::cols_;
+ using MyBase::fx_;
+ using MyBase::fy_;
+ using MyBase::cx_;
+ using MyBase::cy_;
+ T p1_, p2_, k1_, k2_, k3_;
+ int max_iter_;
+};
+
+} // namespace feh
diff --git a/common/rodrigues.h b/common/rodrigues.h
new file mode 100644
index 00000000..f70bc183
--- /dev/null
+++ b/common/rodrigues.h
@@ -0,0 +1,254 @@
+#pragma once
+#include "Eigen/Dense"
+#include "math.h"
+#include
+
+namespace feh {
+
+constexpr int Sum1ToN(int N) { return (N * (N + 1)) >> 1; }
+
+template
+Eigen::Matrix
+dA_dAu(const Eigen::MatrixBase &A) {
+ EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived, N, N);
+
+ Eigen::Matrix D;
+ int idx_u{0}; // indexing to the upper triangular part of the matrix
+ for (int i = 0; i < N; ++i) {
+ for (int j = i; j < N; ++j) {
+ D(i * N + j, idx_u++) = 1;
+ }
+ }
+ return D;
+}
+
+template
+Eigen::Matrix
+unstack(const Eigen::MatrixBase &u, int major = Eigen::RowMajor) {
+ EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived, M * N, 1);
+ Eigen::Matrix m;
+ for (int i = 0; i < M; ++i)
+ for (int j = 0; j < N; ++j) {
+ if (major == Eigen::RowMajor) {
+ m(i, j) = u(i * M + j);
+ } else {
+ m(j, i) = u(i * M + j);
+ }
+ }
+ return m;
+}
+
+template
+Eigen::Matrix
+hat(const Eigen::MatrixBase &u) {
+ EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived, 3, 1);
+ return (Eigen::Matrix{} << 0, -u(2), u(1),
+ u(2), 0, -u(0), -u(1), u(0), 0)
+ .finished();
+}
+
+template Eigen::Matrix dhat() {
+ return (Eigen::Matrix{} << 0, 0, 0, 0, 0, -1, 0, 1, 0, 0, 0, 1, 0, 0,
+ 0, -1, 0, 0, 0, -1, 0, 1, 0, 0, 0, 0, 0)
+ .finished();
+}
+
+template
+Eigen::Matrix
+dhat(const Eigen::MatrixBase &u) {
+ EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived, 3, 1);
+ return dhat();
+}
+
+template
+Eigen::Matrix
+vee(const Eigen::MatrixBase &R) {
+ EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived, 3, 3);
+ return {R(2, 1) - R(1, 2), R(0, 2) - R(2, 0), R(1, 0) - R(0, 1)};
+}
+
+template Eigen::Matrix dvee() {
+ return (Eigen::Matrix{} << 0, 0, 0, 0, 0, -1, 0, 1, 0, 0, 0, 1, 0, 0,
+ 0, -1, 0, 0, 0, -1, 0, 1, 0, 0, 0, 0, 0)
+ .finished();
+}
+
+template
+Eigen::Matrix
+dvee(const Eigen::MatrixBase &R) {
+ EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived, 3, 3);
+ return dvee();
+}
+
+template
+Eigen::Matrix dAt_dA() {
+ Eigen::Matrix D;
+ D.setZero();
+ for (int m = 0; m < M; ++m) {
+ for (int n = 0; n < N; ++n) {
+ D(m * N + n, n * M + m) = 1;
+ }
+ }
+ return D;
+}
+
+template
+Eigen::Matrix
+dAt_dA(const Eigen::MatrixBase &A) {
+ return dAt_dA();
+}
+
+// Note, by default the Eigen matrices arrange data in RowMajor order.
+// This does not affect the way we index the element via () operator.
+// But when using Map<> function to map raw internal data to matrices/vectors,
+// we need to be careful about the order.
+// dC_{n,p}/dA_{n,m}=B_{m,p}
+template
+Eigen::Matrix
+dAB_dA(const Eigen::MatrixBase &A,
+ const Eigen::MatrixBase &B) {
+
+ using T = typename Derived::Scalar;
+ constexpr int N = Derived::RowsAtCompileTime;
+ constexpr int M = Derived::ColsAtCompileTime;
+ constexpr int P = OtherDerived::ColsAtCompileTime;
+
+ static_assert(std::is_same::value,
+ "Operands should have same dtype.");
+ static_assert(M == OtherDerived::RowsAtCompileTime,
+ "Columns of A should match rows of B.");
+
+ Eigen::Matrix D;
+ D.setZero();
+ for (int n = 0; n < N; ++n) {
+ for (int p = 0; p < P; ++p) {
+ for (int m = 0; m < M; ++m) {
+ D(n * P + p, n * M + m) += B(m, p);
+ }
+ }
+ }
+ return D;
+}
+
+// dC_{n,p}/dB_{m,p}=A_{n,m}
+template
+Eigen::Matrix
+dAB_dB(const Eigen::MatrixBase &A,
+ const Eigen::MatrixBase &B) {
+
+ using T = typename Derived::Scalar;
+ constexpr int N = Derived::RowsAtCompileTime;
+ constexpr int M = Derived::ColsAtCompileTime;
+ constexpr int P = OtherDerived::ColsAtCompileTime;
+ static_assert(std::is_same::value,
+ "Operands should have same type.");
+ static_assert(M == OtherDerived::RowsAtCompileTime,
+ "Columns of A should match rows of B.");
+
+ Eigen::Matrix D;
+ D.setZero();
+ for (int n = 0; n < N; ++n) {
+ for (int p = 0; p < P; ++p) {
+ for (int m = 0; m < M; ++m) {
+ D(n * P + p, m * P + p) += A(n, m);
+ }
+ }
+ }
+ return D;
+}
+
+template
+Eigen::Matrix
+rodrigues(const Eigen::MatrixBase &w,
+ Eigen::Matrix *dR_dw = nullptr) {
+
+ EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived, 3, 1);
+ using T = typename Derived::Scalar;
+ Eigen::Matrix R;
+
+ T th = w.norm();
+
+ if (th < 1e-8) {
+ // R = I + hat(w)
+ // std::cout << "small angle approximation" << std::endl;
+ R = Eigen::Matrix::Identity() + hat(w);
+ if (dR_dw) {
+ *dR_dw = dhat(w);
+ }
+ return R;
+ }
+ T inv_th = 1.0 / th;
+ Eigen::Matrix u = w * inv_th;
+
+ // R = I + u.hat * sin(th) + (u.hat)^2 * (1-cos(th))
+ T sin_th = sin(th);
+ T cos_th = cos(th);
+ Eigen::Matrix uhat = hat(u);
+ Eigen::Matrix uhat2 = uhat * uhat;
+ R = Eigen::Matrix::Identity() + uhat * sin_th + uhat2 * (1 - cos_th);
+ if (dR_dw) {
+ Eigen::Matrix dR_du =
+ sin_th * dhat(u) +
+ (1 - cos_th) * (dAB_dA(uhat, uhat) + dAB_dB(uhat, uhat)) * dhat(u);
+ Eigen::Matrix du_dw =
+ inv_th * (Eigen::Matrix::Identity() - u * u.transpose());
+ Eigen::Matrix dR_dth = Eigen::Map>(
+ Eigen::Matrix{uhat * cos_th + uhat2 * sin_th}.data());
+ // Eigen::Matrix dth_dw = u.transpose();
+ *dR_dw = dR_du * du_dw + dR_dth * u.transpose();
+ }
+ return R;
+}
+
+template
+Eigen::Matrix
+invrodrigues(const Eigen::MatrixBase &R,
+ Eigen::Matrix *dw_dR = nullptr) {
+
+ EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived, 3, 3);
+ using T = typename Derived::Scalar;
+
+ Eigen::Matrix w;
+
+ T tmp = 0.5 * (R.trace() - 1);
+ Eigen::Matrix vee_R = vee(R);
+ if (tmp > 1.0 - 1e-10) {
+ // std::cout << "small angle approximation" << std::endl;
+ w = 0.5 * vee_R;
+ if (dw_dR) {
+ *dw_dR = 0.5 * dvee(R);
+ }
+ return w;
+ }
+
+ T th = acos(tmp);
+ T sin_th = sin(th);
+ T inv_sin_th = 1.0 / sin_th;
+ Eigen::Matrix u = 0.5 * vee_R * inv_sin_th;
+
+ w = th * u;
+
+ if (dw_dR) {
+ Eigen::Matrix dth_dR;
+ T dth_dtmp = -1 / sqrt(1 - tmp * tmp);
+ Eigen::Matrix dtmp_dR;
+ dtmp_dR << 1, 0, 0, 0, 1, 0, 0, 0, 1; // d(trace(R)-1)_dR
+ dtmp_dR *= 0.5;
+ dth_dR = dth_dtmp * dtmp_dR;
+
+ Eigen::Matrix du_dR;
+ // u = vee(R) / (2*sin(th));
+ du_dR = 0.5 * (dvee(R) * inv_sin_th -
+ vee(R) * cos(th) * inv_sin_th * inv_sin_th * dth_dR);
+ *dw_dR = u * dth_dR + th * du_dR;
+ }
+ return w;
+}
+}
diff --git a/common/se3.h b/common/se3.h
new file mode 100644
index 00000000..fb9da7e8
--- /dev/null
+++ b/common/se3.h
@@ -0,0 +1,182 @@
+//
+// Created by feixh on 10/29/18.
+//
+#pragma once
+
+#include "rodrigues.h"
+
+namespace feh {
+
+template