# SuperPoint-SuperGlue-TensorRT-CIUS

**Repository Path**: zhuangwu/SuperPoint-SuperGlue-TensorRT-CIUS

## Basic Information

- **Project Name**: SuperPoint-SuperGlue-TensorRT-CIUS
- **Description**: 转自github/SuperPoint-SuperGlue-TensorRT
- **Primary Language**: Unknown
- **License**: Apache-2.0
- **Default Branch**: grid
- **Homepage**: None
- **GVP Project**: No

## Statistics

- **Stars**: 0
- **Forks**: 6
- **Created**: 2024-07-05
- **Last Updated**: 2024-07-05

## Categories & Tags

**Categories**: Uncategorized

**Tags**: None

## README

# CIUS SuperPoint + SuperGlue + TensorRT + **LK_flow**
## Demo
### Dual_channel (SuperPoint + SuperGlue + **LK_flow**) in Lab room
<a href="data/README_File/Dual_Channel_ID_indoor-2023-09-14_16.48.21.mp4">
   <img src="data/README_File/Dual_Channel_ID_indoor.png" width="1080px" />
</a>

### Usage 
Run the code after you install `TensorRT`, See the `CmakeLists.txt` to see the specification.
```sh
# On desktop
roslaunch super_point_glue_track desktop_real_slave.launch 
roslaunch super_point_glue_track desktop_real_master.launch
roslaunch super_point_glue_track desktop_real_opti_grid.launch
```

---

1. zph台式机表现
   - cuda-11.6，TensorRT-8.4.1.5 
   - 先删除原有engine，让其自己生成
   - 可以build superpoint 和 superglue 的engine，
   - 提取点1.7ms，匹配8.5ms

2. 笔记本表现
   - cuda-11.6，TensorRT-8.4.1.5
   - 先删除原有engine，让其自己生成
   - 320*240图像提取250个点，只需要4ms。

3. NVIDIA NX表现
   - Jetpack 5.1  cuda-11.4，TensorRT-8.5.2 cuDNN 8.6.0 OpenCV4.5.4
   - 先删除原有engine，让其自己生成，耗时非常久，大约半小时
   - 提取点14ms，匹配38ms

## 对比方法数据统计
zph台式机测试结果
- SURF
- ORB 匹配时间：提取60个ORB点，经过brute-force match,以及ransac过滤，用时15ms。

## 耗时统计
| Image Size: 640 x 480 | SuperPoint | SuperGlue | normal LK | Fast LK 6 |
|:---------------------:|:----------:|:---------:|:---------:|:---------:|
|       NVIDIA NX       |     26     |    50     |    4.2    |    27     |
|  NVIDIA NX TensorRT   |            |           |           |           |

## 多种匹配方式耗时统计
|            | SSF       | SS      | SURF     | ORB     |
|------------|-----------|---------|----------|---------|
| point: 50  | (23+40)/2 | 34 + 20 | 85  + 10 | 27 + 34 |
| point: 100 |           | 34 + 26 | 105 + 12 | 30 + 35 |
| point: 200 |           | 37 + 33 | 110 + 15 | 30 + 38 |
| point: 500 |           | 40 + 70 | 110 + 30 | 30 + 47 |
写point=500的平均耗时就可以，不同的keypoint点数可以放在曲线图中
在nvidia NX上 光流耗时 CPU：10ms ～ 20ms， CPU：1.5ms ～ 4ms

| SSF                   | SS | SURF | ORB |
|:---------------------:|:----------:|:---------:|:---------:|:---------:|
|       NVIDIA NX       |     26     |    50     |    4.2    |    27     |

## 准确性统计
|           Accuracy           |  Super  | normal LK | Fast LK 3  | Fast LK 5 |
|:----------------------------:|:-------:|:---------:|:----------:|:---------:|
|            Gazebo            | 1.04 MS |     MS    |            |           |
|         Meta_count           | 1.03 MS | 13.32 MS  |            |           |
|            indoor            | 7.47 MS | 58.83 MS  |            |           |
|           outdoor            | 7.47 MS | 58.83 MS  |            |           |

## 测试流程
### 由slave进行匹配方案 slave_core
1. `super_track_master_broadcast.cpp`发布带有深度信息的superpoint消息, 以及对应图像(用于演示match效果)
2. `super_track_slave_match.cpp`
   - 接收来自master的 带有深度信息的superpoint消息 以及 master的图像(用于演示match效果)
   - 进行superGlue匹配，并展示匹配效果
   - 发布匹配好的`MKPTS_XYZ`
3. `super_track_slave_opti.cpp`把`MKPTS_XYZ`和`master`以及`slave`的彩色图像接收，发现深度图不能实时获取，也就作罢
4. **结论**：在使用光流追踪时由于需要master的深度图信息，所以最好还是把匹配过程留在master上做。slave将图片和提取的superpoint发布出来

### 由master匹配方案 master_core
1. `super_track_slave_broadcast.cpp`发布带有深度信息的superpoint消息
2. `super_track_master_match.cpp`
   - 接收来自slave的 带有深度信息的superpoint消息 
   - 进行superGlue匹配
   - 发布匹配好的`MKPTS_XYZ`
3. `super_track__opti.cpp`把`MKPTS_XYZ`和`master`以及`slave`的彩色图像接收，以及`master`的深度图信息
   - 进行光流加速融合。


## 经验
- `keypoint_threshold`对应特征点的score分数，正常场景在0.1~0.7之间，越高的分数，在superpoint集合中点的排列随score呈降序。


## 光流追踪程序思路
1. 对不断传入的img0和img1彩色图片进行`FuseToImgPairs()`进行配对，把图片对作为一组放在map中，并用时间戳索引。
2. 对传来的mkpts数据进行ID关联
   2.1. 首先寻找这次引导数据的时间戳和图片队列中那个时间戳最近，然后依据此时间戳删除图片队列之前的图片对。
   2.2. 判断引导数据是首次还是后续，若是首次则直接给mkpts赋予ID，若是后续，则也是赋予临时ID，都是从0开始，到引导数目为止。
3. 在引导数据下，对存储图片后续队列持续追踪
   3.1. 使用for循环，从队列中依次取出一对图片
   3.2. 若是首次，则进行正常光流跟踪并输出匹配点对; 若是第二次引导，则进入fast track光流，并在后面进行特征引导融合。
4. 特征引导融合
   4.1. 上次追踪的点在`kpts0_map`和`kpts1_map`中，本次引导点在`kpts0_map_temp`和`kpts1_map_temp`中
   4.2. 对img0和img1分别 遍历所有`引导点`，对每个引导点检查上次`追踪点集`中是否有 临近点（8个像素范围以内），若落入临近则认为是`增强点集`。并从`追踪点集`中暂时删除这些点，也暂时删除对应的`引导点`。
   4.3. 然后将剩下`追踪点集`中的没有临近的点称为`衰退点集`，本版本处理中是直接清空这些点集，避免光流跟踪一直保留着这些点。
   4.4. 对`引导点`和`增强点集`都进行0和1的ID交叉检查，删除不一致的ID。
   4.5. 将`引导点`和`增强点集`都加入到最新点集中，作为下一次的`追踪点集`
5. 在没有引导帧的间隔期，对新增图像持续追踪点
   5.1. 由于图像队列一直在塞入新图像，所以用当前时间戳再去队列找，并作为起点，向后续图像队列开始做追踪。并更新当前时间戳。
6. 可视化匹配效果，检查当前深度图并发布消息。


## 计算匹配正确率的方法
使用SuperGlue中提供的`match_pairs.py`，本质是提供两个相机的匹配点`mkpts0`,`mkpts1`，加上cam2相对于cam1的在图像坐标系下相对位姿`T_0_1`，以及两个相机内参`K0`，`K1`
计算特征点对的对极极线误差。最重要的是`model/utils.py`中的`compute_epipolar_error`
使用
```shell
roslaunch super_point_glue_track wirte_image_with_pose.launch
```
可以产生带有图片名称对以及真实相对位姿的txt文件，同时将rostopic的对应图片消息写入磁盘。方便调用匹配正确率的python脚本。
使用superglue匹配示例计算如下
```shell
python match_pairs_changed.py --input_pairs /home/zph/hard_disk/rosbag/match_test/match_accelerate/img_kun01-image-mocap_successful-2023-03-05-16-27-37/image_pairs_with_pose_small_sample.txt --input_dir /home/zph/hard_disk/rosbag/match_test/match_accelerate/img_kun01-image-mocap_successful-2023-03-05-16-27-37/img_assets_small_sample --output_dir /home/zph/hard_disk/rosbag/match_test/match_accelerate/img_kun01-image-mocap_successful-2023-03-05-16-27-37/dump_match_pairs_small_sample --eval --viz --fast_viz
```
或者使用原生的`SuperGluePretrainedNetwork`项目中的
```shell
./match_pairs.py --eval --viz --fast_viz
```

## RoadMap
- [x] 使用cuda::cv进行光流计算加速，参考`https://github.com/pjrambo/VINS-Fusion-gpu`
- [ ] 针对A与B在光流追踪中因一方追踪失败而删除的点，由于再下次检测到该点时会以一个新的ID命名，这样不利于对点的追踪长度的累积统计。以A为主，以B为辅。
拟采用对因为B追踪失败而对应删除A的中的点归为`track_lost`std::map<size_t Id, cv::Point2f>，我们在`Normal_track`中继续对这些点保持追踪，并在一个新的`guide`到来fusion的时机，
我们检查新检测的匹配点是否有和`track_lost`距离相近的点，若判定是同一点则以旧的ID命名，若不是同一点，则以新的ID命名。
  **实验结果**发现很少有点是因为B中跟踪失误而导致A中删除的，反而这些消失的点再次出现时可以继续继承之前的ID，让我怀疑是因为最大数量和有效深度对特征点进行了截取，导致时而是有效匹配，时而不是有效匹配。
## Trouble Known
当刚关闭Gazebo后，可能roscore被占用，或未知状态。此时直接进行slave， master， opti的话。并不能运行。必须重新开一个roscore。

---

# SuperPoint SuperGlue TensorRT
SuperPoint and SuperGlue with TensorRT.

## Demo
<img src="image/superpoint_superglue_tensorrt.gif" width = "640" height = "240"  alt="match_image" border="10" />

* This demo was tested on the Quadro P620 GPU.

## Baseline

| Image Size: 320 x 240  | RTX3080 | Quadro P620 | 
|:----------------------:|:-------:|:-----------:|
| SuperPoint (250 points)| 1.04 MS | 13.61 MS    | 
| SuperPoint (257 points)| 1.03 MS | 13.32 MS    | 
| SuperGlue (256 dims)   | 7.47 MS | 58.83 MS    |
| FPS (superpoint one time + superglue one time)   | $\approx$ 117 FPS | $\approx$ 13 FPS    |

## Docker(Recommand)
```bash
docker pull yuefan2022/tensorrt-ubuntu20.04-cuda11.6:latest
docker run -it --env DISPLAY=$DISPLAY --volume /tmp/.X11-unix:/tmp/.X11-unix --privileged --runtime nvidia --gpus all --volume ${PWD}:/workspace --workdir /workspace --name tensorrt yuefan2022/tensorrt-ubuntu20.04-cuda11.6:latest /bin/bash
```

## Environment required
* CUDA==11.6
* TensorRT==8.4.1.5
* OpenCV>=4.0
* EIGEN
* yaml-cpp

## Convert model(Optional)
The converted model is already provided in the [weights](./weights) folder, if you are using the pretrained model officially provided by [SuperPoint and SuperGlue](https://github.com/magicleap/SuperGluePretrainedNetwork), you do not need to go through this step.
```bash
python convert2onnx/convert_superpoint_to_onnx.py --weight_file superpoint_pth_file_path --output_dir superpoint_onnx_file_dir
python convert2onnx/convert_superglue_to_onnx.py --weight_file superglue_pth_file_path --output_dir superglue_onnx_file_dir
```

## Build and run
```bash
git clone https://github.com/yuefanhao/SuperPoint-SuperGlue-TensorRT.git
cd SuperPoint-SuperGlue-TensorRT
mkdir build
cd build
cmake ..
make
# test on image pairs 100 times, the output image will be saved in the build dir
./superpointglue_image  ../config/config.yaml ../weights/ ${PWD}/../image/image0.png ${PWD}/../image/image1.png
# test on the folder with image sequence, output images will be saved in the param assigned dir
./superpointglue_sequence  ../config/config.yaml ../weights/ ${PWD}/../image/freiburg_sequence/ ${PWD}/../image/freiburg_sequence/match_images/
```
The default image size param is 320x240, if you need to modify the image size in the config file, you should delete the old .engine file in the weights dir.

## Samples
```c++
#include "super_point.h"
#include "super_glue.h"

// read image
cv::Mat image0 = cv::imread("../image/image0.png", cv::IMREAD_GRAYSCALE);
cv::Mat image1 = cv::imread("../image/image1.png", cv::IMREAD_GRAYSCALE);

// read config from file
Configs configs("../config/config.yaml", "../weights/");

// create superpoint detector and superglue matcher
auto superpoint = std::make_shared<SuperPoint>(configs.superpoint_config);
auto superglue = std::make_shared<SuperGlue>(configs.superglue_config);

// build engine
superpoint->build();
superglue->build();

// infer superpoint
Eigen::Matrix<double, 259, Eigen::Dynamic> feature_points0, feature_points1;
superpoint->infer(image0, feature_points0);
superpoint->infer(image1, feature_points1);

// infer superglue
std::vector<cv::DMatch> superglue_matches;
superglue->matching_points(feature_points0, feature_points1, superglue_matches);
 
```

## Acknowledgements
[SuperPoint](https://github.com/magicleap/SuperPointPretrainedNetwork) [SuperGlue](https://github.com/magicleap/SuperGluePretrainedNetwork) [TensorRT](https://github.com/NVIDIA/TensorRT) [AirVO](https://github.com/xukuanHIT/AirVO)