Overview

This repository contains an implementation of a new attempt to solve the dual-view position estimation problem. Classically, the problem is solved by first extracting and matching features followed by outlier elimination and pose estimation using geometric verification methods. We try to break the information bottleneck in this pipeline by optimizing feature correspondences and pose jointly in a unified optimization problem.

Quick Start

Dependencies

Feature matching requires a Python virtual environment with the hierarchical localization toolbox installed. Correspondence and pose optimization and benchmarks are tested in the following C++ environment:

• ROS Noetic, Google Logging Library:

  sudo apt install libgoogle-glog-dev

• OpenCV (≥ 4.4, where SIFT is supported).

• We use the RANSAC implementation in OpenGV.

• We use the GNC implementation in the following version of GTSAM:

  git clone git@github.com:borglab/gtsam.git
  git checkout f19138
  cmake .. -DGTSAM_USE_SYSTEM_EIGEN=yes && make

• We solve the joint optimization of correspondence and pose using Gurobi (10.0.2) and Ceres (2.1.0).

Compilation

The code is released as a ROS package, please run the following command in a ROS workspace to compile it:

catkin_make -DCMAKE_BUILD_TYPE=Release -DCMAKE_EXPORT_COMPILE_COMMANDS=Yes

Run a Demo

To run the released demo, run the following commands in a terminal:

source devel/setup.zsh
roslaunch rpe online_rpe.launch

In another terminal where the prepared Python virtual environment is activated, run the feature matching server:

python src/rpe/python/matching_server.py --feature_extraction_conf disk --matching_conf disk+lightglue

For 3D visualization:

rviz -d src/rpe/config/monitor.rviz

Then play the demo bag:

rosbag play -l demo.bag

After receiving images, the online_rpe_node enters the INIT state and a frame selection window will pop up, where you can select the first frame in the dual-view pose estimation by typing Space in the terminal, which will transfer the online_rpe_node to the SOLVING_BY_TRIGGER state. In this state, you can select the second frame of the dual-view pose estimation by typing t in the terminal and trigger the pose solution.

After the joint optimization of correspondence and pose is completed, you can see the difference between the feature correspondences before and after the optimization in the pop-up window, as well as the 3D results of different methods in RViz.

Licence

The source code is released under GPLv3 license.

Maintaince

We are still working on extending the proposed system and improving code reliability.

For any technical issues, please contact Yunfeng Li (yunfengli@zju.edu.cn).

For commercial inquiries, please contact Fei GAO (fgaoaa@zju.edu.cn).