├── src └── StereoLoopDetector.cmake.in ├── cmake ├── DownloadProject.CMakeLists.cmake.in └── DownloadProject.cmake ├── resources ├── DBoW2_PR-43.patch ├── DBoW2_PR-68.patch ├── DBoW2_PR-69.patch └── fs_stereo_parameters.yaml ├── include └── StereoLoopDetector │ ├── DLoopDetector.h │ ├── StereoParameters.h │ └── TemplatedLoopDetector.h ├── demo ├── ORBextractor.h ├── RowMatcher.hpp ├── RowMatcher.cpp ├── demo_stereo.cpp ├── demoDetectorStereo.h └── ORBextractor.cc ├── LICENSE.txt ├── CMakeLists.txt └── README.md /src/StereoLoopDetector.cmake.in: -------------------------------------------------------------------------------- 1 | FIND_LIBRARY(StereoLoopDetector_LIBRARY StereoLoopDetector 2 | PATHS @CMAKE_INSTALL_PREFIX@/lib 3 | ) 4 | FIND_PATH(StereoLoopDetector_INCLUDE_DIR StereoLoopDetectorConfig.cmake 5 | PATHS @CMAKE_INSTALL_PREFIX@/include/@PROJECT_NAME@ 6 | ) 7 | SET(StereoLoopDetector_LIBRARIES ${StereoLoopDetector_LIBRARY}) 8 | SET(StereoLoopDetector_LIBS ${StereoLoopDetector_LIBRARY}) 9 | SET(StereoLoopDetector_INCLUDE_DIRS ${StereoLoopDetector_INCLUDE_DIR}) -------------------------------------------------------------------------------- /cmake/DownloadProject.CMakeLists.cmake.in: -------------------------------------------------------------------------------- 1 | # Distributed under the OSI-approved MIT License. See accompanying 2 | # file LICENSE or https://github.com/Crascit/DownloadProject for details. 3 | 4 | cmake_minimum_required(VERSION 2.8.2) 5 | 6 | project(${DL_ARGS_PROJ}-download NONE) 7 | 8 | include(ExternalProject) 9 | ExternalProject_Add(${DL_ARGS_PROJ}-download 10 | ${DL_ARGS_UNPARSED_ARGUMENTS} 11 | SOURCE_DIR "${DL_ARGS_SOURCE_DIR}" 12 | BINARY_DIR "${DL_ARGS_BINARY_DIR}" 13 | CONFIGURE_COMMAND "" 14 | BUILD_COMMAND "" 15 | INSTALL_COMMAND "" 16 | TEST_COMMAND "" 17 | ) 18 | -------------------------------------------------------------------------------- /resources/DBoW2_PR-43.patch: -------------------------------------------------------------------------------- 1 | From 9a605d571c99099f6f6afc5292c00a84754c5ca2 Mon Sep 17 00:00:00 2001 2 | From: Matthias Kraft 3 | Date: Thu, 13 Dec 2018 12:26:30 +0100 4 | Subject: [PATCH] Fixed assignment operator 5 | 6 | --- 7 | build/dependencies/src/DBoW2/include/DBoW2/TemplatedDatabase.h | 5 ++++- 8 | 1 file changed, 4 insertions(+), 1 deletion(-) 9 | 10 | diff --git a/include/DBoW2/TemplatedDatabase.h b/include/DBoW2/TemplatedDatabase.h 11 | index e0768b0c..f44231b8 100644 12 | --- a/include/DBoW2/TemplatedDatabase.h 13 | +++ b/include/DBoW2/TemplatedDatabase.h 14 | @@ -384,12 +384,15 @@ TemplatedDatabase& TemplatedDatabase::operator= 15 | { 16 | if(this != &db) 17 | { 18 | + setVocabulary(*db.m_voc); // The vocabulary must be set before the other 19 | + // fields as setVocabulary calls the clear() 20 | + // function. 21 | + 22 | m_dfile = db.m_dfile; 23 | m_dilevels = db.m_dilevels; 24 | m_ifile = db.m_ifile; 25 | m_nentries = db.m_nentries; 26 | m_use_di = db.m_use_di; 27 | - setVocabulary(*db.m_voc); 28 | } 29 | return *this; 30 | } 31 | -------------------------------------------------------------------------------- /include/StereoLoopDetector/DLoopDetector.h: -------------------------------------------------------------------------------- 1 | /* 2 | * File: DLoopDetector.h 3 | * Date: November 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: Generic include file for the DLoopDetector classes and 6 | * the specialized loop detectors 7 | * License: see the LICENSE.txt file 8 | * 9 | */ 10 | 11 | /*! \mainpage DLoopDetector Library 12 | * 13 | * DLoopDetector library for C++: 14 | * Loop detector for monocular image sequences based on bags of words 15 | * 16 | * Written by Dorian Galvez-Lopez, 17 | * University of Zaragoza 18 | * 19 | * Check my website to obtain updates: http://webdiis.unizar.es/~dorian 20 | * 21 | * \section requirements Requirements 22 | * This library requires the DUtils, DUtilsCV, DVision, DBoW2 and OpenCV libraries, 23 | * as well as the boost::dynamic_bitset class. 24 | * 25 | * \section citation Citation 26 | * If you use this software in academic works, please cite: 27 | 
28 |    @@ARTICLE{GalvezTRO12,
29 |     author={Galvez-Lopez, Dorian and Tardos, J. D.}, 
30 |     journal={IEEE Transactions on Robotics},
31 |     title={Bags of Binary Words for Fast Place Recognition in Image Sequences},
32 |     year={2012},
33 |     month={October},
34 |     volume={28},
35 |     number={5},
36 |     pages={1188--1197},
37 |     doi={10.1109/TRO.2012.2197158},
38 |     ISSN={1552-3098}
39 |   }
40 |
41 | * 42 | */ 43 | 44 | 45 | #ifndef __D_T_LOOP_DETECTOR__ 46 | #define __D_T_LOOP_DETECTOR__ 47 | 48 | /// Loop detector for sequences of monocular images 49 | namespace DLoopDetector 50 | { 51 | } 52 | 53 | #include "DBoW2.h" 54 | #include "TemplatedLoopDetector.h" 55 | #include "FORB.h" 56 | #include "FBrief.h" 57 | 58 | /// SURF64 Loop Detector 59 | typedef DLoopDetector::TemplatedLoopDetector 60 | OrbLoopDetector; 61 | 62 | /// BRIEF Loop Detector 63 | typedef DLoopDetector::TemplatedLoopDetector 64 | BriefLoopDetector; 65 | 66 | #endif 67 | 68 | -------------------------------------------------------------------------------- /resources/DBoW2_PR-68.patch: -------------------------------------------------------------------------------- 1 | From b1657991c49a5c76a5c1571e98563bb9b0ade647 Mon Sep 17 00:00:00 2001 2 | From: IronSublimate 3 | Date: Thu, 18 Nov 2021 19:56:43 +0800 4 | Subject: [PATCH] fix load too slow bug 5 | 6 | --- 7 | build/dependencies/src/DBoW2/include/DBoW2/TemplatedVocabulary.h | 19 ++++++++++--------- 8 | 1 file changed, 10 insertions(+), 9 deletions(-) 9 | 10 | diff --git a/include/DBoW2/TemplatedVocabulary.h b/include/DBoW2/TemplatedVocabulary.h 11 | index 6d8d6210..1237d851 100644 12 | --- a/include/DBoW2/TemplatedVocabulary.h 13 | +++ b/include/DBoW2/TemplatedVocabulary.h 14 | @@ -1472,13 +1472,13 @@ void TemplatedVocabulary::load(const cv::FileStorage &fs, 15 | 16 | m_nodes.resize(fn.size() + 1); // +1 to include root 17 | m_nodes[0].id = 0; 18 | - 19 | - for(unsigned int i = 0; i < fn.size(); ++i) 20 | + cv::FileNodeIterator end = fn.end(); 21 | + for(cv::FileNodeIterator it = fn.begin(); it < end; ++it) 22 | { 23 | - NodeId nid = (int)fn[i]["nodeId"]; 24 | - NodeId pid = (int)fn[i]["parentId"]; 25 | - WordValue weight = (WordValue)fn[i]["weight"]; 26 | - std::string d = (std::string)fn[i]["descriptor"]; 27 | + NodeId nid = (int)(*it)["nodeId"]; 28 | + NodeId pid = (int)(*it)["parentId"]; 29 | + WordValue weight = (WordValue)(*it)["weight"]; 30 | + std::string d = (std::string)(*it)["descriptor"]; 31 | 32 | m_nodes[nid].id = nid; 33 | m_nodes[nid].parent = pid; 34 | @@ -1492,11 +1492,12 @@ void TemplatedVocabulary::load(const cv::FileStorage &fs, 35 | fn = fvoc["words"]; 36 | 37 | m_words.resize(fn.size()); 38 | + end = fn.end(); 39 | 40 | - for(unsigned int i = 0; i < fn.size(); ++i) 41 | + for(cv::FileNodeIterator it = fn.begin(); it < end; ++it) 42 | { 43 | - NodeId wid = (int)fn[i]["wordId"]; 44 | - NodeId nid = (int)fn[i]["nodeId"]; 45 | + NodeId wid = (int)(*it)["wordId"]; 46 | + NodeId nid = (int)(*it)["nodeId"]; 47 | 48 | m_nodes[nid].word_id = wid; 49 | m_words[wid] = &m_nodes[nid]; 50 | -------------------------------------------------------------------------------- /resources/DBoW2_PR-69.patch: -------------------------------------------------------------------------------- 1 | From d6d0f589fd47eb727bd5ae2518ce50d58cf6aeaa Mon Sep 17 00:00:00 2001 2 | From: Yassine HABIB 3 | Date: Wed, 24 Nov 2021 12:16:05 +0100 4 | Subject: [PATCH] Added FileNodeIterator for OpenCV 4.X to optimize vocabulary 5 | loading time 6 | 7 | --- 8 | build/dependencies/src/DBoW2/include/TemplatedVocabulary.h | 21 +++++++++++++-------- 9 | 1 file changed, 13 insertions(+), 8 deletions(-) 10 | 11 | diff --git a/include/DBOW2/TemplatedVocabulary.h b/include/TemplatedVocabulary.h 12 | index 6d8d6210..7e145944 100644 13 | --- a/include/DBoW2/TemplatedVocabulary.h 14 | +++ b/include/DBoW2/TemplatedVocabulary.h 15 | @@ -1473,12 +1473,14 @@ void TemplatedVocabulary::load(const cv::FileStorage &fs, 16 | m_nodes.resize(fn.size() + 1); // +1 to include root 17 | m_nodes[0].id = 0; 18 | 19 | - for(unsigned int i = 0; i < fn.size(); ++i) 20 | + unsigned int i = 0; 21 | + for(cv::FileNodeIterator it = fn.begin(); it != fn.end(); it++, i++) 22 | { 23 | - NodeId nid = (int)fn[i]["nodeId"]; 24 | - NodeId pid = (int)fn[i]["parentId"]; 25 | - WordValue weight = (WordValue)fn[i]["weight"]; 26 | - std::string d = (std::string)fn[i]["descriptor"]; 27 | + cv::FileNode fni = *it; 28 | + NodeId nid = (int)fni["nodeId"]; 29 | + NodeId pid = (int)fni["parentId"]; 30 | + WordValue weight = (WordValue)fni["weight"]; 31 | + std::string d = (std::string)fni["descriptor"]; 32 | 33 | m_nodes[nid].id = nid; 34 | m_nodes[nid].parent = pid; 35 | @@ -1493,10 +1495,13 @@ void TemplatedVocabulary::load(const cv::FileStorage &fs, 36 | 37 | m_words.resize(fn.size()); 38 | 39 | - for(unsigned int i = 0; i < fn.size(); ++i) 40 | + i = 0; 41 | + for(cv::FileNodeIterator it = fn.begin(); it != fn.end(); it++, i++) 42 | { 43 | - NodeId wid = (int)fn[i]["wordId"]; 44 | - NodeId nid = (int)fn[i]["nodeId"]; 45 | + cv::FileNode fni = *it; 46 | + 47 | + NodeId wid = (int)fni["wordId"]; 48 | + NodeId nid = (int)fni["nodeId"]; 49 | 50 | m_nodes[nid].word_id = wid; 51 | m_words[wid] = &m_nodes[nid]; 52 | -------------------------------------------------------------------------------- /resources/fs_stereo_parameters.yaml: -------------------------------------------------------------------------------- 1 | image_width: 1024 2 | image_height: 544 3 | left_camera_matrix: 4 | rows: 3 5 | cols: 3 6 | data: [ 555.9204711914062 , 0.0 , 498.1905517578125 , 7 | 0.0 , 556.6275634765625 , 252.35089111328125 , 8 | 0.0 , 0.0 , 1.0 ] 9 | left_distortion_coefficients: 10 | rows: 1 11 | cols: 5 12 | data: [0.0030753163155168295, 0.002497022273018956, 0.0003005412872880697, 0.001575434347614646, -0.003454494522884488] 13 | left_rectification_matrix: 14 | rows: 3 15 | cols: 3 16 | data: [ 0.9999634027481079 , -0.000500216381624341 , 0.00853759702295065 , 17 | 0.0005011018947698176 , 0.9999998807907104 , -0.00010158627264900133 , 18 | -0.00853754486888647 , 0.00010586075950413942 , 0.9999635219573975 ] 19 | left_projection_matrix: 20 | rows: 3 21 | cols: 4 22 | data: [580.6427001953125 , 0.0 , 512.0 , 0.0 , 23 | 0.0 , 580.6427001953125 , 254.5 , 0.0 , 24 | 0.0 , 0.0 , 1.0 , 0.0 ] 25 | right_camera_matrix: 26 | rows: 3 27 | cols: 3 28 | data: [ 554.5745849609375 , 0.0 , 504.5379943847656 , 29 | 0.0 , 554.9697875976562 , 243.73471069335938 , 30 | 0.0 , 0.0 , 1.0 ] 31 | right_distortion_coefficients: 32 | rows: 1 33 | cols: 5 34 | data: [0.005892270710319281, -0.009164233691990376, 0.000662550562992692, 0.0009181547211483121, 0.0074353949166834354] 35 | right_rectification_matrix: 36 | rows: 3 37 | cols: 3 38 | data: [ 0.9999528527259827 , 0.0074403309263288975 , 0.006242129486054182 , 39 | -0.007440978195518255 , 0.9999722838401794 , 8.050136239035055e-05 , 40 | -0.006241357419639826 , -0.00012694511679001153 , 0.9999805092811584 ] 41 | right_projection_matrix: 42 | rows: 3 43 | cols: 4 44 | data: [ 580.6427001953125 , 0.0 , 512.0 , -121.7224349975586 , 45 | 0.0 , 580.6427001953125 , 254.5 , 0.0 , 46 | 0.0 , 0.0 , 1.0 , 0.0 ] 47 | -------------------------------------------------------------------------------- /include/StereoLoopDetector/StereoParameters.h: -------------------------------------------------------------------------------- 1 | /** 2 | * File: StereoParameters.h 3 | * Author: Nicolas Soncini 4 | * Date: Aug 23, 2024 5 | * Description: Data structures and functionality to work with stereo 6 | * License: See LICENSE.txt file at the top project folder 7 | * 8 | **/ 9 | 10 | #ifndef __STEREO_PARAMETERS__ 11 | #define __STEREO_PARAMETERS__ 12 | 13 | #include 14 | #include "yaml-cpp/yaml.h" 15 | 16 | // --------------------------------------------------------------------------- 17 | 18 | struct StereoParameters 19 | { 20 | cv::Mat left_camera_matrix; // for PnP 21 | cv::Mat left_dist_coeffs; // for PnP 22 | cv::Mat left_rectification; 23 | cv::Mat left_projection; // for triangulation 24 | cv::Mat right_camera_matrix; 25 | cv::Mat right_dist_coeffs; 26 | cv::Mat right_rectification; 27 | cv::Mat right_projection; // for triangulation 28 | cv::Size2i size; 29 | 30 | StereoParameters() = default; 31 | 32 | StereoParameters(std::string yaml_path) 33 | { 34 | YAML::Node yaml = YAML::LoadFile(yaml_path); 35 | 36 | left_camera_matrix = cv::Mat( 37 | yaml["left_camera_matrix"]["data"] 38 | .as>(), true 39 | ).reshape(0, 3); 40 | left_dist_coeffs = cv::Mat( 41 | yaml["left_distortion_coefficients"]["data"] 42 | .as>(), true 43 | ).reshape(0, 1); 44 | left_rectification = cv::Mat( 45 | yaml["left_rectification_matrix"]["data"] 46 | .as>(), true 47 | ).reshape(0, 3); 48 | left_projection = cv::Mat( 49 | yaml["left_projection_matrix"]["data"] 50 | .as>(), true 51 | ).reshape(0, 3); 52 | 53 | right_camera_matrix = cv::Mat( 54 | yaml["right_camera_matrix"]["data"] 55 | .as>(), true 56 | ).reshape(0, 3); 57 | right_dist_coeffs = cv::Mat( 58 | yaml["right_distortion_coefficients"]["data"] 59 | .as>(), true 60 | ).reshape(0, 1); 61 | right_rectification = cv::Mat( 62 | yaml["right_rectification_matrix"]["data"] 63 | .as>(), true 64 | ).reshape(0, 3); 65 | right_projection = cv::Mat( 66 | yaml["right_projection_matrix"]["data"] 67 | .as>(), true 68 | ).reshape(0, 3); 69 | 70 | size = cv::Size( 71 | yaml["image_width"].as(), yaml["image_height"].as() 72 | ); 73 | } 74 | }; 75 | 76 | // --------------------------------------------------------------------------- 77 | 78 | #endif //__STEREO_PARAMETERS__ -------------------------------------------------------------------------------- /demo/ORBextractor.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM3 3 | * 4 | * Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. 5 | * Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. 6 | * 7 | * ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public 8 | * License as published by the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even 12 | * the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License along with ORB-SLAM3. 16 | * If not, see . 17 | */ 18 | 19 | #ifndef ORBEXTRACTOR_H 20 | #define ORBEXTRACTOR_H 21 | 22 | #include 23 | #include 24 | #include 25 | 26 | 27 | namespace ORB_SLAM3 28 | { 29 | 30 | class ExtractorNode 31 | { 32 | public: 33 | ExtractorNode():bNoMore(false){} 34 | 35 | void DivideNode(ExtractorNode &n1, ExtractorNode &n2, ExtractorNode &n3, ExtractorNode &n4); 36 | 37 | std::vector vKeys; 38 | cv::Point2i UL, UR, BL, BR; 39 | std::list::iterator lit; 40 | bool bNoMore; 41 | }; 42 | 43 | class ORBextractor 44 | { 45 | public: 46 | 47 | enum {HARRIS_SCORE=0, FAST_SCORE=1 }; 48 | 49 | ORBextractor(int nfeatures, float scaleFactor, int nlevels, 50 | int iniThFAST, int minThFAST); 51 | 52 | ~ORBextractor(){} 53 | 54 | // Compute the ORB features and descriptors on an image. 55 | // ORB are dispersed on the image using an octree. 56 | // Mask is ignored in the current implementation. 57 | int operator()( cv::InputArray _image, cv::InputArray _mask, 58 | std::vector& _keypoints, 59 | cv::OutputArray _descriptors, std::vector &vLappingArea); 60 | 61 | int inline GetLevels(){ 62 | return nlevels;} 63 | 64 | float inline GetScaleFactor(){ 65 | return scaleFactor;} 66 | 67 | std::vector inline GetScaleFactors(){ 68 | return mvScaleFactor; 69 | } 70 | 71 | std::vector inline GetInverseScaleFactors(){ 72 | return mvInvScaleFactor; 73 | } 74 | 75 | std::vector inline GetScaleSigmaSquares(){ 76 | return mvLevelSigma2; 77 | } 78 | 79 | std::vector inline GetInverseScaleSigmaSquares(){ 80 | return mvInvLevelSigma2; 81 | } 82 | 83 | std::vector mvImagePyramid; 84 | 85 | protected: 86 | 87 | void ComputePyramid(cv::Mat image); 88 | void ComputeKeyPointsOctTree(std::vector >& allKeypoints); 89 | std::vector DistributeOctTree(const std::vector& vToDistributeKeys, const int &minX, 90 | const int &maxX, const int &minY, const int &maxY, const int &nFeatures, const int &level); 91 | 92 | void ComputeKeyPointsOld(std::vector >& allKeypoints); 93 | std::vector pattern; 94 | 95 | int nfeatures; 96 | double scaleFactor; 97 | int nlevels; 98 | int iniThFAST; 99 | int minThFAST; 100 | 101 | std::vector mnFeaturesPerLevel; 102 | 103 | std::vector umax; 104 | 105 | std::vector mvScaleFactor; 106 | std::vector mvInvScaleFactor; 107 | std::vector mvLevelSigma2; 108 | std::vector mvInvLevelSigma2; 109 | }; 110 | 111 | } //namespace ORB_SLAM 112 | 113 | #endif 114 | 115 | -------------------------------------------------------------------------------- /demo/RowMatcher.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of S-PTAM. 3 | * 4 | * Copyright (C) 2013-2017 Taihú Pire 5 | * Copyright (C) 2014-2017 Thomas Fischer 6 | * Copyright (C) 2016-2017 Gastón Castro 7 | * Copyright (C) 2017 Matias Nitsche 8 | * For more information see 9 | * 10 | * S-PTAM is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * S-PTAM is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with S-PTAM. If not, see . 22 | * 23 | * Authors: Taihú Pire 24 | * Thomas Fischer 25 | * Gastón Castro 26 | * Matías Nitsche 27 | * 28 | * Laboratory of Robotics and Embedded Systems 29 | * Department of Computer Science 30 | * Faculty of Exact and Natural Sciences 31 | * University of Buenos Aires 32 | */ 33 | #pragma once 34 | 35 | #include 36 | #include 37 | 38 | /** 39 | * Matches two sets of descriptors, asuming that the matches 40 | * will be found on the same pixel row, which makes the match search 41 | * much more efficient. 42 | * This is a fair assumption when computing matches from rectified 43 | * stereo frames, where projected points share the same baseline. 44 | */ 45 | class RowMatcher 46 | { 47 | public: 48 | 49 | /** 50 | * 51 | * @param rowRange 52 | * the range of rows around the row corresponding to the epipolar 53 | * line that will be searched for potential matches. 54 | * 0 is strict, usually 1 is enough. 55 | * 56 | * @param maxDistance 57 | * the maximum allowed distance between descriptors 58 | * to be considered as a match 59 | */ 60 | RowMatcher(double maxDistance, cv::Ptr descriptorMatcher, double rowRange = 1.0); 61 | 62 | /** 63 | * @param normType 64 | * The norm used to measure distance between descriptors. 65 | * Suggested values are: 66 | * - NORM_L1 or NORM_L2 for SIFT and SURF descriptors 67 | * - NORM_HAMMING for ORB, BRISK, and BRIEF 68 | * - NORM_HAMMING2 should be used with ORB when WTA_K==3 or 4 69 | * 70 | * @param crossCheck 71 | * If true, the match() method will only return pairs (i,j) 72 | * such that for i-th query descriptor the j-th descriptor 73 | * in the matcher’s collection is the nearest and vice versa, 74 | * i.e. it will only return consistent pairs. 75 | * Such technique usually produces best results with minimal 76 | * number of outliers when there are enough matches. 77 | * This is alternative to the ratio test, used by D. Lowe 78 | * in SIFT paper. 79 | * OpenCV doesnt support using masks with crossCheck enabled. 80 | * http://answers.opencv.org/question/30670/using-descriptormatcher-with-mask-and-crosscheck/ 81 | * 82 | * @param rowRange 83 | * the range of rows around the row corresponding to the epipolar 84 | * line that will be searched for potential matches. 85 | * 0 is strict, usually 1 is enough. 86 | * 87 | * @param maxDistance 88 | * the maximum allowed distance between descriptors 89 | * to be considered as a match 90 | */ 91 | RowMatcher(double maxDistance, int normType, bool crossCheck = false, double rowRange = 1.0); 92 | 93 | /** 94 | * matches each query descriptor to a train descriptor in the same 95 | * row. For robustness, a keypoint in the +-range is also allowed 96 | * to match. If no match exists, the match index output is -1. 97 | * 98 | * @param keypoints1 99 | * query keypoints 100 | * 101 | * @param descriptors1 102 | * query descriptors 103 | * 104 | * @param keypoints2 105 | * training keypoints 106 | * 107 | * @param descriptors2 108 | * training descriptors 109 | * 110 | * @param matches 111 | * resulting match indexes for each query keypoint, which point 112 | * to the matched training keypoint. If no match was found, 113 | * the index value is -1. 114 | */ 115 | void match( 116 | const std::vector& keypoints1, const cv::Mat& descriptors1, 117 | const std::vector& keypoints2, const cv::Mat& descriptors2, 118 | std::vector& matches) const; 119 | 120 | static void match( 121 | const cv::DescriptorMatcher& matcher, double matchingDistanceThreshold, 122 | const std::vector& keypoints1, const cv::Mat& descriptors1, 123 | const std::vector& keypoints2, const cv::Mat& descriptors2, 124 | std::vector& matches, double range = 1.0 125 | ); 126 | 127 | private: 128 | 129 | const cv::Ptr descriptorMatcher_; 130 | 131 | const double matchingDistanceThreshold_; 132 | 133 | const double range_; 134 | }; 135 | -------------------------------------------------------------------------------- /LICENSE.txt: -------------------------------------------------------------------------------- 1 | StereoLoopDetector: loop detector for stereo image sequences 2 | 3 | Copyright 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | 1. Redistributions of source code must retain the above copyright notice, 9 | this list of conditions and the following disclaimer. 10 | 11 | 2. Redistributions in binary form must reproduce the above copyright notice, 12 | this list of conditions and the following disclaimer in the documentation 13 | and/or other materials provided with the distribution. 14 | 15 | 3. Neither the name of the copyright holder nor the names of its contributors 16 | may be used to endorse or promote products derived from this software without 17 | specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS “AS IS” 20 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 23 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 24 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 25 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 26 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 27 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 28 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | 30 | If used in academic work please cite: 31 | 32 | @article{GalvezTRO12, 33 | author={G\'alvez-L\'opez, Dorian and Tard\'os, J. D.}, 34 | journal={IEEE Transactions on Robotics}, 35 | title={Bags of Binary Words for Fast Place Recognition in Image Sequences}, 36 | year={2012}, 37 | month={October}, 38 | volume={28}, 39 | number={5}, 40 | pages={1188--1197}, 41 | doi={10.1109/TRO.2012.2197158}, 42 | ISSN={1552-3098} 43 | } 44 | 45 | ------------------------------------------------------------------------------- 46 | 47 | DLoopDetector: loop detector for monocular image sequences. 48 | 49 | Copyright (c) 2015 Dorian Galvez-Lopez. http://doriangalvez.com 50 | All rights reserved. 51 | 52 | Redistribution and use in source and binary forms, with or without 53 | modification, are permitted provided that the following conditions 54 | are met: 55 | 1. Redistributions of source code must retain the above copyright 56 | notice, this list of conditions and the following disclaimer. 57 | 2. Redistributions in binary form must reproduce the above copyright 58 | notice, this list of conditions and the following disclaimer in the 59 | documentation and/or other materials provided with the distribution. 60 | 3. The original author of the work must be notified of any 61 | redistribution of source code or in binary form. 62 | 4. Neither the name of copyright holders nor the names of its 63 | contributors may be used to endorse or promote products derived 64 | from this software without specific prior written permission. 65 | 66 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 67 | ''AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 68 | TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 69 | PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDERS OR CONTRIBUTORS 70 | BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 71 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 72 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 73 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 74 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 75 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 76 | POSSIBILITY OF SUCH DAMAGE. 77 | 78 | If you use it in an academic work, please cite: 79 | 80 | @ARTICLE{GalvezTRO12, 81 | author={G\'alvez-L\'opez, Dorian and Tard\'os, J. D.}, 82 | journal={IEEE Transactions on Robotics}, 83 | title={Bags of Binary Words for Fast Place Recognition in Image Sequences}, 84 | year={2012}, 85 | month={October}, 86 | volume={28}, 87 | number={5}, 88 | pages={1188--1197}, 89 | doi={10.1109/TRO.2012.2197158}, 90 | ISSN={1552-3098} 91 | } 92 | 93 | ------------------------------------------------------------------------------- 94 | 95 | CMake 3 Tools: A set of CMake modules to assist in building code, with some 96 | tools for common general use packages. 97 | 98 | BSD 3-Clause License 99 | 100 | Copyright (c) 2017, University of Cincinnati 101 | All rights reserved. 102 | 103 | Redistribution and use in source and binary forms, with or without 104 | modification, are permitted provided that the following conditions are met: 105 | 106 | * Redistributions of source code must retain the above copyright notice, this 107 | list of conditions and the following disclaimer. 108 | 109 | * Redistributions in binary form must reproduce the above copyright notice, 110 | this list of conditions and the following disclaimer in the documentation 111 | and/or other materials provided with the distribution. 112 | 113 | * Neither the name of the copyright holder nor the names of its 114 | contributors may be used to endorse or promote products derived from 115 | this software without specific prior written permission. 116 | 117 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 118 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 119 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 120 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 121 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 122 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 123 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 124 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 125 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 126 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -------------------------------------------------------------------------------- /demo/RowMatcher.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of S-PTAM. 3 | * 4 | * Copyright (C) 2013-2017 Taihú Pire 5 | * Copyright (C) 2014-2017 Thomas Fischer 6 | * Copyright (C) 2016-2017 Gastón Castro 7 | * Copyright (C) 2017 Matias Nitsche 8 | * For more information see 9 | * 10 | * S-PTAM is free software: you can redistribute it and/or modify 11 | * it under the terms of the GNU General Public License as published by 12 | * the Free Software Foundation, either version 3 of the License, or 13 | * (at your option) any later version. 14 | * 15 | * S-PTAM is distributed in the hope that it will be useful, 16 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | * GNU General Public License for more details. 19 | * 20 | * You should have received a copy of the GNU General Public License 21 | * along with S-PTAM. If not, see . 22 | * 23 | * Authors: Taihú Pire 24 | * Thomas Fischer 25 | * Gastón Castro 26 | * Matías Nitsche 27 | * 28 | * Laboratory of Robotics and Embedded Systems 29 | * Department of Computer Science 30 | * Faculty of Exact and Natural Sciences 31 | * University of Buenos Aires 32 | */ 33 | 34 | #include "RowMatcher.hpp" 35 | // #include "utils/macros.hpp" 36 | #include 37 | #include // std::begin, std::end 38 | #include // std::iota 39 | #include // std::lower_bound, std::upper_bound, std::sort 40 | #include // std::bind 41 | 42 | /** useful macros */ 43 | #define forn(i,n) for(size_t i=0;i<(n);i++) 44 | #define fornr(i,n) for(size_t i=(n)-1;0<=i;i--) 45 | #define forsn(i,s,n) for(size_t i=(s);i<(n);i++) 46 | #define forsnr(i,s,n) for(size_t i=(n)-1;(s)<=i;i--) 47 | #define forall(it,X) for(decltype((X).begin()) it=(X).begin();it!=(X).end();it++) 48 | #define forallr(it,X) for(decltype((X).rbegin()) it=(X).rbegin();it!=(X).rend();it++) 49 | 50 | 51 | bool compare(int a, int b, const std::vector& keypoints) 52 | { 53 | return keypoints[a].pt.y < keypoints[b].pt.y; 54 | } 55 | 56 | /* lower_bound uses compare function as (*it < value) */ 57 | bool compareLowerBound(int index, double value, const std::vector& keypoints) 58 | { 59 | return keypoints[index].pt.y < value; 60 | } 61 | 62 | /* upper_bound uses compare function as (value < *it) */ 63 | bool compareUpperBound(double value, int index, const std::vector& keypoints) 64 | { 65 | return value < keypoints[index].pt.y; 66 | } 67 | 68 | RowMatcher::RowMatcher(double maxDistance, cv::Ptr descriptorMatcher, double rowRange) 69 | : descriptorMatcher_( descriptorMatcher->clone() ), matchingDistanceThreshold_( maxDistance ), range_( rowRange ) 70 | {} 71 | 72 | RowMatcher::RowMatcher(double maxDistance, int normType, bool crossCheck, double rowRange) 73 | : descriptorMatcher_( new cv::BFMatcher(normType, crossCheck) ), matchingDistanceThreshold_( maxDistance ), range_( rowRange ) 74 | {} 75 | 76 | void RowMatcher::match( 77 | const std::vector& keypoints1, const cv::Mat& descriptors1, 78 | const std::vector& keypoints2, const cv::Mat& descriptors2, 79 | std::vector& matches 80 | ) const 81 | { 82 | RowMatcher::match( *descriptorMatcher_, matchingDistanceThreshold_, keypoints1, descriptors1, keypoints2, descriptors2, matches, range_ ); 83 | } 84 | 85 | void RowMatcher::match( 86 | const cv::DescriptorMatcher& matcher, double matchingDistanceThreshold, 87 | const std::vector& keypoints1, const cv::Mat& descriptors1, 88 | const std::vector& keypoints2, const cv::Mat& descriptors2, 89 | std::vector& matches, double range 90 | ) 91 | { 92 | // compute a vector of sorted keypoint / descriptor indexes, 93 | // sorted by row value 94 | std::vector sorted_indexes_1( keypoints1.size() ); 95 | std::vector sorted_indexes_2( keypoints2.size() ); 96 | 97 | // fill the indexes with {0,1,2...} 98 | std::iota(sorted_indexes_1.begin(), sorted_indexes_1.end(), 0); 99 | std::iota(sorted_indexes_2.begin(), sorted_indexes_2.end(), 0); 100 | 101 | // sort the indexes by row value 102 | std::sort(sorted_indexes_1.begin(), sorted_indexes_1.end(), std::bind(compare, std::placeholders::_1, std::placeholders::_2, keypoints1)); 103 | std::sort(sorted_indexes_2.begin(), sorted_indexes_2.end(), std::bind(compare, std::placeholders::_1, std::placeholders::_2, keypoints2)); 104 | 105 | // compute the sorted train descriptor matrix 106 | cv::Mat descriptors2_sorted( descriptors2.rows, descriptors2.cols, descriptors2.type() ); 107 | 108 | forn(i, keypoints2.size()) { 109 | descriptors2_sorted.row( i ) = descriptors2.row( sorted_indexes_2[ i ] ); 110 | } 111 | 112 | // for each query keypoint / descriptor, find a match 113 | matches.reserve(keypoints1.size()); 114 | forn(i1, keypoints1.size()) 115 | { 116 | int idx1 = sorted_indexes_1[ i1 ]; 117 | 118 | const cv::Mat& query_descriptor = descriptors1.row( idx1 ); 119 | const cv::Point2d query_measurement = keypoints1[ idx1 ].pt; 120 | 121 | // extract potential matches by row value 122 | // TODO optimize after testing module, min and max should be 123 | // incremented in each step from the last one 124 | 125 | // get range iterators for potential matches 126 | auto min = std::lower_bound(std::begin(sorted_indexes_2), std::end(sorted_indexes_2), 127 | query_measurement.y - range, std::bind(compareLowerBound, std::placeholders::_1, std::placeholders::_2, keypoints2)); 128 | auto max = std::upper_bound(std::begin(sorted_indexes_2), std::end(sorted_indexes_2), 129 | query_measurement.y + range, std::bind(compareUpperBound, std::placeholders::_1, std::placeholders::_2, keypoints2)); 130 | 131 | // if there are no descriptors in the specified row range 132 | // skip to next query point 133 | if ( not (max - min) ) 134 | continue; 135 | 136 | // get potential descriptors in row range 137 | cv::Mat mask(cv::Mat::zeros(1, descriptors2.rows, CV_8UC1)); 138 | for(auto it = min; it != max; it++) { 139 | int valid_index = *it; 140 | mask.at(valid_index) = true; 141 | } 142 | 143 | std::vector match_candidates; 144 | matcher.match(query_descriptor, descriptors2, match_candidates, mask); 145 | 146 | // was found a match? 147 | if (match_candidates.empty()) 148 | continue; 149 | 150 | // check if satisfy the matching distance threshold 151 | if (match_candidates[0].distance > matchingDistanceThreshold) 152 | continue; 153 | 154 | match_candidates[0].queryIdx = idx1; 155 | 156 | matches.push_back(match_candidates[0]); 157 | } 158 | } 159 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8...3.28) 2 | project(StereoLoopDetector 3 | VERSION 1.0 4 | DESCRIPTION "Stereo Loop Detector by Nicolas Soncini" 5 | LANGUAGES CXX) 6 | 7 | set(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake" ${CMAKE_MODULE_PATH}) 8 | include(ExternalProject) 9 | include(FetchContent) 10 | 11 | option(BUILD_DemoStereo "Build demo stereo application" ON) 12 | 13 | if(NOT CMAKE_BUILD_TYPE AND NOT CMAKE_CONFIGURATION_TYPES) 14 | set(CMAKE_BUILD_TYPE Release CACHE STRING "Choose the type of build." FORCE) 15 | set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS "Debug" "Release" 16 | "MinSizeRel" "RelWithDebInfo") 17 | endif() 18 | 19 | ## Options 20 | add_compile_options(-Wall -Wextra -Wpedantic) 21 | 22 | ## Headers 23 | set(HDRS 24 | include/StereoLoopDetector/DLoopDetector.h 25 | include/StereoLoopDetector/TemplatedLoopDetector.h 26 | include/StereoLoopDetector/StereoParameters.h) 27 | 28 | ## Dependencies 29 | set(DEPENDENCY_DIR 30 | ${CMAKE_CURRENT_BINARY_DIR}/dependencies) 31 | set(DEPENDENCY_INSTALL_DIR 32 | ${DEPENDENCY_DIR}/install) 33 | set(DEPENDENCY_CMAKE_ARGS 34 | -DCMAKE_BUILD_TYPE={CMAKE_BUILD_TYPE}) 35 | 36 | # OpenCV 37 | if(DEFINED OpenCV_DIR) 38 | set(DEPENDENCY_CMAKE_ARGS 39 | ${DEPENDENCY_CMAKE_ARGS} 40 | -DOpenCV_DIR=${OpenCV_DIR}) 41 | endif() 42 | find_package(OpenCV 4.9.0 REQUIRED 43 | COMPONENTS sfm) 44 | include_directories(${OpenCV_INCLUDE_DIRS}) 45 | 46 | # Boost (Needed for option parsing on the stereo demo, logging in the future) 47 | find_package(Boost 1.74.0 REQUIRED 48 | COMPONENTS program_options log) 49 | if(Boost_FOUND) 50 | include_directories(${Boost_INCLUDE_DIRS}) 51 | endif() 52 | 53 | # Sophus 54 | find_package(Sophus REQUIRED) 55 | if(Sophus_FOUND) 56 | include_directories(${Sophus_INCLUDE_DIRS}) 57 | endif() 58 | 59 | # Yaml-cpp (Needed for stereo param parsing on the stereo demo) 60 | FetchContent_Declare( 61 | yaml-cpp 62 | GIT_REPOSITORY https://github.com/jbeder/yaml-cpp.git 63 | GIT_TAG yaml-cpp-0.7.0) 64 | FetchContent_GetProperties(yaml-cpp) 65 | if(NOT yaml-cpp_POPULATED) 66 | message(STATUS "Fetching yaml-cpp...") 67 | FetchContent_Populate(yaml-cpp) 68 | add_subdirectory(${yaml-cpp_SOURCE_DIR} ${yaml-cpp_BINARY_DIR}) 69 | endif() 70 | 71 | # TODO: reupload to our servers and replace with DownloadProject macro 72 | # to ease the patch application, which is clunky right now 73 | macro(GetDependency name other_dependency) 74 | find_package(${name} QUIET 75 | PATHS ${DEPENDENCY_INSTALL_DIR}) 76 | if(${${name}_FOUND}) 77 | message("${name} library found, using it from the system") 78 | include_directories(${${name}_INCLUDE_DIRS}) 79 | add_custom_target(${name}_dep) 80 | else(${${name}_FOUND}) 81 | message("${name} library not found in the system, it will be downloaded on build") 82 | option(DOWNLOAD_${name}_dependency "Download ${name} dependency" ON) 83 | if(${DOWNLOAD_${name}_dependency}) 84 | if(NOT ${other_dependency}) 85 | set(dependency ${other_dependency}_dep) 86 | endif() 87 | ExternalProject_Add(${name} 88 | PREFIX ${DEPENDENCY_DIR} 89 | GIT_REPOSITORY http://github.com/dorian3d/${name} 90 | GIT_TAG master 91 | INSTALL_DIR ${DEPENDENCY_INSTALL_DIR} 92 | CMAKE_ARGS -DCMAKE_INSTALL_PREFIX= ${DEPENDENCY_CMAKE_ARGS} 93 | DEPENDS ${dependency}) 94 | add_custom_target(${name}_dep ${CMAKE_COMMAND} ${CMAKE_SOURCE_DIR} DEPENDS ${name}) 95 | else() 96 | message(SEND_ERROR "Please, activate DOWNLOAD_${name}_dependency option or download manually") 97 | endif(${DOWNLOAD_${name}_dependency}) 98 | endif(${${name}_FOUND}) 99 | endmacro(GetDependency) 100 | 101 | # DLib 102 | GetDependency(DLib "" "") 103 | 104 | # DBoW2 105 | # TODO: incorporate extra optional step of patching to above macro 106 | find_package(DBoW2 QUIET 107 | PATHS ${DEPENDENCY_INSTALL_DIR}) 108 | if(${DBoW2_FOUND}) 109 | message("DBoW2 library found, using it from the system") 110 | include_directories(${DBoW2_INCLUDE_DIRS}) 111 | add_custom_target(DBoW2_dep) 112 | else(${DBoW2_FOUND}) 113 | message("DBoW2 library not found in the system, it will be downloaded on build") 114 | option(DOWNLOAD_DBoW2_dependency "Download DBoW2 dependency" ON) 115 | if(${DOWNLOAD_DBoW2_dependency}) 116 | if(NOT ${other_dependency}) 117 | set(dependency ${other_dependency}_dep) 118 | endif() 119 | ExternalProject_Add(DBoW2 120 | PREFIX ${DEPENDENCY_DIR} 121 | GIT_REPOSITORY http://github.com/dorian3d/DBoW2 122 | GIT_TAG master 123 | INSTALL_DIR ${DEPENDENCY_INSTALL_DIR} 124 | CMAKE_ARGS -DCMAKE_INSTALL_PREFIX= ${DEPENDENCY_CMAKE_ARGS} 125 | DEPENDS DLib_dep 126 | PATCH_COMMAND git --git-dir= apply ${CMAKE_SOURCE_DIR}/resources/DBoW2_PR-43.patch ${CMAKE_SOURCE_DIR}/resources/DBoW2_PR-68.patch) 127 | add_custom_target(DBoW2_dep ${CMAKE_COMMAND} ${CMAKE_SOURCE_DIR} DEPENDS DBoW2) 128 | else() 129 | message(SEND_ERROR "Please, activate DOWNLOAD_DBoW2_dependency option or download manually") 130 | endif(${DOWNLOAD_DBoW2_dependency}) 131 | endif(${DBoW2_FOUND}) 132 | # GetDependency(DBoW2 DLib 'git --git-dir= apply ${CMAKE_SOURCE_DIR}/resources/DBoW2_PR-43.patch ${CMAKE_SOURCE_DIR}/resources/DBoW2_PR-68.patch') 133 | 134 | add_custom_target(Dependencies 135 | ${CMAKE_COMMAND} 136 | ${CMAKE_SOURCE_DIR} 137 | DEPENDS DBoW2_dep DLib_dep) 138 | 139 | include_directories(include/StereoLoopDetector/) 140 | 141 | ## Applications / Binaries 142 | if(BUILD_DemoStereo) 143 | add_executable(demo_stereo demo/demo_stereo.cpp 144 | demo/RowMatcher.cpp demo/RowMatcher.hpp 145 | demo/ORBextractor.cc demo/ORBextractor.h) 146 | add_dependencies(demo_stereo DLib_dep DBoW2_dep) 147 | target_link_libraries(demo_stereo 148 | ${OpenCV_LIBS} 149 | ${DLib_LIBS} 150 | ${DBoW2_LIBS} 151 | Boost::program_options 152 | yaml-cpp::yaml-cpp) 153 | target_include_directories(demo_stereo PUBLIC 154 | ${DLib_INCLUDE_DIRS} 155 | ${DBoW2_INCLUDE_DIRS}) 156 | set_target_properties(demo_stereo PROPERTIES CXX_STANDARD 17) 157 | endif(BUILD_DemoStereo) 158 | 159 | configure_file(src/StereoLoopDetector.cmake.in 160 | "${PROJECT_BINARY_DIR}/StereoLoopDetector.cmake" @ONLY) 161 | 162 | install(DIRECTORY include/StereoLoopDetector DESTINATION ${CMAKE_INSTALL_PREFIX}/include) 163 | install(FILES "${CMAKE_CURRENT_BINARY_DIR}/StereoLoopDetector.cmake" 164 | DESTINATION ${CMAKE_INSTALL_PREFIX}/include/${PROJECT_NAME}) 165 | install(FILES "${PROJECT_BINARY_DIR}/StereoLoopDetector.cmake" 166 | DESTINATION ${CMAKE_INSTALL_PREFIX}/lib/cmake/StereoLoopDetector/) 167 | install(DIRECTORY ${DEPENDENCY_INSTALL_DIR}/ DESTINATION ${CMAKE_INSTALL_PREFIX} OPTIONAL) 168 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Stereo Loop Detector 2 | 3 | Code for the article ["Addressing the challenges of loop detection in 4 | agricultural environments"](https://onlinelibrary.wiley.com/doi/10.1002/rob.22414), published in the Journal of Field Robotics in 2024. 5 | A pre-print version can be found in arxiv.org: https://arxiv.org/abs/2408.15761 6 | An explanatory video can be found on YouTube: https://www.youtube.com/watch?v=DujsHfyyhNI 7 | 8 | [![YouTube](http://i.ytimg.com/vi/DujsHfyyhNI/hqdefault.jpg)](https://www.youtube.com/watch?v=DujsHfyyhNI) 9 | 10 | If you use this work in academic work please cite: 11 | 12 | ```text 13 | @article{soncini2024argiloopchallenges, 14 | author = {Nicol\'as, Soncini and Javier, Civera and Taih\'u, Pire}, 15 | title = {Addressing the challenges of loop detection in agricultural environments}, 16 | journal = {Journal of Field Robotics}, 17 | volume = {}, 18 | number = {}, 19 | pages = {}, 20 | keywords = {agricultural fields, loop detection, open fields, SLAM, visual place recognition}, 21 | doi = {https://doi.org/10.1002/rob.22414}, 22 | url = {https://onlinelibrary.wiley.com/doi/abs/10.1002/rob.22414}, 23 | eprint = {https://onlinelibrary.wiley.com/doi/pdf/10.1002/rob.22414} 24 | } 25 | ``` 26 | and also 27 | ```text 28 | @article{GalvezTRO12, 29 | author={G\'alvez-L\'opez, Dorian and Tard\'os, J. D.}, 30 | journal={IEEE Transactions on Robotics}, 31 | title={Bags of Binary Words for Fast Place Recognition in Image Sequences}, 32 | year={2012}, 33 | month={October}, 34 | volume={28}, 35 | number={5}, 36 | pages={1188--1197}, 37 | doi={10.1109/TRO.2012.2197158}, 38 | ISSN={1552-3098} 39 | } 40 | ``` 41 | 42 | Make sure to read the LICENSE uploaded to this repo if you intend to use or modify this code. 43 | 44 | For any comments or issues with the code please submit an issue/ticket through the platform or send us an e-mail to the address listed in the article. 45 | 46 | ## Description 47 | StereoLoopDetector works by detecting loops in sequences of stereo images collected by a robot or mobile platform and returning an approximate relative pose between them. 48 | It implements the algorithm presented in Gálvez-López D. et al. (2012) with additional modifications to support stereo images and relative pose estimation as presented in Soncini N. et al. (2024). 49 | 50 | ## Building & Installing 51 | All of the software was ran and tested with Ubuntu 22.04 and 24.04. 52 | 53 | ### Dependencies 54 | First you will have to install the dependencies, if you don't have them already installed and discoverable by CMake. 55 | 56 | - [cmake](https://cmake.org) 57 | can be installed via aptitude `sudo apt install cmake` 58 | - [opencv + opencv_contrib](https://opencv.org/) (tested w/version 4.9.x) 59 | contrib modules used: sfm, ... 60 | usually has to be compiled from source for contrib modules 61 | - [boost](https://www.boost.org/) 62 | additional modules used: boost::dynamic_bitset (for BRIEF), boost::program_options (for demos) 63 | can be installed via aptitude `sudo apt install libboost-dev` 64 | usually has to be compiled for these extra modules 65 | usually has to be compiled from source for additional modules 66 | - [yaml-cpp](https://github.com/jbeder/yaml-cpp) 67 | used for parsing stereo parameters, can be removed from code easily 68 | installed automatically when cmake + make runs 69 | - [DLib](https://github.com/dorian3d/DLib) + PR patches 70 | installed automatically when cmake + make runs 71 | - [DBoW2](https://github.com/dorian3d/DBoW2) + PR patches 72 | installed automatically when cmake + make runs 73 | 74 | ### Building 75 | 76 | To configure the project using cmake in a `build` folder: 77 | ```bash 78 | cmake -S . -B build 79 | ``` 80 | add `-GNinja` if you have ninja installed. 81 | 82 | To build: 83 | ```bash 84 | cmake --build build 85 | ``` 86 | or use `make` from the `build` folder. 87 | 88 | ## Usage 89 | To run the code you need to have 5 things ready: 90 | - the stereo images 91 | - two files with newline-separated full paths to the images (one for the left and one for the right camera) 92 | - a stereo camera calibration file (example provided in `resources/fs_stereo_parameters.yaml`) 93 | - a file with the poses of the images (or empty file if not displaying anything) 94 | - a vocabulary generated by DBoW2 of whichever feature detector+descriptor used (we recommend checking ORB_SLAM3, S S-PTAM or similar open-sourced libraries for pre-generated ones) 95 | 96 | Then calling the already built StereoLoopDetector is as easy as: 97 | ```bash 98 | cd build/ 99 | ./demo_stereo -h 100 | ``` 101 | and completing the necessary listed parameters. 102 | 103 | ## Reproducing our results 104 | 105 | Our article showcases the results of the StereoLoopDetector on the [FieldSAFE dataset](https://vision.eng.au.dk/fieldsafe/), for which we had to extract the images, synchronize them with the GPS positions, project the GPS coordinates to a planar projection (for plotting), generate the calibration files, etc. 106 | 107 | We cannot provide the dataset as it is not of our authorship, but we can provide a step-by-step to download and test on them. 108 | The scripts for evaluation and visualization are not included just yet. 109 | 110 | Steps to reproduce: 111 | 1. download the FieldSAFE session of your choice from the dataset website (e.g. [FieldSAFE Dynamic obstacle session #1](https://vision.eng.au.dk/?download=/data/FieldSAFE/2016-10-25-11-41-21.bag)) 112 | 2. extract the stereo images from the rosbag, we chose to name them with the timestamp and saving the left and right stereo images to separate folders, 113 | 3. filter the images to be certain that every left image has a right image (we detected this was not always the case, even though they were supposedly hardware triggered), 114 | 4. (optional) create the poses .csv file by extracting the GPS poses, projecting them to a plane and making sure that each image has a corresponding pose (interpolate if needed); optionally just use the empty file in `resources/fs_empty_poses.txt`, just be aware that the visualziation tools that were carried on from DLoopDetector will not work, 115 | 5. find out the camera parameters and create the stereo parameters yaml (or use the one in `resources/fs_stereo_parameters.yaml`), 116 | 6. download a vocabulary file for the ORB features (e.g. [ORB_SLAM3 Vocabulary file](https://github.com/UZ-SLAMLab/ORB_SLAM3/raw/master/Vocabulary/ORBvoc.txt.tar.gz) pairs with our ORB_SLAM3 ORB extractor) 117 | 7. run the demo! 118 | 119 | 120 | ## Acknowledgements 121 | We thank the original authors of DLoopDetector for making the code open and available to let us _stand on the shoulders of giants_ to more easily contribute our enhancements for agricultural stereo loop detection. 122 | We thank the authors of S-PTAM for makign their code open and allowing us to take inspiration on stereo SLAM. 123 | We thank the autors of ORB_SLAM for making their ORB feature detector and descriptor open along with their DBoW2 vocabulary so we didn't have to retrain one of our own for initial tests. 124 | We thank ourselves, because getting experimental code to be mostly readable and usable by the community takes time. -------------------------------------------------------------------------------- /cmake/DownloadProject.cmake: -------------------------------------------------------------------------------- 1 | # Distributed under the OSI-approved MIT License. See accompanying 2 | # file LICENSE or https://github.com/Crascit/DownloadProject for details. 3 | # 4 | # MODULE: DownloadProject 5 | # 6 | # PROVIDES: 7 | # download_project( PROJ projectName 8 | # [PREFIX prefixDir] 9 | # [DOWNLOAD_DIR downloadDir] 10 | # [SOURCE_DIR srcDir] 11 | # [BINARY_DIR binDir] 12 | # [QUIET] 13 | # ... 14 | # ) 15 | # 16 | # Provides the ability to download and unpack a tarball, zip file, git repository, 17 | # etc. at configure time (i.e. when the cmake command is run). How the downloaded 18 | # and unpacked contents are used is up to the caller, but the motivating case is 19 | # to download source code which can then be included directly in the build with 20 | # add_subdirectory() after the call to download_project(). Source and build 21 | # directories are set up with this in mind. 22 | # 23 | # The PROJ argument is required. The projectName value will be used to construct 24 | # the following variables upon exit (obviously replace projectName with its actual 25 | # value): 26 | # 27 | # projectName_SOURCE_DIR 28 | # projectName_BINARY_DIR 29 | # 30 | # The SOURCE_DIR and BINARY_DIR arguments are optional and would not typically 31 | # need to be provided. They can be specified if you want the downloaded source 32 | # and build directories to be located in a specific place. The contents of 33 | # projectName_SOURCE_DIR and projectName_BINARY_DIR will be populated with the 34 | # locations used whether you provide SOURCE_DIR/BINARY_DIR or not. 35 | # 36 | # The DOWNLOAD_DIR argument does not normally need to be set. It controls the 37 | # location of the temporary CMake build used to perform the download. 38 | # 39 | # The PREFIX argument can be provided to change the base location of the default 40 | # values of DOWNLOAD_DIR, SOURCE_DIR and BINARY_DIR. If all of those three arguments 41 | # are provided, then PREFIX will have no effect. The default value for PREFIX is 42 | # CMAKE_BINARY_DIR. 43 | # 44 | # The QUIET option can be given if you do not want to show the output associated 45 | # with downloading the specified project. 46 | # 47 | # In addition to the above, any other options are passed through unmodified to 48 | # ExternalProject_Add() to perform the actual download, patch and update steps. 49 | # The following ExternalProject_Add() options are explicitly prohibited (they 50 | # are reserved for use by the download_project() command): 51 | # 52 | # CONFIGURE_COMMAND 53 | # BUILD_COMMAND 54 | # INSTALL_COMMAND 55 | # TEST_COMMAND 56 | # 57 | # Only those ExternalProject_Add() arguments which relate to downloading, patching 58 | # and updating of the project sources are intended to be used. Also note that at 59 | # least one set of download-related arguments are required. 60 | # 61 | # If using CMake 3.2 or later, the UPDATE_DISCONNECTED option can be used to 62 | # prevent a check at the remote end for changes every time CMake is run 63 | # after the first successful download. See the documentation of the ExternalProject 64 | # module for more information. It is likely you will want to use this option if it 65 | # is available to you. Note, however, that the ExternalProject implementation contains 66 | # bugs which result in incorrect handling of the UPDATE_DISCONNECTED option when 67 | # using the URL download method or when specifying a SOURCE_DIR with no download 68 | # method. Fixes for these have been created, the last of which is scheduled for 69 | # inclusion in CMake 3.8.0. Details can be found here: 70 | # 71 | # https://gitlab.kitware.com/cmake/cmake/commit/bdca68388bd57f8302d3c1d83d691034b7ffa70c 72 | # https://gitlab.kitware.com/cmake/cmake/issues/16428 73 | # 74 | # If you experience build errors related to the update step, consider avoiding 75 | # the use of UPDATE_DISCONNECTED. 76 | # 77 | # EXAMPLE USAGE: 78 | # 79 | # include(DownloadProject) 80 | # download_project(PROJ googletest 81 | # GIT_REPOSITORY https://github.com/google/googletest.git 82 | # GIT_TAG master 83 | # UPDATE_DISCONNECTED 1 84 | # QUIET 85 | # ) 86 | # 87 | # add_subdirectory(${googletest_SOURCE_DIR} ${googletest_BINARY_DIR}) 88 | # 89 | #======================================================================================== 90 | 91 | 92 | set(_DownloadProjectDir "${CMAKE_CURRENT_LIST_DIR}") 93 | 94 | include(CMakeParseArguments) 95 | 96 | function(download_project) 97 | 98 | set(options QUIET) 99 | set(oneValueArgs 100 | PROJ 101 | PREFIX 102 | DOWNLOAD_DIR 103 | SOURCE_DIR 104 | BINARY_DIR 105 | # Prevent the following from being passed through 106 | CONFIGURE_COMMAND 107 | BUILD_COMMAND 108 | INSTALL_COMMAND 109 | TEST_COMMAND 110 | ) 111 | set(multiValueArgs "") 112 | 113 | cmake_parse_arguments(DL_ARGS "${options}" "${oneValueArgs}" "${multiValueArgs}" ${ARGN}) 114 | 115 | # Hide output if requested 116 | if (DL_ARGS_QUIET) 117 | set(OUTPUT_QUIET "OUTPUT_QUIET") 118 | else() 119 | unset(OUTPUT_QUIET) 120 | message(STATUS "Downloading/updating ${DL_ARGS_PROJ}") 121 | endif() 122 | 123 | # Set up where we will put our temporary CMakeLists.txt file and also 124 | # the base point below which the default source and binary dirs will be 125 | if (NOT DL_ARGS_PREFIX) 126 | set(DL_ARGS_PREFIX "${CMAKE_BINARY_DIR}") 127 | endif() 128 | if (NOT DL_ARGS_DOWNLOAD_DIR) 129 | set(DL_ARGS_DOWNLOAD_DIR "${DL_ARGS_PREFIX}/${DL_ARGS_PROJ}-download") 130 | endif() 131 | 132 | # Ensure the caller can know where to find the source and build directories 133 | if (NOT DL_ARGS_SOURCE_DIR) 134 | set(DL_ARGS_SOURCE_DIR "${DL_ARGS_PREFIX}/${DL_ARGS_PROJ}-src") 135 | endif() 136 | if (NOT DL_ARGS_BINARY_DIR) 137 | set(DL_ARGS_BINARY_DIR "${DL_ARGS_PREFIX}/${DL_ARGS_PROJ}-build") 138 | endif() 139 | set(${DL_ARGS_PROJ}_SOURCE_DIR "${DL_ARGS_SOURCE_DIR}" PARENT_SCOPE) 140 | set(${DL_ARGS_PROJ}_BINARY_DIR "${DL_ARGS_BINARY_DIR}" PARENT_SCOPE) 141 | 142 | # Create and build a separate CMake project to carry out the download. 143 | # If we've already previously done these steps, they will not cause 144 | # anything to be updated, so extra rebuilds of the project won't occur. 145 | configure_file("${_DownloadProjectDir}/DownloadProject.CMakeLists.cmake.in" 146 | "${DL_ARGS_DOWNLOAD_DIR}/CMakeLists.txt") 147 | execute_process(COMMAND ${CMAKE_COMMAND} -G "${CMAKE_GENERATOR}" . 148 | RESULT_VARIABLE result 149 | ${OUTPUT_QUIET} 150 | WORKING_DIRECTORY "${DL_ARGS_DOWNLOAD_DIR}" 151 | ) 152 | if(result) 153 | message(FATAL_ERROR "CMake step for ${DL_ARGS_PROJ} failed: ${result}") 154 | endif() 155 | execute_process(COMMAND ${CMAKE_COMMAND} --build . 156 | RESULT_VARIABLE result 157 | ${OUTPUT_QUIET} 158 | WORKING_DIRECTORY "${DL_ARGS_DOWNLOAD_DIR}" 159 | ) 160 | if(result) 161 | message(FATAL_ERROR "Build step for ${DL_ARGS_PROJ} failed: ${result}") 162 | endif() 163 | 164 | endfunction() 165 | -------------------------------------------------------------------------------- /demo/demo_stereo.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include 6 | 7 | // DLoopDetector and DBoW2 8 | #include "DLoopDetector.h" 9 | #include "StereoParameters.h" 10 | #include 11 | #include 12 | #include "ORBextractor.h" // ORB_SLAM3 ORB Implementation 13 | 14 | // OpenCV 15 | #include 16 | #include 17 | 18 | #include "demoDetectorStereo.h" 19 | 20 | using namespace DLoopDetector; 21 | using namespace DBoW2; 22 | using namespace DVision; 23 | using namespace std; 24 | namespace po = boost::program_options; 25 | 26 | // TODO: remove dependency on these constants if possible 27 | static const int IMAGE_W = 1280; // image size 28 | static const int IMAGE_H = 720; 29 | 30 | // FAST + BRIEF Keypoint Detector and Descriptor Configuration 31 | static const int FAST_THRESH = 20; // corner detector response threshold 32 | static const bool FAST_NMS = true; // perform non-maxima-suppression 33 | static const int FAST_RETAIN_BEST = 2000; //retain the best keypoints only 34 | static const char *BRIEF_PATTERN_FILE = "./resources/brief_pattern.yml"; 35 | static const char *BRIEF_VOC_FILE = "./resources/brief_k10L6.voc.gz"; 36 | 37 | // ORB Keypoint Detector and Descriptor Configuration 38 | static const char *ORB_VOC_FILE = "../resources/ORBvoc.yml.gz"; 39 | static const int ORB_NFEATURES = 2000; 40 | static const float ORB_SCALEFACTOR = 1.2f; 41 | static const int ORB_NLEVELS = 8; 42 | 43 | // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 44 | 45 | // This functor extracts BRIEF descriptors in the required format 46 | class BriefExtractor: public FeatureExtractor 47 | { 48 | public: 49 | /** 50 | * Extracts features from an image 51 | * @param im image 52 | * @param keys keypoints extracted 53 | * @param descriptors descriptors extracted 54 | */ 55 | void operator()(const cv::Mat &im, 56 | vector &keys, vector &descriptors) const override; 57 | 58 | /** 59 | * Creates the brief extractor with the given pattern file 60 | * @param pattern_file 61 | */ 62 | BriefExtractor(const std::string &pattern_file); 63 | 64 | private: 65 | 66 | /// BRIEF descriptor extractor 67 | BRIEF256 m_brief; 68 | }; 69 | 70 | // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 71 | 72 | class OrbExtractor: public FeatureExtractor 73 | { 74 | public: 75 | /** 76 | * Extracts features from an image 77 | * @param im image 78 | * @param keys keypoints extracted 79 | * @param descriptors descriptors extracted 80 | */ 81 | void operator()(const cv::Mat &im, 82 | vector &keys, vector &descriptors) const override; 83 | 84 | /** 85 | * Creates the brief extractor with the given pattern file 86 | * @param pattern_file 87 | */ 88 | OrbExtractor(int nfeatures, float scalefactor, int nlevels); 89 | 90 | private: 91 | 92 | // ORB descriptor and detector 93 | ORB_SLAM3::ORBextractor *orbext; 94 | }; 95 | 96 | // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 97 | 98 | int main(int argc, char* argv[]) 99 | { 100 | std::string path_left; 101 | std::string path_right; 102 | std::string path_calibration; 103 | std::string poses_path; 104 | std::string type, voc_path; 105 | // Program Options (BOOST library w/special compilation needed) 106 | po::options_description options_desc( 107 | "StereoLoopDetector demo usage with ORB or BRIEF features.\n\n" 108 | "Allowed options"); 109 | options_desc.add_options() 110 | ("help,h", "produce help message") 111 | ("input-left,l", po::value(&path_left)->required(), 112 | "path to file with newline separated paths to left camera input images") 113 | ("input-right,r", po::value(&path_right)->required(), 114 | "path to file with newline separated paths to right camera input images") 115 | ("calibration", po::value(&path_calibration)->required(), 116 | "path to file with calibration parameters") 117 | ("poses-file", po::value(&poses_path)->required(), 118 | "path to file with CSV poses x,y") 119 | ("type", po::value(&type)->default_value("ORB"), "Type of keypoint extractor/descriptor") 120 | ("voc", po::value(&voc_path), "Path to vocabulary file for specified type") 121 | ("noshow", po::value()->default_value(true), "Don't display results") 122 | ; 123 | 124 | // Parse program options 125 | po::variables_map options_vm; 126 | try { 127 | po::store(po::parse_command_line(argc, argv, options_desc), options_vm); 128 | if (options_vm.count("help")) { 129 | std::cout << options_desc << std::endl; 130 | return 0; 131 | } 132 | po::notify(options_vm); 133 | } catch (const po::error &ex) { 134 | std::cerr << ex.what() << std::endl; 135 | std::cout << std::endl; 136 | std::cout << options_desc << std::endl; 137 | return 1; 138 | } 139 | 140 | // Store options 141 | bool show = !options_vm["noshow"].as(); 142 | 143 | // Validate options 144 | if (!((type == "BRIEF") || (type == "ORB"))) { 145 | std::cerr << "\"type\" option should be one of BRIEF or ORB, not " << 146 | type << std::endl; 147 | throw po::validation_error(po::validation_error::invalid_option_value, "type"); 148 | } 149 | 150 | StereoParameters sparams(path_calibration); 151 | 152 | // prepares the demo 153 | try 154 | { 155 | std::string vocabulary; 156 | if (type == "BRIEF") { 157 | vocabulary = options_vm.count("voc")? voc_path : BRIEF_VOC_FILE; 158 | demoDetectorStereo 159 | demo(vocabulary, path_left, path_right, poses_path, 160 | sparams, show); 161 | BriefExtractor extractor(BRIEF_PATTERN_FILE); 162 | demo.run(type, extractor); 163 | } else if (type == "ORB") { 164 | vocabulary = options_vm.count("voc")? voc_path : ORB_VOC_FILE; 165 | demoDetectorStereo 166 | demo(vocabulary, path_left, path_right, poses_path, 167 | sparams, show); 168 | OrbExtractor extractor(ORB_NFEATURES, ORB_SCALEFACTOR, ORB_NLEVELS); 169 | demo.run(type, extractor); 170 | } 171 | } 172 | catch(const std::string &ex) 173 | { 174 | cout << "Error: " << ex << endl; 175 | } 176 | 177 | return 0; 178 | } 179 | 180 | 181 | // ---------------------------------------------------------------------------- 182 | 183 | BriefExtractor::BriefExtractor(const std::string &pattern_file) 184 | { 185 | // The DVision::BRIEF extractor computes a random pattern by default when 186 | // the object is created. 187 | // We load the pattern that we used to build the vocabulary, to make 188 | // the descriptors compatible with the predefined vocabulary 189 | 190 | // loads the pattern 191 | cv::FileStorage fs(pattern_file.c_str(), cv::FileStorage::READ); 192 | if(!fs.isOpened()) throw string("Could not open file ") + pattern_file; 193 | 194 | vector x1, y1, x2, y2; 195 | fs["x1"] >> x1; 196 | fs["x2"] >> x2; 197 | fs["y1"] >> y1; 198 | fs["y2"] >> y2; 199 | 200 | m_brief.importPairs(x1, y1, x2, y2); 201 | } 202 | 203 | // ---------------------------------------------------------------------------- 204 | 205 | void BriefExtractor::operator() (const cv::Mat &im, 206 | vector &keys, vector &descriptors) const 207 | { 208 | // extract FAST keypoints with opencv 209 | cv::FAST(im, keys, FAST_THRESH, FAST_NMS); 210 | cv::KeyPointsFilter::retainBest(keys, FAST_RETAIN_BEST); 211 | 212 | // compute their BRIEF descriptor 213 | m_brief.compute(im, keys, descriptors); 214 | } 215 | 216 | // ---------------------------------------------------------------------------- 217 | 218 | OrbExtractor::OrbExtractor(int nfeatures, float scalefactor, int nlevels) 219 | { 220 | std::cout << "Creating ORB extractor with nfeatures=" << nfeatures << 221 | ", scalefactor=" << scalefactor << ", and nlevels=" << nlevels << 222 | std::endl; 223 | orbext = new ORB_SLAM3::ORBextractor(nfeatures, scalefactor, nlevels, 20, 7); 224 | } 225 | 226 | // ---------------------------------------------------------------------------- 227 | 228 | void OrbExtractor::operator() ( 229 | const cv::Mat &im, vector &keys, 230 | vector &descriptors) const 231 | { 232 | cv::Mat descs; 233 | vector vLapping = {0,0}; //Overlapping Pixels (stereo?) FIXME 234 | (*orbext)(im, cv::Mat(), keys, descs, vLapping); 235 | descriptors.resize(descs.rows); 236 | for(int i = 0; i < descs.rows; ++i) 237 | { 238 | descriptors[i] = descs.row(i); 239 | } 240 | } 241 | 242 | // ---------------------------------------------------------------------------- 243 | -------------------------------------------------------------------------------- /demo/demoDetectorStereo.h: -------------------------------------------------------------------------------- 1 | /** 2 | * File: demoDetectorStereo.h 3 | * Author: Nicolas Soncini (other attributions below) 4 | * Date: Aug 23, 2024 5 | * Description: Demo for loop detection on stereo images 6 | * License: See LICENSE.txt file at the top project folder 7 | * 8 | * This file has been modified from a different version, attribution: 9 | * Original File: demoDetector.h 10 | * Original Author: Dorian Galvez-Lopez 11 | * Original Last Modified Date: - 12 | * 13 | **/ 14 | 15 | #ifndef __DEMO_DETECTOR_STEREO__ 16 | #define __DEMO_DETECTOR_STEREO__ 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | // OpenCV 25 | #include 26 | #include 27 | #include 28 | 29 | // DLoopDetector and DBoW2 30 | #include 31 | #include "DLoopDetector.h" 32 | #include "TemplatedLoopDetector.h" 33 | #include 34 | #include 35 | #include 36 | 37 | #include "RowMatcher.hpp" 38 | #include "StereoParameters.h" 39 | 40 | using namespace DLoopDetector; 41 | using namespace DBoW2; 42 | using namespace std; 43 | 44 | // -------------------------------------------------------------------------- 45 | 46 | /// Generic class to create functors to extract features 47 | template 48 | class FeatureExtractor 49 | { 50 | public: 51 | /** 52 | * Extracts features 53 | * @param im image 54 | * @param keys keypoints extracted 55 | * @param descriptors descriptors extracted 56 | */ 57 | virtual void operator()(const cv::Mat &im, 58 | vector &keys, vector &descriptors) const = 0; 59 | }; 60 | 61 | // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 62 | 63 | /// @param TVocabulary vocabulary class (e.g: BriefVocabulary) 64 | /// @param TDetector detector class (e.g: BriefLoopDetector) 65 | /// @param TDescriptor descriptor class (e.g: bitset for Brief) 66 | template 67 | /// Class to run the demo 68 | class demoDetectorStereo 69 | { 70 | public: 71 | 72 | /** 73 | * @param vocfile vocabulary file to load 74 | * @param imagedir directory to read images from 75 | * @param posefile pose file 76 | * @param sparam stereo camera parameters 77 | */ 78 | demoDetectorStereo(const std::string &vocfile, const std::string &imagedir1, 79 | const std::string &imagedir2, const std::string &posefile, 80 | StereoParameters &stereoparams, bool show); 81 | 82 | ~demoDetectorStereo(){} 83 | 84 | /** 85 | * Runs the demo 86 | * @param name demo name 87 | * @param extractor functor to extract features 88 | */ 89 | void run(const std::string &name, const FeatureExtractor &extractor); 90 | 91 | protected: 92 | 93 | /** 94 | * Reads the robot poses from a file 95 | * @param filename file 96 | * @param xs 97 | * @param ys 98 | */ 99 | void readPoseFile(const char *filename, std::vector &xs, 100 | std::vector &ys) const; 101 | 102 | vector read_file_list(std::string path) const; 103 | 104 | void readPoseFileCSV( 105 | const char *filename, std::vector &xs, std::vector &ys) const; 106 | 107 | protected: 108 | 109 | std::string m_vocfile; 110 | std::string m_imagedir1; 111 | std::string m_imagedir2; 112 | std::string m_posefile; 113 | StereoParameters m_stereoparams; 114 | bool m_show; 115 | }; 116 | 117 | // --------------------------------------------------------------------------- 118 | 119 | template 120 | demoDetectorStereo::demoDetectorStereo 121 | (const std::string &vocfile, 122 | const std::string &imagedir1, const std::string &imagedir2, 123 | const std::string &posefile, StereoParameters &stereoparams, 124 | bool show) 125 | : m_vocfile(vocfile), m_imagedir1(imagedir1), m_imagedir2(imagedir2), 126 | m_posefile(posefile), m_stereoparams(stereoparams), m_show(show) 127 | { 128 | } 129 | 130 | // --------------------------------------------------------------------------- 131 | 132 | // Function to convert std::bitset to cv::Mat 133 | cv::Mat toMat(const std::vector>& descriptors) { 134 | // Assuming each bitset<256> corresponds to one descriptor 135 | int descriptorCount = descriptors.size(); 136 | int bitsPerDescriptor = 256; 137 | 138 | // Create an empty Mat to store the descriptors 139 | cv::Mat descriptorsMat(descriptorCount, bitsPerDescriptor, CV_8UC1); 140 | 141 | // Convert each bitset to Mat 142 | for (int i = 0; i < descriptorCount; ++i) { 143 | for (int j = 0; j < bitsPerDescriptor; ++j) { 144 | descriptorsMat.at(i, j) = descriptors[i][j] ? 255 : 0; 145 | } 146 | } 147 | 148 | return descriptorsMat; 149 | } 150 | 151 | cv::Mat toMat(const std::vector& descriptors){ 152 | int descriptorCount = descriptors.size(); 153 | cv::Mat descriptorsMat; 154 | 155 | for (int i = 0; i < descriptorCount; ++i) { 156 | descriptorsMat.push_back(descriptors.at(i)); 157 | } 158 | 159 | return descriptorsMat; 160 | } 161 | 162 | // --------------------------------------------------------------------------- 163 | 164 | // Function to extract matching elements from two arrays 165 | template 166 | void extractMatchingElements(const std::vector& matches, 167 | const std::vector& array1, 168 | const std::vector& array2, 169 | std::vector& matchingElements1, 170 | std::vector& matchingElements2) { 171 | matchingElements1.clear(); 172 | matchingElements2.clear(); 173 | 174 | // Iterate through the matches 175 | for (const auto& match : matches) { 176 | // Get indices from the match 177 | int index1 = match.queryIdx; 178 | int index2 = match.trainIdx; 179 | 180 | // Add matching elements from array1 and array2 to respective vectors 181 | matchingElements1.push_back(array1[index1]); 182 | matchingElements2.push_back(array2[index2]); 183 | } 184 | 185 | } 186 | 187 | // --------------------------------------------------------------------------- 188 | 189 | template 190 | void demoDetectorStereo::run 191 | (const std::string &name, const FeatureExtractor &extractor) 192 | { 193 | // Set loop detector parameters 194 | // Some references for frequency: 195 | // EuRoC 20fps, FieldSAFE stereo 10fps / webcam 30fps 196 | float m_frequency = 10; 197 | int m_height = m_stereoparams.size.height; 198 | int m_width = m_stereoparams.size.width; 199 | typename TDetector::Parameters params(m_height, m_width, m_frequency); 200 | 201 | // Parameters given by default are: 202 | // use nss = true 203 | // alpha = 0.3 204 | // k = 3 205 | // geom checking = GEOM_DI 206 | // di levels = 0 207 | 208 | // We are going to change these values individually: 209 | params.use_nss = true; // use normalized similarity instead of raw score 210 | params.alpha = 0.3; // nss threshold 211 | params.k = 5; // a loop must be consistent with k previous matches 212 | params.geom_check = GEOM_EXHAUSTIVE_STEREO; 213 | params.stereo_params = m_stereoparams; 214 | params.di_levels = 2; // number of direct index levels 215 | params.near_distance = 0.4; // min meters for triangulated points 216 | params.far_distance = 50; // max meters for triangulated points 217 | // params.dislocal = 20; // number of frames to consider close in time 218 | params.min_Fpoints = 20; // min points to compute fundamental matrix 219 | 220 | // To verify loops you can select one of the next geometrical checkings: 221 | // GEOM_EXHAUSTIVE_STEREO: correspondence points are computed by comparing 222 | // all the features between the two stereo pairs, keeping the ones that 223 | // match between them and performing PnP to return a transformation 224 | // between the triangulated 3D points of the first pair. 225 | // GEOM_FLANN: as above, but the comparisons are done with a Flann structure, 226 | // which makes them faster. However, creating the flann structure may 227 | // be slow. 228 | // GEOM_DI: the direct index is used to select correspondence points between 229 | // those features whose vocabulary node at a certain level is the same. 230 | // The level at which the comparison is done is set by the parameter 231 | // di_levels: 232 | // di_levels = 0 -> features must belong to the same leaf (word). 233 | // This is the fastest configuration and the most restrictive one. 234 | // di_levels = l (l < L) -> node at level l starting from the leaves. 235 | // The higher l, the slower the geometrical checking, but higher 236 | // recall as well. 237 | // Here, L stands for the depth levels of the vocabulary tree. 238 | // di_levels = L -> the same as the exhaustive technique. 239 | // GEOM_NONE: no geometrical checking is done. 240 | // 241 | // In general, with a 10^6 vocabulary, GEOM_DI with 2 <= di_levels <= 4 242 | // yields the best results in recall/time. 243 | // Check the T-RO paper for more information. 244 | // 245 | 246 | // Load the vocabulary to use 247 | cout << "Loading " << name << " vocabulary..." << endl; 248 | TVocabulary voc(m_vocfile); 249 | cout << "... done" << endl; 250 | 251 | // Initiate loop detector with the vocabulary 252 | cout << "Initializing detector..." << endl; 253 | TDetector detector(voc, params); 254 | cout << "... done" << endl; 255 | 256 | // Initialize row keypoint matcher 257 | // TODO: if we decide to make SURF descriptors available this would need to 258 | // change to NORM_L2 or similar, maybe passing the stereo matcher as a 259 | // parameter. Maybe passing the stereo matcher already initialized in the 260 | // parameters? 261 | cv::Ptr descriptor_matcher = 262 | cv::BFMatcher::create(cv::NORM_HAMMING); 263 | double max_distance = 50.0; 264 | double row_range = 1.0; 265 | RowMatcher stereo_matcher(max_distance, descriptor_matcher, row_range); 266 | 267 | // Process images 268 | std::vector keys1, keys2; 269 | std::vector descriptors1, descriptors2; 270 | 271 | // load image filenames 272 | cout << "Collecting image paths..." << endl; 273 | std::vector filenames1 = 274 | read_file_list(m_imagedir1); 275 | // DUtils::FileFunctions::Dir(m_imagedir.c_str(), ".png", true); 276 | std::vector filenames2 = 277 | read_file_list(m_imagedir2); 278 | // DUtils::FileFunctions::Dir(m_imagedir.c_str(), ".png", true); 279 | assert(filenames1.size() == filenames2.size()); 280 | cout << "... done" << endl; 281 | 282 | // load robot poses 283 | cout << "Loading poses..." << endl; 284 | std::vector xs, ys; 285 | // readPoseFile(m_posefile.c_str(), xs, ys); 286 | readPoseFileCSV(m_posefile.c_str(), xs, ys); 287 | cout << "done..." << endl; 288 | 289 | // we can allocate memory for the expected number of images 290 | detector.allocate(filenames1.size()); 291 | 292 | // prepare visualization windows 293 | DUtilsCV::GUI::tWinHandler win = "Current image"; 294 | DUtilsCV::GUI::tWinHandler winplot = "Trajectory"; 295 | 296 | DUtilsCV::Drawing::Plot::Style normal_style(2); // thickness 297 | DUtilsCV::Drawing::Plot::Style loop_style('r', 2); // color, thickness 298 | 299 | DUtilsCV::Drawing::Plot implot(240, 320, 300 | - *std::max_element(xs.begin(), xs.end()), 301 | - *std::min_element(xs.begin(), xs.end()), 302 | *std::min_element(ys.begin(), ys.end()), 303 | *std::max_element(ys.begin(), ys.end()), 20); 304 | 305 | // prepare profiler to measure times 306 | DUtils::Profiler profiler; 307 | 308 | // Loop Detection Matrix to store and save 309 | cv::Mat2i ldmat(filenames1.size(), filenames1.size(), (int) 0); 310 | 311 | // Loop Metric Distance Matrix to store and save 312 | cv::Mat2f lmmat(filenames1.size(), filenames1.size(), (float) 0); 313 | 314 | // Create a file to save detection info 315 | auto t = std::time(nullptr); 316 | auto tm = *std::localtime(&t); 317 | std::ostringstream dtime; 318 | dtime << std::put_time(&tm, "%d-%m-%Y_%H-%M-%S"); 319 | std::string fstorepath = dtime.str() + "_results.yml"; 320 | cv::FileStorage fstore(fstorepath, cv::FileStorage::WRITE); 321 | 322 | int count = 0; 323 | 324 | // go 325 | for(unsigned int i = 0; i < filenames1.size(); ++i) 326 | { 327 | std::cout << "Adding image " << i << std::endl; 328 | 329 | // get images 330 | cv::Mat im1 = cv::imread(filenames1[i].c_str(), 0); 331 | cv::Mat im2 = cv::imread(filenames2[i].c_str(), 0); 332 | 333 | // show image 334 | if(m_show) 335 | DUtilsCV::GUI::showImage(im1, true, &win, 10); 336 | // DUtilsCV::GUI::showImage(im2, true, &win, 10); 337 | 338 | // get features 339 | profiler.profile("features"); 340 | extractor(im1, keys1, descriptors1); 341 | extractor(im2, keys2, descriptors2); 342 | profiler.stop(); 343 | std::cout << "[StereoMatcher] found " << keys1.size() << " and " << 344 | keys2.size() << " keypoints" << std::endl; 345 | 346 | // We should only pass matching key/descriptors to detectLoop stereo 347 | std::vector stereo_matches; 348 | // transforms descriptors to cv::Mat for compatibility 349 | cv::Mat mat_descriptors1 = toMat(descriptors1); 350 | cv::Mat mat_descriptors2 = toMat(descriptors2); 351 | stereo_matcher.match( 352 | keys1, mat_descriptors1, keys2, mat_descriptors2, stereo_matches 353 | ); 354 | std::cout << "[StereoMatcher] found " << stereo_matches.size() 355 | << " stereo matches" << std::endl; 356 | // reorder keypoints and descriptors to match in order 357 | std::vector s_keys1, s_keys2; 358 | extractMatchingElements( 359 | stereo_matches, keys1, keys2, s_keys1, s_keys2 360 | ); 361 | std::vector s_descriptors1, s_descriptors2; 362 | extractMatchingElements( 363 | stereo_matches, descriptors1, descriptors2, s_descriptors1, s_descriptors2 364 | ); 365 | 366 | // add image to the collection and check if there is some loop 367 | DetectionResult result; 368 | 369 | profiler.profile("detection"); 370 | detector.detectLoop( 371 | s_keys1, s_descriptors1, result, s_keys2, s_descriptors2); 372 | profiler.stop(); 373 | 374 | if(result.detection()) 375 | { 376 | cout << "- Loop found with image " << result.match << "!" << endl; 377 | cout << "- \t with translation: " << result.transform << endl; 378 | ++count; 379 | } 380 | else 381 | { 382 | cout << "- No loop: "; 383 | switch(result.status) 384 | { 385 | case CLOSE_MATCHES_ONLY: 386 | cout << "All the images in the database are very recent" << endl; 387 | break; 388 | 389 | case NO_DB_RESULTS: 390 | cout << "There are no matches against the database (few features in" 391 | " the image?)" << endl; 392 | break; 393 | 394 | case LOW_NSS_FACTOR: 395 | cout << "Little overlap between this image and the previous one" 396 | << endl; 397 | break; 398 | 399 | case LOW_SCORES: 400 | cout << "No match reaches the score threshold (alpha: " << 401 | params.alpha << ")" << endl; 402 | break; 403 | 404 | case NO_GROUPS: 405 | cout << "Not enough close matches to create groups. " 406 | << "Best candidate: " << result.match << endl; 407 | break; 408 | 409 | case NO_TEMPORAL_CONSISTENCY: 410 | cout << "No temporal consistency (k: " << params.k << "). " 411 | << "Best candidate: " << result.match << endl; 412 | break; 413 | 414 | case NO_GEOMETRICAL_CONSISTENCY: 415 | cout << "No geometrical consistency. Best candidate: " 416 | << result.match << endl; 417 | break; 418 | 419 | default: 420 | break; 421 | } 422 | } 423 | 424 | cout << endl; 425 | 426 | // show trajectory 427 | if(m_show && i > 0) 428 | { 429 | if(result.detection()) 430 | implot.line(-xs[i-1], ys[i-1], -xs[i], ys[i], loop_style); 431 | else 432 | implot.line(-xs[i-1], ys[i-1], -xs[i], ys[i], normal_style); 433 | 434 | DUtilsCV::GUI::showImage(implot.getImage(), true, &winplot, 10); 435 | } 436 | } 437 | 438 | fstore.release(); 439 | 440 | if(count == 0) 441 | { 442 | cout << "No loops found in this image sequence" << endl; 443 | } 444 | else 445 | { 446 | cout << count << " loops found in this image sequence!" << endl; 447 | } 448 | 449 | cout << endl << "Execution time:" << endl 450 | << " - Feature computation: " << profiler.getMeanTime("features") * 1e3 451 | << " ms/image" << endl 452 | << " - Loop detection: " << profiler.getMeanTime("detection") * 1e3 453 | << " ms/image" << endl; 454 | 455 | if(m_show) { 456 | cout << endl << "Press a key to finish..." << endl; 457 | DUtilsCV::GUI::showImage(implot.getImage(), true, &winplot, 0); 458 | } 459 | } 460 | 461 | // --------------------------------------------------------------------------- 462 | 463 | template 464 | void demoDetectorStereo::readPoseFile 465 | (const char *filename, std::vector &xs, std::vector &ys) 466 | const 467 | { 468 | xs.clear(); 469 | ys.clear(); 470 | 471 | fstream f(filename, ios::in); 472 | 473 | string s; 474 | double ts, x, y, t; 475 | while(!f.eof()) 476 | { 477 | getline(f, s); 478 | if(!f.eof() && !s.empty()) 479 | { 480 | sscanf(s.c_str(), "%lf, %lf, %lf, %lf", &ts, &x, &y, &t); 481 | xs.push_back(x); 482 | ys.push_back(y); 483 | } 484 | } 485 | 486 | f.close(); 487 | } 488 | 489 | // --------------------------------------------------------------------------- 490 | 491 | template 492 | void demoDetectorStereo::readPoseFileCSV 493 | (const char *filename, std::vector &xs, std::vector &ys) 494 | const 495 | { 496 | // ASSUMES CSV HEADERS: 'nanoseconds,x,y,ow,ox,oy,oz' 497 | xs.clear(); 498 | ys.clear(); 499 | 500 | fstream f(filename, ios::in); 501 | 502 | string s; 503 | // double ts, x, y, t; 504 | double x, y; 505 | while(!f.eof()) 506 | { 507 | getline(f, s); 508 | if(!f.eof() && !s.empty()) 509 | { 510 | sscanf(s.c_str(), "%*d,%lf,%lf,%*s", &x, &y); 511 | xs.push_back(x); 512 | ys.push_back(y); 513 | } 514 | } 515 | 516 | f.close(); 517 | } 518 | 519 | // --------------------------------------------------------------------------- 520 | 521 | template 522 | std::vector 523 | demoDetectorStereo::read_file_list 524 | (std::string path) 525 | const 526 | { 527 | std::ifstream file; 528 | std::string line; 529 | std::vector list; 530 | 531 | file.open(path); 532 | assert(file.is_open()); 533 | while (getline(file, line)) { 534 | list.push_back(line); 535 | } 536 | return list; 537 | } 538 | 539 | // --------------------------------------------------------------------------- 540 | 541 | #endif 542 | -------------------------------------------------------------------------------- /demo/ORBextractor.cc: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of ORB-SLAM3 3 | * 4 | * Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. 5 | * Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. 6 | * 7 | * ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public 8 | * License as published by the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even 12 | * the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License along with ORB-SLAM3. 16 | * If not, see . 17 | */ 18 | 19 | /** 20 | * Software License Agreement (BSD License) 21 | * 22 | * Copyright (c) 2009, Willow Garage, Inc. 23 | * All rights reserved. 24 | * 25 | * Redistribution and use in source and binary forms, with or without 26 | * modification, are permitted provided that the following conditions 27 | * are met: 28 | * 29 | * * Redistributions of source code must retain the above copyright 30 | * notice, this list of conditions and the following disclaimer. 31 | * * Redistributions in binary form must reproduce the above 32 | * copyright notice, this list of conditions and the following 33 | * disclaimer in the documentation and/or other materials provided 34 | * with the distribution. 35 | * * Neither the name of the Willow Garage nor the names of its 36 | * contributors may be used to endorse or promote products derived 37 | * from this software without specific prior written permission. 38 | * 39 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 40 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 41 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 42 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 43 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 44 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 45 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 46 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 47 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 48 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 49 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 50 | * POSSIBILITY OF SUCH DAMAGE. 51 | * 52 | */ 53 | 54 | 55 | #include 56 | #include 57 | #include 58 | #include 59 | #include 60 | #include 61 | 62 | #include "ORBextractor.h" 63 | 64 | 65 | using namespace cv; 66 | using namespace std; 67 | 68 | namespace ORB_SLAM3 69 | { 70 | 71 | const int PATCH_SIZE = 31; 72 | const int HALF_PATCH_SIZE = 15; 73 | const int EDGE_THRESHOLD = 19; 74 | 75 | 76 | static float IC_Angle(const Mat& image, Point2f pt, const vector & u_max) 77 | { 78 | int m_01 = 0, m_10 = 0; 79 | 80 | const uchar* center = &image.at (cvRound(pt.y), cvRound(pt.x)); 81 | 82 | // Treat the center line differently, v=0 83 | for (int u = -HALF_PATCH_SIZE; u <= HALF_PATCH_SIZE; ++u) 84 | m_10 += u * center[u]; 85 | 86 | // Go line by line in the circuI853lar patch 87 | int step = (int)image.step1(); 88 | for (int v = 1; v <= HALF_PATCH_SIZE; ++v) 89 | { 90 | // Proceed over the two lines 91 | int v_sum = 0; 92 | int d = u_max[v]; 93 | for (int u = -d; u <= d; ++u) 94 | { 95 | int val_plus = center[u + v*step], val_minus = center[u - v*step]; 96 | v_sum += (val_plus - val_minus); 97 | m_10 += u * (val_plus + val_minus); 98 | } 99 | m_01 += v * v_sum; 100 | } 101 | 102 | return fastAtan2((float)m_01, (float)m_10); 103 | } 104 | 105 | 106 | const float factorPI = (float)(CV_PI/180.f); 107 | static void computeOrbDescriptor(const KeyPoint& kpt, 108 | const Mat& img, const Point* pattern, 109 | uchar* desc) 110 | { 111 | float angle = (float)kpt.angle*factorPI; 112 | float a = (float)cos(angle), b = (float)sin(angle); 113 | 114 | const uchar* center = &img.at(cvRound(kpt.pt.y), cvRound(kpt.pt.x)); 115 | const int step = (int)img.step; 116 | 117 | #define GET_VALUE(idx) \ 118 | center[cvRound(pattern[idx].x*b + pattern[idx].y*a)*step + \ 119 | cvRound(pattern[idx].x*a - pattern[idx].y*b)] 120 | 121 | 122 | for (int i = 0; i < 32; ++i, pattern += 16) 123 | { 124 | int t0, t1, val; 125 | t0 = GET_VALUE(0); t1 = GET_VALUE(1); 126 | val = t0 < t1; 127 | t0 = GET_VALUE(2); t1 = GET_VALUE(3); 128 | val |= (t0 < t1) << 1; 129 | t0 = GET_VALUE(4); t1 = GET_VALUE(5); 130 | val |= (t0 < t1) << 2; 131 | t0 = GET_VALUE(6); t1 = GET_VALUE(7); 132 | val |= (t0 < t1) << 3; 133 | t0 = GET_VALUE(8); t1 = GET_VALUE(9); 134 | val |= (t0 < t1) << 4; 135 | t0 = GET_VALUE(10); t1 = GET_VALUE(11); 136 | val |= (t0 < t1) << 5; 137 | t0 = GET_VALUE(12); t1 = GET_VALUE(13); 138 | val |= (t0 < t1) << 6; 139 | t0 = GET_VALUE(14); t1 = GET_VALUE(15); 140 | val |= (t0 < t1) << 7; 141 | 142 | desc[i] = (uchar)val; 143 | } 144 | 145 | #undef GET_VALUE 146 | } 147 | 148 | 149 | static int bit_pattern_31_[256*4] = 150 | { 151 | 8,-3, 9,5/*mean (0), correlation (0)*/, 152 | 4,2, 7,-12/*mean (1.12461e-05), correlation (0.0437584)*/, 153 | -11,9, -8,2/*mean (3.37382e-05), correlation (0.0617409)*/, 154 | 7,-12, 12,-13/*mean (5.62303e-05), correlation (0.0636977)*/, 155 | 2,-13, 2,12/*mean (0.000134953), correlation (0.085099)*/, 156 | 1,-7, 1,6/*mean (0.000528565), correlation (0.0857175)*/, 157 | -2,-10, -2,-4/*mean (0.0188821), correlation (0.0985774)*/, 158 | -13,-13, -11,-8/*mean (0.0363135), correlation (0.0899616)*/, 159 | -13,-3, -12,-9/*mean (0.121806), correlation (0.099849)*/, 160 | 10,4, 11,9/*mean (0.122065), correlation (0.093285)*/, 161 | -13,-8, -8,-9/*mean (0.162787), correlation (0.0942748)*/, 162 | -11,7, -9,12/*mean (0.21561), correlation (0.0974438)*/, 163 | 7,7, 12,6/*mean (0.160583), correlation (0.130064)*/, 164 | -4,-5, -3,0/*mean (0.228171), correlation (0.132998)*/, 165 | -13,2, -12,-3/*mean (0.00997526), correlation (0.145926)*/, 166 | -9,0, -7,5/*mean (0.198234), correlation (0.143636)*/, 167 | 12,-6, 12,-1/*mean (0.0676226), correlation (0.16689)*/, 168 | -3,6, -2,12/*mean (0.166847), correlation (0.171682)*/, 169 | -6,-13, -4,-8/*mean (0.101215), correlation (0.179716)*/, 170 | 11,-13, 12,-8/*mean (0.200641), correlation (0.192279)*/, 171 | 4,7, 5,1/*mean (0.205106), correlation (0.186848)*/, 172 | 5,-3, 10,-3/*mean (0.234908), correlation (0.192319)*/, 173 | 3,-7, 6,12/*mean (0.0709964), correlation (0.210872)*/, 174 | -8,-7, -6,-2/*mean (0.0939834), correlation (0.212589)*/, 175 | -2,11, -1,-10/*mean (0.127778), correlation (0.20866)*/, 176 | -13,12, -8,10/*mean (0.14783), correlation (0.206356)*/, 177 | -7,3, -5,-3/*mean (0.182141), correlation (0.198942)*/, 178 | -4,2, -3,7/*mean (0.188237), correlation (0.21384)*/, 179 | -10,-12, -6,11/*mean (0.14865), correlation (0.23571)*/, 180 | 5,-12, 6,-7/*mean (0.222312), correlation (0.23324)*/, 181 | 5,-6, 7,-1/*mean (0.229082), correlation (0.23389)*/, 182 | 1,0, 4,-5/*mean (0.241577), correlation (0.215286)*/, 183 | 9,11, 11,-13/*mean (0.00338507), correlation (0.251373)*/, 184 | 4,7, 4,12/*mean (0.131005), correlation (0.257622)*/, 185 | 2,-1, 4,4/*mean (0.152755), correlation (0.255205)*/, 186 | -4,-12, -2,7/*mean (0.182771), correlation (0.244867)*/, 187 | -8,-5, -7,-10/*mean (0.186898), correlation (0.23901)*/, 188 | 4,11, 9,12/*mean (0.226226), correlation (0.258255)*/, 189 | 0,-8, 1,-13/*mean (0.0897886), correlation (0.274827)*/, 190 | -13,-2, -8,2/*mean (0.148774), correlation (0.28065)*/, 191 | -3,-2, -2,3/*mean (0.153048), correlation (0.283063)*/, 192 | -6,9, -4,-9/*mean (0.169523), correlation (0.278248)*/, 193 | 8,12, 10,7/*mean (0.225337), correlation (0.282851)*/, 194 | 0,9, 1,3/*mean (0.226687), correlation (0.278734)*/, 195 | 7,-5, 11,-10/*mean (0.00693882), correlation (0.305161)*/, 196 | -13,-6, -11,0/*mean (0.0227283), correlation (0.300181)*/, 197 | 10,7, 12,1/*mean (0.125517), correlation (0.31089)*/, 198 | -6,-3, -6,12/*mean (0.131748), correlation (0.312779)*/, 199 | 10,-9, 12,-4/*mean (0.144827), correlation (0.292797)*/, 200 | -13,8, -8,-12/*mean (0.149202), correlation (0.308918)*/, 201 | -13,0, -8,-4/*mean (0.160909), correlation (0.310013)*/, 202 | 3,3, 7,8/*mean (0.177755), correlation (0.309394)*/, 203 | 5,7, 10,-7/*mean (0.212337), correlation (0.310315)*/, 204 | -1,7, 1,-12/*mean (0.214429), correlation (0.311933)*/, 205 | 3,-10, 5,6/*mean (0.235807), correlation (0.313104)*/, 206 | 2,-4, 3,-10/*mean (0.00494827), correlation (0.344948)*/, 207 | -13,0, -13,5/*mean (0.0549145), correlation (0.344675)*/, 208 | -13,-7, -12,12/*mean (0.103385), correlation (0.342715)*/, 209 | -13,3, -11,8/*mean (0.134222), correlation (0.322922)*/, 210 | -7,12, -4,7/*mean (0.153284), correlation (0.337061)*/, 211 | 6,-10, 12,8/*mean (0.154881), correlation (0.329257)*/, 212 | -9,-1, -7,-6/*mean (0.200967), correlation (0.33312)*/, 213 | -2,-5, 0,12/*mean (0.201518), correlation (0.340635)*/, 214 | -12,5, -7,5/*mean (0.207805), correlation (0.335631)*/, 215 | 3,-10, 8,-13/*mean (0.224438), correlation (0.34504)*/, 216 | -7,-7, -4,5/*mean (0.239361), correlation (0.338053)*/, 217 | -3,-2, -1,-7/*mean (0.240744), correlation (0.344322)*/, 218 | 2,9, 5,-11/*mean (0.242949), correlation (0.34145)*/, 219 | -11,-13, -5,-13/*mean (0.244028), correlation (0.336861)*/, 220 | -1,6, 0,-1/*mean (0.247571), correlation (0.343684)*/, 221 | 5,-3, 5,2/*mean (0.000697256), correlation (0.357265)*/, 222 | -4,-13, -4,12/*mean (0.00213675), correlation (0.373827)*/, 223 | -9,-6, -9,6/*mean (0.0126856), correlation (0.373938)*/, 224 | -12,-10, -8,-4/*mean (0.0152497), correlation (0.364237)*/, 225 | 10,2, 12,-3/*mean (0.0299933), correlation (0.345292)*/, 226 | 7,12, 12,12/*mean (0.0307242), correlation (0.366299)*/, 227 | -7,-13, -6,5/*mean (0.0534975), correlation (0.368357)*/, 228 | -4,9, -3,4/*mean (0.099865), correlation (0.372276)*/, 229 | 7,-1, 12,2/*mean (0.117083), correlation (0.364529)*/, 230 | -7,6, -5,1/*mean (0.126125), correlation (0.369606)*/, 231 | -13,11, -12,5/*mean (0.130364), correlation (0.358502)*/, 232 | -3,7, -2,-6/*mean (0.131691), correlation (0.375531)*/, 233 | 7,-8, 12,-7/*mean (0.160166), correlation (0.379508)*/, 234 | -13,-7, -11,-12/*mean (0.167848), correlation (0.353343)*/, 235 | 1,-3, 12,12/*mean (0.183378), correlation (0.371916)*/, 236 | 2,-6, 3,0/*mean (0.228711), correlation (0.371761)*/, 237 | -4,3, -2,-13/*mean (0.247211), correlation (0.364063)*/, 238 | -1,-13, 1,9/*mean (0.249325), correlation (0.378139)*/, 239 | 7,1, 8,-6/*mean (0.000652272), correlation (0.411682)*/, 240 | 1,-1, 3,12/*mean (0.00248538), correlation (0.392988)*/, 241 | 9,1, 12,6/*mean (0.0206815), correlation (0.386106)*/, 242 | -1,-9, -1,3/*mean (0.0364485), correlation (0.410752)*/, 243 | -13,-13, -10,5/*mean (0.0376068), correlation (0.398374)*/, 244 | 7,7, 10,12/*mean (0.0424202), correlation (0.405663)*/, 245 | 12,-5, 12,9/*mean (0.0942645), correlation (0.410422)*/, 246 | 6,3, 7,11/*mean (0.1074), correlation (0.413224)*/, 247 | 5,-13, 6,10/*mean (0.109256), correlation (0.408646)*/, 248 | 2,-12, 2,3/*mean (0.131691), correlation (0.416076)*/, 249 | 3,8, 4,-6/*mean (0.165081), correlation (0.417569)*/, 250 | 2,6, 12,-13/*mean (0.171874), correlation (0.408471)*/, 251 | 9,-12, 10,3/*mean (0.175146), correlation (0.41296)*/, 252 | -8,4, -7,9/*mean (0.183682), correlation (0.402956)*/, 253 | -11,12, -4,-6/*mean (0.184672), correlation (0.416125)*/, 254 | 1,12, 2,-8/*mean (0.191487), correlation (0.386696)*/, 255 | 6,-9, 7,-4/*mean (0.192668), correlation (0.394771)*/, 256 | 2,3, 3,-2/*mean (0.200157), correlation (0.408303)*/, 257 | 6,3, 11,0/*mean (0.204588), correlation (0.411762)*/, 258 | 3,-3, 8,-8/*mean (0.205904), correlation (0.416294)*/, 259 | 7,8, 9,3/*mean (0.213237), correlation (0.409306)*/, 260 | -11,-5, -6,-4/*mean (0.243444), correlation (0.395069)*/, 261 | -10,11, -5,10/*mean (0.247672), correlation (0.413392)*/, 262 | -5,-8, -3,12/*mean (0.24774), correlation (0.411416)*/, 263 | -10,5, -9,0/*mean (0.00213675), correlation (0.454003)*/, 264 | 8,-1, 12,-6/*mean (0.0293635), correlation (0.455368)*/, 265 | 4,-6, 6,-11/*mean (0.0404971), correlation (0.457393)*/, 266 | -10,12, -8,7/*mean (0.0481107), correlation (0.448364)*/, 267 | 4,-2, 6,7/*mean (0.050641), correlation (0.455019)*/, 268 | -2,0, -2,12/*mean (0.0525978), correlation (0.44338)*/, 269 | -5,-8, -5,2/*mean (0.0629667), correlation (0.457096)*/, 270 | 7,-6, 10,12/*mean (0.0653846), correlation (0.445623)*/, 271 | -9,-13, -8,-8/*mean (0.0858749), correlation (0.449789)*/, 272 | -5,-13, -5,-2/*mean (0.122402), correlation (0.450201)*/, 273 | 8,-8, 9,-13/*mean (0.125416), correlation (0.453224)*/, 274 | -9,-11, -9,0/*mean (0.130128), correlation (0.458724)*/, 275 | 1,-8, 1,-2/*mean (0.132467), correlation (0.440133)*/, 276 | 7,-4, 9,1/*mean (0.132692), correlation (0.454)*/, 277 | -2,1, -1,-4/*mean (0.135695), correlation (0.455739)*/, 278 | 11,-6, 12,-11/*mean (0.142904), correlation (0.446114)*/, 279 | -12,-9, -6,4/*mean (0.146165), correlation (0.451473)*/, 280 | 3,7, 7,12/*mean (0.147627), correlation (0.456643)*/, 281 | 5,5, 10,8/*mean (0.152901), correlation (0.455036)*/, 282 | 0,-4, 2,8/*mean (0.167083), correlation (0.459315)*/, 283 | -9,12, -5,-13/*mean (0.173234), correlation (0.454706)*/, 284 | 0,7, 2,12/*mean (0.18312), correlation (0.433855)*/, 285 | -1,2, 1,7/*mean (0.185504), correlation (0.443838)*/, 286 | 5,11, 7,-9/*mean (0.185706), correlation (0.451123)*/, 287 | 3,5, 6,-8/*mean (0.188968), correlation (0.455808)*/, 288 | -13,-4, -8,9/*mean (0.191667), correlation (0.459128)*/, 289 | -5,9, -3,-3/*mean (0.193196), correlation (0.458364)*/, 290 | -4,-7, -3,-12/*mean (0.196536), correlation (0.455782)*/, 291 | 6,5, 8,0/*mean (0.1972), correlation (0.450481)*/, 292 | -7,6, -6,12/*mean (0.199438), correlation (0.458156)*/, 293 | -13,6, -5,-2/*mean (0.211224), correlation (0.449548)*/, 294 | 1,-10, 3,10/*mean (0.211718), correlation (0.440606)*/, 295 | 4,1, 8,-4/*mean (0.213034), correlation (0.443177)*/, 296 | -2,-2, 2,-13/*mean (0.234334), correlation (0.455304)*/, 297 | 2,-12, 12,12/*mean (0.235684), correlation (0.443436)*/, 298 | -2,-13, 0,-6/*mean (0.237674), correlation (0.452525)*/, 299 | 4,1, 9,3/*mean (0.23962), correlation (0.444824)*/, 300 | -6,-10, -3,-5/*mean (0.248459), correlation (0.439621)*/, 301 | -3,-13, -1,1/*mean (0.249505), correlation (0.456666)*/, 302 | 7,5, 12,-11/*mean (0.00119208), correlation (0.495466)*/, 303 | 4,-2, 5,-7/*mean (0.00372245), correlation (0.484214)*/, 304 | -13,9, -9,-5/*mean (0.00741116), correlation (0.499854)*/, 305 | 7,1, 8,6/*mean (0.0208952), correlation (0.499773)*/, 306 | 7,-8, 7,6/*mean (0.0220085), correlation (0.501609)*/, 307 | -7,-4, -7,1/*mean (0.0233806), correlation (0.496568)*/, 308 | -8,11, -7,-8/*mean (0.0236505), correlation (0.489719)*/, 309 | -13,6, -12,-8/*mean (0.0268781), correlation (0.503487)*/, 310 | 2,4, 3,9/*mean (0.0323324), correlation (0.501938)*/, 311 | 10,-5, 12,3/*mean (0.0399235), correlation (0.494029)*/, 312 | -6,-5, -6,7/*mean (0.0420153), correlation (0.486579)*/, 313 | 8,-3, 9,-8/*mean (0.0548021), correlation (0.484237)*/, 314 | 2,-12, 2,8/*mean (0.0616622), correlation (0.496642)*/, 315 | -11,-2, -10,3/*mean (0.0627755), correlation (0.498563)*/, 316 | -12,-13, -7,-9/*mean (0.0829622), correlation (0.495491)*/, 317 | -11,0, -10,-5/*mean (0.0843342), correlation (0.487146)*/, 318 | 5,-3, 11,8/*mean (0.0929937), correlation (0.502315)*/, 319 | -2,-13, -1,12/*mean (0.113327), correlation (0.48941)*/, 320 | -1,-8, 0,9/*mean (0.132119), correlation (0.467268)*/, 321 | -13,-11, -12,-5/*mean (0.136269), correlation (0.498771)*/, 322 | -10,-2, -10,11/*mean (0.142173), correlation (0.498714)*/, 323 | -3,9, -2,-13/*mean (0.144141), correlation (0.491973)*/, 324 | 2,-3, 3,2/*mean (0.14892), correlation (0.500782)*/, 325 | -9,-13, -4,0/*mean (0.150371), correlation (0.498211)*/, 326 | -4,6, -3,-10/*mean (0.152159), correlation (0.495547)*/, 327 | -4,12, -2,-7/*mean (0.156152), correlation (0.496925)*/, 328 | -6,-11, -4,9/*mean (0.15749), correlation (0.499222)*/, 329 | 6,-3, 6,11/*mean (0.159211), correlation (0.503821)*/, 330 | -13,11, -5,5/*mean (0.162427), correlation (0.501907)*/, 331 | 11,11, 12,6/*mean (0.16652), correlation (0.497632)*/, 332 | 7,-5, 12,-2/*mean (0.169141), correlation (0.484474)*/, 333 | -1,12, 0,7/*mean (0.169456), correlation (0.495339)*/, 334 | -4,-8, -3,-2/*mean (0.171457), correlation (0.487251)*/, 335 | -7,1, -6,7/*mean (0.175), correlation (0.500024)*/, 336 | -13,-12, -8,-13/*mean (0.175866), correlation (0.497523)*/, 337 | -7,-2, -6,-8/*mean (0.178273), correlation (0.501854)*/, 338 | -8,5, -6,-9/*mean (0.181107), correlation (0.494888)*/, 339 | -5,-1, -4,5/*mean (0.190227), correlation (0.482557)*/, 340 | -13,7, -8,10/*mean (0.196739), correlation (0.496503)*/, 341 | 1,5, 5,-13/*mean (0.19973), correlation (0.499759)*/, 342 | 1,0, 10,-13/*mean (0.204465), correlation (0.49873)*/, 343 | 9,12, 10,-1/*mean (0.209334), correlation (0.49063)*/, 344 | 5,-8, 10,-9/*mean (0.211134), correlation (0.503011)*/, 345 | -1,11, 1,-13/*mean (0.212), correlation (0.499414)*/, 346 | -9,-3, -6,2/*mean (0.212168), correlation (0.480739)*/, 347 | -1,-10, 1,12/*mean (0.212731), correlation (0.502523)*/, 348 | -13,1, -8,-10/*mean (0.21327), correlation (0.489786)*/, 349 | 8,-11, 10,-6/*mean (0.214159), correlation (0.488246)*/, 350 | 2,-13, 3,-6/*mean (0.216993), correlation (0.50287)*/, 351 | 7,-13, 12,-9/*mean (0.223639), correlation (0.470502)*/, 352 | -10,-10, -5,-7/*mean (0.224089), correlation (0.500852)*/, 353 | -10,-8, -8,-13/*mean (0.228666), correlation (0.502629)*/, 354 | 4,-6, 8,5/*mean (0.22906), correlation (0.498305)*/, 355 | 3,12, 8,-13/*mean (0.233378), correlation (0.503825)*/, 356 | -4,2, -3,-3/*mean (0.234323), correlation (0.476692)*/, 357 | 5,-13, 10,-12/*mean (0.236392), correlation (0.475462)*/, 358 | 4,-13, 5,-1/*mean (0.236842), correlation (0.504132)*/, 359 | -9,9, -4,3/*mean (0.236977), correlation (0.497739)*/, 360 | 0,3, 3,-9/*mean (0.24314), correlation (0.499398)*/, 361 | -12,1, -6,1/*mean (0.243297), correlation (0.489447)*/, 362 | 3,2, 4,-8/*mean (0.00155196), correlation (0.553496)*/, 363 | -10,-10, -10,9/*mean (0.00239541), correlation (0.54297)*/, 364 | 8,-13, 12,12/*mean (0.0034413), correlation (0.544361)*/, 365 | -8,-12, -6,-5/*mean (0.003565), correlation (0.551225)*/, 366 | 2,2, 3,7/*mean (0.00835583), correlation (0.55285)*/, 367 | 10,6, 11,-8/*mean (0.00885065), correlation (0.540913)*/, 368 | 6,8, 8,-12/*mean (0.0101552), correlation (0.551085)*/, 369 | -7,10, -6,5/*mean (0.0102227), correlation (0.533635)*/, 370 | -3,-9, -3,9/*mean (0.0110211), correlation (0.543121)*/, 371 | -1,-13, -1,5/*mean (0.0113473), correlation (0.550173)*/, 372 | -3,-7, -3,4/*mean (0.0140913), correlation (0.554774)*/, 373 | -8,-2, -8,3/*mean (0.017049), correlation (0.55461)*/, 374 | 4,2, 12,12/*mean (0.01778), correlation (0.546921)*/, 375 | 2,-5, 3,11/*mean (0.0224022), correlation (0.549667)*/, 376 | 6,-9, 11,-13/*mean (0.029161), correlation (0.546295)*/, 377 | 3,-1, 7,12/*mean (0.0303081), correlation (0.548599)*/, 378 | 11,-1, 12,4/*mean (0.0355151), correlation (0.523943)*/, 379 | -3,0, -3,6/*mean (0.0417904), correlation (0.543395)*/, 380 | 4,-11, 4,12/*mean (0.0487292), correlation (0.542818)*/, 381 | 2,-4, 2,1/*mean (0.0575124), correlation (0.554888)*/, 382 | -10,-6, -8,1/*mean (0.0594242), correlation (0.544026)*/, 383 | -13,7, -11,1/*mean (0.0597391), correlation (0.550524)*/, 384 | -13,12, -11,-13/*mean (0.0608974), correlation (0.55383)*/, 385 | 6,0, 11,-13/*mean (0.065126), correlation (0.552006)*/, 386 | 0,-1, 1,4/*mean (0.074224), correlation (0.546372)*/, 387 | -13,3, -9,-2/*mean (0.0808592), correlation (0.554875)*/, 388 | -9,8, -6,-3/*mean (0.0883378), correlation (0.551178)*/, 389 | -13,-6, -8,-2/*mean (0.0901035), correlation (0.548446)*/, 390 | 5,-9, 8,10/*mean (0.0949843), correlation (0.554694)*/, 391 | 2,7, 3,-9/*mean (0.0994152), correlation (0.550979)*/, 392 | -1,-6, -1,-1/*mean (0.10045), correlation (0.552714)*/, 393 | 9,5, 11,-2/*mean (0.100686), correlation (0.552594)*/, 394 | 11,-3, 12,-8/*mean (0.101091), correlation (0.532394)*/, 395 | 3,0, 3,5/*mean (0.101147), correlation (0.525576)*/, 396 | -1,4, 0,10/*mean (0.105263), correlation (0.531498)*/, 397 | 3,-6, 4,5/*mean (0.110785), correlation (0.540491)*/, 398 | -13,0, -10,5/*mean (0.112798), correlation (0.536582)*/, 399 | 5,8, 12,11/*mean (0.114181), correlation (0.555793)*/, 400 | 8,9, 9,-6/*mean (0.117431), correlation (0.553763)*/, 401 | 7,-4, 8,-12/*mean (0.118522), correlation (0.553452)*/, 402 | -10,4, -10,9/*mean (0.12094), correlation (0.554785)*/, 403 | 7,3, 12,4/*mean (0.122582), correlation (0.555825)*/, 404 | 9,-7, 10,-2/*mean (0.124978), correlation (0.549846)*/, 405 | 7,0, 12,-2/*mean (0.127002), correlation (0.537452)*/, 406 | -1,-6, 0,-11/*mean (0.127148), correlation (0.547401)*/ 407 | }; 408 | 409 | ORBextractor::ORBextractor(int _nfeatures, float _scaleFactor, int _nlevels, 410 | int _iniThFAST, int _minThFAST): 411 | nfeatures(_nfeatures), scaleFactor(_scaleFactor), nlevels(_nlevels), 412 | iniThFAST(_iniThFAST), minThFAST(_minThFAST) 413 | { 414 | mvScaleFactor.resize(nlevels); 415 | mvLevelSigma2.resize(nlevels); 416 | mvScaleFactor[0]=1.0f; 417 | mvLevelSigma2[0]=1.0f; 418 | for(int i=1; i= vmin; --v) 463 | { 464 | while (umax[v0] == umax[v0 + 1]) 465 | ++v0; 466 | umax[v] = v0; 467 | ++v0; 468 | } 469 | } 470 | 471 | static void computeOrientation(const Mat& image, vector& keypoints, const vector& umax) 472 | { 473 | for (vector::iterator keypoint = keypoints.begin(), 474 | keypointEnd = keypoints.end(); keypoint != keypointEnd; ++keypoint) 475 | { 476 | keypoint->angle = IC_Angle(image, keypoint->pt, umax); 477 | } 478 | } 479 | 480 | void ExtractorNode::DivideNode(ExtractorNode &n1, ExtractorNode &n2, ExtractorNode &n3, ExtractorNode &n4) 481 | { 482 | const int halfX = ceil(static_cast(UR.x-UL.x)/2); 483 | const int halfY = ceil(static_cast(BR.y-UL.y)/2); 484 | 485 | //Define boundaries of childs 486 | n1.UL = UL; 487 | n1.UR = cv::Point2i(UL.x+halfX,UL.y); 488 | n1.BL = cv::Point2i(UL.x,UL.y+halfY); 489 | n1.BR = cv::Point2i(UL.x+halfX,UL.y+halfY); 490 | n1.vKeys.reserve(vKeys.size()); 491 | 492 | n2.UL = n1.UR; 493 | n2.UR = UR; 494 | n2.BL = n1.BR; 495 | n2.BR = cv::Point2i(UR.x,UL.y+halfY); 496 | n2.vKeys.reserve(vKeys.size()); 497 | 498 | n3.UL = n1.BL; 499 | n3.UR = n1.BR; 500 | n3.BL = BL; 501 | n3.BR = cv::Point2i(n1.BR.x,BL.y); 502 | n3.vKeys.reserve(vKeys.size()); 503 | 504 | n4.UL = n3.UR; 505 | n4.UR = n2.BR; 506 | n4.BL = n3.BR; 507 | n4.BR = BR; 508 | n4.vKeys.reserve(vKeys.size()); 509 | 510 | //Associate points to childs 511 | for(size_t i=0;i& e1, pair& e2){ 539 | if(e1.first < e2.first){ 540 | return true; 541 | } 542 | else if(e1.first > e2.first){ 543 | return false; 544 | } 545 | else{ 546 | if(e1.second->UL.x < e2.second->UL.x){ 547 | return true; 548 | } 549 | else{ 550 | return false; 551 | } 552 | } 553 | } 554 | 555 | vector ORBextractor::DistributeOctTree(const vector& vToDistributeKeys, const int &minX, 556 | const int &maxX, const int &minY, const int &maxY, const int &N, const int &level) 557 | { 558 | // Compute how many initial nodes 559 | const int nIni = round(static_cast(maxX-minX)/(maxY-minY)); 560 | 561 | const float hX = static_cast(maxX-minX)/nIni; 562 | 563 | list lNodes; 564 | 565 | vector vpIniNodes; 566 | vpIniNodes.resize(nIni); 567 | 568 | for(int i=0; i(i),0); 572 | ni.UR = cv::Point2i(hX*static_cast(i+1),0); 573 | ni.BL = cv::Point2i(ni.UL.x,maxY-minY); 574 | ni.BR = cv::Point2i(ni.UR.x,maxY-minY); 575 | ni.vKeys.reserve(vToDistributeKeys.size()); 576 | 577 | lNodes.push_back(ni); 578 | vpIniNodes[i] = &lNodes.back(); 579 | } 580 | 581 | //Associate points to childs 582 | for(size_t i=0;ivKeys.push_back(kp); 586 | } 587 | 588 | list::iterator lit = lNodes.begin(); 589 | 590 | while(lit!=lNodes.end()) 591 | { 592 | if(lit->vKeys.size()==1) 593 | { 594 | lit->bNoMore=true; 595 | lit++; 596 | } 597 | else if(lit->vKeys.empty()) 598 | lit = lNodes.erase(lit); 599 | else 600 | lit++; 601 | } 602 | 603 | bool bFinish = false; 604 | 605 | int iteration = 0; 606 | 607 | vector > vSizeAndPointerToNode; 608 | vSizeAndPointerToNode.reserve(lNodes.size()*4); 609 | 610 | while(!bFinish) 611 | { 612 | iteration++; 613 | 614 | int prevSize = lNodes.size(); 615 | 616 | lit = lNodes.begin(); 617 | 618 | int nToExpand = 0; 619 | 620 | vSizeAndPointerToNode.clear(); 621 | 622 | while(lit!=lNodes.end()) 623 | { 624 | if(lit->bNoMore) 625 | { 626 | // If node only contains one point do not subdivide and continue 627 | lit++; 628 | continue; 629 | } 630 | else 631 | { 632 | // If more than one point, subdivide 633 | ExtractorNode n1,n2,n3,n4; 634 | lit->DivideNode(n1,n2,n3,n4); 635 | 636 | // Add childs if they contain points 637 | if(n1.vKeys.size()>0) 638 | { 639 | lNodes.push_front(n1); 640 | if(n1.vKeys.size()>1) 641 | { 642 | nToExpand++; 643 | vSizeAndPointerToNode.push_back(make_pair(n1.vKeys.size(),&lNodes.front())); 644 | lNodes.front().lit = lNodes.begin(); 645 | } 646 | } 647 | if(n2.vKeys.size()>0) 648 | { 649 | lNodes.push_front(n2); 650 | if(n2.vKeys.size()>1) 651 | { 652 | nToExpand++; 653 | vSizeAndPointerToNode.push_back(make_pair(n2.vKeys.size(),&lNodes.front())); 654 | lNodes.front().lit = lNodes.begin(); 655 | } 656 | } 657 | if(n3.vKeys.size()>0) 658 | { 659 | lNodes.push_front(n3); 660 | if(n3.vKeys.size()>1) 661 | { 662 | nToExpand++; 663 | vSizeAndPointerToNode.push_back(make_pair(n3.vKeys.size(),&lNodes.front())); 664 | lNodes.front().lit = lNodes.begin(); 665 | } 666 | } 667 | if(n4.vKeys.size()>0) 668 | { 669 | lNodes.push_front(n4); 670 | if(n4.vKeys.size()>1) 671 | { 672 | nToExpand++; 673 | vSizeAndPointerToNode.push_back(make_pair(n4.vKeys.size(),&lNodes.front())); 674 | lNodes.front().lit = lNodes.begin(); 675 | } 676 | } 677 | 678 | lit=lNodes.erase(lit); 679 | continue; 680 | } 681 | } 682 | 683 | // Finish if there are more nodes than required features 684 | // or all nodes contain just one point 685 | if((int)lNodes.size()>=N || (int)lNodes.size()==prevSize) 686 | { 687 | bFinish = true; 688 | } 689 | else if(((int)lNodes.size()+nToExpand*3)>N) 690 | { 691 | 692 | while(!bFinish) 693 | { 694 | 695 | prevSize = lNodes.size(); 696 | 697 | vector > vPrevSizeAndPointerToNode = vSizeAndPointerToNode; 698 | vSizeAndPointerToNode.clear(); 699 | 700 | sort(vPrevSizeAndPointerToNode.begin(),vPrevSizeAndPointerToNode.end(),compareNodes); 701 | for(int j=vPrevSizeAndPointerToNode.size()-1;j>=0;j--) 702 | { 703 | ExtractorNode n1,n2,n3,n4; 704 | vPrevSizeAndPointerToNode[j].second->DivideNode(n1,n2,n3,n4); 705 | 706 | // Add childs if they contain points 707 | if(n1.vKeys.size()>0) 708 | { 709 | lNodes.push_front(n1); 710 | if(n1.vKeys.size()>1) 711 | { 712 | vSizeAndPointerToNode.push_back(make_pair(n1.vKeys.size(),&lNodes.front())); 713 | lNodes.front().lit = lNodes.begin(); 714 | } 715 | } 716 | if(n2.vKeys.size()>0) 717 | { 718 | lNodes.push_front(n2); 719 | if(n2.vKeys.size()>1) 720 | { 721 | vSizeAndPointerToNode.push_back(make_pair(n2.vKeys.size(),&lNodes.front())); 722 | lNodes.front().lit = lNodes.begin(); 723 | } 724 | } 725 | if(n3.vKeys.size()>0) 726 | { 727 | lNodes.push_front(n3); 728 | if(n3.vKeys.size()>1) 729 | { 730 | vSizeAndPointerToNode.push_back(make_pair(n3.vKeys.size(),&lNodes.front())); 731 | lNodes.front().lit = lNodes.begin(); 732 | } 733 | } 734 | if(n4.vKeys.size()>0) 735 | { 736 | lNodes.push_front(n4); 737 | if(n4.vKeys.size()>1) 738 | { 739 | vSizeAndPointerToNode.push_back(make_pair(n4.vKeys.size(),&lNodes.front())); 740 | lNodes.front().lit = lNodes.begin(); 741 | } 742 | } 743 | 744 | lNodes.erase(vPrevSizeAndPointerToNode[j].second->lit); 745 | 746 | if((int)lNodes.size()>=N) 747 | break; 748 | } 749 | 750 | if((int)lNodes.size()>=N || (int)lNodes.size()==prevSize) 751 | bFinish = true; 752 | 753 | } 754 | } 755 | } 756 | 757 | // Retain the best point in each node 758 | vector vResultKeys; 759 | vResultKeys.reserve(nfeatures); 760 | for(list::iterator lit=lNodes.begin(); lit!=lNodes.end(); lit++) 761 | { 762 | vector &vNodeKeys = lit->vKeys; 763 | cv::KeyPoint* pKP = &vNodeKeys[0]; 764 | float maxResponse = pKP->response; 765 | 766 | for(size_t k=1;kmaxResponse) 769 | { 770 | pKP = &vNodeKeys[k]; 771 | maxResponse = vNodeKeys[k].response; 772 | } 773 | } 774 | 775 | vResultKeys.push_back(*pKP); 776 | } 777 | 778 | return vResultKeys; 779 | } 780 | 781 | void ORBextractor::ComputeKeyPointsOctTree(vector >& allKeypoints) 782 | { 783 | allKeypoints.resize(nlevels); 784 | 785 | const float W = 35; 786 | 787 | for (int level = 0; level < nlevels; ++level) 788 | { 789 | const int minBorderX = EDGE_THRESHOLD-3; 790 | const int minBorderY = minBorderX; 791 | const int maxBorderX = mvImagePyramid[level].cols-EDGE_THRESHOLD+3; 792 | const int maxBorderY = mvImagePyramid[level].rows-EDGE_THRESHOLD+3; 793 | 794 | vector vToDistributeKeys; 795 | vToDistributeKeys.reserve(nfeatures*10); 796 | 797 | const float width = (maxBorderX-minBorderX); 798 | const float height = (maxBorderY-minBorderY); 799 | 800 | const int nCols = width/W; 801 | const int nRows = height/W; 802 | const int wCell = ceil(width/nCols); 803 | const int hCell = ceil(height/nRows); 804 | 805 | for(int i=0; i=maxBorderY-3) 811 | continue; 812 | if(maxY>maxBorderY) 813 | maxY = maxBorderY; 814 | 815 | for(int j=0; j=maxBorderX-6) 820 | continue; 821 | if(maxX>maxBorderX) 822 | maxX = maxBorderX; 823 | 824 | vector vKeysCell; 825 | 826 | FAST(mvImagePyramid[level].rowRange(iniY,maxY).colRange(iniX,maxX), 827 | vKeysCell,iniThFAST,true); 828 | 829 | /*if(bRight && j <= 13){ 830 | FAST(mvImagePyramid[level].rowRange(iniY,maxY).colRange(iniX,maxX), 831 | vKeysCell,10,true); 832 | } 833 | else if(!bRight && j >= 16){ 834 | FAST(mvImagePyramid[level].rowRange(iniY,maxY).colRange(iniX,maxX), 835 | vKeysCell,10,true); 836 | } 837 | else{ 838 | FAST(mvImagePyramid[level].rowRange(iniY,maxY).colRange(iniX,maxX), 839 | vKeysCell,iniThFAST,true); 840 | }*/ 841 | 842 | 843 | if(vKeysCell.empty()) 844 | { 845 | FAST(mvImagePyramid[level].rowRange(iniY,maxY).colRange(iniX,maxX), 846 | vKeysCell,minThFAST,true); 847 | /*if(bRight && j <= 13){ 848 | FAST(mvImagePyramid[level].rowRange(iniY,maxY).colRange(iniX,maxX), 849 | vKeysCell,5,true); 850 | } 851 | else if(!bRight && j >= 16){ 852 | FAST(mvImagePyramid[level].rowRange(iniY,maxY).colRange(iniX,maxX), 853 | vKeysCell,5,true); 854 | } 855 | else{ 856 | FAST(mvImagePyramid[level].rowRange(iniY,maxY).colRange(iniX,maxX), 857 | vKeysCell,minThFAST,true); 858 | }*/ 859 | } 860 | 861 | if(!vKeysCell.empty()) 862 | { 863 | for(vector::iterator vit=vKeysCell.begin(); vit!=vKeysCell.end();vit++) 864 | { 865 | (*vit).pt.x+=j*wCell; 866 | (*vit).pt.y+=i*hCell; 867 | vToDistributeKeys.push_back(*vit); 868 | } 869 | } 870 | 871 | } 872 | } 873 | 874 | vector & keypoints = allKeypoints[level]; 875 | keypoints.reserve(nfeatures); 876 | 877 | keypoints = DistributeOctTree(vToDistributeKeys, minBorderX, maxBorderX, 878 | minBorderY, maxBorderY,mnFeaturesPerLevel[level], level); 879 | 880 | const int scaledPatchSize = PATCH_SIZE*mvScaleFactor[level]; 881 | 882 | // Add border to coordinates and scale information 883 | const int nkps = keypoints.size(); 884 | for(int i=0; i > &allKeypoints) 899 | { 900 | allKeypoints.resize(nlevels); 901 | 902 | float imageRatio = (float)mvImagePyramid[0].cols/mvImagePyramid[0].rows; 903 | 904 | for (int level = 0; level < nlevels; ++level) 905 | { 906 | const int nDesiredFeatures = mnFeaturesPerLevel[level]; 907 | 908 | const int levelCols = sqrt((float)nDesiredFeatures/(5*imageRatio)); 909 | const int levelRows = imageRatio*levelCols; 910 | 911 | const int minBorderX = EDGE_THRESHOLD; 912 | const int minBorderY = minBorderX; 913 | const int maxBorderX = mvImagePyramid[level].cols-EDGE_THRESHOLD; 914 | const int maxBorderY = mvImagePyramid[level].rows-EDGE_THRESHOLD; 915 | 916 | const int W = maxBorderX - minBorderX; 917 | const int H = maxBorderY - minBorderY; 918 | const int cellW = ceil((float)W/levelCols); 919 | const int cellH = ceil((float)H/levelRows); 920 | 921 | const int nCells = levelRows*levelCols; 922 | const int nfeaturesCell = ceil((float)nDesiredFeatures/nCells); 923 | 924 | vector > > cellKeyPoints(levelRows, vector >(levelCols)); 925 | 926 | vector > nToRetain(levelRows,vector(levelCols,0)); 927 | vector > nTotal(levelRows,vector(levelCols,0)); 928 | vector > bNoMore(levelRows,vector(levelCols,false)); 929 | vector iniXCol(levelCols); 930 | vector iniYRow(levelRows); 931 | int nNoMore = 0; 932 | int nToDistribute = 0; 933 | 934 | 935 | float hY = cellH + 6; 936 | 937 | for(int i=0; infeaturesCell) 992 | { 993 | nToRetain[i][j] = nfeaturesCell; 994 | bNoMore[i][j] = false; 995 | } 996 | else 997 | { 998 | nToRetain[i][j] = nKeys; 999 | nToDistribute += nfeaturesCell-nKeys; 1000 | bNoMore[i][j] = true; 1001 | nNoMore++; 1002 | } 1003 | 1004 | } 1005 | } 1006 | 1007 | 1008 | // Retain by score 1009 | 1010 | while(nToDistribute>0 && nNoMorenNewFeaturesCell) 1022 | { 1023 | nToRetain[i][j] = nNewFeaturesCell; 1024 | bNoMore[i][j] = false; 1025 | } 1026 | else 1027 | { 1028 | nToRetain[i][j] = nTotal[i][j]; 1029 | nToDistribute += nNewFeaturesCell-nTotal[i][j]; 1030 | bNoMore[i][j] = true; 1031 | nNoMore++; 1032 | } 1033 | } 1034 | } 1035 | } 1036 | } 1037 | 1038 | vector & keypoints = allKeypoints[level]; 1039 | keypoints.reserve(nDesiredFeatures*2); 1040 | 1041 | const int scaledPatchSize = PATCH_SIZE*mvScaleFactor[level]; 1042 | 1043 | // Retain by score and transform coordinates 1044 | for(int i=0; i &keysCell = cellKeyPoints[i][j]; 1049 | KeyPointsFilter::retainBest(keysCell,nToRetain[i][j]); 1050 | if((int)keysCell.size()>nToRetain[i][j]) 1051 | keysCell.resize(nToRetain[i][j]); 1052 | 1053 | 1054 | for(size_t k=0, kend=keysCell.size(); knDesiredFeatures) 1066 | { 1067 | KeyPointsFilter::retainBest(keypoints,nDesiredFeatures); 1068 | keypoints.resize(nDesiredFeatures); 1069 | } 1070 | } 1071 | 1072 | // and compute orientations 1073 | for (int level = 0; level < nlevels; ++level) 1074 | computeOrientation(mvImagePyramid[level], allKeypoints[level], umax); 1075 | } 1076 | 1077 | static void computeDescriptors(const Mat& image, vector& keypoints, Mat& descriptors, 1078 | const vector& pattern) 1079 | { 1080 | descriptors = Mat::zeros((int)keypoints.size(), 32, CV_8UC1); 1081 | 1082 | for (size_t i = 0; i < keypoints.size(); i++) 1083 | computeOrbDescriptor(keypoints[i], image, &pattern[0], descriptors.ptr((int)i)); 1084 | } 1085 | 1086 | int ORBextractor::operator()( InputArray _image, InputArray _mask, vector& _keypoints, 1087 | OutputArray _descriptors, std::vector &vLappingArea) 1088 | { 1089 | //cout << "[ORBextractor]: Max Features: " << nfeatures << endl; 1090 | if(_image.empty()) 1091 | return -1; 1092 | 1093 | Mat image = _image.getMat(); 1094 | assert(image.type() == CV_8UC1 ); 1095 | 1096 | // Pre-compute the scale pyramid 1097 | ComputePyramid(image); 1098 | 1099 | vector < vector > allKeypoints; 1100 | ComputeKeyPointsOctTree(allKeypoints); 1101 | //ComputeKeyPointsOld(allKeypoints); 1102 | 1103 | Mat descriptors; 1104 | 1105 | int nkeypoints = 0; 1106 | for (int level = 0; level < nlevels; ++level) 1107 | nkeypoints += (int)allKeypoints[level].size(); 1108 | if( nkeypoints == 0 ) 1109 | _descriptors.release(); 1110 | else 1111 | { 1112 | _descriptors.create(nkeypoints, 32, CV_8U); 1113 | descriptors = _descriptors.getMat(); 1114 | } 1115 | 1116 | //_keypoints.clear(); 1117 | //_keypoints.reserve(nkeypoints); 1118 | _keypoints = vector(nkeypoints); 1119 | 1120 | int offset = 0; 1121 | //Modified for speeding up stereo fisheye matching 1122 | int monoIndex = 0, stereoIndex = nkeypoints-1; 1123 | for (int level = 0; level < nlevels; ++level) 1124 | { 1125 | vector& keypoints = allKeypoints[level]; 1126 | int nkeypointsLevel = (int)keypoints.size(); 1127 | 1128 | if(nkeypointsLevel==0) 1129 | continue; 1130 | 1131 | // preprocess the resized image 1132 | Mat workingMat = mvImagePyramid[level].clone(); 1133 | GaussianBlur(workingMat, workingMat, Size(7, 7), 2, 2, BORDER_REFLECT_101); 1134 | 1135 | // Compute the descriptors 1136 | //Mat desc = descriptors.rowRange(offset, offset + nkeypointsLevel); 1137 | Mat desc = cv::Mat(nkeypointsLevel, 32, CV_8U); 1138 | computeDescriptors(workingMat, keypoints, desc, pattern); 1139 | 1140 | offset += nkeypointsLevel; 1141 | 1142 | 1143 | float scale = mvScaleFactor[level]; //getScale(level, firstLevel, scaleFactor); 1144 | int i = 0; 1145 | for (vector::iterator keypoint = keypoints.begin(), 1146 | keypointEnd = keypoints.end(); keypoint != keypointEnd; ++keypoint){ 1147 | 1148 | // Scale keypoint coordinates 1149 | if (level != 0){ 1150 | keypoint->pt *= scale; 1151 | } 1152 | 1153 | if(keypoint->pt.x >= vLappingArea[0] && keypoint->pt.x <= vLappingArea[1]){ 1154 | _keypoints.at(stereoIndex) = (*keypoint); 1155 | desc.row(i).copyTo(descriptors.row(stereoIndex)); 1156 | stereoIndex--; 1157 | } 1158 | else{ 1159 | _keypoints.at(monoIndex) = (*keypoint); 1160 | desc.row(i).copyTo(descriptors.row(monoIndex)); 1161 | monoIndex++; 1162 | } 1163 | i++; 1164 | } 1165 | } 1166 | //cout << "[ORBextractor]: extracted " << _keypoints.size() << " KeyPoints" << endl; 1167 | return monoIndex; 1168 | } 1169 | 1170 | void ORBextractor::ComputePyramid(cv::Mat image) 1171 | { 1172 | for (int level = 0; level < nlevels; ++level) 1173 | { 1174 | float scale = mvInvScaleFactor[level]; 1175 | Size sz(cvRound((float)image.cols*scale), cvRound((float)image.rows*scale)); 1176 | Size wholeSize(sz.width + EDGE_THRESHOLD*2, sz.height + EDGE_THRESHOLD*2); 1177 | Mat temp(wholeSize, image.type()), masktemp; 1178 | mvImagePyramid[level] = temp(Rect(EDGE_THRESHOLD, EDGE_THRESHOLD, sz.width, sz.height)); 1179 | 1180 | // Compute the resized image 1181 | if( level != 0 ) 1182 | { 1183 | resize(mvImagePyramid[level-1], mvImagePyramid[level], sz, 0, 0, INTER_LINEAR); 1184 | 1185 | copyMakeBorder(mvImagePyramid[level], temp, EDGE_THRESHOLD, EDGE_THRESHOLD, EDGE_THRESHOLD, EDGE_THRESHOLD, 1186 | BORDER_REFLECT_101+BORDER_ISOLATED); 1187 | } 1188 | else 1189 | { 1190 | copyMakeBorder(image, temp, EDGE_THRESHOLD, EDGE_THRESHOLD, EDGE_THRESHOLD, EDGE_THRESHOLD, 1191 | BORDER_REFLECT_101); 1192 | } 1193 | } 1194 | 1195 | } 1196 | 1197 | } //namespace ORB_SLAM 1198 | -------------------------------------------------------------------------------- /include/StereoLoopDetector/TemplatedLoopDetector.h: -------------------------------------------------------------------------------- 1 | /** 2 | * File: TemplatedLoopDetector.h 3 | * Author: Nicolas Soncini (other attributions below) 4 | * Date: Aug 23, 2024 5 | * Description: Templated structures for generic loop detection 6 | * License: See LICENSE.txt file at the top project folder 7 | * 8 | * This file has been modified from the original version, attribution: 9 | * Original File: TemplatedLoopDetector.h 10 | * Original Author: Dorian Galvez-Lopez 11 | * Original Last Modified Date: May 7, 2020 12 | * 13 | **/ 14 | 15 | #ifndef __D_T_TEMPLATED_LOOP_DETECTOR__ 16 | #define __D_T_TEMPLATED_LOOP_DETECTOR__ 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | #include 24 | #include 25 | #include 26 | #include //FIXME 27 | 28 | #include "TemplatedVocabulary.h" 29 | #include "TemplatedDatabase.h" 30 | #include "QueryResults.h" 31 | #include "BowVector.h" 32 | #include "DUtils.h" 33 | #include "DUtilsCV.h" 34 | #include "DVision.h" 35 | 36 | #include "StereoParameters.h" 37 | 38 | using namespace std; 39 | using namespace DUtils; 40 | using namespace DBoW2; 41 | 42 | // -------------------------------------------------------------------------- 43 | 44 | namespace DLoopDetector { 45 | 46 | /// Geometrical checking methods 47 | enum GeometricalCheck 48 | { 49 | /// Exhaustive search 50 | GEOM_EXHAUSTIVE, 51 | /// Use direct index 52 | GEOM_DI, 53 | /// Use a Flann structure 54 | GEOM_FLANN, 55 | /// Use exhaustive stereo checking 56 | GEOM_EXHAUSTIVE_STEREO, 57 | /// Use direct index with stereo 58 | GEOM_SDI, 59 | /// Do not perform geometrical checking 60 | GEOM_NONE 61 | }; 62 | 63 | /// Reasons for dismissing loops 64 | enum DetectionStatus 65 | { 66 | /// Loop correctly detected 67 | LOOP_DETECTED, 68 | /// All the matches are very recent 69 | CLOSE_MATCHES_ONLY, 70 | /// No matches against the database 71 | NO_DB_RESULTS, 72 | /// Score of current image against previous one too low 73 | LOW_NSS_FACTOR, 74 | /// Scores (or NS Scores) were below the alpha threshold 75 | LOW_SCORES, 76 | /// Not enough matches to create groups 77 | NO_GROUPS, 78 | /// Not enough temporary consistent matches (k) 79 | NO_TEMPORAL_CONSISTENCY, 80 | /// The geometrical consistency failed 81 | NO_GEOMETRICAL_CONSISTENCY 82 | }; 83 | 84 | /// Result of a detection 85 | struct DetectionResult 86 | { 87 | /// Detection status. LOOP_DETECTED iff loop detected 88 | DetectionStatus status; 89 | /// Query id 90 | EntryId query; 91 | /// Matched id if loop detected, otherwise, best candidate 92 | EntryId match; 93 | 94 | /// Matched transformation if loop detected, otherwise undefined behavior 95 | cv::Vec3d transform = {0.0, 0.0, 0.0}; 96 | 97 | /** 98 | * Checks if the loop was detected 99 | * @return true iff a loop was detected 100 | */ 101 | inline bool detection() const 102 | { 103 | return status == LOOP_DETECTED; 104 | } 105 | }; 106 | 107 | /// TDescriptor: class of descriptor 108 | /// F: class of descriptor functions 109 | template 110 | /// Generic Loop detector 111 | class TemplatedLoopDetector 112 | { 113 | public: 114 | /// Parameters to create a loop detector 115 | struct Parameters 116 | { 117 | /// Height of expected images 118 | int image_rows; 119 | /// Width of expected images 120 | int image_cols; 121 | 122 | // Main loop detector parameters 123 | 124 | /// Use normalized similarity score? 125 | bool use_nss; 126 | /// Alpha threshold 127 | float alpha; 128 | /// Min consistent matches to pass the temporal check 129 | int k; 130 | /// Geometrical check 131 | GeometricalCheck geom_check; 132 | /// If using direct index for geometrical checking, direct index levels 133 | int di_levels; 134 | 135 | // These are less deciding parameters of the system 136 | 137 | /// Distance between entries to be consider a match 138 | int dislocal; 139 | /// Max number of results from db queries to consider 140 | int max_db_results; 141 | /// Min raw score between current entry and previous one to consider a match 142 | float min_nss_factor; 143 | /// Min number of close matches to consider some of them 144 | int min_matches_per_group; 145 | /// Max separation between matches to consider them of the same group 146 | int max_intragroup_gap; 147 | /// Max separation between groups of matches to consider them consistent 148 | int max_distance_between_groups; 149 | /// Max separation between two queries to consider them consistent 150 | int max_distance_between_queries; 151 | 152 | // These are for the RANSAC to compute the F 153 | 154 | /// Min number of inliers when computing a fundamental matrix 155 | int min_Fpoints; 156 | /// Max number of iterations of RANSAC 157 | int max_ransac_iterations; 158 | /// Success probability of RANSAC 159 | double ransac_probability; 160 | /// Max reprojection error of fundamental matrices 161 | double max_reprojection_error; 162 | 163 | // This is to compute correspondences 164 | 165 | /// Max value of the neighbour-ratio of accepted correspondences 166 | double max_neighbor_ratio; 167 | /// Camera calibration information for triangulation and PnP 168 | StereoParameters stereo_params; 169 | /// Distances for 3d point culling 170 | int near_distance; 171 | int far_distance; 172 | 173 | /** 174 | * Creates parameters by default 175 | */ 176 | Parameters(); 177 | 178 | /** 179 | * Creates parameters by default 180 | * @param height image height 181 | * @param width image width 182 | * @param frequency set the value of some parameters according to the 183 | * expected working frequency (Hz) 184 | * @param nss use normalized similarity score 185 | * @param _alpha alpha parameter 186 | * @param _k k parameter (number of temporary consistent matches) 187 | * @param geom type of geometrical check 188 | * @param dilevels direct index levels when geom == GEOM_DI 189 | */ 190 | Parameters(int height, int width, float frequency = 1, bool nss = true, 191 | float _alpha = 0.3, int _k = 3, 192 | GeometricalCheck geom = GEOM_DI, int dilevels = 0); 193 | 194 | private: 195 | /** 196 | * Sets some parameters according to the frequency 197 | * @param frequency 198 | */ 199 | void set(float frequency); 200 | }; 201 | 202 | public: 203 | 204 | /** 205 | * Empty constructor 206 | */ 207 | TemplatedLoopDetector(const Parameters ¶ms = Parameters()); 208 | 209 | /** 210 | * Creates a loop detector with the given parameters and with a BoW2 database 211 | * with the given vocabulary 212 | * @param voc vocabulary 213 | * @param params loop detector parameters 214 | */ 215 | TemplatedLoopDetector(const TemplatedVocabulary &voc, 216 | const Parameters ¶ms = Parameters()); 217 | 218 | /** 219 | * Creates a loop detector with a copy of the given database, but clearing 220 | * its contents 221 | * @param db database to copy 222 | * @param params loop detector parameters 223 | */ 224 | TemplatedLoopDetector(const TemplatedDatabase &db, 225 | const Parameters ¶ms = Parameters()); 226 | 227 | /** 228 | * Creates a loop detector with a copy of the given database, but clearing 229 | * its contents 230 | * @param T class derived from TemplatedDatabase 231 | * @param db database to copy 232 | * @param params loop detector parameters 233 | */ 234 | template 235 | TemplatedLoopDetector(const T &db, const Parameters ¶ms = Parameters()); 236 | 237 | /** 238 | * Destructor 239 | */ 240 | virtual ~TemplatedLoopDetector(void); 241 | 242 | /** 243 | * Retrieves a reference to the database used by the loop detector 244 | * @return const reference to database 245 | */ 246 | inline const TemplatedDatabase& getDatabase() const; 247 | 248 | /** 249 | * Retrieves a reference to the vocabulary used by the loop detector 250 | * @return const reference to vocabulary 251 | */ 252 | inline const TemplatedVocabulary& getVocabulary() const; 253 | 254 | /** 255 | * Sets the database to use. The contents of the database and the detector 256 | * entries are cleared 257 | * @param T class derived from TemplatedDatabase 258 | * @param db database to copy 259 | */ 260 | template 261 | void setDatabase(const T &db); 262 | 263 | /** 264 | * Sets a new DBoW2 database created from the given vocabulary 265 | * @param voc vocabulary to copy 266 | */ 267 | void setVocabulary(const TemplatedVocabulary& voc); 268 | 269 | /** 270 | * Allocates some memory for the first entries 271 | * @param nentries number of expected entries 272 | * @param nkeys number of keypoints per image expected 273 | */ 274 | void allocate(int nentries, int nkeys = 0); 275 | 276 | /** 277 | * Adds the given tuple to the database 278 | * and returns the match if any 279 | * @param keys keypoints of the image 280 | * @param descriptors descriptors associated to the given keypoints 281 | * @param match (out) match or failing information 282 | * @param keys2 optional keypoints of the stereo second image 283 | * @param descriptors2 optional descriptors associated to the keypoints 284 | * of the stereo second image 285 | * @return true iff there was match 286 | */ 287 | bool detectLoop(const std::vector &keys, 288 | const std::vector &descriptors, 289 | DetectionResult &match, 290 | const std::vector &keys2 = std::vector(), 291 | const std::vector &descriptors2 = std::vector()); 292 | 293 | /** 294 | * Resets the detector and clears the database, such that the next entry 295 | * will be 0 again 296 | */ 297 | inline void clear(); 298 | 299 | protected: 300 | 301 | /// Matching island 302 | struct tIsland 303 | { 304 | /// Island starting entry 305 | EntryId first; 306 | /// Island ending entry 307 | EntryId last; 308 | /// Island score 309 | double score; // score of island 310 | 311 | /// Entry of the island with the highest score 312 | EntryId best_entry; // id and score of the entry with the highest score 313 | /// Highest single score in the island 314 | double best_score; // in the island 315 | 316 | /** 317 | * Creates an empty island 318 | */ 319 | tIsland(){} 320 | 321 | /** 322 | * Creates an island 323 | * @param f first entry 324 | * @param l last entry 325 | */ 326 | tIsland(EntryId f, EntryId l): first(f), last(l){} 327 | 328 | /** 329 | * Creates an island 330 | * @param f first entry 331 | * @param l last entry 332 | * @param s island score 333 | */ 334 | tIsland(EntryId f, EntryId l, double s): first(f), last(l), score(s){} 335 | 336 | /** 337 | * Says whether this score is less than the score of another island 338 | * @param b 339 | * @return true iff this score < b.score 340 | */ 341 | inline bool operator < (const tIsland &b) const 342 | { 343 | return this->score < b.score; 344 | } 345 | 346 | /** 347 | * Says whether this score is greater than the score of another island 348 | * @param b 349 | * @return true iff this score > b.score 350 | */ 351 | inline bool operator > (const tIsland &b) const 352 | { 353 | return this->score > b.score; 354 | } 355 | 356 | /** 357 | * Returns true iff a > b 358 | * This function is used to sort in descending order 359 | * @param a 360 | * @param b 361 | * @return a > b 362 | */ 363 | static inline bool gt(const tIsland &a, const tIsland &b) 364 | { 365 | return a.score > b.score; 366 | } 367 | 368 | /** 369 | * Returns true iff entry ids of a are less then those of b. 370 | * Assumes there is no overlap between the islands 371 | * @param a 372 | * @param b 373 | * @return a.first < b.first 374 | */ 375 | static inline bool ltId(const tIsland &a, const tIsland &b) 376 | { 377 | return a.first < b.first; 378 | } 379 | 380 | /** 381 | * Returns the length of the island 382 | * @return length of island 383 | */ 384 | inline int length() const { return last - first + 1; } 385 | 386 | /** 387 | * Returns a printable version of the island 388 | * @return printable island 389 | */ 390 | std::string toString() const 391 | { 392 | stringstream ss; 393 | ss << "[" << first << "-" << last << ": " << score << " | best: <" 394 | << best_entry << ": " << best_score << "> ] "; 395 | return ss.str(); 396 | } 397 | }; 398 | 399 | protected: 400 | 401 | /// Temporal consistency window 402 | struct tTemporalWindow 403 | { 404 | /// Island matched in the last query 405 | tIsland last_matched_island; 406 | /// Last query id 407 | EntryId last_query_id; 408 | /// Number of consistent entries in the window 409 | int nentries; 410 | 411 | /** 412 | * Creates an empty temporal window 413 | */ 414 | tTemporalWindow(): nentries(0) {} 415 | }; 416 | 417 | protected: 418 | 419 | /** 420 | * Removes from q those results whose score is lower than threshold 421 | * (that should be alpha * ns_factor) 422 | * @param q results from query 423 | * @param threshold min value of the resting results 424 | */ 425 | void removeLowScores(QueryResults &q, double threshold) const; 426 | 427 | /** 428 | * Returns the islands of the given matches in ascending order of entry ids 429 | * @param q 430 | * @param islands (out) computed islands 431 | */ 432 | void computeIslands(QueryResults &q, vector &islands) const; 433 | 434 | /** 435 | * Returns the score of the island composed of the entries of q whose indices 436 | * are in [i_first, i_last] (both included) 437 | * @param q 438 | * @param i_first first index of q of the island 439 | * @param i_last last index of q of the island 440 | * @return island score 441 | */ 442 | double calculateIslandScore(const QueryResults &q, unsigned int i_first, 443 | unsigned int i_last) const; 444 | 445 | /** 446 | * Updates the temporal window by adding the given match , such 447 | * that the window will contain only those islands which are consistent 448 | * @param matched_island 449 | * @param entry_id 450 | */ 451 | void updateTemporalWindow(const tIsland &matched_island, EntryId entry_id); 452 | 453 | /** 454 | * Returns the number of consistent islands in the temporal window 455 | * @return number of temporal consistent islands 456 | */ 457 | inline int getConsistentEntries() const 458 | { 459 | return m_window.nentries; 460 | } 461 | 462 | /** 463 | * Check if an old entry is geometrically consistent (by calculating a 464 | * fundamental matrix) with the given set of keys and descriptors 465 | * @param old_entry entry id of the stored image to check 466 | * @param keys current keypoints 467 | * @param descriptors current descriptors associated to the given keypoints 468 | * @param curvec feature vector of the current entry 469 | */ 470 | bool isGeometricallyConsistent_DI(EntryId old_entry, 471 | const std::vector &keys, 472 | const std::vector &descriptors, 473 | const FeatureVector &curvec) const; 474 | 475 | /** 476 | * Checks if an old entry is geometrically consistent (by using FLANN and 477 | * computing an essential matrix by using the neighbour ratio 0.6) 478 | * with the given set of keys and descriptors 479 | * @param old_entry entry id of the stored image to check 480 | * @param keys current keypoints 481 | * @param descriptors current descriptors 482 | * @param flann_structure flann structure with the descriptors of the current entry 483 | */ 484 | bool isGeometricallyConsistent_Flann(EntryId old_entry, 485 | const std::vector &keys, 486 | const std::vector &descriptors, 487 | cv::FlannBasedMatcher &flann_structure) const; 488 | 489 | /** 490 | * Creates a flann structure from a set of descriptors to perform queries 491 | * @param descriptors 492 | * @param flann_structure (out) flann matcher 493 | */ 494 | void getFlannStructure(const std::vector &descriptors, 495 | cv::FlannBasedMatcher &flann_structure) const; 496 | 497 | /** 498 | * Check if an old entry is geometrically consistent (by calculating a 499 | * fundamental matrix from left-right correspondences) with the given set 500 | * of keys and descriptors, 501 | * without using the direct index 502 | * @param old_keys keys of old entry 503 | * @param old_descriptors descriptors of old keys 504 | * @param cur_keys keys of current entry 505 | * @param cur_descriptors descriptors of cur keys 506 | */ 507 | bool isGeometricallyConsistent_Exhaustive( 508 | const std::vector &old_keys, 509 | const std::vector &old_descriptors, 510 | const std::vector &cur_keys, 511 | const std::vector &cur_descriptors) const; 512 | 513 | /** 514 | * Check if an old entry is spatially consistent with a given set of stereo 515 | * keypoints and descriptors by matching keypoints in all four images, 516 | * calculating the 3d point projections of those for a given pair and 517 | * performing PnP matching with a single image of the other stereo pair. 518 | * Does not use direct index. 519 | * @param old_keys keys of old entry 520 | * @param old_descriptors descriptors of old keys 521 | * @param cur_keys keys of current entry 522 | * @param cur_descriptors descriptors of cur keys 523 | */ 524 | bool isGeometricallyConsistent_Exhaustive_Stereo( 525 | const std::vector &old_keys1, 526 | const std::vector &old_descriptors1, 527 | const std::vector &old_keys2, 528 | const std::vector &old_descriptors2, 529 | const std::vector &cur_keys1, 530 | const std::vector &cur_descriptors1, 531 | const std::vector &cur_keys2, 532 | const std::vector &cur_descriptors2, 533 | cv::Vec3d &transform ) const; 534 | 535 | /** 536 | * Calculate the matches between the descriptors A[i_A] and the descriptors 537 | * B[i_B]. Applies a left-right matching without neighbour ratio 538 | * @param A set A of descriptors 539 | * @param i_A only descriptors A[i_A] will be checked 540 | * @param B set B of descriptors 541 | * @param i_B only descriptors B[i_B] will be checked 542 | * @param i_match_A (out) indices of descriptors matched (s.t. A[i_match_A]) 543 | * @param i_match_B (out) indices of descriptors matched (s.t. B[i_match_B]) 544 | */ 545 | void getMatches_neighratio(const std::vector &A, 546 | const vector &i_A, const vector &B, 547 | const vector &i_B, 548 | vector &i_match_A, vector &i_match_B) const; 549 | 550 | protected: 551 | 552 | /// Database 553 | // The loop detector stores its own copy of the database 554 | TemplatedDatabase *m_database; 555 | 556 | /// KeyPoints of images 557 | vector > m_image_keys; 558 | 559 | /// Descriptors of images 560 | vector > m_image_descriptors; 561 | 562 | /// KeyPoints of stereo second images 563 | vector > m_image_keys2; 564 | 565 | /// Descriptors of stereo second images 566 | vector > m_image_descriptors2; 567 | 568 | /// Last bow vector added to database 569 | BowVector m_last_bowvec; 570 | 571 | /// Temporal consistency window 572 | tTemporalWindow m_window; 573 | 574 | /// Parameters of loop detector 575 | Parameters m_params; 576 | 577 | /// To compute the fundamental matrix 578 | DVision::FSolver m_fsolver; 579 | 580 | }; 581 | 582 | // -------------------------------------------------------------------------- 583 | 584 | template 585 | TemplatedLoopDetector::Parameters::Parameters(): 586 | use_nss(true), alpha(0.3), k(4), geom_check(GEOM_DI), di_levels(0) 587 | { 588 | set(1); 589 | } 590 | 591 | // -------------------------------------------------------------------------- 592 | 593 | template 594 | TemplatedLoopDetector::Parameters::Parameters 595 | (int height, int width, float frequency, bool nss, float _alpha, 596 | int _k, GeometricalCheck geom, int dilevels) 597 | : image_rows(height), image_cols(width), use_nss(nss), alpha(_alpha), k(_k), 598 | geom_check(geom), di_levels(dilevels) 599 | { 600 | set(frequency); 601 | } 602 | 603 | // -------------------------------------------------------------------------- 604 | 605 | template 606 | void TemplatedLoopDetector::Parameters::set(float f) 607 | { 608 | dislocal = 20 * f; 609 | max_db_results = 50 * f; 610 | min_nss_factor = 0.005; 611 | min_matches_per_group = f; 612 | max_intragroup_gap = 3 * f; 613 | max_distance_between_groups = 3 * f; 614 | max_distance_between_queries = 2 * f; 615 | 616 | min_Fpoints = 12; 617 | max_ransac_iterations = 500; 618 | ransac_probability = 0.99; 619 | max_reprojection_error = 2.0; 620 | 621 | max_neighbor_ratio = 0.6; 622 | } 623 | 624 | // -------------------------------------------------------------------------- 625 | 626 | // -------------------------------------------------------------------------- 627 | 628 | template 629 | TemplatedLoopDetector::TemplatedLoopDetector 630 | (const Parameters ¶ms) 631 | : m_database(NULL), m_params(params) 632 | { 633 | } 634 | 635 | // -------------------------------------------------------------------------- 636 | 637 | template 638 | TemplatedLoopDetector::TemplatedLoopDetector 639 | (const TemplatedVocabulary &voc, const Parameters ¶ms) 640 | : m_params(params) 641 | { 642 | m_database = new TemplatedDatabase(voc, 643 | params.geom_check == GEOM_DI, params.di_levels); 644 | 645 | m_fsolver.setImageSize(params.image_cols, params.image_rows); 646 | } 647 | 648 | // -------------------------------------------------------------------------- 649 | 650 | template 651 | void TemplatedLoopDetector::setVocabulary 652 | (const TemplatedVocabulary& voc) 653 | { 654 | delete m_database; 655 | m_database = new TemplatedDatabase(voc, 656 | m_params.geom_check == GEOM_DI, m_params.di_levels); 657 | } 658 | 659 | // -------------------------------------------------------------------------- 660 | 661 | template 662 | TemplatedLoopDetector::TemplatedLoopDetector 663 | (const TemplatedDatabase &db, const Parameters ¶ms) 664 | : m_params(params) 665 | { 666 | m_database = new TemplatedDatabase(db.getVocabulary(), 667 | params.geom_check == GEOM_DI, params.di_levels); 668 | 669 | m_fsolver.setImageSize(params.image_cols, params.image_rows); 670 | } 671 | 672 | // -------------------------------------------------------------------------- 673 | 674 | template 675 | template 676 | TemplatedLoopDetector::TemplatedLoopDetector 677 | (const T &db, const Parameters ¶ms) 678 | : m_params(params) 679 | { 680 | m_database = new T(db); 681 | m_database->clear(); 682 | 683 | m_fsolver.setImageSize(params.image_cols, params.image_rows); 684 | } 685 | 686 | // -------------------------------------------------------------------------- 687 | 688 | template 689 | template 690 | void TemplatedLoopDetector::setDatabase(const T &db) 691 | { 692 | delete m_database; 693 | m_database = new T(db); 694 | clear(); 695 | } 696 | 697 | // -------------------------------------------------------------------------- 698 | 699 | template 700 | TemplatedLoopDetector::~TemplatedLoopDetector(void) 701 | { 702 | delete m_database; 703 | m_database = NULL; 704 | } 705 | 706 | // -------------------------------------------------------------------------- 707 | 708 | template 709 | void TemplatedLoopDetector::allocate 710 | (int nentries, int nkeys) 711 | { 712 | const int sz = (const int)m_image_keys.size(); 713 | 714 | if(sz < nentries) 715 | { 716 | m_image_keys.resize(nentries); 717 | m_image_keys2.resize(nentries); 718 | m_image_descriptors.resize(nentries); 719 | m_image_descriptors2.resize(nentries); 720 | } 721 | 722 | if(nkeys > 0) 723 | { 724 | for(int i = sz; i < nentries; ++i) 725 | { 726 | m_image_keys[i].reserve(nkeys); 727 | m_image_keys2[i].reserve(nkeys); 728 | m_image_descriptors[i].reserve(nkeys); 729 | m_image_descriptors2[i].reserve(nkeys); 730 | } 731 | } 732 | 733 | m_database->allocate(nentries, nkeys); 734 | } 735 | 736 | // -------------------------------------------------------------------------- 737 | 738 | template 739 | inline const TemplatedDatabase& 740 | TemplatedLoopDetector::getDatabase() const 741 | { 742 | return *m_database; 743 | } 744 | 745 | // -------------------------------------------------------------------------- 746 | 747 | template 748 | inline const TemplatedVocabulary& 749 | TemplatedLoopDetector::getVocabulary() const 750 | { 751 | return m_database->getVocabulary(); 752 | } 753 | 754 | // -------------------------------------------------------------------------- 755 | 756 | /** 757 | * Detects a loop based on keypoints and descriptors of an image. 758 | * Will optionally take the keypoints and descriptors of a second image for 759 | * stereo loop detection. Its assumed that the keypoints for the stereo pair 760 | * have already been matched and only their matching keys and descriptors are 761 | * given in matched order (i.e. keys[i] matches keys2[i] forall i and the same 762 | * for descriptors). 763 | * 764 | * @param keys keypoints for the monocular or first stereo image 765 | * @param descriptors descriptors for the monocular or first stereo image 766 | * @param match information about the match if a loop was detected 767 | * @param keys2 keypoints for the second stereo image 768 | * @param descriptors2 descriptors for the second stereo image 769 | * @return boolean representing if the loop was detected or not 770 | */ 771 | template 772 | bool TemplatedLoopDetector::detectLoop( 773 | const std::vector &keys, 774 | const std::vector &descriptors, 775 | DetectionResult &match, 776 | const std::vector &keys2, 777 | const std::vector &descriptors2) 778 | { 779 | // assert(keys.size() == descriptors.size()) 780 | // assert(keys2.size() == descriptors2.size()); 781 | 782 | // We first find matches for the monocular or first stereo image (leaving 783 | // the geometric consistency or stereo geometric consistency for later). 784 | EntryId entry_id = m_database->size(); 785 | match.query = entry_id; 786 | 787 | // Check if we have stereo information 788 | bool stereo = false; 789 | if (keys2.size() != 0) { 790 | stereo = true; 791 | } 792 | 793 | BowVector bowvec, bowvec2; 794 | FeatureVector featvec, featvec2; 795 | 796 | if(m_params.geom_check == GEOM_DI) 797 | m_database->getVocabulary()->transform(descriptors, bowvec, featvec, 798 | m_params.di_levels); 799 | else 800 | m_database->getVocabulary()->transform(descriptors, bowvec); 801 | 802 | if((int)entry_id <= m_params.dislocal) 803 | { 804 | // for the first `dislocal` entries there's no need to check as 805 | // there's certainly not enough distance between them, so we 806 | // only add the entry to the database and finish 807 | m_database->add(bowvec, featvec); 808 | match.status = CLOSE_MATCHES_ONLY; 809 | } 810 | else 811 | { 812 | int max_id = (int)entry_id - m_params.dislocal; 813 | 814 | QueryResults qret; 815 | m_database->query(bowvec, qret, m_params.max_db_results, max_id); 816 | 817 | // update database 818 | m_database->add(bowvec, featvec); // returns entry_id 819 | 820 | if(!qret.empty()) 821 | { 822 | // factor to compute normalized similarity score, if necessary 823 | double ns_factor = 1.0; 824 | 825 | if(m_params.use_nss) 826 | { 827 | ns_factor = m_database->getVocabulary()->score(bowvec, m_last_bowvec); 828 | } 829 | 830 | if(!m_params.use_nss || ns_factor >= m_params.min_nss_factor) 831 | { 832 | // scores in qret must be divided by ns_factor to obtain the 833 | // normalized similarity score, but we can 834 | // speed this up by moving ns_factor to alpha's 835 | 836 | // remove those scores whose nss is lower than alpha 837 | // (ret is sorted in descending score order now) 838 | std::cout << "Normalized Score based on factor " << ns_factor << std::endl; 839 | std::cout << "Removing scores lower than: " << m_params.alpha * ns_factor << std::endl; 840 | removeLowScores(qret, m_params.alpha * ns_factor); 841 | 842 | if(!qret.empty()) 843 | { 844 | // the best candidate is the one with highest score by now 845 | match.match = qret[0].Id; 846 | 847 | // compute islands 848 | vector islands; 849 | computeIslands(qret, islands); 850 | // this modifies qret and changes the score order 851 | 852 | // get best island 853 | if(!islands.empty()) 854 | { 855 | const tIsland& island = 856 | *std::max_element(islands.begin(), islands.end()); 857 | 858 | // check temporal consistency of this island 859 | updateTemporalWindow(island, entry_id); 860 | 861 | // get the best candidate (maybe match) 862 | match.match = island.best_entry; 863 | 864 | if(getConsistentEntries() > m_params.k) 865 | { 866 | // candidate loop detected 867 | // check geometry 868 | bool detection; 869 | 870 | if(m_params.geom_check == GEOM_DI) 871 | { 872 | // all the DI stuff is implicit in the database 873 | detection = isGeometricallyConsistent_DI(island.best_entry, 874 | keys, descriptors, featvec); 875 | } 876 | else if(m_params.geom_check == GEOM_FLANN) 877 | { 878 | cv::FlannBasedMatcher flann_structure; 879 | getFlannStructure(descriptors, flann_structure); 880 | 881 | detection = isGeometricallyConsistent_Flann(island.best_entry, 882 | keys, descriptors, flann_structure); 883 | } 884 | else if(m_params.geom_check == GEOM_EXHAUSTIVE) 885 | { 886 | detection = isGeometricallyConsistent_Exhaustive( 887 | m_image_keys[island.best_entry], 888 | m_image_descriptors[island.best_entry], 889 | keys, descriptors); 890 | } 891 | else if(m_params.geom_check == GEOM_EXHAUSTIVE_STEREO) 892 | { 893 | assert(stereo); 894 | detection = isGeometricallyConsistent_Exhaustive_Stereo( 895 | m_image_keys[island.best_entry], 896 | m_image_descriptors[island.best_entry], 897 | m_image_keys2[island.best_entry], 898 | m_image_descriptors2[island.best_entry], 899 | keys, descriptors, keys2, descriptors2, 900 | match.transform); 901 | } 902 | else // GEOM_NONE, accept the match 903 | { 904 | detection = true; 905 | } 906 | 907 | if(detection) 908 | { 909 | match.status = LOOP_DETECTED; 910 | } 911 | else 912 | { 913 | match.status = NO_GEOMETRICAL_CONSISTENCY; 914 | } 915 | 916 | } // if enough temporal matches 917 | else 918 | { 919 | match.status = NO_TEMPORAL_CONSISTENCY; 920 | } 921 | 922 | } // if there is some island 923 | else 924 | { 925 | match.status = NO_GROUPS; 926 | } 927 | } // if !qret empty after removing low scores 928 | else 929 | { 930 | match.status = LOW_SCORES; 931 | } 932 | } // if (ns_factor > min normal score) 933 | else 934 | { 935 | match.status = LOW_NSS_FACTOR; 936 | } 937 | } // if(!qret.empty()) 938 | else 939 | { 940 | match.status = NO_DB_RESULTS; 941 | } 942 | } 943 | 944 | // update record 945 | // m_image_keys and m_image_descriptors have the same length 946 | // std::cout << "m_image_keys.size(): " << m_image_keys.size() << std::endl; 947 | // std::cout << "entry_id: " << entry_id << std::endl; 948 | if(m_image_keys.size() == entry_id) 949 | { 950 | m_image_keys.push_back(keys); 951 | m_image_descriptors.push_back(descriptors); 952 | if (stereo) { 953 | m_image_keys2.push_back(keys2); 954 | m_image_descriptors2.push_back(descriptors2); 955 | } 956 | } 957 | else 958 | { 959 | m_image_keys[entry_id] = keys; 960 | m_image_descriptors[entry_id] = descriptors; 961 | if (stereo) { 962 | m_image_keys2[entry_id] = keys2; 963 | m_image_descriptors2[entry_id] = descriptors2; 964 | } 965 | } 966 | 967 | // store this bowvec if we are going to use it in next iteratons 968 | if(m_params.use_nss && (int)entry_id + 1 > m_params.dislocal) 969 | { 970 | m_last_bowvec = bowvec; 971 | } 972 | 973 | return match.detection(); 974 | } 975 | 976 | // -------------------------------------------------------------------------- 977 | 978 | template 979 | inline void TemplatedLoopDetector::clear() 980 | { 981 | m_database->clear(); 982 | m_window.nentries = 0; 983 | } 984 | 985 | // -------------------------------------------------------------------------- 986 | 987 | template 988 | void TemplatedLoopDetector::computeIslands 989 | (QueryResults &q, vector &islands) const 990 | { 991 | islands.clear(); 992 | 993 | if(q.size() == 1) 994 | { 995 | islands.push_back(tIsland(q[0].Id, q[0].Id, calculateIslandScore(q, 0, 0))); 996 | islands.back().best_entry = q[0].Id; 997 | islands.back().best_score = q[0].Score; 998 | } 999 | else if(!q.empty()) 1000 | { 1001 | // sort query results in ascending order of ids 1002 | std::sort(q.begin(), q.end(), Result::ltId); 1003 | 1004 | // create long enough islands 1005 | QueryResults::const_iterator dit = q.begin(); 1006 | int first_island_entry = dit->Id; 1007 | int last_island_entry = dit->Id; 1008 | 1009 | // these are indices of q 1010 | unsigned int i_first = 0; 1011 | unsigned int i_last = 0; 1012 | 1013 | double best_score = dit->Score; 1014 | EntryId best_entry = dit->Id; 1015 | 1016 | ++dit; 1017 | for(unsigned int idx = 1; dit != q.end(); ++dit, ++idx) 1018 | { 1019 | if((int)dit->Id - last_island_entry < m_params.max_intragroup_gap) 1020 | { 1021 | // go on until find the end of the island 1022 | last_island_entry = dit->Id; 1023 | i_last = idx; 1024 | if(dit->Score > best_score) 1025 | { 1026 | best_score = dit->Score; 1027 | best_entry = dit->Id; 1028 | } 1029 | } 1030 | else 1031 | { 1032 | // end of island reached 1033 | int length = last_island_entry - first_island_entry + 1; 1034 | if(length >= m_params.min_matches_per_group) 1035 | { 1036 | islands.push_back( tIsland(first_island_entry, last_island_entry, 1037 | calculateIslandScore(q, i_first, i_last)) ); 1038 | 1039 | islands.back().best_score = best_score; 1040 | islands.back().best_entry = best_entry; 1041 | } 1042 | 1043 | // prepare next island 1044 | first_island_entry = last_island_entry = dit->Id; 1045 | i_first = i_last = idx; 1046 | best_score = dit->Score; 1047 | best_entry = dit->Id; 1048 | } 1049 | } 1050 | // add last island 1051 | if(last_island_entry - first_island_entry + 1 >= 1052 | m_params.min_matches_per_group) 1053 | { 1054 | islands.push_back( tIsland(first_island_entry, last_island_entry, 1055 | calculateIslandScore(q, i_first, i_last)) ); 1056 | 1057 | islands.back().best_score = best_score; 1058 | islands.back().best_entry = best_entry; 1059 | } 1060 | } 1061 | 1062 | } 1063 | 1064 | // -------------------------------------------------------------------------- 1065 | 1066 | template 1067 | double TemplatedLoopDetector::calculateIslandScore( 1068 | const QueryResults &q, unsigned int i_first, unsigned int i_last) const 1069 | { 1070 | // get the sum of the scores 1071 | double sum = 0; 1072 | while(i_first <= i_last) sum += q[i_first++].Score; 1073 | return sum; 1074 | } 1075 | 1076 | // -------------------------------------------------------------------------- 1077 | 1078 | template 1079 | void TemplatedLoopDetector::updateTemporalWindow 1080 | (const tIsland &matched_island, EntryId entry_id) 1081 | { 1082 | // if m_window.nentries > 0, island > m_window.last_matched_island and 1083 | // entry_id > m_window.last_query_id hold 1084 | 1085 | if(m_window.nentries == 0 || int(entry_id - m_window.last_query_id) 1086 | > m_params.max_distance_between_queries) 1087 | { 1088 | m_window.nentries = 1; 1089 | } 1090 | else 1091 | { 1092 | EntryId a1 = m_window.last_matched_island.first; 1093 | EntryId a2 = m_window.last_matched_island.last; 1094 | EntryId b1 = matched_island.first; 1095 | EntryId b2 = matched_island.last; 1096 | 1097 | bool fit = (b1 <= a1 && a1 <= b2) || (a1 <= b1 && b1 <= a2); 1098 | 1099 | if(!fit) 1100 | { 1101 | int d1 = (int)a1 - (int)b2; 1102 | int d2 = (int)b1 - (int)a2; 1103 | int gap = (d1 > d2 ? d1 : d2); 1104 | 1105 | fit = (gap <= m_params.max_distance_between_groups); 1106 | } 1107 | 1108 | if(fit) m_window.nentries++; 1109 | else m_window.nentries = 1; 1110 | } 1111 | 1112 | m_window.last_matched_island = matched_island; 1113 | m_window.last_query_id = entry_id; 1114 | } 1115 | 1116 | // -------------------------------------------------------------------------- 1117 | 1118 | template 1119 | bool TemplatedLoopDetector::isGeometricallyConsistent_DI( 1120 | EntryId old_entry, const std::vector &keys, 1121 | const std::vector &descriptors, 1122 | const FeatureVector &bowvec) const 1123 | { 1124 | const FeatureVector &oldvec = m_database->retrieveFeatures(old_entry); 1125 | 1126 | // for each word in common, get the closest descriptors 1127 | 1128 | vector i_old, i_cur; 1129 | 1130 | FeatureVector::const_iterator old_it, cur_it; 1131 | const FeatureVector::const_iterator old_end = oldvec.end(); 1132 | const FeatureVector::const_iterator cur_end = bowvec.end(); 1133 | 1134 | old_it = oldvec.begin(); 1135 | cur_it = bowvec.begin(); 1136 | 1137 | while(old_it != old_end && cur_it != cur_end) 1138 | { 1139 | if(old_it->first == cur_it->first) 1140 | { 1141 | // compute matches between 1142 | // features old_it->second of m_image_keys[old_entry] and 1143 | // features cur_it->second of keys 1144 | vector i_old_now, i_cur_now; 1145 | 1146 | getMatches_neighratio( 1147 | m_image_descriptors[old_entry], old_it->second, 1148 | descriptors, cur_it->second, 1149 | i_old_now, i_cur_now); 1150 | 1151 | i_old.insert(i_old.end(), i_old_now.begin(), i_old_now.end()); 1152 | i_cur.insert(i_cur.end(), i_cur_now.begin(), i_cur_now.end()); 1153 | 1154 | // move old_it and cur_it forward 1155 | ++old_it; 1156 | ++cur_it; 1157 | } 1158 | else if(old_it->first < cur_it->first) 1159 | { 1160 | // move old_it forward 1161 | old_it = oldvec.lower_bound(cur_it->first); 1162 | // old_it = (first element >= cur_it.id) 1163 | } 1164 | else 1165 | { 1166 | // move cur_it forward 1167 | cur_it = bowvec.lower_bound(old_it->first); 1168 | // cur_it = (first element >= old_it.id) 1169 | } 1170 | } 1171 | 1172 | // calculate now the fundamental matrix 1173 | if((int)i_old.size() >= m_params.min_Fpoints) 1174 | { 1175 | vector old_points, cur_points; 1176 | 1177 | // add matches to the vectors to calculate the fundamental matrix 1178 | vector::const_iterator oit, cit; 1179 | oit = i_old.begin(); 1180 | cit = i_cur.begin(); 1181 | 1182 | for(; oit != i_old.end(); ++oit, ++cit) 1183 | { 1184 | const cv::KeyPoint &old_k = m_image_keys[old_entry][*oit]; 1185 | const cv::KeyPoint &cur_k = keys[*cit]; 1186 | 1187 | old_points.push_back(old_k.pt); 1188 | cur_points.push_back(cur_k.pt); 1189 | } 1190 | 1191 | cv::Mat oldMat(old_points.size(), 2, CV_32F, &old_points[0]); 1192 | cv::Mat curMat(cur_points.size(), 2, CV_32F, &cur_points[0]); 1193 | 1194 | return m_fsolver.checkFundamentalMat(oldMat, curMat, 1195 | m_params.max_reprojection_error, m_params.min_Fpoints, 1196 | m_params.ransac_probability, m_params.max_ransac_iterations); 1197 | } 1198 | 1199 | return false; 1200 | } 1201 | 1202 | // -------------------------------------------------------------------------- 1203 | 1204 | template 1205 | bool TemplatedLoopDetector:: 1206 | isGeometricallyConsistent_Exhaustive( 1207 | const std::vector &old_keys, 1208 | const std::vector &old_descriptors, 1209 | const std::vector &cur_keys, 1210 | const std::vector &cur_descriptors) const 1211 | { 1212 | vector i_old, i_cur; 1213 | vector i_all_old, i_all_cur; 1214 | 1215 | i_all_old.reserve(old_keys.size()); 1216 | i_all_cur.reserve(cur_keys.size()); 1217 | 1218 | for(unsigned int i = 0; i < old_keys.size(); ++i) 1219 | { 1220 | i_all_old.push_back(i); 1221 | } 1222 | 1223 | for(unsigned int i = 0; i < cur_keys.size(); ++i) 1224 | { 1225 | i_all_cur.push_back(i); 1226 | } 1227 | 1228 | getMatches_neighratio(old_descriptors, i_all_old, 1229 | cur_descriptors, i_all_cur, i_old, i_cur); 1230 | 1231 | std::cout << "[Exhaustive] found matches for " << (int)i_old.size() 1232 | << " keypoints" << std::endl; 1233 | if((int)i_old.size() >= m_params.min_Fpoints) 1234 | { 1235 | // add matches to the vectors to calculate the fundamental matrix 1236 | vector::const_iterator oit, cit; 1237 | oit = i_old.begin(); 1238 | cit = i_cur.begin(); 1239 | 1240 | vector old_points, cur_points; 1241 | old_points.reserve(i_old.size()); 1242 | cur_points.reserve(i_cur.size()); 1243 | 1244 | for(; oit != i_old.end(); ++oit, ++cit) 1245 | { 1246 | const cv::KeyPoint &old_k = old_keys[*oit]; 1247 | const cv::KeyPoint &cur_k = cur_keys[*cit]; 1248 | 1249 | old_points.push_back(old_k.pt); 1250 | cur_points.push_back(cur_k.pt); 1251 | } 1252 | 1253 | cv::Mat oldMat(old_points.size(), 2, CV_32F, &old_points[0]); 1254 | cv::Mat curMat(cur_points.size(), 2, CV_32F, &cur_points[0]); 1255 | 1256 | return m_fsolver.checkFundamentalMat(oldMat, curMat, 1257 | m_params.max_reprojection_error, m_params.min_Fpoints, 1258 | m_params.ransac_probability, m_params.max_ransac_iterations); 1259 | } 1260 | 1261 | return false; 1262 | } 1263 | 1264 | // -------------------------------------------------------------------------- 1265 | 1266 | template 1267 | void TemplatedLoopDetector::getFlannStructure( 1268 | const std::vector &descriptors, 1269 | cv::FlannBasedMatcher &flann_structure) const 1270 | { 1271 | vector features(1); 1272 | F::toMat32F(descriptors, features[0]); 1273 | 1274 | flann_structure.clear(); 1275 | flann_structure.add(features); 1276 | flann_structure.train(); 1277 | } 1278 | 1279 | // -------------------------------------------------------------------------- 1280 | 1281 | template 1282 | bool TemplatedLoopDetector::isGeometricallyConsistent_Flann 1283 | (EntryId old_entry, 1284 | const std::vector &keys, 1285 | const std::vector &, 1286 | cv::FlannBasedMatcher &flann_structure) const 1287 | { 1288 | vector i_old, i_cur; // indices of correspondences 1289 | 1290 | const vector& old_keys = m_image_keys[old_entry]; 1291 | const vector& old_descs = m_image_descriptors[old_entry]; 1292 | const vector& cur_keys = keys; 1293 | 1294 | vector queryDescs_v(1); 1295 | F::toMat32F(old_descs, queryDescs_v[0]); 1296 | 1297 | vector > matches; 1298 | 1299 | flann_structure.knnMatch(queryDescs_v[0], matches, 2); 1300 | 1301 | for(int old_idx = 0; old_idx < (int)matches.size(); ++old_idx) 1302 | { 1303 | if(!matches[old_idx].empty()) 1304 | { 1305 | int cur_idx = matches[old_idx][0].trainIdx; 1306 | float dist = matches[old_idx][0].distance; 1307 | 1308 | bool ok = true; 1309 | if(matches[old_idx].size() >= 2) 1310 | { 1311 | float dist_ratio = dist / matches[old_idx][1].distance; 1312 | ok = dist_ratio <= m_params.max_neighbor_ratio; 1313 | } 1314 | 1315 | if(ok) 1316 | { 1317 | vector::iterator cit = 1318 | std::find(i_cur.begin(), i_cur.end(), cur_idx); 1319 | 1320 | if(cit == i_cur.end()) 1321 | { 1322 | i_old.push_back(old_idx); 1323 | i_cur.push_back(cur_idx); 1324 | } 1325 | else 1326 | { 1327 | int idx = i_old[ cit - i_cur.begin() ]; 1328 | if(dist < matches[idx][0].distance) 1329 | { 1330 | i_old[ cit - i_cur.begin() ] = old_idx; 1331 | } 1332 | } 1333 | } 1334 | } 1335 | } 1336 | 1337 | if((int)i_old.size() >= m_params.min_Fpoints) 1338 | { 1339 | // add matches to the vectors for calculating the fundamental matrix 1340 | vector::const_iterator oit, cit; 1341 | oit = i_old.begin(); 1342 | cit = i_cur.begin(); 1343 | 1344 | vector old_points, cur_points; 1345 | old_points.reserve(i_old.size()); 1346 | cur_points.reserve(i_cur.size()); 1347 | 1348 | for(; oit != i_old.end(); ++oit, ++cit) 1349 | { 1350 | const cv::KeyPoint &old_k = old_keys[*oit]; 1351 | const cv::KeyPoint &cur_k = cur_keys[*cit]; 1352 | 1353 | old_points.push_back(old_k.pt); 1354 | cur_points.push_back(cur_k.pt); 1355 | } 1356 | 1357 | cv::Mat oldMat(old_points.size(), 2, CV_32F, &old_points[0]); 1358 | cv::Mat curMat(cur_points.size(), 2, CV_32F, &cur_points[0]); 1359 | 1360 | return m_fsolver.checkFundamentalMat(oldMat, curMat, 1361 | m_params.max_reprojection_error, m_params.min_Fpoints, 1362 | m_params.ransac_probability, m_params.max_ransac_iterations); 1363 | } 1364 | 1365 | return false; 1366 | } 1367 | 1368 | // -------------------------------------------------------------------------- 1369 | 1370 | // Stereo version of Exhaustive 1371 | // Its assumed that keys and descriptors of a stereo image pair have been 1372 | // matched beforehand and only their matches are passed here in order. 1373 | // i.e.: keys1[m] and keys2[m] are stereo matches for all m. 1374 | template 1375 | bool TemplatedLoopDetector:: 1376 | isGeometricallyConsistent_Exhaustive_Stereo( 1377 | const std::vector &old_keys1, 1378 | const std::vector &old_descriptors1, 1379 | const std::vector &old_keys2, 1380 | const std::vector &old_descriptors2, 1381 | const std::vector &cur_keys1, 1382 | const std::vector &cur_descriptors1, 1383 | const std::vector &cur_keys2, 1384 | const std::vector &cur_descriptors2, 1385 | cv::Vec3d& transform) const 1386 | { 1387 | // We already have left/right stereo matches, we calculate the 1388 | // left/left and right/right matches, then we filter for those that 1389 | // satisfy all 4 constraints (l1/r1 & l2/r2 & l1/l2 & r1/r2) 1390 | std::vector i_old1, i_cur1, i_old2, i_cur2; 1391 | std::vector i_all_old, i_all_cur; 1392 | i_all_old.resize(old_keys1.size()); 1393 | i_all_cur.resize(cur_keys1.size()); 1394 | std::iota(i_all_old.begin(), i_all_old.begin() + old_keys1.size(), 0); 1395 | std::iota(i_all_cur.begin(), i_all_cur.begin() + cur_keys1.size(), 0); 1396 | 1397 | getMatches_neighratio(old_descriptors1, i_all_old, 1398 | cur_descriptors1, i_all_cur, i_old1, i_cur1); 1399 | getMatches_neighratio(old_descriptors2, i_all_old, 1400 | cur_descriptors2, i_all_cur, i_old2, i_cur2); 1401 | 1402 | // we keep the min_Fpoints as a threshold value for computing stereo 1403 | // but we dont use it as intended 1404 | // FIXME 1405 | if (((int)i_old1.size() >= m_params.min_Fpoints) && 1406 | ((int)i_old2.size() >= m_params.min_Fpoints)) 1407 | { 1408 | // we filter matches by the condition that they agree on both 1409 | // the stereo configurations 1410 | std::vector i_old, i_cur; // final agreeing indices 1411 | std::vector::const_iterator oit1, cit1, oit2, cit2; 1412 | oit1 = i_old1.begin(); 1413 | cit1 = i_cur1.begin(); 1414 | for(; oit1 != i_old1.end(); ++oit1, ++cit1) 1415 | { 1416 | oit2 = i_old2.begin(); 1417 | cit2 = i_cur2.begin(); 1418 | for(; oit2 != i_old2.end(); ++oit2, ++cit2) 1419 | { 1420 | if (*oit1 == *oit2 && *cit1 == *cit2) 1421 | { 1422 | // theres a pair (oit1, cit1) == (oit2, cit2) 1423 | i_old.push_back(*oit1); 1424 | i_cur.push_back(*cit1); 1425 | } 1426 | } 1427 | } 1428 | 1429 | // check enough matches to compute camera translation 1430 | // FIXME 1431 | if (!(i_old.size() >= m_params.min_Fpoints)) 1432 | { 1433 | return false; 1434 | } 1435 | 1436 | // calculate the stereo triangulation of the first stereo pair 1437 | std::vector left_matches, right_matches; 1438 | std::vector cur_matches, cur_matches2; 1439 | std::vector::const_iterator oit, cit; 1440 | oit = i_old.begin(); 1441 | cit = i_cur.begin(); 1442 | for (; oit != i_old.end(); ++oit, ++cit) 1443 | { 1444 | left_matches.push_back(old_keys1[*oit]); 1445 | right_matches.push_back(old_keys2[*oit]); 1446 | cur_matches.push_back(cur_keys1[*cit]); 1447 | } 1448 | 1449 | // Prepare the data for the interface of triangulatePoints 1450 | // FIXME: Looks repetitive, get it out of here 1451 | cv::Mat left_points(2, left_matches.size(), CV_32F); 1452 | for (size_t col = 0; col < left_matches.size(); col++) { 1453 | left_points.at(0, col) = left_matches[col].pt.x; 1454 | left_points.at(1, col) = left_matches[col].pt.y; 1455 | } 1456 | cv::Mat right_points(2, right_matches.size(), CV_32F); 1457 | for (size_t col = 0; col < right_matches.size(); col++) { 1458 | right_points.at(0, col) = right_matches[col].pt.x; 1459 | right_points.at(1, col) = right_matches[col].pt.y; 1460 | } 1461 | cv::Mat cur_points(2, cur_matches.size(), CV_32F); 1462 | for (size_t col = 0; col < cur_matches.size(); col++) { 1463 | cur_points.at(0, col) = cur_matches[col].pt.x; 1464 | cur_points.at(1, col) = cur_matches[col].pt.y; 1465 | } 1466 | cv::transpose(cur_points, cur_points); // to Nx2 1467 | std::vector stereo_points = {left_points, right_points}; 1468 | std::vector stereo_projection_matrices = 1469 | {m_params.stereo_params.left_projection, 1470 | m_params.stereo_params.right_projection}; 1471 | 1472 | // Triangulate the stereo 3D points 1473 | cv::Mat stereo_3d_points; 1474 | std::cout << "left projection:\n" << m_params.stereo_params.left_projection << std::endl; 1475 | cv::sfm::triangulatePoints( 1476 | stereo_points, stereo_projection_matrices, stereo_3d_points 1477 | ); // stereo_3d_points is 3xN 1478 | cv::transpose(stereo_3d_points, stereo_3d_points); // to Nx3 1479 | 1480 | // Filter based on distance to keep only 3D points that agree with the 1481 | // stereo camera's baseline threshold. 1482 | // NOTE: we assume the left camera set at the (0,0,0) and 1483 | // the triangulated points are expressed in the left camera frame 1484 | cv::Mat good_3d_points, good_cur_points; 1485 | std::vector distances; 1486 | for (int row = 0; row < stereo_3d_points.rows; row++) { 1487 | float dist = cv::norm(stereo_3d_points.row(row)); 1488 | if ((dist > this->m_params.near_distance) && 1489 | (dist < this->m_params.far_distance)) 1490 | { 1491 | distances.push_back(dist); 1492 | good_3d_points.push_back(stereo_3d_points.row(row)); 1493 | good_cur_points.push_back(cur_points.row(row)); 1494 | } 1495 | } 1496 | 1497 | // Check enough 3D points to perform PnP 1498 | if (!(good_3d_points.rows >= (m_params.min_Fpoints))) 1499 | { 1500 | return false; 1501 | } 1502 | 1503 | // Calculate the position of the current left image to the previously 1504 | // triangulated points via PnP of 2D matches to 3D points 1505 | try { 1506 | cv::Mat rotation, translation; 1507 | cv::solvePnP( 1508 | good_3d_points, good_cur_points, 1509 | m_params.stereo_params.left_camera_matrix, 1510 | m_params.stereo_params.left_dist_coeffs, 1511 | rotation, translation, cv::SOLVEPNP_ITERATIVE); 1512 | 1513 | // TODO: test this works 1514 | transform = cv::Vec3d(translation.reshape(1).at()); 1515 | return true; 1516 | } catch (cv::Exception const& e) { 1517 | std::cerr << "[SExhaustive] failed PnP calculations with exception: " << 1518 | e.err << std::endl; 1519 | } 1520 | } 1521 | 1522 | return false; 1523 | } 1524 | 1525 | // -------------------------------------------------------------------------- 1526 | 1527 | template 1528 | void TemplatedLoopDetector::getMatches_neighratio( 1529 | const vector &A, const vector &i_A, 1530 | const vector &B, const vector &i_B, 1531 | vector &i_match_A, vector &i_match_B) const 1532 | { 1533 | i_match_A.resize(0); 1534 | i_match_B.resize(0); 1535 | i_match_A.reserve( min(i_A.size(), i_B.size()) ); 1536 | i_match_B.reserve( min(i_A.size(), i_B.size()) ); 1537 | 1538 | vector::const_iterator ait, bit; 1539 | unsigned int i, j; 1540 | i = 0; 1541 | for(ait = i_A.begin(); ait != i_A.end(); ++ait, ++i) 1542 | { 1543 | int best_j_now = -1; 1544 | double best_dist_1 = 1e9; 1545 | double best_dist_2 = 1e9; 1546 | 1547 | j = 0; 1548 | for(bit = i_B.begin(); bit != i_B.end(); ++bit, ++j) 1549 | { 1550 | double d = F::distance(A[*ait], B[*bit]); 1551 | 1552 | // in i 1553 | if(d < best_dist_1) 1554 | { 1555 | best_j_now = j; 1556 | best_dist_2 = best_dist_1; 1557 | best_dist_1 = d; 1558 | } 1559 | else if(d < best_dist_2) 1560 | { 1561 | best_dist_2 = d; 1562 | } 1563 | } 1564 | 1565 | if(best_dist_1 / best_dist_2 <= m_params.max_neighbor_ratio) 1566 | { 1567 | unsigned int idx_B = i_B[best_j_now]; 1568 | bit = find(i_match_B.begin(), i_match_B.end(), idx_B); 1569 | 1570 | if(bit == i_match_B.end()) 1571 | { 1572 | i_match_B.push_back(idx_B); 1573 | i_match_A.push_back(*ait); 1574 | } 1575 | else 1576 | { 1577 | unsigned int idx_A = i_match_A[ bit - i_match_B.begin() ]; 1578 | double d = F::distance(A[idx_A], B[idx_B]); 1579 | if(best_dist_1 < d) 1580 | { 1581 | i_match_A[ bit - i_match_B.begin() ] = *ait; 1582 | } 1583 | } 1584 | 1585 | } 1586 | } 1587 | } 1588 | 1589 | // -------------------------------------------------------------------------- 1590 | 1591 | template 1592 | void TemplatedLoopDetector::removeLowScores(QueryResults &q, 1593 | double threshold) const 1594 | { 1595 | // remember scores in q are in descending order now 1596 | //QueryResults::iterator qit = 1597 | // lower_bound(q.begin(), q.end(), threshold, Result::geqv); 1598 | 1599 | Result aux(0, threshold); 1600 | QueryResults::iterator qit = 1601 | lower_bound(q.begin(), q.end(), aux, Result::geq); 1602 | 1603 | // qit = first element < m_alpha_minus || end 1604 | 1605 | if(qit != q.end()) 1606 | { 1607 | int valid_entries = qit - q.begin(); 1608 | q.resize(valid_entries); 1609 | } 1610 | } 1611 | 1612 | // -------------------------------------------------------------------------- 1613 | 1614 | } // namespace DLoopDetector 1615 | 1616 | #endif --------------------------------------------------------------------------------