129 lines
4.9 KiB
Markdown
129 lines
4.9 KiB
Markdown
# MSCKF\_VIO
|
|
|
|
|
|
The `MSCKF_VIO` package is a stereo-photometric version of MSCKF. The software takes in synchronized stereo images and IMU messages and generates real-time 6DOF pose estimation of the IMU frame.
|
|
|
|
This approach is based on the paper written by Ke Sun et al.
|
|
[https://arxiv.org/abs/1712.00036](https://arxiv.org/abs/1712.00036) and their Stereo MSCKF implementation, which tightly fuse the matched feature information of a stereo image pair into a 6DOF Pose.
|
|
The approach implemented in this repository follows the semi-dense msckf approach tightly fusing the photometric
|
|
information around the matched featues into the covariance matrix, as described and derived in the master thesis [Pose Estimation using a Stereo-Photometric Multi-State Constraint Kalman Filter](http://raphael.maenle.net/resources/sp-msckf/maenle_master_thesis.pdf).
|
|
|
|
It's positioning is comparable to the approach from Ke Sun et al. with the photometric approach, with a higher
|
|
computational load, especially with larger image patches around the feature. A video explaining the approach can be
|
|
found on [https://youtu.be/HrqQywAnenQ](https://youtu.be/HrqQywAnenQ):
|
|
<br/>
|
|
<p align="center">
|
|
[](https://www.youtube.com/watch?v=HrqQywAnenQ)
|
|
</p>
|
|
|
|
<br/>
|
|
This software should be deployed using ROS Kinetic on Ubuntu 16.04 or 18.04.
|
|
|
|
|
|
## License
|
|
|
|
Penn Software License. See LICENSE.txt for further details.
|
|
|
|
## Dependencies
|
|
|
|
Most of the dependencies are standard including `Eigen`, `OpenCV`, and `Boost`. The standard shipment from Ubuntu 16.04 and ROS Kinetic works fine. One special requirement is `suitesparse`, which can be installed through,
|
|
|
|
```
|
|
sudo apt-get install libsuitesparse-dev
|
|
```
|
|
|
|
## Compling
|
|
The software is a standard catkin package. Make sure the package is on `ROS_PACKAGE_PATH` after cloning the package to your workspace. And the normal procedure for compiling a catkin package should work.
|
|
|
|
```
|
|
cd your_work_space
|
|
catkin_make --pkg msckf_vio --cmake-args -DCMAKE_BUILD_TYPE=Release
|
|
```
|
|
|
|
## EuRoC and UPenn Fast flight dataset example usage
|
|
|
|
First obtain either the [EuRoC](https://projects.asl.ethz.ch/datasets/doku.php?id=kmavvisualinertialdatasets) or the [UPenn fast flight](https://github.com/KumarRobotics/msckf_vio/wiki/Dataset) dataset.
|
|
|
|
Recommended EuRoC ROS Bags:
|
|
- [Vicon Room 1 01](http://robotics.ethz.ch/~asl-datasets/ijrr_euroc_mav_dataset/vicon_room1/V1_01_easy/V1_01_easy.bag)
|
|
- [Vicon Room 1 02](http://robotics.ethz.ch/~asl-datasets/ijrr_euroc_mav_dataset/vicon_room1/V1_02_easy/V1_02_easy.bag)
|
|
|
|
Once the `msckf_vio` is built and sourced (via `source <path to catkin_ws>/devel/setup.bash`), there are two launch files prepared for the [EuRoC](https://projects.asl.ethz.ch/datasets/doku.php?id=kmavvisualinertialdatasets) and [UPenn fast flight](https://github.com/KumarRobotics/msckf_vio/wiki/Dataset) dataset named `msckf_vio_euroc.launch` and `msckf_vio_fla.launch` respectively. Each launch files instantiates two ROS nodes:
|
|
|
|
* `image_processor` processes stereo images to detect and track features
|
|
* `vio` obtains feature measurements from the `image_processor` and tightly fuses them with the IMU messages to estimate pose.
|
|
|
|
These launch files can be executed via
|
|
|
|
```
|
|
roslaunch msckf_vio msckf_vio_euroc.launch
|
|
```
|
|
or
|
|
|
|
```
|
|
roslaunch msckf_vio msckf_vio_fla.launch
|
|
```
|
|
|
|
Once the nodes are running you need to run the dataset rosbags (in a different terminal), for example:
|
|
|
|
```
|
|
rosbag play V1_01_easy.bag
|
|
```
|
|
|
|
As mentioned in the previous section, **The robot is required to start from a stationary state in order to initialize the VIO successfully.**
|
|
|
|
To visualize the pose and feature estimates you can use the provided rviz configurations found in `msckf_vio/rviz` folder (EuRoC: `rviz_euroc_config.rviz`, Fast dataset: `rviz_fla_config.rviz`).
|
|
|
|
|
|
## ROS Nodes
|
|
|
|
The general structure is similar to the structure of the MSCKF implementation this repository is derived from.
|
|
|
|
### `image_processor` node
|
|
|
|
**Subscribed Topics**
|
|
|
|
`imu` (`sensor_msgs/Imu`)
|
|
|
|
IMU messages is used for compensating rotation in feature tracking, and 2-point RANSAC.
|
|
|
|
`cam[x]_image` (`sensor_msgs/Image`)
|
|
|
|
Synchronized stereo images.
|
|
|
|
**Published Topics**
|
|
|
|
`features` (`msckf_vio/CameraMeasurement`)
|
|
|
|
Records the feature measurements on the current stereo image pair.
|
|
|
|
`tracking_info` (`msckf_vio/TrackingInfo`)
|
|
|
|
Records the feature tracking status for debugging purpose.
|
|
|
|
`debug_stereo_img` (`sensor_msgs::Image`)
|
|
|
|
Draw current features on the stereo images for debugging purpose. Note that this debugging image is only generated upon subscription.
|
|
|
|
### `vio` node
|
|
|
|
**Subscribed Topics**
|
|
|
|
`imu` (`sensor_msgs/Imu`)
|
|
|
|
IMU measurements.
|
|
|
|
`features` (`msckf_vio/CameraMeasurement`)
|
|
|
|
Stereo feature measurements from the `image_processor` node.
|
|
|
|
**Published Topics**
|
|
|
|
`odom` (`nav_msgs/Odometry`)
|
|
|
|
Odometry of the IMU frame including a proper covariance.
|
|
|
|
`feature_point_cloud` (`sensor_msgs/PointCloud2`)
|
|
|
|
Shows current features in the map which is used for estimation.
|