This source code provides the efficient incremental bundle adjustment implementation, which is part of our RKD-SLAM.
We present RKD-SLAM, a robust keyframe-based dense SLAM approach for an RGB-D camera that can robustly handle fast motion and dense loop closure, run without time limitation in a moderate size scene. For reducing accumulation error, we also introduce a very efficient incremental bundle adjustment (EIBA) algorithm, that can provide nearly the same solution as global BA, but with significantly less computation time, which is proportional to how many variables are actually changed.
Haomin Liu, Chen Li, Guojun Chen, Guofeng Zhang, Michael Kaess and Hujun Bao. Robust Keyframe-based Dense SLAM with an RGB-D Camera [J]. arXiv preprint arXiv:1711.05166, 2017. [arXiv report].
EIBA is released under a Apache License 2.0. Please contact Guofeng Zhang if you have any questions.
If you use this source code for your academic publication, please cite our paper:
@article{LiuLCZKB2017,
title={Robust Keyframe-based Dense SLAM with an RGB-D Camera},
author={Haomin Liu and Chen Li and Guojun Chen and Guofeng Zhang and Michael Kaess and Hujun Bao},
journal={arXiv preprint arXiv:1711.05166},
year={2017}
}
git clone https://github.com/jbeder/yaml-cpp.git
cd yaml-cpp
mkdir build && cd build
cmake ..
make -j4
sudo make install
The project has been tested in ubuntu 16.04.
Then you can build and run example:
cd /path/to/this/project
mkdir build && cd build
cmake ..
make -j4
./ExamlpeYAML ../data/
We use inverse depth parameterization for 3D points, which means that all the feature points are parameterized in the form of inverse depth and the corresponding camera location from which they were first observed. The first observations are also called source features. In our program, we only optimize camera pose and source feature inverse depth.
We provide an interface class BAInterface
, which can be used to push feature measurements PushKeyframeFeatures(*)
and frame constraintsPushBetweenFrameConstraint()
, then you can call Optimize(*)
to run optimization and get optimized camera pose and inverse depths.
Besides, we also provide a function PushFrameInfoFromYAML(*)
, which can be used to load our example YAML format data files and push data to BA system.
- Call this to push feature observations
- Initial guess of current frame camera pose (rotation and position)
- Initial guess of last frame source feature's inverse depth
- Current frame source features (2D feature measurements )
- Measured features (matches) and covariance
- The correspondence for measured features to the matched source features is indicated by matched keyframe index (globally) and matched source feature index (locally in its keyframe).
-
Call this to push frame constrains
-
The function minimize the energy of ![](http://latex.codecogs.com/gif.latex?C_1 C_2^{-1}T_{12})
- Call this to run EIBA optimization
- Optimized result would been stored in
optKFs
- Optimized keyframes and depths
- if keyframes or depths have been updated by EIBA, the corresponding boolean mark would be set true.
- Call this to set optimization parameters
- Please refer to the code comments for more informations.
- We use rotation and postion model for input and output camera pose, that means
, where
) is the point in world frame,
) is the point in camera frame.
- All the 2D features should have been undistorted and normalized to z=1 plane.
- If measured depth is invalid, set the corrresponding inverse depth source observation
source_features[i].m_inv_depth
to 0.
We provide an example ExampleYAML.cpp
to call EIBA by providing YAML format frame data, which is generated by the tracking part of RKD-SLAM. This example data is recorded from TUM RGBD fr3/long_office_household, which contains 93 keyframes.
Here is a simplified example of YAML data format:
# keyframe feature measurement
features:
# initial guess
initial_guess:
# current frame camera pose (rotation and position)
current_camera_pose_guess:
- [1, 0, 0, 0]
- [0, 1, 0, 0]
- [0, 0, 1, 0]
# last frame inverse depth guess
last_inv_depth_guess:
- [0.5]
# source features (first observation of features)
# point2D (normalized in z=1 plane) and inverse depth
source_features:
- [0.2, -0.08, 0.3]
# measured feature matches
# matched source feature index is indicated by its keyframe index, and source feature index
# kf_idx: matched keyframe index (globally)
# ftr_idx: matched source feature index (locally)
# pt: point2D (normalized in z=1 plane) and inverse depth
# cov2: covariance
measured_features:
- kf_idx: 0
ftr_idx: 15
pt: [0.33, -0.42, 0.2]
cov2:
- [280000, 600]
- [600, 280000]
# between frame constraints
# see README.md for more information
# keyframe_index_1/2: two indices of frame constraint
# relative_pose_Rp: T_{12}
# cov6: covariance
frame_constraints:
- keyframe_index_1: 0
keyframe_index_2: 1
relative_pose_Rp:
- [1, 0, 0, 0]
- [0, 1, 0, 0]
- [0, 0, 1, 0]
cov6:
- [3000, 0, 0, 0, 0, 0]
- [0, 3000, 0, 0, 0, 0]
- [0, 0, 3000, 0, 0, 0]
- [0, 0, 0, 100, 0, 0]
- [0, 0, 0, 0, 100, 0]
- [0, 0, 0, 0, 0, 100]