This is the cerebro module for VINS-Fusion. The aim of this project is better loop detection and recover from kidnap. The cerebro node connects to the vins_estimator nodes of VINS-Fusion (with ros interface). The DataManager class handles all the incoming data. Visualization handles all the visualization. Cerebro class handles the loop closure intelligence part. It publishes a LoopMsg which contains timestamps of the identified loopcandidate along with the computed relative pose between the pair. The pose computation needs a stereo pair for reliable pose computation. This is a multi-threaded object oriented implementation and I observe a CPU load factor of about 2.0. A separate node handles pose graph solver (it is in github-repo ).
A MORE UPTO DATE README IS AVAILABLE IN THE ORIGINAL REPO (mpkuse/cerebro )
Manuscript: Preprint : https://arxiv.org/abs/1904.06962
Alternate link to highlight video: Dailymotion
We show our system's performance for AR tasks under kidnap. We are able to track and relocalize despite long kidnaps.
Our system is able to save the constructed map to disk. It is also able to load a previously stored map and relocalize live and in realtime from that map. We demo the effect of relocalization from map with our group's SurfelMapping code
For more demonstration, have a look at my youtube playlist
- ETHZ EuroC Dataset
- TUM Visual Inertial Dataset
- UPenn - Penncosyvio
- ADVIO ARKIT, Tango logging.
- Our MyntEye (Stereo+IMU) One-drive-link
I highly recommend the already deployed packages with docker. Run the roscore on your host pc and all the packages run inside of docker container. rviz runs on the host pc.
I assume you have a PC with a graphics card and cuda9 working smoothly and nvidia-docker installed.
$(host) export ROS_HOSTNAME=`hostname`
$(host) roscore
# assume that host has the ip address 172.17.0.1 in docker-network aka docker0
$(host) docker run --runtime=nvidia -it \
--add-host `hostname`:172.17.0.1 \
--env ROS_MASTER_URI=http://`hostname`:11311/ \
--env CUDA_VISIBLE_DEVICES=0 \
--hostname happy_go \
--name happy_go \
mpkuse/kusevisionkit:vins-kidnap bash
$(host) rviz # inside rviz open config cerebro/config/good-viz.rviz. If you open rviz in a new tab you might need to do set ROS_HOSTNAME again.
$(docker) roslaunch cerebro mynteye_vinsfusion.launch
OR
$(docker) roslaunch cerebro euroc_vinsfusion.launch
$(host) rosbag play 1.bag
Edit the launch file as needed.
If you are unfamiliar with docker, you may want to read my blog post on using docker for computer vision researchers. You might want to have a look at my test ros-package to ensure things work with docker docker_ros_test.
You will need a) VINS-Fusion (with modification for reset by mpkuse), b) cerebro
and c) solve_keyframe_pose_graph. Besure to setup a catkin_ws and make sure
your ROS works correctly.
- ROS Kinetic
- Eigen3
- Ceres
- OpenCV3 (should also work with 2.4 (not tested), 3.3 and 3.4)
- Theia-sfm - OpenImageIO. tested with version Release-1.7.6RC1 - RocksDB. tested with version v5.9.2
Get VINS-Fusion Working GIT
I recommend you use my fork of VINS-Fusion, in which I have fixed some bugs and added mechanism for reseting the VINS.
cd catkin_ws/src
#git clone https://github.com/HKUST-Aerial-Robotics/VINS-Fusion.git
git clone https://github.com/mpkuse/VINS-Fusion
cd ../
catkin_make
source catkin_ws/devel/setup.bash
Make sure your vins-fusion can compile and run correctly. See vins-fusion github repo for the latest information on prerequisites and compilation instructions. For compatibility I recommend using my fork of vins-mono/vins-fusion. Some minor modifications have been made by me for working with kidnap cases.
Cerebro GIT
cd catkin_ws/src/
git clone https://github.com/mpkuse/cerebro
cd ../
catkin_make
This has 2 exectables. a) ros server that takes as input an image and returns a image descriptor. b) cerebro_node, this finds the loop candidates and computes the relative poses. I have also included my trained model (about 4 MB) in this package (located scripts/keras.model). The pose computation uses the stereo pair in this node. This node publishes the loopcandidate's relative pose which is expected to be consumed the pose-graph solver.
If you wish to train your own model, you may use my learning code here.
Threads:
- Main Thread : ros-callbacks
- data_association_th : Sync the incoming image data and incoming data vins_estimator.
- desc_th : Consumes the images to produce whole-image-descriptors.
- dot_product_th : Dot product of current image descriptor with all the previous ones.
- loopcandidate_consumer_th : Computes the relative pose at the loopcandidates. Publishes the loopEdge.
- kidnap_th : Identifies kidnap. If kidnap publishes the reset signals for vins_estimator.
- viz_th : Publishes the image-pair, and more things for debugging and analysis.
Pose Graph Solver GIT
Use my pose graph solver, github-repo. The differences between this implementation and the original from VINS-Fusion is that this can handle kidnap cases, handles multiple world co-ordinate frames and it uses a switch-constraint formulation of the pose-graph problem. It uses the disjoint set forest to maintain a set association of world co-ordinate systems.
cd catkin_ws/src/
git clone https://github.com/mpkuse/solve_keyframe_pose_graph
cd ../
catkin_make
Threads:
- Main thread : ros-callbacks for odometry poses (from vins_estimator) and LoopMsg (from cerebro).
- SLAM : Monitors the node-poses and loop-edges, on new loop-edges constructs and solves the pose-graph optimization problem.
- th4 : Publish latest odometry.
- th5 : Display image to visualize disjoint set datastructure.
- th6 : Publish corrected poses, Different color for nodes in different co-ordinate systems.
AR Demo GIT
To make ar_demo similar to my video above you could use this package. It takes the corrected pose from the pose graph solver (along with the worldID) and renders a polygonal mesh on the camera-image. It also have support for ground plane estimation, checkout the readme of that package.
cd catkin_ws/src
git clone https://github.com/mpkuse/ar_demo
cd ../
catkin_make
With cerebro node it is possible to live run the vins and make it log all the details to file for further analysis/debugging. This might be useful for researchers and other Ph.D. student to help VINS-Fusion improve further. see github/mpkuse/vins_mono. It basically contains unit tests and some standalone tools which might come in handy. If you are looking to help improve VINS-fusion or cerebro also look at 'Development Guidelines'.
roslaunch cerebro mynteye_vinsfusion.launch
You can get some of my bag files collected with the mynteye camera HERE. More
example launch files in folder launch, all the config files which contains
calibration info are in folder config.
If you are developing I still recommend using docker. with -v flags in docker you could mount your
pc's folders on the docker. I recommend keeping all the packages in folder docker_ws_slam/catkin_ws/src
on your host pc. And all the rosbags in folder /media/mpkuse/Bulk_Data. And then mount these
two folders on the docker-container. Edit the following command as needed.
docker run --runtime=nvidia -it -v /media/mpkuse/Bulk_Data/:/Bulk_Data -v /home/mpkuse/docker_ws_slam:/app --add-host `hostname`:172.17.0.1 --env ROS_MASTER_URI=http://`hostname`:11311/ --env CUDA_VISIBLE_DEVICES=0 --hostname happy_go --name happy_go mpkuse/kusevisionkit:ros-kinetic-vins bash
Each of my classes can export the data they hold as json objects and image files. Look at the
end of main() in cerebro_node.cpp and modify as needed to extract more debug data. Similarly
the pose graph solver can also be debugged.
For streamlining printing messages
I have preprocessor macros at the start of function implementation (of classes DataManager and Cerebro),
read the comments there and edit as per need. Try to implement your algorithms in
object oriented way and using the producer-consumer paradigm. Look at my thread-mains for example.
Finally, sensible PR with bug fixes, enhancements are welcome!
Manohar Kuse mpkuse@connect.ust.hk