OpenVINS Vehicle Updates

This is the official implementation of the paper Visual-Inertial Odometry aided by Speed and Steering Angle Measurements. It enables OpenVINS to use speed, steering angle, and wheel speeds measurements in different variants for ground vehicles such as cars or wheeled robots. All edits to the code are marked with the string OVVU. This way it should be fairly easy to detect all changes.

Usage

This code is currently tested on Ubuntu 18.04 and ROS 1 melodic as well as ROS 2 eloquent and foxy the monocular camera IMU setup. Additional speed and steering measurements are supplied with a stamped Ackermann drive message available from standard ROS packages on the topic /ackermann0. When using wheel speed measurements and the differential drive model you need to use the custom Wheel speeds message on the topic /wheel_speeds0 added to ov_core in the msg folder.

Different vehicle-related updates are set with the ros parameter vehicle_update_mode. The best performing method for cars is VEHICLE_UPDATE_PREINTEGRATED_SINGLE_TRACK. The other options are given in the following enum: c++ enum VehicleUpdateMode { // Basic OpenVINS algorithm without vehicle updates VEHICLE_UPDATE_NONE, // Basic OpenVINS algorithm with speed update, propagating the state at every speed measurement VEHICLE_UPDATE_SPEED, // Basic OpenVINS algorithm with steering update, propagating the state at every steering measurement VEHICLE_UPDATE_STEERING, // Basic OpenVINS algorithm with speed and steering update, propagating the state at every speed and steering measurement VEHICLE_UPDATE_SPEED_AND_STEERING, // Basic OpenVINS algorithm with 3DOF odometry (x, y, yaw) update using single track model VEHICLE_UPDATE_PREINTEGRATED_SINGLE_TRACK, // Basic OpenVINS algorithm with 3DOF odometry (x, y, yaw) update using differential drive model VEHICLE_UPDATE_PREINTEGRATED_DIFFERENTIAL_DRIVE, // When the mode is not known we exit the program VEHICLE_UPDATE_UNKNOWN, };

ROS 1 Usage

I added new launch files for the serial usage serialvehicleupdates.launch and regular topic subscription method subscribevehicleupdates.launch. Take a look at them for an overview of the additional vehicle-related parameters.

To run the algorithm on a rosbag use the following command:

roslaunch ovmsckf serialvehicleupdates.launch bag:=/path/to/bag.bag vehicleupdatemode:=VEHICLEUPDATEPREINTEGRATEDSINGLETRACK pathest:=/path/to/OpenVINS-format/result.txt pathrpgest:=/path/to/RPGtrajectoryevaluation-format/stampedtrajestimate.txt path_time:=/path/to/computation-time-results.txt

Note that I added the parameter path_rpg_est to output the results in a format that enables the usage of the rpgtrajectoryevaluation toolbox.

ROS 2 Usage

I added new launch files for regular topic subscription nodes subscribevehicleupdates_eloquent.launch.py and subscribevehicleupdates.launch.py. The node can be started as follows:

ros2 launch ovmsckf subscribevehicleupdates.launch.py vehicleupdatemode:=VEHICLEUPDATEPREINTEGRATEDSINGLETRACK savetotalstate:=true filepathposeestrpg:=/path/to/RPGtrajectoryevaluation-format/stampedtrajestimate.txt

In another terminal a rosbag has to be played: ros2 bag play aros2bag.db3

Different updating schemes

The following image display the different updating schemes for vehicle-related measurements. Speed and steering measurements can be used whenever the respective measurement arrives. The time scheme is displayed in the center column of the image. Furthermore, speed and steering measurements in conjunction as well as wheel speed measurements can be used to perform a preintegrated odometry update, much like the visual update of the Multi-State Constraint Kalman Filter. This updating scheme is represented on the right column.

Single-track and differential drive model

Two kinematic motion models are applied: the single-track model and the differential drive model. While both can be used for cars, the first one is better suited and reports lower odometry errors in our experiments. The latter can be used for wheeled robots which do not supply steering angle information.

In the following table all updating methods and their corresponding vehicle_update_mode, legend entry for subsequent plots, and applied motion models are summarized:

| vehicleupdatemode | legend | Update frequency | Motion model | | :------------------------------------------ | :-------------- | :--------------: | :----------------: | | VEHICLEUPDATENONE | ov | N/A | N/A | | VEHICLEUPDATESPEEDPROPAGATE | ovvs | CAN sensor | N/A | | VEHICLEUPDATESPEEDANDSTEERINGPROPAGATE | ovvs+sa | CAN sensor | single-track | | VEHICLEUPDATEPREINTEGRATEDSINGLETRACK | ovsingle-track | camera ensor | single-track | | VEHICLEUPDATEPREINTEGRATEDDIFFERENTIAL | ov_diff-drive | camera sensor | differential drive |

Results

A top view plot for a typical urban drive of roughly 4 kilometers and a median speed of 38 km/h is displayed here. The end points of each algorithm are marked with crosses. ovsingle-track performs the best.

<img src="docs/readmeimages/eschborn2topreadme.png" title="Eschborn2 top plot" width="700px">

A relative translation error plot for the given dataset displays the typical performance of each variant. The estimation results are aligned multiple times for the given subtrajectory lengths and the error metrics are calculated.

Citation

For more details refer to our paper and if you use this work please cite the following: txt @Conference{Serov2022Fusion, Title = {Visual-Inertial Odometry aided by Speed and Steering Angle Measurements}, Author = {Andreas Serov and Joachim Clemens and Kerstin Schill}, Booktitle = {IEEE 25th International Conference on Information Fusion (Fusion)}, Year = {2022}, Address = {Linkping, Sweden}, Url = {\url{https://github.com/aserbremen/open_vins}} } If you have any question, don't hesitate to open an issue.

Original Credit / Licensing

This code is based on OpenVINS by the Robot Perception and Navigation Group (RPNG) at the University of Delaware. For researchers that have leveraged or compared to this work, please cite the following:

txt @Conference{Geneva2020ICRA, Title = {{OpenVINS}: A Research Platform for Visual-Inertial Estimation}, Author = {Patrick Geneva and Kevin Eckenhoff and Woosik Lee and Yulin Yang and Guoquan Huang}, Booktitle = {Proc. of the IEEE International Conference on Robotics and Automation}, Year = {2020}, Address = {Paris, France}, Url = {\url{https://github.com/rpng/open_vins}}, Doi = {https://doi.org/10.23919/FUSION49751.2022.9841243, }

The codebase and documentation is licensed under the GNU General Public License v3 (GPL-3). You must preserve the copyright and license notices in your derivative work and make available the complete source code with modifications under the same license (see this; this is not legal advice).