This is an example secondary thread which provides loop closure in a loosely coupled manner for OpenVINS. This code was originally developed by the HKUST aerial robotics group and can be found in the VINS-Fusion repository. Here we stress that this is a loosely coupled method, thus no information is returned to the estimator to improve the underlying OpenVINS odometry. This codebase has been modified in a few key areas including: exposing more loop closure parameters, subscribing to camera intrinsics, simplifying configuration such that only topics need to be supplied, and some tweaks to the loop closure detection to improve frequency. Please see the below sections on the dependencies of the system along with a discussion of how the loop closure logic works, its improvements, and limitations.
This code is provided, as is, and is an example of how to use a secondary thread with the OpenVINS system. Thus, this code has been directly adapted from the VINS-Fusion repository without a thorough investigation of the underlying code. Therefore we don't have any guarantee of the accuracy or correctness. Additionally, for questions about the underlying implementation (besides discussing the changes introduced from the original codebase) we might not be able to answer in detail.
# setup our workspace
mkdir -p catkin_ws_ov/src/
cd catkin_ws_ov
catkin init
# repositories to clone
cd src
git clone https://github.com/rpng/open_vins.git
git clone https://github.com/rpng/ov_secondary.git
# go back to root and build
cd ..
catkin build -j4
# run the OpenVINS system and loop node
source devel/setup.bash
roslaunch ov_msckf pgeneva_ros_eth.launch
roslaunch loop_fusion posegraph.launch
We found that while the secondary pose graph works well in some cases, in many other cases it fails to improve performance or even hurt it. For example on the EurocMav dataset there isn't a clear winner in terms of ATE accuracy of the OpenVINS odometry poses being fed into the loop closure module and its published states. On these small room datasets, many loop closure candidates are rejected, and thus there are maybe 2-4 loop closures, with V1_02_medium dataset having none over the whole trajectory in most cases. Also to ensure there are enough points for PnP, the number of tracked features in ov_msckf needed to be increased to around 300.
On the other hand, there are cases where the loop closure clearly helps. For example, on a long dataset such as the corridor1 from the TUM-VI dataset the system running in monocular mode drifts towards the end of the dataset (blue). The loop closure (red) is able to correct this and ensure that the ending pose is correct relative to the start (the sensor system returns to the same location in the bottom left of this trajectory).
Looking at some more quantitative results on the TUM-VI dataset, a few runs on the room datasets can clearly show the advantage of loop closure. These room datasets are limits to the same vicon room environment, and get very frequent loop closures due to this. From the below table, it is very clear that using the secondary loop closure thread in most cases has a clear performance gain as compared to the standard odometry.
pose_graph_node.cpp
we get an image, pointcloud, odometry messages from the OpenVINS system.
window_keypoints
and keypoints
respectivelypose_graph.cpp
the addKeyFrame()
function will call on detectLoop()
to try to find a keyframeoptimize_buf
and will be read by the optimize4DoF()
threadyaw_drift,r_drift,t_drift
)vio_callback()
function)