├── README.md ├── SLAM ├── Zdzn.yaml ├── launch │ └── SP.launch └── src │ ├── .ros_sp.cc.swp │ └── ros_sp.cc ├── SimEnv └── README.md ├── SuperPointPretrainedNetwork ├── README.md ├── assets │ ├── DL4VSLAM_talk.pdf │ ├── icl_snippet │ │ ├── 250.png │ │ ├── 254.png │ │ ├── 258.png │ │ ├── 262.png │ │ ├── 266.png │ │ ├── 270.png │ │ ├── 274.png │ │ ├── 278.png │ │ ├── 282.png │ │ ├── 286.png │ │ ├── 290.png │ │ ├── 294.png │ │ ├── 298.png │ │ ├── 302.png │ │ ├── 306.png │ │ ├── 310.png │ │ ├── 314.png │ │ ├── 318.png │ │ ├── 322.png │ │ ├── 326.png │ │ ├── 330.png │ │ ├── 334.png │ │ ├── 338.png │ │ ├── 342.png │ │ ├── 346.png │ │ ├── 350.png │ │ ├── 354.png │ │ ├── 358.png │ │ ├── 362.png │ │ ├── 366.png │ │ └── magicleap.png │ ├── magicleap.png │ ├── nyu_snippet.mp4 │ ├── processed_freiburg.gif │ ├── processed_icl.gif │ ├── processed_kitti.gif │ ├── processed_monovo.gif │ ├── processed_ms7.gif │ └── processed_nyu.gif ├── demo_superpoint.py ├── superpoint_v1.pth └── test.png └── modules.png /README.md: -------------------------------------------------------------------------------- 1 | # SuperPoint_SLAM 2 | 使用SuperPoint方法提取特征点，来代替ORB-SLAM中的ORB特征。整个项目会由三个部分，分别是： 3 | 4 | - 仿真环境**SimEnv**。主要实现仿真环境的构建、机器人及其传感器的放置、机器人与环境的交互，具有键盘控制机器人运动的功能，需要发布`/camera/rgb/`和`/camera/depth/`的相关消息。 5 | - **SLAM模块**。基于ORB-SLAM2框架,将ORB特征全面替换为SuperPoint特征，其中为了实现建图功能，额外添加了`PointCloud.cc`，用于生成点云，然后建图。 6 | - SuperPoint特征提取**SuperPointPretrainedNetwork**。运行其中的`demo_superpoint.py`，能够实时处理RGB图片，生成特征点文件。 7 | 8 | ### 模块间关系 9 | 10 | ![](./modules.png) 11 | 12 | 为了保证时间能够对上，图中的四步能够保持一致（在特征点处理图片时，不会再有新的图片保存下来），我们设计了如下方法： 13 | 14 | 基本思想：通过往`/dev/shm/tmpSP/`中读写文件实现同步，`/dev/shm/`这个文件夹下的文件能够实现快速读写。 15 | 16 | ```c++ 17 | cv::imwrite(filename, RGB); 18 | system("touch /dev/shm/tmpSP/ready.txt"); 19 | while( ! file_exist("/dev/shm/tmpSP/done.txt")){ 20 | usleep(5000); 21 | } 22 | Camera_Pose = mpSLAM->TrackRGBD(cv_ptrRGB->image,cv_ptrD->image,cv_ptrRGB->header.stamp.toSec()); 23 | system("rm -r /dev/shm/tmpSP/*"); 24 | ``` 25 | 26 | SLAM模块中先保存接收到的图片，然后创建一个`ready.txt`，用来标记图片已保存，然后会一直查找文件夹下有无`done.txt`，若查找到，才可继续运行并接收下一张图片，在接收图片前要清空文件夹。 27 | 28 | ```python 29 | while ( not os.path.exists(listenlingfolder+'ready.txt') ): 30 | time.sleep(0.01) 31 | # print("1234") 32 | os.remove(listenlingfolder+'ready.txt') 33 | f_list = glob.glob( listenlingfolder+'/*.png' ) 34 | assert len(f_list) == 1 35 | for file_i in f_list: 36 | if file_i.split('.')[-1] == 'png': 37 | print(file_i) 38 | targetpre = re.sub(r'\.png', "", file_i) 39 | print(targetpre) 40 | getImage2SP(file_i,targetpre,fe) 41 | os.remove(file_i) 42 | pathlib.Path(listenlingfolder + '/done.txt').touch() 43 | while ( os.path.exists(listenlingfolder+'/done.txt') ): 44 | time.sleep(0.01) 45 | ``` 46 | 47 | 特征点模块中会一直查询有无`ready.txt`文件，查找到了，说明图片已存在，就可开始处理。处理前先删除`ready.txt`，除了完后会生成`kpts.txt`和`feats.txt`，同时再新建一个`done.txt`，用来告诉SLAM模块上一帧已经处理完毕，可以接收新的图片了。 48 | 49 | ### 主要问题 50 | 51 | - **特征点数不够**，导致效果不佳。主要原因可能还是仿真环境不够逼真，仿真环境中的RGBD相机获取到的图片与实际场景的照片差距较大。解决方案：使用AirSim替换Gazebo、改变原先使用的相机。 52 | - **各模块间的帧率不匹配**，造成仿真时较卡顿。解决方案：仿真时具体调整。 53 | - 优化处理速度，尽可能提高帧率。 -------------------------------------------------------------------------------- /SLAM/Zdzn.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #-------------------------------------------------------------------------------------------- 4 | # Camera Parameters. Adjust them! 5 | #-------------------------------------------------------------------------------------------- 6 | 7 | # Camera calibration and distortion parameters (OpenCV) 8 | Camera.fx: 277.19135641132203 9 | Camera.fy: 277.19135641132203 10 | Camera.cx: 160.5 11 | Camera.cy: 120.5 12 | 13 | Camera.k1: 0.0 14 | Camera.k2: 0.0 15 | Camera.p1: 0.0 16 | Camera.p2: 0.0 17 | 18 | Camera.width: 320 19 | Camera.height: 240 20 | 21 | # Camera frames per second 22 | Camera.fps: 50.0 23 | 24 | # IR projector baseline times fx (aprox.) 25 | Camera.bf: 40.0 26 | 27 | # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) 28 | Camera.RGB: 1 29 | 30 | # Close/Far threshold. Baseline times. 31 | ThDepth: 40.0 32 | 33 | # Deptmap values factor 34 | DepthMapFactor: 1.0 35 | 36 | #-------------------------------------------------------------------------------------------- 37 | # ORB Parameters 38 | #-------------------------------------------------------------------------------------------- 39 | 40 | # ORB Extractor: Number of features per image 41 | ORBextractor.nFeatures: 1000 42 | 43 | # ORB Extractor: Scale factor between levels in the scale pyramid 44 | ORBextractor.scaleFactor: 1.2 45 | 46 | # ORB Extractor: Number of levels in the scale pyramid 47 | ORBextractor.nLevels: 8 48 | 49 | # ORB Extractor: Fast threshold 50 | # Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. 51 | # Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST 52 | # You can lower these values if your images have low contrast 53 | ORBextractor.iniThFAST: 20 54 | ORBextractor.minThFAST: 7 55 | 56 | #-------------------------------------------------------------------------------------------- 57 | # Viewer Parameters 58 | #-------------------------------------------------------------------------------------------- 59 | Viewer.KeyFrameSize: 0.05 60 | Viewer.KeyFrameLineWidth: 1 61 | Viewer.GraphLineWidth: 0.9 62 | Viewer.PointSize:2 63 | Viewer.CameraSize: 0.08 64 | Viewer.CameraLineWidth: 3 65 | Viewer.ViewpointX: 0 66 | Viewer.ViewpointY: -0.7 67 | Viewer.ViewpointZ: -1.8 68 | Viewer.ViewpointF: 500 69 | PointCloudMapping.Resolution: 0.05 70 | -------------------------------------------------------------------------------- /SLAM/launch/SP.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /SLAM/src/.ros_sp.cc.swp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/FarawaySail/SuperPoint_SLAM/e34cc9fdf321096797f7eb8c6fce33be2f4225ef/SLAM/src/.ros_sp.cc.swp -------------------------------------------------------------------------------- /SLAM/src/ros_sp.cc: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM2. 3 | * 4 | * Copyright (C) 2014-2016 Raúl Mur-Artal (University of Zaragoza) 5 | * For more information see 6 | * 7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify 8 | * it under the terms of the GNU General Public License as published by 9 | * the Free Software Foundation, either version 3 of the License, or 10 | * (at your option) any later version. 11 | * 12 | * ORB-SLAM2 is distributed in the hope that it will be useful, 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | * GNU General Public License for more details. 16 | * 17 | * You should have received a copy of the GNU General Public License 18 | * along with ORB-SLAM2. If not, see . 19 | */ 20 | 21 | 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include "geometry_msgs/PoseWithCovarianceStamped.h" 43 | 44 | #include 45 | 46 | #include"../../../include/System.h" 47 | 48 | cv::Mat RGB; 49 | cv::Mat DEPTH; 50 | ros::Time rgbTime; 51 | bool newImgFlg=false; 52 | using namespace std; 53 | 54 | ros::Publisher CamPose_Pub; 55 | ros::Publisher Camodom_Pub; 56 | ros::Publisher odom_pub; 57 | 58 | geometry_msgs::PoseStamped Cam_Pose; 59 | geometry_msgs::PoseWithCovarianceStamped Cam_odom; 60 | 61 | cv::Mat Camera_Pose; 62 | tf::Transform orb_slam; 63 | tf::TransformBroadcaster * orb_slam_broadcaster; 64 | std::vector Pose_quat(4); 65 | std::vector Pose_trans(3); 66 | 67 | ros::Time current_time, last_time; 68 | double lastx=0,lasty=0,lastth=0; 69 | 70 | void Pub_CamPose(cv::Mat &pose); 71 | 72 | class ImageGrabber 73 | { 74 | public: 75 | ImageGrabber(ORB_SLAM2::System* pSLAM):mpSLAM(pSLAM){} 76 | 77 | void GrabRGBD(const sensor_msgs::ImageConstPtr& msgRGB,const sensor_msgs::ImageConstPtr& msgD); 78 | 79 | ORB_SLAM2::System* mpSLAM; 80 | }; 81 | 82 | int main(int argc, char **argv) 83 | { 84 | ros::init(argc, argv, "SP"); 85 | ros::start(); 86 | 87 | if(argc != 3) 88 | { 89 | cerr << endl << "Usage: rosrun ORB_SLAM2 RGBD path_to_vocabulary path_to_settings" << endl; 90 | ros::shutdown(); 91 | return 1; 92 | } 93 | 94 | // Create SLAM system. It initializes all system threads and gets ready to process frames. 95 | ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::RGBD, true, false, false, false, false); //viewer, localmap, loop, bow, trackonly 96 | 97 | ImageGrabber igb(&SLAM); 98 | 99 | ros::NodeHandle nh; 100 | message_filters::Subscriber rgb_sub(nh, "/camera/rgb/image_raw", 1); 101 | message_filters::Subscriber depth_sub(nh, "/camera/depth_registered/image_raw", 1); 102 | typedef message_filters::sync_policies::ApproximateTime sync_pol; 103 | message_filters::Synchronizer sync(sync_pol(10), rgb_sub,depth_sub); 104 | sync.registerCallback(boost::bind(&ImageGrabber::GrabRGBD,&igb,_1,_2)); 105 | 106 | CamPose_Pub = nh.advertise("/Camera_Pose",1); 107 | Camodom_Pub = nh.advertise("/Camera_Odom", 1); 108 | odom_pub = nh.advertise("/odom", 50); 109 | 110 | while( ros::ok() ) 111 | { 112 | ros::spinOnce(); 113 | if(!newImgFlg) 114 | { 115 | continue; 116 | } 117 | Pub_CamPose(Camera_Pose); 118 | newImgFlg=false; 119 | } 120 | // Stop all threads 121 | SLAM.Shutdown(); 122 | 123 | // Save camera trajectory 124 | SLAM.SaveKeyFrameTrajectoryTUM("/home/tsui/KeyFrameTrajectory.txt"); 125 | 126 | ros::shutdown(); 127 | 128 | return 0; 129 | } 130 | 131 | inline bool file_exist (const std::string& name) { 132 | ifstream f(name.c_str()); 133 | return f.good(); 134 | } 135 | 136 | int GetFiles(char** file_list, const char* dirname) 137 | { 138 | DIR * dir; 139 | struct dirent * ptr; 140 | int i=0; 141 | dir = opendir(dirname); //打开一个目录 142 | while((ptr = readdir(dir)) != NULL) //循环读取目录数据 143 | { 144 | //printf("d_name : %s\n", ptr->d_name); //输出文件名 145 | strcpy(file_list[i],ptr->d_name ); //存储到数组 146 | if ( ++i>=5 ) break; 147 | } 148 | closedir(dir);//关闭目录指针 149 | return (i - 2); 150 | } 151 | 152 | void ImageGrabber::GrabRGBD(const sensor_msgs::ImageConstPtr& msgRGB,const sensor_msgs::ImageConstPtr& msgD) 153 | { 154 | // Copy the ros image message to cv::Mat. 155 | newImgFlg=true; 156 | cv_bridge::CvImageConstPtr cv_ptrRGB; 157 | try 158 | { 159 | cv_ptrRGB = cv_bridge::toCvShare(msgRGB); 160 | } 161 | catch (cv_bridge::Exception& e) 162 | { 163 | ROS_ERROR("cv_bridge exception: %s", e.what()); 164 | return; 165 | } 166 | 167 | cv_bridge::CvImageConstPtr cv_ptrD; 168 | try 169 | { 170 | cv_ptrD = cv_bridge::toCvShare(msgD); 171 | } 172 | catch (cv_bridge::Exception& e) 173 | { 174 | ROS_ERROR("cv_bridge exception: %s", e.what()); 175 | return; 176 | } 177 | 178 | // Get RGB and Depth image 179 | RGB=cv_ptrRGB->image; 180 | DEPTH=cv_ptrD->image; 181 | rgbTime=msgRGB->header.stamp; 182 | 183 | string dir = "/dev/shm/tmpSP/"; //文件夹 184 | string filename = dir + to_string(rgbTime.toSec()) + ".png";; //RGB图文件名 185 | // 读取特征点文件信息 186 | 187 | cv::imwrite(filename, RGB); 188 | system("touch /dev/shm/tmpSP/ready.txt"); 189 | while( ! file_exist("/dev/shm/tmpSP/done.txt")){ 190 | usleep(5000); 191 | } 192 | Camera_Pose = mpSLAM->TrackRGBD(cv_ptrRGB->image,cv_ptrD->image,cv_ptrRGB->header.stamp.toSec()); 193 | system("rm -r /dev/shm/tmpSP/*"); 194 | } 195 | 196 | void Pub_CamPose(cv::Mat &pose) 197 | { 198 | cv::Mat Rwc(3,3,CV_32F); 199 | cv::Mat twc(3,1,CV_32F); 200 | Eigen::Matrix rotationMat; 201 | orb_slam_broadcaster = new tf::TransformBroadcaster; 202 | if(pose.dims<2 || pose.rows < 3) 203 | { 204 | Rwc = Rwc; 205 | twc = twc; 206 | } 207 | else 208 | { 209 | Rwc = pose.rowRange(0,3).colRange(0,3).t(); 210 | twc = -Rwc*pose.rowRange(0,3).col(3); 211 | 212 | rotationMat << Rwc.at(0,0), Rwc.at(0,1), Rwc.at(0,2), 213 | Rwc.at(1,0), Rwc.at(1,1), Rwc.at(1,2), 214 | Rwc.at(2,0), Rwc.at(2,1), Rwc.at(2,2); 215 | Eigen::Quaterniond Q(rotationMat); 216 | 217 | Pose_quat[0] = Q.x(); Pose_quat[1] = Q.y(); 218 | Pose_quat[2] = Q.z(); Pose_quat[3] = Q.w(); 219 | 220 | Pose_trans[0] = twc.at(0); 221 | Pose_trans[1] = twc.at(1); 222 | Pose_trans[2] = twc.at(2); 223 | 224 | orb_slam.setOrigin(tf::Vector3(Pose_trans[2], -Pose_trans[0], -Pose_trans[1])); 225 | orb_slam.setRotation(tf::Quaternion(Q.z(), -Q.x(), -Q.y(), Q.w())); 226 | orb_slam_broadcaster->sendTransform(tf::StampedTransform(orb_slam, ros::Time::now(), "/map", "/base_link")); 227 | 228 | Cam_Pose.header.stamp = ros::Time::now(); 229 | Cam_Pose.header.frame_id = "/map"; 230 | tf::pointTFToMsg(orb_slam.getOrigin(), Cam_Pose.pose.position); 231 | tf::quaternionTFToMsg(orb_slam.getRotation(), Cam_Pose.pose.orientation); 232 | 233 | Cam_odom.header.stamp = ros::Time::now(); 234 | Cam_odom.header.frame_id = "/map"; 235 | tf::pointTFToMsg(orb_slam.getOrigin(), Cam_odom.pose.pose.position); 236 | tf::quaternionTFToMsg(orb_slam.getRotation(), Cam_odom.pose.pose.orientation); 237 | Cam_odom.pose.covariance = {0.01, 0, 0, 0, 0, 0, 238 | 0, 0.01, 0, 0, 0, 0, 239 | 0, 0, 0.01, 0, 0, 0, 240 | 0, 0, 0, 0.01, 0, 0, 241 | 0, 0, 0, 0, 0.01, 0, 242 | 0, 0, 0, 0, 0, 0.01}; 243 | 244 | CamPose_Pub.publish(Cam_Pose); 245 | Camodom_Pub.publish(Cam_odom); 246 | 247 | nav_msgs::Odometry odom; 248 | odom.header.stamp =ros::Time::now(); 249 | odom.header.frame_id = "/map"; 250 | 251 | // Set the position 252 | odom.pose.pose.position = Cam_odom.pose.pose.position; 253 | odom.pose.pose.orientation = Cam_odom.pose.pose.orientation; 254 | 255 | // Set the velocity 256 | odom.child_frame_id = "/base_link"; 257 | current_time = ros::Time::now(); 258 | double dt = (current_time - last_time).toSec(); 259 | double vx = (Cam_odom.pose.pose.position.x - lastx)/dt; 260 | double vy = (Cam_odom.pose.pose.position.y - lasty)/dt; 261 | double vth = (Cam_odom.pose.pose.orientation.z - lastth)/dt; 262 | 263 | odom.twist.twist.linear.x = vx; 264 | odom.twist.twist.linear.y = vy; 265 | odom.twist.twist.angular.z = vth; 266 | 267 | // Publish the message 268 | odom_pub.publish(odom); 269 | 270 | last_time = current_time; 271 | lastx = Cam_odom.pose.pose.position.x; 272 | lasty = Cam_odom.pose.pose.position.y; 273 | lastth = Cam_odom.pose.pose.orientation.z; 274 | } 275 | } 276 | 277 | 278 | -------------------------------------------------------------------------------- /SimEnv/README.md: -------------------------------------------------------------------------------- 1 | # 仿真环境 2 | 3 | 待补全 -------------------------------------------------------------------------------- /SuperPointPretrainedNetwork/README.md: -------------------------------------------------------------------------------- 1 | 2 | 3 | ### Research @ Magic Leap 4 | 5 | # SuperPoint Weights File and Demo Script 6 | 7 | ## Introduction 8 | This repo contains the pretrained SuperPoint network, as implemented by the originating authors. SuperPoint is a research project at Magic Leap. The SuperPoint network is a fully convolutional deep neural network trained to detect interest points and compute their accompanying descriptors. The detected points and descriptors can thus be used for various image-to-image matching tasks. For more details please see 9 | 10 | * Full paper PDF: [SuperPoint: Self-Supervised Interest Point Detection and Description](https://arxiv.org/abs/1712.07629) 11 | 12 | * Presentation PDF: [Talk at CVPR Deep Learning for Visual SLAM Workshop 2018](assets/DL4VSLAM_talk.pdf) 13 | 14 | * Authors: *Daniel DeTone, Tomasz Malisiewicz, Andrew Rabinovich* 15 | 16 | This demo showcases a simple sparse optical flow point tracker that uses SuperPoint to detect points and match them across video sequences. The repo contains two core files (1) a PyTorch weights file and (2) a python deployment script that defines the network, loads images and runs the pytorch weights file on them, creating a sparse optical flow visualization. Here are videos of the demo running on various publically available datsets: 17 | 18 | Freiburg RGBD: 19 | 20 | 21 | KITTI: 22 | 23 | 24 | Microsoft 7 Scenes: 25 | 26 | 27 | MonoVO: 28 | 29 | 30 | 31 | ## Dependencies 32 | * [OpenCV](https://opencv.org/) python >= 3.4 33 | * [PyTorch](https://pytorch.org/) >= 0.4 34 | 35 | This repo depends on a few standard pythonic modules, plus OpenCV and PyTorch. These commands usually work (tested on Mac and Ubuntu) for installing the two libraries: 36 | 37 | ```sh 38 | pip install opencv-python 39 | pip install torch 40 | ``` 41 | 42 | ## Running the Demo 43 | This demo will run the SuperPoint network on an image sequence and compute points and descriptors from the images, using a helper class called `SuperPointFrontend`. The tracks are formed by the `PointTracker` class which finds sequential pair-wise nearest neighbors using two-way matching of the points' descriptors. The demo script uses a helper class called `VideoStreamer` which can process inputs from three different input streams: 44 | 45 | 1. A directory of images, such as .png or .jpg 46 | 2. A video file, such as .mp4 or .avi 47 | 3. A USB Webcam 48 | 49 | ### Run the demo on provided directory of images in CPU-mode: 50 | 51 | ```sh 52 | ./demo_superpoint.py assets/icl_snippet/ 53 | ``` 54 | You should see the following output from the ICL-NUIM sequence snippet: 55 | 56 | 57 | 58 | ### Run the demo on provided .mp4 file in GPU-mode: 59 | 60 | ```sh 61 | ./demo_superpoint.py assets/nyu_snippet.mp4 --cuda 62 | ``` 63 | You should see the following output from the NYU sequence snippet: 64 | 65 | 66 | 67 | ### Run a live demo via webcam (id #1) in CPU-mode: 68 | 69 | ```sh 70 | ./demo_superpoint.py camera --camid=1 71 | ``` 72 | 73 | ### Run the demo on a remote GPU (no display) on 640x480 images and write the output to `myoutput/` 74 | ```sh 75 | ./demo_superpoint.py assets/icl_snippet/ --W=640 --H=480 --no_display --write --write_dir=myoutput/ 76 | ``` 77 | 78 | ### Additional useful command line parameters 79 | 80 | * Use `--H` to change the input image height (default: 120). 81 | * Use `--W` to change the input image width (default: 160). 82 | * Use `--display_scale` to scale the output visualization image height and width (default: 2). 83 | * Use `--cuda` flag to enable the GPU. 84 | * Use `--img_glob` to change the image file extension (default: *.png). 85 | * Use `--min_length` to change the minimum track length (default: 2). 86 | * Use `--max_length` to change the maximum track length (default: 5). 87 | * Use `--conf_thresh` to change the point confidence threshold (default: 0.015). 88 | * Use `--nn_thresh` to change the descriptor matching distance threshold (default: 0.7). 89 | * Use `--show_extra` to show more computer vision outputs. 90 | * Press the `q` key to quit. 91 | 92 | 93 | ## BibTeX Citation 94 | ```txt 95 | @inproceedings{detone18superpoint, 96 | author = {Daniel DeTone and 97 | Tomasz Malisiewicz and 98 | Andrew Rabinovich}, 99 | title = {SuperPoint: Self-Supervised Interest Point Detection and Description}, 100 | booktitle = {CVPR Deep Learning for Visual SLAM Workshop}, 101 | year = {2018}, 102 | url = {http://arxiv.org/abs/1712.07629} 103 | } 104 | ``` 105 | 106 | ## Additional Notes 107 | * We do not intend to release the SuperPoint training or evaluation code, please do not email us to ask for it. 108 | * We do not intend to release the Synthetic Shapes dataset used to bootstrap the SuperPoint training, please do not email us to ask for it. 109 | * We use bi-linear interpolation rather than the bi-cubic interpolation described in the paper to sample the descriptor as it is faster and gave us similar results. 110 | -------------------------------------------------------------------------------- /SuperPointPretrainedNetwork/assets/DL4VSLAM_talk.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/FarawaySail/SuperPoint_SLAM/e34cc9fdf321096797f7eb8c6fce33be2f4225ef/SuperPointPretrainedNetwork/assets/DL4VSLAM_talk.pdf -------------------------------------------------------------------------------- /SuperPointPretrainedNetwork/assets/icl_snippet/250.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/FarawaySail/SuperPoint_SLAM/e34cc9fdf321096797f7eb8c6fce33be2f4225ef/SuperPointPretrainedNetwork/assets/icl_snippet/250.png -------------------------------------------------------------------------------- /SuperPointPretrainedNetwork/assets/icl_snippet/254.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/FarawaySail/SuperPoint_SLAM/e34cc9fdf321096797f7eb8c6fce33be2f4225ef/SuperPointPretrainedNetwork/assets/icl_snippet/254.png -------------------------------------------------------------------------------- /SuperPointPretrainedNetwork/assets/icl_snippet/258.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/FarawaySail/SuperPoint_SLAM/e34cc9fdf321096797f7eb8c6fce33be2f4225ef/SuperPointPretrainedNetwork/assets/icl_snippet/258.png -------------------------------------------------------------------------------- /SuperPointPretrainedNetwork/assets/icl_snippet/262.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/FarawaySail/SuperPoint_SLAM/e34cc9fdf321096797f7eb8c6fce33be2f4225ef/SuperPointPretrainedNetwork/assets/icl_snippet/262.png -------------------------------------------------------------------------------- /SuperPointPretrainedNetwork/assets/icl_snippet/266.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/FarawaySail/SuperPoint_SLAM/e34cc9fdf321096797f7eb8c6fce33be2f4225ef/SuperPointPretrainedNetwork/assets/icl_snippet/266.png -------------------------------------------------------------------------------- /SuperPointPretrainedNetwork/assets/icl_snippet/270.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/FarawaySail/SuperPoint_SLAM/e34cc9fdf321096797f7eb8c6fce33be2f4225ef/SuperPointPretrainedNetwork/assets/icl_snippet/270.png -------------------------------------------------------------------------------- /SuperPointPretrainedNetwork/assets/icl_snippet/274.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/FarawaySail/SuperPoint_SLAM/e34cc9fdf321096797f7eb8c6fce33be2f4225ef/SuperPointPretrainedNetwork/assets/icl_snippet/274.png -------------------------------------------------------------------------------- /SuperPointPretrainedNetwork/assets/icl_snippet/278.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/FarawaySail/SuperPoint_SLAM/e34cc9fdf321096797f7eb8c6fce33be2f4225ef/SuperPointPretrainedNetwork/assets/icl_snippet/278.png -------------------------------------------------------------------------------- /SuperPointPretrainedNetwork/assets/icl_snippet/282.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/FarawaySail/SuperPoint_SLAM/e34cc9fdf321096797f7eb8c6fce33be2f4225ef/SuperPointPretrainedNetwork/assets/icl_snippet/282.png -------------------------------------------------------------------------------- /SuperPointPretrainedNetwork/assets/icl_snippet/286.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/FarawaySail/SuperPoint_SLAM/e34cc9fdf321096797f7eb8c6fce33be2f4225ef/SuperPointPretrainedNetwork/assets/icl_snippet/286.png -------------------------------------------------------------------------------- /SuperPointPretrainedNetwork/assets/icl_snippet/290.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/FarawaySail/SuperPoint_SLAM/e34cc9fdf321096797f7eb8c6fce33be2f4225ef/SuperPointPretrainedNetwork/assets/icl_snippet/290.png -------------------------------------------------------------------------------- /SuperPointPretrainedNetwork/assets/icl_snippet/294.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/FarawaySail/SuperPoint_SLAM/e34cc9fdf321096797f7eb8c6fce33be2f4225ef/SuperPointPretrainedNetwork/assets/icl_snippet/294.png -------------------------------------------------------------------------------- /SuperPointPretrainedNetwork/assets/icl_snippet/298.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/FarawaySail/SuperPoint_SLAM/e34cc9fdf321096797f7eb8c6fce33be2f4225ef/SuperPointPretrainedNetwork/assets/icl_snippet/298.png -------------------------------------------------------------------------------- /SuperPointPretrainedNetwork/assets/icl_snippet/302.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/FarawaySail/SuperPoint_SLAM/e34cc9fdf321096797f7eb8c6fce33be2f4225ef/SuperPointPretrainedNetwork/assets/icl_snippet/302.png -------------------------------------------------------------------------------- /SuperPointPretrainedNetwork/assets/icl_snippet/306.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/FarawaySail/SuperPoint_SLAM/e34cc9fdf321096797f7eb8c6fce33be2f4225ef/SuperPointPretrainedNetwork/assets/icl_snippet/306.png -------------------------------------------------------------------------------- /SuperPointPretrainedNetwork/assets/icl_snippet/310.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/FarawaySail/SuperPoint_SLAM/e34cc9fdf321096797f7eb8c6fce33be2f4225ef/SuperPointPretrainedNetwork/assets/icl_snippet/310.png -------------------------------------------------------------------------------- /SuperPointPretrainedNetwork/assets/icl_snippet/314.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/FarawaySail/SuperPoint_SLAM/e34cc9fdf321096797f7eb8c6fce33be2f4225ef/SuperPointPretrainedNetwork/assets/icl_snippet/314.png -------------------------------------------------------------------------------- /SuperPointPretrainedNetwork/assets/icl_snippet/318.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/FarawaySail/SuperPoint_SLAM/e34cc9fdf321096797f7eb8c6fce33be2f4225ef/SuperPointPretrainedNetwork/assets/icl_snippet/318.png -------------------------------------------------------------------------------- /SuperPointPretrainedNetwork/assets/icl_snippet/322.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/FarawaySail/SuperPoint_SLAM/e34cc9fdf321096797f7eb8c6fce33be2f4225ef/SuperPointPretrainedNetwork/assets/icl_snippet/322.png -------------------------------------------------------------------------------- /SuperPointPretrainedNetwork/assets/icl_snippet/326.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/FarawaySail/SuperPoint_SLAM/e34cc9fdf321096797f7eb8c6fce33be2f4225ef/SuperPointPretrainedNetwork/assets/icl_snippet/326.png -------------------------------------------------------------------------------- /SuperPointPretrainedNetwork/assets/icl_snippet/330.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/FarawaySail/SuperPoint_SLAM/e34cc9fdf321096797f7eb8c6fce33be2f4225ef/SuperPointPretrainedNetwork/assets/icl_snippet/330.png -------------------------------------------------------------------------------- /SuperPointPretrainedNetwork/assets/icl_snippet/334.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/FarawaySail/SuperPoint_SLAM/e34cc9fdf321096797f7eb8c6fce33be2f4225ef/SuperPointPretrainedNetwork/assets/icl_snippet/334.png -------------------------------------------------------------------------------- /SuperPointPretrainedNetwork/assets/icl_snippet/338.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/FarawaySail/SuperPoint_SLAM/e34cc9fdf321096797f7eb8c6fce33be2f4225ef/SuperPointPretrainedNetwork/assets/icl_snippet/338.png -------------------------------------------------------------------------------- /SuperPointPretrainedNetwork/assets/icl_snippet/342.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/FarawaySail/SuperPoint_SLAM/e34cc9fdf321096797f7eb8c6fce33be2f4225ef/SuperPointPretrainedNetwork/assets/icl_snippet/342.png -------------------------------------------------------------------------------- /SuperPointPretrainedNetwork/assets/icl_snippet/346.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/FarawaySail/SuperPoint_SLAM/e34cc9fdf321096797f7eb8c6fce33be2f4225ef/SuperPointPretrainedNetwork/assets/icl_snippet/346.png -------------------------------------------------------------------------------- /SuperPointPretrainedNetwork/assets/icl_snippet/350.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/FarawaySail/SuperPoint_SLAM/e34cc9fdf321096797f7eb8c6fce33be2f4225ef/SuperPointPretrainedNetwork/assets/icl_snippet/350.png -------------------------------------------------------------------------------- /SuperPointPretrainedNetwork/assets/icl_snippet/354.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/FarawaySail/SuperPoint_SLAM/e34cc9fdf321096797f7eb8c6fce33be2f4225ef/SuperPointPretrainedNetwork/assets/icl_snippet/354.png -------------------------------------------------------------------------------- /SuperPointPretrainedNetwork/assets/icl_snippet/358.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/FarawaySail/SuperPoint_SLAM/e34cc9fdf321096797f7eb8c6fce33be2f4225ef/SuperPointPretrainedNetwork/assets/icl_snippet/358.png -------------------------------------------------------------------------------- /SuperPointPretrainedNetwork/assets/icl_snippet/362.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/FarawaySail/SuperPoint_SLAM/e34cc9fdf321096797f7eb8c6fce33be2f4225ef/SuperPointPretrainedNetwork/assets/icl_snippet/362.png -------------------------------------------------------------------------------- /SuperPointPretrainedNetwork/assets/icl_snippet/366.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/FarawaySail/SuperPoint_SLAM/e34cc9fdf321096797f7eb8c6fce33be2f4225ef/SuperPointPretrainedNetwork/assets/icl_snippet/366.png -------------------------------------------------------------------------------- /SuperPointPretrainedNetwork/assets/icl_snippet/magicleap.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/FarawaySail/SuperPoint_SLAM/e34cc9fdf321096797f7eb8c6fce33be2f4225ef/SuperPointPretrainedNetwork/assets/icl_snippet/magicleap.png -------------------------------------------------------------------------------- /SuperPointPretrainedNetwork/assets/magicleap.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/FarawaySail/SuperPoint_SLAM/e34cc9fdf321096797f7eb8c6fce33be2f4225ef/SuperPointPretrainedNetwork/assets/magicleap.png -------------------------------------------------------------------------------- /SuperPointPretrainedNetwork/assets/nyu_snippet.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/FarawaySail/SuperPoint_SLAM/e34cc9fdf321096797f7eb8c6fce33be2f4225ef/SuperPointPretrainedNetwork/assets/nyu_snippet.mp4 -------------------------------------------------------------------------------- /SuperPointPretrainedNetwork/assets/processed_freiburg.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/FarawaySail/SuperPoint_SLAM/e34cc9fdf321096797f7eb8c6fce33be2f4225ef/SuperPointPretrainedNetwork/assets/processed_freiburg.gif -------------------------------------------------------------------------------- /SuperPointPretrainedNetwork/assets/processed_icl.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/FarawaySail/SuperPoint_SLAM/e34cc9fdf321096797f7eb8c6fce33be2f4225ef/SuperPointPretrainedNetwork/assets/processed_icl.gif -------------------------------------------------------------------------------- /SuperPointPretrainedNetwork/assets/processed_kitti.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/FarawaySail/SuperPoint_SLAM/e34cc9fdf321096797f7eb8c6fce33be2f4225ef/SuperPointPretrainedNetwork/assets/processed_kitti.gif -------------------------------------------------------------------------------- /SuperPointPretrainedNetwork/assets/processed_monovo.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/FarawaySail/SuperPoint_SLAM/e34cc9fdf321096797f7eb8c6fce33be2f4225ef/SuperPointPretrainedNetwork/assets/processed_monovo.gif -------------------------------------------------------------------------------- /SuperPointPretrainedNetwork/assets/processed_ms7.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/FarawaySail/SuperPoint_SLAM/e34cc9fdf321096797f7eb8c6fce33be2f4225ef/SuperPointPretrainedNetwork/assets/processed_ms7.gif -------------------------------------------------------------------------------- /SuperPointPretrainedNetwork/assets/processed_nyu.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/FarawaySail/SuperPoint_SLAM/e34cc9fdf321096797f7eb8c6fce33be2f4225ef/SuperPointPretrainedNetwork/assets/processed_nyu.gif -------------------------------------------------------------------------------- /SuperPointPretrainedNetwork/demo_superpoint.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # 3 | # %BANNER_BEGIN% 4 | # --------------------------------------------------------------------- 5 | # %COPYRIGHT_BEGIN% 6 | # 7 | # Magic Leap, Inc. ("COMPANY") CONFIDENTIAL 8 | # 9 | # Unpublished Copyright (c) 2018 10 | # Magic Leap, Inc., All Rights Reserved. 11 | # 12 | # NOTICE: All information contained herein is, and remains the property 13 | # of COMPANY. The intellectual and technical concepts contained herein 14 | # are proprietary to COMPANY and may be covered by U.S. and Foreign 15 | # Patents, patents in process, and are protected by trade secret or 16 | # copyright law. Dissemination of this information or reproduction of 17 | # this material is strictly forbidden unless prior written permission is 18 | # obtained from COMPANY. Access to the source code contained herein is 19 | # hereby forbidden to anyone except current COMPANY employees, managers 20 | # or contractors who have executed Confidentiality and Non-disclosure 21 | # agreements explicitly covering such access. 22 | # 23 | # The copyright notice above does not evidence any actual or intended 24 | # publication or disclosure of this source code, which includes 25 | # information that is confidential and/or proprietary, and is a trade 26 | # secret, of COMPANY. ANY REPRODUCTION, MODIFICATION, DISTRIBUTION, 27 | # PUBLIC PERFORMANCE, OR PUBLIC DISPLAY OF OR THROUGH USE OF THIS 28 | # SOURCE CODE WITHOUT THE EXPRESS WRITTEN CONSENT OF COMPANY IS 29 | # STRICTLY PROHIBITED, AND IN VIOLATION OF APPLICABLE LAWS AND 30 | # INTERNATIONAL TREATIES. THE RECEIPT OR POSSESSION OF THIS SOURCE 31 | # CODE AND/OR RELATED INFORMATION DOES NOT CONVEY OR IMPLY ANY RIGHTS 32 | # TO REPRODUCE, DISCLOSE OR DISTRIBUTE ITS CONTENTS, OR TO MANUFACTURE, 33 | # USE, OR SELL ANYTHING THAT IT MAY DESCRIBE, IN WHOLE OR IN PART. 34 | # 35 | # %COPYRIGHT_END% 36 | # ---------------------------------------------------------------------- 37 | # %AUTHORS_BEGIN% 38 | # 39 | # Originating Authors: Daniel DeTone (ddetone) 40 | # Tomasz Malisiewicz (tmalisiewicz) 41 | # 42 | # %AUTHORS_END% 43 | # --------------------------------------------------------------------*/ 44 | # %BANNER_END% 45 | 46 | 47 | import argparse 48 | import glob 49 | import numpy as np 50 | import os 51 | import time 52 | import re 53 | 54 | 55 | import cv2 56 | import torch 57 | import pathlib 58 | 59 | # Stub to warn about opencv version. 60 | if int(cv2.__version__[0]) < 3: # pragma: no cover 61 | print('Warning: OpenCV 3 is not installed') 62 | 63 | # Jet colormap for visualization. 64 | myjet = np.array([[0. , 0. , 0.5 ], 65 | [0. , 0. , 0.99910873], 66 | [0. , 0.37843137, 1. ], 67 | [0. , 0.83333333, 1. ], 68 | [0.30044276, 1. , 0.66729918], 69 | [0.66729918, 1. , 0.30044276], 70 | [1. , 0.90123457, 0. ], 71 | [1. , 0.48002905, 0. ], 72 | [0.99910873, 0.07334786, 0. ], 73 | [0.5 , 0. , 0. ]]) 74 | 75 | class SuperPointNet(torch.nn.Module): 76 | """ Pytorch definition of SuperPoint Network. """ 77 | def __init__(self): 78 | super(SuperPointNet, self).__init__() 79 | self.relu = torch.nn.ReLU(inplace=True) 80 | self.pool = torch.nn.MaxPool2d(kernel_size=2, stride=2) 81 | c1, c2, c3, c4, c5, d1 = 64, 64, 128, 128, 256, 256 82 | # Shared Encoder. 83 | self.conv1a = torch.nn.Conv2d(1, c1, kernel_size=3, stride=1, padding=1) 84 | self.conv1b = torch.nn.Conv2d(c1, c1, kernel_size=3, stride=1, padding=1) 85 | self.conv2a = torch.nn.Conv2d(c1, c2, kernel_size=3, stride=1, padding=1) 86 | self.conv2b = torch.nn.Conv2d(c2, c2, kernel_size=3, stride=1, padding=1) 87 | self.conv3a = torch.nn.Conv2d(c2, c3, kernel_size=3, stride=1, padding=1) 88 | self.conv3b = torch.nn.Conv2d(c3, c3, kernel_size=3, stride=1, padding=1) 89 | self.conv4a = torch.nn.Conv2d(c3, c4, kernel_size=3, stride=1, padding=1) 90 | self.conv4b = torch.nn.Conv2d(c4, c4, kernel_size=3, stride=1, padding=1) 91 | # Detector Head. 92 | self.convPa = torch.nn.Conv2d(c4, c5, kernel_size=3, stride=1, padding=1) 93 | self.convPb = torch.nn.Conv2d(c5, 65, kernel_size=1, stride=1, padding=0) 94 | # Descriptor Head. 95 | self.convDa = torch.nn.Conv2d(c4, c5, kernel_size=3, stride=1, padding=1) 96 | self.convDb = torch.nn.Conv2d(c5, d1, kernel_size=1, stride=1, padding=0) 97 | 98 | def forward(self, x): 99 | """ Forward pass that jointly computes unprocessed point and descriptor 100 | tensors. 101 | Input 102 | x: Image pytorch tensor shaped N x 1 x H x W. 103 | Output 104 | semi: Output point pytorch tensor shaped N x 65 x H/8 x W/8. 105 | desc: Output descriptor pytorch tensor shaped N x 256 x H/8 x W/8. 106 | """ 107 | # Shared Encoder. 108 | x = self.relu(self.conv1a(x)) 109 | x = self.relu(self.conv1b(x)) 110 | x = self.pool(x) 111 | x = self.relu(self.conv2a(x)) 112 | x = self.relu(self.conv2b(x)) 113 | x = self.pool(x) 114 | x = self.relu(self.conv3a(x)) 115 | x = self.relu(self.conv3b(x)) 116 | x = self.pool(x) 117 | x = self.relu(self.conv4a(x)) 118 | x = self.relu(self.conv4b(x)) 119 | # Detector Head. 120 | cPa = self.relu(self.convPa(x)) 121 | semi = self.convPb(cPa) 122 | # Descriptor Head. 123 | cDa = self.relu(self.convDa(x)) 124 | desc = self.convDb(cDa) 125 | dn = torch.norm(desc, p=2, dim=1) # Compute the norm. 126 | desc = desc.div(torch.unsqueeze(dn, 1)) # Divide by norm to normalize. 127 | return semi, desc 128 | 129 | 130 | class SuperPointFrontend(object): 131 | """ Wrapper around pytorch net to help with pre and post image processing. """ 132 | def __init__(self, weights_path, nms_dist, conf_thresh, nn_thresh, 133 | cuda=False): 134 | self.name = 'SuperPoint' 135 | self.cuda = cuda 136 | self.nms_dist = nms_dist 137 | self.conf_thresh = conf_thresh 138 | self.nn_thresh = nn_thresh # L2 descriptor distance for good match. 139 | self.cell = 8 # Size of each output cell. Keep this fixed. 140 | self.border_remove = 4 # Remove points this close to the border. 141 | 142 | # Load the network in inference mode. 143 | self.net = SuperPointNet() 144 | if cuda: 145 | # Train on GPU, deploy on GPU. 146 | self.net.load_state_dict(torch.load(weights_path)) 147 | self.net = self.net.cuda() 148 | else: 149 | # Train on GPU, deploy on CPU. 150 | self.net.load_state_dict(torch.load(weights_path, 151 | map_location=lambda storage, loc: storage)) 152 | self.net.eval() 153 | 154 | def nms_fast(self, in_corners, H, W, dist_thresh): 155 | """ 156 | Run a faster approximate Non-Max-Suppression on numpy corners shaped: 157 | 3xN [x_i,y_i,conf_i]^T 158 | 159 | Algo summary: Create a grid sized HxW. Assign each corner location a 1, rest 160 | are zeros. Iterate through all the 1's and convert them either to -1 or 0. 161 | Suppress points by setting nearby values to 0. 162 | 163 | Grid Value Legend: 164 | -1 : Kept. 165 | 0 : Empty or suppressed. 166 | 1 : To be processed (converted to either kept or supressed). 167 | 168 | NOTE: The NMS first rounds points to integers, so NMS distance might not 169 | be exactly dist_thresh. It also assumes points are within image boundaries. 170 | 171 | Inputs 172 | in_corners - 3xN numpy array with corners [x_i, y_i, confidence_i]^T. 173 | H - Image height. 174 | W - Image width. 175 | dist_thresh - Distance to suppress, measured as an infinty norm distance. 176 | Returns 177 | nmsed_corners - 3xN numpy matrix with surviving corners. 178 | nmsed_inds - N length numpy vector with surviving corner indices. 179 | """ 180 | grid = np.zeros((H, W)).astype(int) # Track NMS data. 181 | inds = np.zeros((H, W)).astype(int) # Store indices of points. 182 | # Sort by confidence and round to nearest int. 183 | inds1 = np.argsort(-in_corners[2,:]) 184 | corners = in_corners[:,inds1] 185 | rcorners = corners[:2,:].round().astype(int) # Rounded corners. 186 | # Check for edge case of 0 or 1 corners. 187 | if rcorners.shape[1] == 0: 188 | return np.zeros((3,0)).astype(int), np.zeros(0).astype(int) 189 | if rcorners.shape[1] == 1: 190 | out = np.vstack((rcorners, in_corners[2])).reshape(3,1) 191 | return out, np.zeros((1)).astype(int) 192 | # Initialize the grid. 193 | for i, rc in enumerate(rcorners.T): 194 | grid[rcorners[1,i], rcorners[0,i]] = 1 195 | inds[rcorners[1,i], rcorners[0,i]] = i 196 | # Pad the border of the grid, so that we can NMS points near the border. 197 | pad = dist_thresh 198 | grid = np.pad(grid, ((pad,pad), (pad,pad)), mode='constant') 199 | # Iterate through points, highest to lowest conf, suppress neighborhood. 200 | count = 0 201 | for i, rc in enumerate(rcorners.T): 202 | # Account for top and left padding. 203 | pt = (rc[0]+pad, rc[1]+pad) 204 | if grid[pt[1], pt[0]] == 1: # If not yet suppressed. 205 | grid[pt[1]-pad:pt[1]+pad+1, pt[0]-pad:pt[0]+pad+1] = 0 206 | grid[pt[1], pt[0]] = -1 207 | count += 1 208 | # Get all surviving -1's and return sorted array of remaining corners. 209 | keepy, keepx = np.where(grid==-1) 210 | keepy, keepx = keepy - pad, keepx - pad 211 | inds_keep = inds[keepy, keepx] 212 | out = corners[:, inds_keep] 213 | values = out[-1, :] 214 | inds2 = np.argsort(-values) 215 | out = out[:, inds2] 216 | out_inds = inds1[inds_keep[inds2]] 217 | return out, out_inds 218 | 219 | def run(self, img): 220 | """ Process a numpy image to extract points and descriptors. 221 | Input 222 | img - HxW numpy float32 input image in range [0,1]. 223 | Output 224 | corners - 3xN numpy array with corners [x_i, y_i, confidence_i]^T. 225 | desc - 256xN numpy array of corresponding unit normalized descriptors. 226 | heatmap - HxW numpy heatmap in range [0,1] of point confidences. 227 | """ 228 | assert img.ndim == 2, 'Image must be grayscale.' 229 | assert img.dtype == np.float32, 'Image must be float32.' 230 | H, W = img.shape[0], img.shape[1] 231 | inp = img.copy() 232 | inp = (inp.reshape(1, H, W)) 233 | inp = torch.from_numpy(inp) 234 | inp = torch.autograd.Variable(inp).view(1, 1, H, W) 235 | if self.cuda: 236 | inp = inp.cuda() 237 | # Forward pass of network. 238 | outs = self.net.forward(inp) 239 | semi, coarse_desc = outs[0], outs[1] 240 | # Convert pytorch -> numpy. 241 | semi = semi.data.cpu().numpy().squeeze() 242 | # --- Process points. 243 | dense = np.exp(semi) # Softmax. 244 | dense = dense / (np.sum(dense, axis=0)+.00001) # Should sum to 1. 245 | # Remove dustbin. 246 | nodust = dense[:-1, :, :] 247 | # Reshape to get full resolution heatmap. 248 | Hc = int(H / self.cell) 249 | Wc = int(W / self.cell) 250 | nodust = nodust.transpose(1, 2, 0) 251 | heatmap = np.reshape(nodust, [Hc, Wc, self.cell, self.cell]) 252 | heatmap = np.transpose(heatmap, [0, 2, 1, 3]) 253 | heatmap = np.reshape(heatmap, [Hc*self.cell, Wc*self.cell]) 254 | xs, ys = np.where(heatmap >= self.conf_thresh) # Confidence threshold. 255 | if len(xs) == 0: 256 | return np.zeros((3, 0)), None, None 257 | pts = np.zeros((3, len(xs))) # Populate point data sized 3xN. 258 | pts[0, :] = ys 259 | pts[1, :] = xs 260 | pts[2, :] = heatmap[xs, ys] 261 | pts, _ = self.nms_fast(pts, H, W, dist_thresh=self.nms_dist) # Apply NMS. 262 | inds = np.argsort(pts[2,:]) 263 | pts = pts[:,inds[::-1]] # Sort by confidence. 264 | # Remove points along border. 265 | bord = self.border_remove 266 | toremoveW = np.logical_or(pts[0, :] < bord, pts[0, :] >= (W-bord)) 267 | toremoveH = np.logical_or(pts[1, :] < bord, pts[1, :] >= (H-bord)) 268 | toremove = np.logical_or(toremoveW, toremoveH) 269 | pts = pts[:, ~toremove] 270 | # --- Process descriptor. 271 | D = coarse_desc.shape[1] 272 | if pts.shape[1] == 0: 273 | desc = np.zeros((D, 0)) 274 | else: 275 | # Interpolate into descriptor map using 2D point locations. 276 | samp_pts = torch.from_numpy(pts[:2, :].copy()) 277 | samp_pts[0, :] = (samp_pts[0, :] / (float(W)/2.)) - 1. 278 | samp_pts[1, :] = (samp_pts[1, :] / (float(H)/2.)) - 1. 279 | samp_pts = samp_pts.transpose(0, 1).contiguous() 280 | samp_pts = samp_pts.view(1, 1, -1, 2) 281 | samp_pts = samp_pts.float() 282 | if self.cuda: 283 | samp_pts = samp_pts.cuda() 284 | desc = torch.nn.functional.grid_sample(coarse_desc, samp_pts) 285 | desc = desc.data.cpu().numpy().reshape(D, -1) 286 | desc /= np.linalg.norm(desc, axis=0)[np.newaxis, :] 287 | return pts, desc, heatmap 288 | 289 | 290 | class PointTracker(object): 291 | """ Class to manage a fixed memory of points and descriptors that enables 292 | sparse optical flow point tracking. 293 | 294 | Internally, the tracker stores a 'tracks' matrix sized M x (2+L), of M 295 | tracks with maximum length L, where each row corresponds to: 296 | row_m = [track_id_m, avg_desc_score_m, point_id_0_m, ..., point_id_L-1_m]. 297 | """ 298 | 299 | def __init__(self, max_length, nn_thresh): 300 | if max_length < 2: 301 | raise ValueError('max_length must be greater than or equal to 2.') 302 | self.maxl = max_length 303 | self.nn_thresh = nn_thresh 304 | self.all_pts = [] 305 | for n in range(self.maxl): 306 | self.all_pts.append(np.zeros((2, 0))) 307 | self.last_desc = None 308 | self.tracks = np.zeros((0, self.maxl+2)) 309 | self.track_count = 0 310 | self.max_score = 9999 311 | 312 | def nn_match_two_way(self, desc1, desc2, nn_thresh): 313 | """ 314 | Performs two-way nearest neighbor matching of two sets of descriptors, such 315 | that the NN match from descriptor A->B must equal the NN match from B->A. 316 | 317 | Inputs: 318 | desc1 - NxM numpy matrix of N corresponding M-dimensional descriptors. 319 | desc2 - NxM numpy matrix of N corresponding M-dimensional descriptors. 320 | nn_thresh - Optional descriptor distance below which is a good match. 321 | 322 | Returns: 323 | matches - 3xL numpy array, of L matches, where L <= N and each column i is 324 | a match of two descriptors, d_i in image 1 and d_j' in image 2: 325 | [d_i index, d_j' index, match_score]^T 326 | """ 327 | assert desc1.shape[0] == desc2.shape[0] 328 | if desc1.shape[1] == 0 or desc2.shape[1] == 0: 329 | return np.zeros((3, 0)) 330 | if nn_thresh < 0.0: 331 | raise ValueError('\'nn_thresh\' should be non-negative') 332 | # Compute L2 distance. Easy since vectors are unit normalized. 333 | dmat = np.dot(desc1.T, desc2) 334 | dmat = np.sqrt(2-2*np.clip(dmat, -1, 1)) 335 | # Get NN indices and scores. 336 | idx = np.argmin(dmat, axis=1) 337 | scores = dmat[np.arange(dmat.shape[0]), idx] 338 | # Threshold the NN matches. 339 | keep = scores < nn_thresh 340 | # Check if nearest neighbor goes both directions and keep those. 341 | idx2 = np.argmin(dmat, axis=0) 342 | keep_bi = np.arange(len(idx)) == idx2[idx] 343 | keep = np.logical_and(keep, keep_bi) 344 | idx = idx[keep] 345 | scores = scores[keep] 346 | # Get the surviving point indices. 347 | m_idx1 = np.arange(desc1.shape[1])[keep] 348 | m_idx2 = idx 349 | # Populate the final 3xN match data structure. 350 | matches = np.zeros((3, int(keep.sum()))) 351 | matches[0, :] = m_idx1 352 | matches[1, :] = m_idx2 353 | matches[2, :] = scores 354 | return matches 355 | 356 | def get_offsets(self): 357 | """ Iterate through list of points and accumulate an offset value. Used to 358 | index the global point IDs into the list of points. 359 | 360 | Returns 361 | offsets - N length array with integer offset locations. 362 | """ 363 | # Compute id offsets. 364 | offsets = [] 365 | offsets.append(0) 366 | for i in range(len(self.all_pts)-1): # Skip last camera size, not needed. 367 | offsets.append(self.all_pts[i].shape[1]) 368 | offsets = np.array(offsets) 369 | offsets = np.cumsum(offsets) 370 | return offsets 371 | 372 | def update(self, pts, desc): 373 | """ Add a new set of point and descriptor observations to the tracker. 374 | 375 | Inputs 376 | pts - 3xN numpy array of 2D point observations. 377 | desc - DxN numpy array of corresponding D dimensional descriptors. 378 | """ 379 | if pts is None or desc is None: 380 | print('PointTracker: Warning, no points were added to tracker.') 381 | return 382 | assert pts.shape[1] == desc.shape[1] 383 | # Initialize last_desc. 384 | if self.last_desc is None: 385 | self.last_desc = np.zeros((desc.shape[0], 0)) 386 | # Remove oldest points, store its size to update ids later. 387 | remove_size = self.all_pts[0].shape[1] 388 | self.all_pts.pop(0) 389 | self.all_pts.append(pts) 390 | # Remove oldest point in track. 391 | self.tracks = np.delete(self.tracks, 2, axis=1) 392 | # Update track offsets. 393 | for i in range(2, self.tracks.shape[1]): 394 | self.tracks[:, i] -= remove_size 395 | self.tracks[:, 2:][self.tracks[:, 2:] < -1] = -1 396 | offsets = self.get_offsets() 397 | # Add a new -1 column. 398 | self.tracks = np.hstack((self.tracks, -1*np.ones((self.tracks.shape[0], 1)))) 399 | # Try to append to existing tracks. 400 | matched = np.zeros((pts.shape[1])).astype(bool) 401 | matches = self.nn_match_two_way(self.last_desc, desc, self.nn_thresh) 402 | for match in matches.T: 403 | # Add a new point to it's matched track. 404 | id1 = int(match[0]) + offsets[-2] 405 | id2 = int(match[1]) + offsets[-1] 406 | found = np.argwhere(self.tracks[:, -2] == id1) 407 | if found.shape[0] > 0: 408 | matched[int(match[1])] = True 409 | row = int(found) 410 | self.tracks[row, -1] = id2 411 | if self.tracks[row, 1] == self.max_score: 412 | # Initialize track score. 413 | self.tracks[row, 1] = match[2] 414 | else: 415 | # Update track score with running average. 416 | # NOTE(dd): this running average can contain scores from old matches 417 | # not contained in last max_length track points. 418 | track_len = (self.tracks[row, 2:] != -1).sum() - 1. 419 | frac = 1. / float(track_len) 420 | self.tracks[row, 1] = (1.-frac)*self.tracks[row, 1] + frac*match[2] 421 | # Add unmatched tracks. 422 | new_ids = np.arange(pts.shape[1]) + offsets[-1] 423 | new_ids = new_ids[~matched] 424 | new_tracks = -1*np.ones((new_ids.shape[0], self.maxl + 2)) 425 | new_tracks[:, -1] = new_ids 426 | new_num = new_ids.shape[0] 427 | new_trackids = self.track_count + np.arange(new_num) 428 | new_tracks[:, 0] = new_trackids 429 | new_tracks[:, 1] = self.max_score*np.ones(new_ids.shape[0]) 430 | self.tracks = np.vstack((self.tracks, new_tracks)) 431 | self.track_count += new_num # Update the track count. 432 | # Remove empty tracks. 433 | keep_rows = np.any(self.tracks[:, 2:] >= 0, axis=1) 434 | self.tracks = self.tracks[keep_rows, :] 435 | # Store the last descriptors. 436 | self.last_desc = desc.copy() 437 | return 438 | 439 | def get_tracks(self, min_length): 440 | """ Retrieve point tracks of a given minimum length. 441 | Input 442 | min_length - integer >= 1 with minimum track length 443 | Output 444 | returned_tracks - M x (2+L) sized matrix storing track indices, where 445 | M is the number of tracks and L is the maximum track length. 446 | """ 447 | if min_length < 1: 448 | raise ValueError('\'min_length\' too small.') 449 | valid = np.ones((self.tracks.shape[0])).astype(bool) 450 | good_len = np.sum(self.tracks[:, 2:] != -1, axis=1) >= min_length 451 | # Remove tracks which do not have an observation in most recent frame. 452 | not_headless = (self.tracks[:, -1] != -1) 453 | keepers = np.logical_and.reduce((valid, good_len, not_headless)) 454 | returned_tracks = self.tracks[keepers, :].copy() 455 | return returned_tracks 456 | 457 | def draw_tracks(self, out, tracks): 458 | """ Visualize tracks all overlayed on a single image. 459 | Inputs 460 | out - numpy uint8 image sized HxWx3 upon which tracks are overlayed. 461 | tracks - M x (2+L) sized matrix storing track info. 462 | """ 463 | # Store the number of points per camera. 464 | pts_mem = self.all_pts 465 | N = len(pts_mem) # Number of cameras/images. 466 | # Get offset ids needed to reference into pts_mem. 467 | offsets = self.get_offsets() 468 | # Width of track and point circles to be drawn. 469 | stroke = 1 470 | # Iterate through each track and draw it. 471 | for track in tracks: 472 | clr = myjet[int(np.clip(np.floor(track[1]*10), 0, 9)), :]*255 473 | for i in range(N-1): 474 | if track[i+2] == -1 or track[i+3] == -1: 475 | continue 476 | offset1 = offsets[i] 477 | offset2 = offsets[i+1] 478 | idx1 = int(track[i+2]-offset1) 479 | idx2 = int(track[i+3]-offset2) 480 | pt1 = pts_mem[i][:2, idx1] 481 | pt2 = pts_mem[i+1][:2, idx2] 482 | p1 = (int(round(pt1[0])), int(round(pt1[1]))) 483 | p2 = (int(round(pt2[0])), int(round(pt2[1]))) 484 | cv2.line(out, p1, p2, clr, thickness=stroke, lineType=16) 485 | # Draw end points of each track. 486 | if i == N-2: 487 | clr2 = (255, 0, 0) 488 | cv2.circle(out, p2, stroke, clr2, -1, lineType=16) 489 | 490 | class VideoStreamer(object): 491 | """ Class to help process image streams. Three types of possible inputs:" 492 | 1.) USB Webcam. 493 | 2.) A directory of images (files in directory matching 'img_glob'). 494 | 3.) A video file, such as an .mp4 or .avi file. 495 | """ 496 | def __init__(self, basedir, camid, height, width, skip, img_glob): 497 | self.cap = [] 498 | self.camera = False 499 | self.video_file = False 500 | self.listing = [] 501 | self.sizer = [height, width] 502 | self.i = 0 503 | self.skip = skip 504 | self.maxlen = 1000000 505 | # If the "basedir" string is the word camera, then use a webcam. 506 | if basedir == "camera/" or basedir == "camera": 507 | print('==> Processing Webcam Input.') 508 | self.cap = cv2.VideoCapture(camid) 509 | self.listing = range(0, self.maxlen) 510 | self.camera = True 511 | else: 512 | # Try to open as a video. 513 | self.cap = cv2.VideoCapture(basedir) 514 | lastbit = basedir[-4:len(basedir)] 515 | if (type(self.cap) == list or not self.cap.isOpened()) and (lastbit == '.mp4'): 516 | raise IOError('Cannot open movie file') 517 | elif type(self.cap) != list and self.cap.isOpened() and (lastbit != '.txt'): 518 | print('==> Processing Video Input.') 519 | num_frames = int(self.cap.get(cv2.CAP_PROP_FRAME_COUNT)) 520 | self.listing = range(0, num_frames) 521 | self.listing = self.listing[::self.skip] 522 | self.camera = True 523 | self.video_file = True 524 | self.maxlen = len(self.listing) 525 | else: 526 | print('==> Processing Image Directory Input.') 527 | search = os.path.join(basedir, img_glob) 528 | self.listing = glob.glob(search) 529 | self.listing.sort() 530 | self.listing = self.listing[::self.skip] 531 | self.maxlen = len(self.listing) 532 | if self.maxlen == 0: 533 | raise IOError('No images were found (maybe bad \'--img_glob\' parameter?)') 534 | 535 | def read_image(self, impath, img_size): 536 | """ Read image as grayscale and resize to img_size. 537 | Inputs 538 | impath: Path to input image. 539 | img_size: (W, H) tuple specifying resize size. 540 | Returns 541 | grayim: float32 numpy array sized H x W with values in range [0, 1]. 542 | """ 543 | grayim = cv2.imread(impath, 0) 544 | if grayim is None: 545 | raise Exception('Error reading image %s' % impath) 546 | # Image is resized via opencv. 547 | interp = cv2.INTER_AREA 548 | grayim = cv2.resize(grayim, (img_size[1], img_size[0]), interpolation=interp) 549 | grayim = (grayim.astype('float32') / 255.) 550 | return grayim 551 | 552 | def next_frame(self): 553 | """ Return the next frame, and increment internal counter. 554 | Returns 555 | image: Next H x W image. 556 | status: True or False depending whether image was loaded. 557 | """ 558 | if self.i == self.maxlen: 559 | return (None, False) 560 | if self.camera: 561 | ret, input_image = self.cap.read() 562 | if ret is False: 563 | print('VideoStreamer: Cannot get image from camera (maybe bad --camid?)') 564 | return (None, False) 565 | if self.video_file: 566 | self.cap.set(cv2.CAP_PROP_POS_FRAMES, self.listing[self.i]) 567 | input_image = cv2.resize(input_image, (self.sizer[1], self.sizer[0]), 568 | interpolation=cv2.INTER_AREA) 569 | input_image = cv2.cvtColor(input_image, cv2.COLOR_RGB2GRAY) 570 | input_image = input_image.astype('float')/255.0 571 | else: 572 | image_file = self.listing[self.i] 573 | input_image = self.read_image(image_file, self.sizer) 574 | # Increment internal counter. 575 | self.i = self.i + 1 576 | input_image = input_image.astype('float32') 577 | return (input_image, True) 578 | 579 | def writeSPtofile(pts, desc,sizeratio, filename_pre, maxpoints = 1000): 580 | maxlen = min( pts.shape[1] , maxpoints) 581 | pts = pts[:,0:maxlen] 582 | outpts_x = pts[0]*sizeratio[0] 583 | outpts_y = pts[1]*sizeratio[1] 584 | desc = desc[:,0:maxlen] 585 | with open(filename_pre+'_kpts.txt','w') as outfile: 586 | for index in range(0,len(outpts_x) ): 587 | print('{x} {y}'.format( x=int(outpts_x[index]),y=int(outpts_y[index]) ),file=outfile) 588 | with open(filename_pre+'_feats.txt','w') as outfile: 589 | for index in range(0,desc.shape[1] ): 590 | for element in desc[:,index]: 591 | print(element,file=outfile,end=' ') 592 | print('',file=outfile) 593 | 594 | 595 | def getImage2SP(img_name, filename_pre, fe): 596 | grayim = cv2.imread(img_name,0) 597 | ori_size = grayim.shape 598 | interp = cv2.INTER_AREA 599 | grayim = cv2.resize(grayim, ( 480 , 640 ), interpolation=interp) 600 | sizeratio = (ori_size[0]/480 , ori_size[1]/640) 601 | grayim = (grayim.astype('float32') / 255.) 602 | pts, desc, _ = fe.run(grayim) 603 | writeSPtofile(pts,desc,sizeratio,filename_pre) 604 | tmp = 1 605 | 606 | def test2SP(img_name, fe): 607 | grayim = cv2.imread(img_name,0) 608 | ori_size = grayim.shape 609 | interp = cv2.INTER_AREA 610 | grayim = cv2.resize(grayim, ( 480 , 640 ), interpolation=interp) 611 | sizeratio = (ori_size[0]/480 , ori_size[1]/640) 612 | grayim = (grayim.astype('float32') / 255.) 613 | pts, desc, _ = fe.run(grayim) 614 | maxlen = min( pts.shape[1] , 200) 615 | pts = pts[:,0:maxlen] 616 | outpts_x = pts[0]*sizeratio[0] 617 | outpts_y = pts[1]*sizeratio[1] 618 | desc = desc[:,0:maxlen] 619 | 620 | for index in range(0,len(outpts_x) ): 621 | cv2.circle(grayim, (int(outpts_x[index]),int(outpts_y[index])), 1, (0, 255, 0), -1, lineType=16 ) 622 | cv2.imshow("Test",grayim) 623 | cv2.waitKey() 624 | 625 | 626 | tmp = 1 627 | 628 | if __name__ == '__main__': 629 | fe = SuperPointFrontend(weights_path='superpoint_v1.pth', 630 | nms_dist=4, 631 | conf_thresh=0.005, 632 | nn_thresh=0.7, 633 | cuda=True) 634 | print('==> Successfully loaded pre-trained network.') 635 | 636 | listenlingfolder = '/dev/shm/tmpSP/' 637 | 638 | # test2SP('test.png',fe) 639 | 640 | while(True): 641 | while ( not os.path.exists(listenlingfolder+'ready.txt') ): 642 | time.sleep(0.01) 643 | # print("1234") 644 | os.remove(listenlingfolder+'ready.txt') 645 | f_list = glob.glob( listenlingfolder+'/*.png' ) 646 | assert len(f_list) == 1 647 | for file_i in f_list: 648 | if file_i.split('.')[-1] == 'png': 649 | print(file_i) 650 | targetpre = re.sub(r'\.png', "", file_i) 651 | print(targetpre) 652 | getImage2SP(file_i,targetpre,fe) 653 | os.remove(file_i) 654 | pathlib.Path(listenlingfolder + '/done.txt').touch() 655 | while ( os.path.exists(listenlingfolder+'/done.txt') ): 656 | time.sleep(0.01) 657 | # print("4321") 658 | 659 | 660 | 661 | 662 | if __name__ == '__main__12': 663 | 664 | # Parse command line arguments. 665 | parser = argparse.ArgumentParser(description='PyTorch SuperPoint Demo.') 666 | parser.add_argument('input', type=str, default='', 667 | help='Image directory or movie file or "camera" (for webcam).') 668 | parser.add_argument('--weights_path', type=str, default='superpoint_v1.pth', 669 | help='Path to pretrained weights file (default: superpoint_v1.pth).') 670 | parser.add_argument('--img_glob', type=str, default='*.png', 671 | help='Glob match if directory of images is specified (default: \'*.png\').') 672 | parser.add_argument('--skip', type=int, default=1, 673 | help='Images to skip if input is movie or directory (default: 1).') 674 | parser.add_argument('--show_extra', action='store_true', 675 | help='Show extra debug outputs (default: False).') 676 | parser.add_argument('--H', type=int, default=120, 677 | help='Input image height (default: 120).') 678 | parser.add_argument('--W', type=int, default=160, 679 | help='Input image width (default:160).') 680 | parser.add_argument('--display_scale', type=int, default=2, 681 | help='Factor to scale output visualization (default: 2).') 682 | parser.add_argument('--min_length', type=int, default=2, 683 | help='Minimum length of point tracks (default: 2).') 684 | parser.add_argument('--max_length', type=int, default=5, 685 | help='Maximum length of point tracks (default: 5).') 686 | parser.add_argument('--nms_dist', type=int, default=4, 687 | help='Non Maximum Suppression (NMS) distance (default: 4).') 688 | parser.add_argument('--conf_thresh', type=float, default=0.015, 689 | help='Detector confidence threshold (default: 0.015).') 690 | parser.add_argument('--nn_thresh', type=float, default=0.7, 691 | help='Descriptor matching threshold (default: 0.7).') 692 | parser.add_argument('--camid', type=int, default=0, 693 | help='OpenCV webcam video capture ID, usually 0 or 1 (default: 0).') 694 | parser.add_argument('--waitkey', type=int, default=1, 695 | help='OpenCV waitkey time in ms (default: 1).') 696 | parser.add_argument('--cuda', action='store_true', 697 | help='Use cuda GPU to speed up network processing speed (default: False)') 698 | parser.add_argument('--no_display', action='store_true', 699 | help='Do not display images to screen. Useful if running remotely (default: False).') 700 | parser.add_argument('--write', action='store_true', 701 | help='Save output frames to a directory (default: False)') 702 | parser.add_argument('--write_dir', type=str, default='tracker_outputs/', 703 | help='Directory where to write output frames (default: tracker_outputs/).') 704 | opt = parser.parse_args() 705 | print(opt) 706 | 707 | # This class helps load input images from different sources. 708 | vs = VideoStreamer(opt.input, opt.camid, opt.H, opt.W, opt.skip, opt.img_glob) 709 | 710 | print('==> Loading pre-trained network.') 711 | # This class runs the SuperPoint network and processes its outputs. 712 | fe = SuperPointFrontend(weights_path=opt.weights_path, 713 | nms_dist=opt.nms_dist, 714 | conf_thresh=opt.conf_thresh, 715 | nn_thresh=opt.nn_thresh, 716 | cuda=opt.cuda) 717 | print('==> Successfully loaded pre-trained network.') 718 | 719 | # This class helps merge consecutive point matches into tracks. 720 | tracker = PointTracker(opt.max_length, nn_thresh=fe.nn_thresh) 721 | 722 | # Create a window to display the demo. 723 | if not opt.no_display: 724 | win = 'SuperPoint Tracker' 725 | cv2.namedWindow(win) 726 | else: 727 | print('Skipping visualization, will not show a GUI.') 728 | 729 | # Font parameters for visualizaton. 730 | font = cv2.FONT_HERSHEY_DUPLEX 731 | font_clr = (255, 255, 255) 732 | font_pt = (4, 12) 733 | font_sc = 0.4 734 | 735 | # Create output directory if desired. 736 | if opt.write: 737 | print('==> Will write outputs to %s' % opt.write_dir) 738 | if not os.path.exists(opt.write_dir): 739 | os.makedirs(opt.write_dir) 740 | 741 | print('==> Running Demo.') 742 | while True: 743 | 744 | start = time.time() 745 | 746 | # Get a new image. 747 | img, status = vs.next_frame() 748 | if status is False: 749 | break 750 | 751 | # Get points and descriptors. 752 | start1 = time.time() 753 | pts, desc, heatmap = fe.run(img) 754 | end1 = time.time() 755 | 756 | # Add points and descriptors to the tracker. 757 | tracker.update(pts, desc) 758 | 759 | # Get tracks for points which were match successfully across all frames. 760 | tracks = tracker.get_tracks(opt.min_length) 761 | 762 | # Primary output - Show point tracks overlayed on top of input image. 763 | out1 = (np.dstack((img, img, img)) * 255.).astype('uint8') 764 | tracks[:, 1] /= float(fe.nn_thresh) # Normalize track scores to [0,1]. 765 | tracker.draw_tracks(out1, tracks) 766 | if opt.show_extra: 767 | cv2.putText(out1, 'Point Tracks', font_pt, font, font_sc, font_clr, lineType=16) 768 | 769 | # Extra output -- Show current point detections. 770 | out2 = (np.dstack((img, img, img)) * 255.).astype('uint8') 771 | for pt in pts.T: 772 | pt1 = (int(round(pt[0])), int(round(pt[1]))) 773 | cv2.circle(out2, pt1, 1, (0, 255, 0), -1, lineType=16) 774 | cv2.putText(out2, 'Raw Point Detections', font_pt, font, font_sc, font_clr, lineType=16) 775 | 776 | # Extra output -- Show the point confidence heatmap. 777 | if heatmap is not None: 778 | min_conf = 0.001 779 | heatmap[heatmap < min_conf] = min_conf 780 | heatmap = -np.log(heatmap) 781 | heatmap = (heatmap - heatmap.min()) / (heatmap.max() - heatmap.min() + .00001) 782 | out3 = myjet[np.round(np.clip(heatmap*10, 0, 9)).astype('int'), :] 783 | out3 = (out3*255).astype('uint8') 784 | else: 785 | out3 = np.zeros_like(out2) 786 | cv2.putText(out3, 'Raw Point Confidences', font_pt, font, font_sc, font_clr, lineType=16) 787 | 788 | # Resize final output. 789 | if opt.show_extra: 790 | out = np.hstack((out1, out2, out3)) 791 | out = cv2.resize(out, (3*opt.display_scale*opt.W, opt.display_scale*opt.H)) 792 | else: 793 | out = cv2.resize(out1, (opt.display_scale*opt.W, opt.display_scale*opt.H)) 794 | 795 | # Display visualization image to screen. 796 | if not opt.no_display: 797 | cv2.imshow(win, out) 798 | key = cv2.waitKey(opt.waitkey) & 0xFF 799 | if key == ord('q'): 800 | print('Quitting, \'q\' pressed.') 801 | break 802 | 803 | # Optionally write images to disk. 804 | if opt.write: 805 | out_file = os.path.join(opt.write_dir, 'frame_%05d.png' % vs.i) 806 | print('Writing image to %s' % out_file) 807 | cv2.imwrite(out_file, out) 808 | 809 | end = time.time() 810 | net_t = (1./ float(end1 - start)) 811 | total_t = (1./ float(end - start)) 812 | if opt.show_extra: 813 | print('Processed image %d (net+post_process: %.2f FPS, total: %.2f FPS).'\ 814 | % (vs.i, net_t, total_t)) 815 | 816 | # Close any remaining windows. 817 | cv2.destroyAllWindows() 818 | 819 | print('==> Finshed Demo.') 820 | -------------------------------------------------------------------------------- /SuperPointPretrainedNetwork/superpoint_v1.pth: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/FarawaySail/SuperPoint_SLAM/e34cc9fdf321096797f7eb8c6fce33be2f4225ef/SuperPointPretrainedNetwork/superpoint_v1.pth -------------------------------------------------------------------------------- /SuperPointPretrainedNetwork/test.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/FarawaySail/SuperPoint_SLAM/e34cc9fdf321096797f7eb8c6fce33be2f4225ef/SuperPointPretrainedNetwork/test.png -------------------------------------------------------------------------------- /modules.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/FarawaySail/SuperPoint_SLAM/e34cc9fdf321096797f7eb8c6fce33be2f4225ef/modules.png --------------------------------------------------------------------------------