The QPEP (Quadratic Pose Estimation Problems) Enhanced VINS-Mono (Originated from https://github.com/HKUST-Aerial-Robotics/VINS-Fusion) . The details of the QPEP can be found in https://github.com/zarathustr/LibQPEP.
We use the QPEP to solve the problem that in previous version of VINS-Mono, the PnP algorithm in the initialization SFM framework was not completely working. The original problem was that, due to the usage of OpenCV PnP and improper initial guess (e.g. R = I and t = 0), the PnP algorithm, being a highly non-convex one, was not able to converge to a satisfactory global minimum. This problem has been solved by using the QPEP, which guarantees the global optimality.
Some of the codes are modified to adapt the ROS Noetic and Clang compiler. Therefore, the use of ROS Noetic is recommended. First go to https://github.com/zarathustr/LibQPEP for the repo of LibQPEP. Then, follow the instructions to install the library:
cd LibQPEP
mkdir build
cd build
cmake .. -DCMAKE_BUILD_TYPE=Release
make install
Then, clone the current repo to your catkin workspace and conduct catkin build:
cd catkin_ws/src
git clone https://github.com/zarathustr/VINS-Mono-QPEP
catkin build
Once finished, use the following command to run the demo from EuroC MAV Dataset:
roslaunch vins_estimator_QPEP euroc.launch
In another independent terminal, run
roslaunch vins_estimator vins_rviz.launch
In the terminal window of vins_estimator_QPEP, you will find:
QPEP PnP Converged
if the QPEP has been successfully executed for initialization of VINS-Mono.