├── .gitattributes ├── LICENSE ├── README.md ├── groundTruth ├── groundTruthCityCentre.mat ├── groundTruthEuRoc.mat ├── groundTruthKITTI00.mat ├── groundTruthKITTI02.mat ├── groundTruthKITTI05.mat ├── groundTruthKITTI06.mat ├── groundTruthLip6O.mat ├── groundTruthMalaga6L.mat └── groundTruthNC.mat └── matlab ├── HMMinitialization.m ├── buildingDatabase.m ├── forwardHMM.m ├── geometricalCheck.m ├── guidedFeatureSelection.m ├── incomingVisualData.m ├── initializationBoTW.m ├── locationDefinition.m ├── main.m ├── matchesInitialization.m ├── methodEvaluation.m ├── parametersDefinition.m ├── queryingDatabaseHMM.m ├── timersInitialization.m ├── timesBoTW_LCD.m └── vocabularyManagement.m /.gitattributes: -------------------------------------------------------------------------------- 1 | # Auto detect text files and perform LF normalization 2 | * text=auto 3 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2019 Konstantinos Tsintotas 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Modest-vocabulary loop-closure detection with incremental bag of tracked words 2 | 3 | This open source MATLAB algorith presents an appearance-based loop-closure detection pipeline, which encodes the traversed trajectory by unique visual words generated online through tracking. 4 | The incrementally constructed visual vocabulary is referred to as “Bag of Tracked Words”. 5 | By querying the database through a nearest neighbor voting scheme, probabilistic scores are assigned to all visited locations. 6 | Exploiting the inherent time order appearing in the loop-closure task, the produced scores are processed through a Bayesian filter to estimate the belief state about the robot’s location on the map. 7 | Furthermore, a temporal consistency constraint reduces the searching space, while a geometrical verification step rectifies further the results. 8 | Management is also applied to the resulting vocabulary to lessen its tendency to exceed in size over time, while it constrains the system’s computational complexity and voting ambiguity. 9 | The performance of the proposed approach is experimentally evaluated on several challenging and publicly available datasets, including hand-held, car-mounted, aerial and ground robot courses. 10 | Results demonstrate the method’s adaptability, reaching high recall rates for perfect precision and outperforming most of the compared state-of-the-art algorithms. 11 | The system’s effectiveness is owed to the reduced vocabulary size, which, compared to the ones of other contemporary pipelines, is at least one order of magnitude smaller. 12 | An open research-oriented source code has been made publicly available, which is dubbed as “**BoTW-LCD**”. 13 | 14 | Note that the HMM-BoTW approach is a research code. The authors are not responsible for any errors it may contain. **Use it at your own risk!** 15 | 16 | ## Conditions of use 17 | BoTW-LCD is distributed under the terms of the [MIT License](https://github.com/ktsintotas/HMM-BoTW/blob/master/LICENSE). 18 | 19 | ## Related publication 20 | The details of the algorithm are explained in the [following publication](https://www.sciencedirect.com/science/article/pii/S0921889021000671): 21 | 22 | **Modest-vocabulary loop-closure detection with incremental bag of tracked words
** 23 | Konstantinos A. Tsintotas, Loukas Bampis, and Antonios Gasteratos
24 | Robotics and Autonomous Systems (Elsevier) 25 | 26 | If you use this code, please cite: 27 | 28 | ``` 29 | @article{tsintotas2021botw, 30 | title={Modest-vocabulary loop-closure detection with incremental bag of tracked words}, 31 | author={K. A. Tsintotas and L. Bampis and A. Gasteratos}, 32 | journal={Robotics and Autonomous Systems}, 33 | pages={103782}, 34 | volume={141}, 35 | year={2021}, 36 | month={July}, 37 | publisher={Elsevier}, 38 | doi={10.1016/j.robot.2021.103782} 39 | } 40 | ``` 41 | 42 | ## Contact 43 | If you have problems or questions using this code, please contact the author (e-mail address: ktsintot@pme.duth.gr, ktsintotas@icloud.com). Contributions are totally welcome. 44 | -------------------------------------------------------------------------------- /groundTruth/groundTruthCityCentre.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ktsintotas/BoTW-LCD/3cfec5d3ecf474e73c09b03cc0ccf52c376a2387/groundTruth/groundTruthCityCentre.mat -------------------------------------------------------------------------------- /groundTruth/groundTruthEuRoc.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ktsintotas/BoTW-LCD/3cfec5d3ecf474e73c09b03cc0ccf52c376a2387/groundTruth/groundTruthEuRoc.mat -------------------------------------------------------------------------------- /groundTruth/groundTruthKITTI00.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ktsintotas/BoTW-LCD/3cfec5d3ecf474e73c09b03cc0ccf52c376a2387/groundTruth/groundTruthKITTI00.mat -------------------------------------------------------------------------------- /groundTruth/groundTruthKITTI02.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ktsintotas/BoTW-LCD/3cfec5d3ecf474e73c09b03cc0ccf52c376a2387/groundTruth/groundTruthKITTI02.mat -------------------------------------------------------------------------------- /groundTruth/groundTruthKITTI05.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ktsintotas/BoTW-LCD/3cfec5d3ecf474e73c09b03cc0ccf52c376a2387/groundTruth/groundTruthKITTI05.mat -------------------------------------------------------------------------------- /groundTruth/groundTruthKITTI06.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ktsintotas/BoTW-LCD/3cfec5d3ecf474e73c09b03cc0ccf52c376a2387/groundTruth/groundTruthKITTI06.mat -------------------------------------------------------------------------------- /groundTruth/groundTruthLip6O.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ktsintotas/BoTW-LCD/3cfec5d3ecf474e73c09b03cc0ccf52c376a2387/groundTruth/groundTruthLip6O.mat -------------------------------------------------------------------------------- /groundTruth/groundTruthMalaga6L.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ktsintotas/BoTW-LCD/3cfec5d3ecf474e73c09b03cc0ccf52c376a2387/groundTruth/groundTruthMalaga6L.mat -------------------------------------------------------------------------------- /groundTruth/groundTruthNC.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ktsintotas/BoTW-LCD/3cfec5d3ecf474e73c09b03cc0ccf52c376a2387/groundTruth/groundTruthNC.mat -------------------------------------------------------------------------------- /matlab/HMMinitialization.m: -------------------------------------------------------------------------------- 1 | % 2 | 3 | % Copyright 2020, Konstantinos A. Tsintotas 4 | % ktsintot@pme.duth.gr 5 | % 6 | % This file is part of BoTW-LCD framework for visual loop closure detection 7 | % 8 | % BoTW-LCD framework is free software: you can redistribute 9 | % it and/or modify it under the terms of the MIT License as 10 | % published by the corresponding authors. 11 | % 12 | % BoTW-LCD pipeline is distributed in the hope that it will be 13 | % useful, but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | % MIT License for more details. 16 | 17 | function HMM = HMMinitialization(visualData) 18 | 19 | % observation presented along the procedure 20 | HMM.observations = int8(ones(1, visualData.imagesLoaded)); 21 | % loop states presented along the procedure 22 | HMM.loopStates = int8(ones(1, visualData.imagesLoaded)); 23 | % forward probabilities generated along the procedure 24 | HMM.forwardProb = double(zeros(2, visualData.imagesLoaded)); 25 | 26 | end 27 | -------------------------------------------------------------------------------- /matlab/buildingDatabase.m: -------------------------------------------------------------------------------- 1 | % 2 | 3 | % Copyright 2020, Konstantinos A. Tsintotas 4 | % ktsintot@pme.duth.gr 5 | % 6 | % This file is part of BoTW-LCD framework for visual loop closure detection 7 | % 8 | % BoTW-LCD framework is free software: you can redistribute 9 | % it and/or modify it under the terms of the MIT License as 10 | % published by the corresponding authors. 11 | % 12 | % BoTW-LCD pipeline is distributed in the hope that it will be 13 | % useful, but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | % MIT License for more details. 16 | 17 | function [BoTW, timer] = buildingDatabase(visualData, params, timer) 18 | 19 | % shall we load the visual information and its' extracted variables? 20 | if params.buildingDatabase.load == true && exist('results/buildingDatabase.mat', 'file') 21 | load('results/buildingDatabase.mat'); 22 | 23 | else 24 | 25 | % tracked words' counter 26 | twCounter = single(0); 27 | % initializing the Bag of Tracked Words database 28 | BoTW = initializationBoTW(visualData, params); 29 | % initialization of points' tracking validity based on our conditions 30 | trackObservation = false(params.buildingDatabase.numPointsToTrack, 1); 31 | % initialization of points' representation along consecutive features 32 | pointRepeatability = ones(params.buildingDatabase.numPointsToTrack, 1, 'uint16'); 33 | 34 | for It = uint16(1 : visualData.imagesLoaded) 35 | disp(It) 36 | 37 | % initialization of points and descriptors 38 | if It == 1 39 | if size(visualData.points{It}, 1) > params.buildingDatabase.numPointsToTrack 40 | % initial query points and initial points for the tracker 41 | BoTW.queryPoints{It}= visualData.points{It}.Location(1 : params.buildingDatabase.numPointsToTrack, :); 42 | % initial points, each one into a cell bag 43 | trackedPointsBag = mat2cell(BoTW.queryPoints{It}, ones(1, params.buildingDatabase.numPointsToTrack)); 44 | % initial query descriptors 45 | BoTW.queryDescriptors{It} = visualData.features{It}(1 : params.buildingDatabase.numPointsToTrack, :); 46 | % initial descriptors, each one into a cell bag 47 | trackedDescriptorsBag = mat2cell(BoTW.queryDescriptors{It}, ones(1, params.buildingDatabase.numPointsToTrack)); 48 | else 49 | % initial query points and initial points for the tracker 50 | BoTW.queryPoints{It}= visualData.points{It}.Location(1 : size(visualData.points{It}, 1), :); 51 | % initial points, each one into a cell bag 52 | trackedPointsBag = mat2cell(BoTW.queryPoints{It}, ones(1, size(visualData.points{It}, 1))); 53 | % initial query descriptors 54 | BoTW.queryDescriptors{It} = visualData.features{It}(1 : size(visualData.features{It}, 1), :); 55 | % initial descriptors, each one into a cell bag 56 | trackedDescriptorsBag = mat2cell(BoTW.queryDescriptors{It}, ones(1, size(visualData.features{It}, 1))); 57 | end 58 | 59 | % point tracker object generation that tracks a set of points 60 | pointTracker = vision.PointTracker('NumPyramidLevels', 3, 'MaxBidirectionalError', 3); 61 | initialize(pointTracker, BoTW.queryPoints{It}, visualData.inputImage{It}); 62 | end 63 | 64 | if It > 1 65 | previousIt = uint16(It-1); 66 | if ~isempty(BoTW.queryPoints{previousIt}) && size(visualData.points{It}, 1) > params.queryingDatabase.inliersTheshold 67 | 68 | if It > 2 69 | % objects lock when you call them and the release function unlocks them 70 | release(pointTracker); 71 | % initialize again the tracker with the new and more accurate points 72 | initialize(pointTracker, BoTW.queryPoints{previousIt}, visualData.inputImage{previousIt}); 73 | end 74 | 75 | % start the timer for the Kanade-Lucas-Tomase tracker 76 | tic 77 | % tracked points in the incoming frame 78 | [trackedPoints, trackedPointsValidity] = pointTracker(visualData.inputImage{It}); 79 | % stop the timer for the Kanade-Lucas-Tomase tracker 80 | timer.trackingPoints(It, 1) = toc; 81 | 82 | % GUIDED FEATURE SELECTION 83 | [BoTW.queryPoints{It}, BoTW.queryDescriptors{It}, pointRepeatability, trackObservation, pointsToSearch, descriptorsToSearch, trackedPointsBag, trackedDescriptorsBag, timer] = ... 84 | guidedFeatureSelection(params, visualData, previousIt, It, BoTW.queryPoints{previousIt}, BoTW.queryDescriptors{previousIt}, trackedPoints, trackedPointsValidity, pointRepeatability, trackedPointsBag, trackedDescriptorsBag, trackObservation, timer); 85 | 86 | % TRACKED WORD GENERATION 87 | newPointCounter = uint16(0); 88 | deletion = false; 89 | pointsToDelete = false(length(trackObservation), 1); 90 | 91 | 92 | for td = uint16(1 : params.buildingDatabase.numPointsToTrack) 93 | 94 | % When the tracking of a certain point is discontinued, its total length measured in consecutive frames, determines whether a new word should be created 95 | if trackObservation(td) == false && pointRepeatability(td) > params.buildingDatabase.trackLength 96 | twCounter = twCounter + 1; 97 | % Bag of Tracked Words generation from the median of the tracked descriptors the representative TW is computed 98 | BoTW.bagOfTrackedWords(twCounter, :) = median(trackedDescriptorsBag{td}, 1 ); 99 | 100 | if twCounter > 2 101 | [id, dNN] = knnsearch(BoTW.bagOfTrackedWords(1 : twCounter-1, :), BoTW.bagOfTrackedWords(twCounter, :), 'K', 2); 102 | ratio = dNN(1)/dNN(2); 103 | if ratio < params.buildingDatabase.wordsRatio 104 | % renew the visual word 105 | BoTW.bagOfTrackedWords(id(1), :) = median([BoTW.trackedWordDescriptors{id(1)} ; trackedDescriptorsBag{td}], 1 ); 106 | % how many tracked words each location generates 107 | locationsToAdd = uint16(BoTW.twLocationIndex(id(1), It - pointRepeatability(td) : previousIt ) ~= 1); 108 | BoTW.lamda(1, (It - pointRepeatability(td)) : previousIt) = BoTW.lamda(1, (It - pointRepeatability(td)) : previousIt) + locationsToAdd; 109 | % track word to location binary indexing 110 | BoTW.twLocationIndex(id(1), It - pointRepeatability(td) : previousIt) = true; 111 | % decrease the number of counter 112 | twCounter = twCounter-1; 113 | % increase the number of deleted words 114 | BoTW.deleted = BoTW.deleted + 1; 115 | else 116 | % first image where the point is observed //BoTW.twIndex(twCounter, 1) = It - pointRepeatability(tw); 117 | % last image where the feature is observed // BoTW.twIndex(twCounter, 2) = previous; 118 | % tracked word's repeatability // BoTW.twIndex(twCounter, 3) 119 | BoTW.twIndex(twCounter, :) = [(It - pointRepeatability(td)), previousIt, pointRepeatability(td)]; 120 | % track word to location binary indexing 121 | BoTW.twLocationIndex(twCounter, It - pointRepeatability(td) : previousIt) = true; 122 | % points correspond to the generated tracked word 123 | BoTW.trackedWordPoints{twCounter} = trackedPointsBag{td}; 124 | % descriptors correspond to the generated tracked word 125 | BoTW.trackedWordDescriptors{twCounter} = trackedDescriptorsBag{td}; 126 | % how many tracked words each location generates 127 | BoTW.lamda(1, (It - pointRepeatability(td)) : previousIt) = BoTW.lamda(1, (It - pointRepeatability(td)) : previousIt) + 1; 128 | end 129 | else 130 | % first image where the point is observed //BoTW.twIndex(twCounter, 1) = It - pointRepeatability(tw); 131 | % last image where the feature is observed // BoTW.twIndex(twCounter, 2) = previous; 132 | % tracked word's repeatability // BoTW.twIndex(twCounter, 3) 133 | BoTW.twIndex(twCounter, :) = [(It - pointRepeatability(td)), previousIt, pointRepeatability(td)]; 134 | % track word to location binary indexing 135 | BoTW.twLocationIndex(twCounter, It - pointRepeatability(td) : previousIt) = true; 136 | % points correspond to the generated tracked word 137 | BoTW.trackedWordPoints{twCounter} = trackedPointsBag{td}; 138 | % descriptors correspond to the generated tracked word 139 | BoTW.trackedWordDescriptors{twCounter} = trackedDescriptorsBag{td}; 140 | % how many tracked words each location generates 141 | BoTW.lamda(1, BoTW.twIndex(twCounter, 1) : BoTW.twIndex(twCounter, 2)) = BoTW.lamda(1, BoTW.twIndex(twCounter, 1) : BoTW.twIndex(twCounter, 2)) + 1; 142 | end 143 | end 144 | 145 | % replacement of point that either became TW or just lose its track 146 | if trackObservation(td) == false && newPointCounter < size(pointsToSearch, 1) 147 | newPointCounter = newPointCounter + 1; 148 | % new point addition 149 | BoTW.queryPoints{It}(td, :) = pointsToSearch(newPointCounter, : ); 150 | % new query descriptor addition 151 | BoTW.queryDescriptors{It}(td, :) = descriptorsToSearch(newPointCounter, : ); 152 | % new point addition 153 | trackedPointsBag{td} = pointsToSearch(newPointCounter, : ); 154 | % new descriptor addition 155 | trackedDescriptorsBag{td} = descriptorsToSearch(newPointCounter, : ); 156 | % initialization new point representation 157 | pointRepeatability(td) = 1; 158 | % in cases where the plane is not textured enough 159 | elseif trackObservation(td) == false && newPointCounter >= size(pointsToSearch, 1) && size(BoTW.queryPoints{It}, 1) >= td 160 | deletion = true; 161 | pointsToDelete(td) = true; 162 | end 163 | end 164 | 165 | if deletion == true 166 | BoTW.queryPoints{It}((pointsToDelete == true), :) = []; 167 | BoTW.queryDescriptors{It}((pointsToDelete == true), :) = []; 168 | trackedPointsBag((pointsToDelete == true), :) = []; 169 | trackedDescriptorsBag((pointsToDelete == true), :) = []; 170 | pointRepeatability((pointsToDelete == true), :) = []; 171 | pointRepeatability(length(pointRepeatability) + 1 : params.buildingDatabase.numPointsToTrack) = 1; 172 | end 173 | 174 | BoTW.maximumActivePoint(It) = max(pointRepeatability); 175 | else 176 | 177 | % initialization process again 178 | trackObservation = false(params.buildingDatabase.numPointsToTrack, 1); 179 | pointRepeatability = ones(params.buildingDatabase.numPointsToTrack, 1, 'uint16'); 180 | 181 | if size(visualData.points{It}, 1) > params.buildingDatabase.numPointsToTrack 182 | BoTW.queryPoints{It}= visualData.points{It}.Location(1 : params.buildingDatabase.numPointsToTrack, :); 183 | trackedPointsBag = mat2cell(BoTW.queryPoints{It}, ones(1, params.buildingDatabase.numPointsToTrack)); 184 | BoTW.queryDescriptors{It} = visualData.features{It}(1 : params.buildingDatabase.numPointsToTrack, :); 185 | trackedDescriptorsBag = mat2cell(BoTW.queryDescriptors{It}, ones(1, params.buildingDatabase.numPointsToTrack)); 186 | % in case the image texture can not generate enough visual local features 187 | elseif size(visualData.points{It}, 1) < params.buildingDatabase.numPointsToTrack ... 188 | && size(visualData.points{It}, 1) > params.queryingDatabase.inliersTheshold 189 | BoTW.queryPoints{It}= visualData.points{It}.Location(1 : size(visualData.points{It}, 1), :); 190 | trackedPointsBag = mat2cell(BoTW.queryPoints{It}, ones(1, size(visualData.points{It}, 1))); 191 | BoTW.queryDescriptors{It} = visualData.features{It}(1 : size(visualData.features{It}, 1), :); 192 | trackedDescriptorsBag = mat2cell(BoTW.queryDescriptors{It}, ones(1, size(visualData.features{It}, 1))); 193 | end 194 | 195 | end 196 | end 197 | end 198 | 199 | BoTW.bagOfTrackedWords = BoTW.bagOfTrackedWords(1:twCounter, :); 200 | BoTW.twIndex = BoTW.twIndex(1:twCounter, :); 201 | BoTW.twLocationIndex = BoTW.twLocationIndex(1:twCounter, :); 202 | BoTW.trackedWordPoints = BoTW.trackedWordPoints(1, 1 : twCounter); 203 | BoTW.trackedWordDescriptors = BoTW.trackedWordDescriptors(1, 1 : twCounter); 204 | 205 | % save the generated database if not a file exists 206 | if params.buildingDatabase.save 207 | save('results/buildingDatabase', 'BoTW', 'timer', '-v7.3'); 208 | end 209 | 210 | end 211 | end 212 | -------------------------------------------------------------------------------- /matlab/forwardHMM.m: -------------------------------------------------------------------------------- 1 | % 2 | 3 | % Copyright 2020, Konstantinos A. Tsintotas 4 | % ktsintot@pme.duth.gr 5 | % 6 | % This file is part of BoTW-LCD framework for visual loop closure detection 7 | % 8 | % BoTW-LCD framework is free software: you can redistribute 9 | % it and/or modify it under the terms of the MIT License as 10 | % published by the corresponding authors. 11 | % 12 | % BoTW-LCD pipeline is distributed in the hope that it will be 13 | % useful, but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | % MIT License for more details. 16 | 17 | function [HMMresults] = forwardHMM(HMMresults, params, It) 18 | 19 | % observation along the trajectory 20 | Y = HMMresults.observations(It); 21 | Y = [length(Y) + 1, Y]; 22 | fs = zeros(params.queryingDatabase.numStates, length(Y)); 23 | 24 | % apriori state S_t-1 25 | if sum(HMMresults.forwardProb(:, It - 1)) == 0 26 | fs(1, 1) = 1; 27 | else 28 | fs(:, 1) = HMMresults.forwardProb(:, It - 1); 29 | end 30 | 31 | % FILTERING based on the forward algorithm 32 | for i = 2 : length(Y) 33 | for state = 1 : params.queryingDatabase.numStates 34 | fs(state, i) = params.queryingDatabase.HMM.EMIS(state, Y(i)) .* (sum(fs(:, i-1) .* params.queryingDatabase.HMM.TRANS(:, state))); 35 | end 36 | fs(:, i) = fs(:, i)./sum(fs(:, i)); 37 | end 38 | 39 | % probabilities registration 40 | HMMresults.forwardProb(:, It) = fs(:, length(Y)); 41 | % loop closure state 42 | [~, HMMresults.loopStates(It)] = max(HMMresults.forwardProb(: , It)); 43 | 44 | end 45 | -------------------------------------------------------------------------------- /matlab/geometricalCheck.m: -------------------------------------------------------------------------------- 1 | % 2 | 3 | % Copyright 2020, Konstantinos A. Tsintotas 4 | % ktsintot@pme.duth.gr 5 | % 6 | % This file is part of BoTW-LCD framework for visual loop closure detection 7 | % 8 | % BoTW-LCD framework is free software: you can redistribute 9 | % it and/or modify it under the terms of the MIT License as 10 | % published by the corresponding authors. 11 | % 12 | % BoTW-LCD pipeline is distributed in the hope that it will be 13 | % useful, but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | % MIT License for more details. 16 | 17 | function [properImage, inliersTotal] = geometricalCheck(It, iBoTW, params, candidate, visualData) 18 | 19 | properImage = uint16(0); 20 | inliersTotal = uint16(0); 21 | acceptedImage = false; 22 | i = uint16(0); 23 | 24 | if length(candidate) > 10 25 | iterations = 10; 26 | else 27 | iterations = length(candidate); 28 | end 29 | 30 | while acceptedImage == false 31 | 32 | if i < iterations 33 | i = i + 1; 34 | elseif i <= iterations && acceptedImage == false 35 | acceptedImage = true; 36 | end 37 | 38 | indexPairs = matchFeatures(iBoTW.queryDescriptors{It}, ... 39 | visualData.features{candidate(i)}, 'Unique', true, 'Method', 'Exhaustive', 'MatchThreshold', 10.0, 'MaxRatio', params.queryingDatabase.maxRatio); 40 | 41 | if size(indexPairs, 1) >= params.queryingDatabase.inliersTheshold 42 | matchedPoints1 = iBoTW.queryPoints{It}(indexPairs(:, 1), :); 43 | matchedPoints2 = visualData.points{candidate(i)}.Location(indexPairs(:, 2), :); 44 | 45 | try 46 | [~, inliersIndex, ~] = estimateFundamentalMatrix(matchedPoints1, matchedPoints2, ... 47 | 'Method', 'RANSAC', 'DistanceType', 'Algebraic', 'DistanceThreshold', 1); 48 | if sum(inliersIndex) >= params.queryingDatabase.inliersTheshold 49 | properImage = candidate(i); 50 | inliersTotal = sum(inliersIndex); 51 | acceptedImage = true; 52 | end 53 | catch 54 | end 55 | else 56 | continue 57 | end 58 | end 59 | end 60 | -------------------------------------------------------------------------------- /matlab/guidedFeatureSelection.m: -------------------------------------------------------------------------------- 1 | % 2 | 3 | % Copyright 2020, Konstantinos A. Tsintotas 4 | % ktsintot@pme.duth.gr 5 | % 6 | % This file is part of BoTW-LCD framework for visual loop closure detection 7 | % 8 | % BoTW-LCD framework is free software: you can redistribute 9 | % it and/or modify it under the terms of the MIT License as 10 | % published by the corresponding authors. 11 | % 12 | % BoTW-LCD pipeline is distributed in the hope that it will be 13 | % useful, but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | % MIT License for more details. 16 | 17 | function [pointsFedtoTracker, pointsDescriptors, pointRepeatability, trackObservation, pointsToSearch, descriptorsToSearch, trackedPointsBag, trackedDescriptorsBag, timer] = ... 18 | guidedFeatureSelection(params, visualData, previousImg, It, pointsFedtoTracker, pointsDescriptors, trackedPoints, trackedPointsValidity, pointRepeatability, trackedPointsBag, trackedDescriptorsBag, trackObservation, timer) 19 | 20 | pointsToSearch = single(visualData.points{It}.Location); 21 | descriptorsToSearch = visualData.features{It}; 22 | excludedPoint = int16([]); 23 | % allocate the timer 24 | guidedFeatureSelectionTiming = zeros(1, params.buildingDatabase.numPointsToTrack,'single'); 25 | 26 | for j = 1 : params.buildingDatabase.numPointsToTrack 27 | 28 | % exclusion of point that used before from guided point detection in order a duplicate to be avoided 29 | pointsToSearch(excludedPoint, :) = []; 30 | % exclusion of descriptor that used before from guided feature detection in order a duplicate to be avoided 31 | descriptorsToSearch(excludedPoint, :) = []; 32 | 33 | % start the timer for the guided feature selection 34 | tic 35 | 36 | % check if point of the previous image is tracked in the current and if the number of points are lower the desired 37 | if j <= size(trackedPointsValidity, 1) && trackedPointsValidity(j) == 1 && ~isempty(pointsToSearch) 38 | 39 | % nearest neighbor index and points' distance between the tracked point "tp" and SURF points "SP" in I(t) 40 | [IdxNN, pointsDist] = knnsearch(pointsToSearch, trackedPoints(j, :), 'K', 1, 'NSMethod', 'kdtree'); 41 | % SURF Points nearest neighbor in order to find the appropriate descriptor in previous image 42 | [p, ~] = knnsearch(visualData.points{previousImg}.Location, pointsFedtoTracker(j, :), 'K', 1, 'NSMethod', 'kdtree' ); 43 | % descriptors' distance 44 | descriptorsDist= norm(visualData.features{previousImg}(p, :) - descriptorsToSearch(IdxNN, :)); 45 | 46 | % two conditions for acceptance of a tracked point 47 | if pointsDist < params.buildingDatabase.pointsDist && descriptorsDist < params.buildingDatabase.descriptorsDist 48 | % accepted point and descriptor 49 | trackObservation(j) = true; 50 | % to maintain the correct point detected in the current image 51 | pointsFedtoTracker(j, :) = pointsToSearch(IdxNN, :); 52 | % adding points in order to be used for geometrical procedure 53 | trackedPointsBag{j} = [trackedPointsBag{j}; pointsFedtoTracker(j, :)]; 54 | % to maintain the correct descriptor detected in the current image 55 | pointsDescriptors(j, :) = descriptorsToSearch(IdxNN, :); 56 | % adding descriptors in order to be transformed at Tracked Word and also used for geometrical procedure 57 | trackedDescriptorsBag{j} = [trackedDescriptorsBag{j}; pointsDescriptors(j, :)]; 58 | % point repeatability along consecutive images 59 | pointRepeatability(j) = pointRepeatability(j) + 1; 60 | % point to be deleted from the repetitive function 61 | excludedPoint = IdxNN; 62 | else 63 | trackObservation(j) = false; 64 | excludedPoint = []; 65 | end 66 | else 67 | trackObservation(j) = false; 68 | excludedPoint = []; 69 | end 70 | 71 | % stop the timer for the guided feature selection 72 | guidedFeatureSelectionTiming(1, j) = toc; 73 | 74 | end 75 | 76 | guidedFeatureSelectionTiming = mean(guidedFeatureSelectionTiming); 77 | timer.guidedFeatureSelection(It, 1) = guidedFeatureSelectionTiming; 78 | 79 | % exclusion of points and descriptors that used before from guided tracking device for avoidance of duplicate at the new points addition 80 | pointsToSearch(excludedPoint, :) = []; 81 | descriptorsToSearch(excludedPoint, :) = []; 82 | 83 | end 84 | -------------------------------------------------------------------------------- /matlab/incomingVisualData.m: -------------------------------------------------------------------------------- 1 | % 2 | 3 | % Copyright 2020, Konstantinos A. Tsintotas 4 | % ktsintot@pme.duth.gr 5 | % 6 | % This file is part of BoTW-LCD framework for visual loop closure detection 7 | % 8 | % BoTW-LCD framework is free software: you can redistribute 9 | % it and/or modify it under the terms of the MIT License as 10 | % published by the corresponding authors. 11 | % 12 | % BoTW-LCD pipeline is distributed in the hope that it will be 13 | % useful, but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | % MIT License for more details. 16 | 17 | function [visualData, timer] = incomingVisualData(params, dataPath, dataFormat) 18 | 19 | % shall we load the visual information and its' extracted variables? 20 | if params.visualData.load == true && exist('results/visualData.mat', 'file') 21 | load('results/visualData.mat'); 22 | else 23 | % list the dataset's images 24 | images = dir([dataPath dataFormat]); 25 | % fields to be removed from images' structure 26 | fields = {'folder','date','bytes','isdir','datenum'}; 27 | images = rmfield(images, fields); 28 | % the total number of the incomming visual sensory information 29 | visualData.imagesLoaded = uint16(size(images, 1)); 30 | 31 | % % uncomment for downsampled New College dataset 32 | % images(1 : 2 : size(images,1)) = []; % Extracting the left camera measurements 33 | % images = images(1 : 20 : size(images, 1)); 34 | % visualData.imagesLoaded = int16(size(images,1)); 35 | 36 | % % uncomment for City Centre dataset 37 | % images(2 : 2 : size(images,1)) = []; % Extracting the left camera measurements 38 | % visualData.imagesLoaded = uint16(size(images,1)); 39 | 40 | % pre-allocation of images' space 41 | visualData.inputImage = cell(1, visualData.imagesLoaded); 42 | % pre-allocation of images' descriptors space 43 | visualData.features = cell(1, visualData.imagesLoaded); 44 | % pre-allocation of images' points space 45 | visualData.points = cell(1, visualData.imagesLoaded); 46 | 47 | % pre-allocation of timer for feature detection 48 | timer.featuresDetection = zeros(visualData.imagesLoaded, 1, 'single'); 49 | % pre-allocation of timer for feature description 50 | timer.featuresDescription = zeros(visualData.imagesLoaded, 1, 'single'); 51 | 52 | for It = 1 : visualData.imagesLoaded 53 | 54 | % display the current frame 55 | disp(It) 56 | % read the incoming camera measurement 57 | visualData.inputImage{It} = imread([dataPath images(It).name]); 58 | 59 | % if the input data is RGB, then convert it to a grayscale one 60 | if size(visualData.inputImage{It}, 3) == 3 61 | visualData.inputImage{It} = rgb2gray(visualData.inputImage{It}); 62 | end 63 | 64 | % start the timer for the points' detection 65 | tic 66 | % points detection 67 | visualData.points{It} = detectSURFFeatures(visualData.inputImage{It}, 'MetricThreshold', params.incomingVisualData.featuresResponse); 68 | visualData.points{It} = visualData.points{It}.selectStrongest(params.incomingVisualData.strongest); 69 | % stop the timer for the points' detection 70 | timer.featuresDetection(It, 1) = toc; 71 | 72 | % start the timer for the points' description 73 | tic 74 | % Points' description 75 | [visualData.features{It}, visualData.points{It}] = ... 76 | extractFeatures(visualData.inputImage{It}, visualData.points{It}, 'Method', 'Auto', 'FeatureSize', params.incomingVisualData.descriptorDimension); 77 | % stop the timer for the points' description 78 | timer.featuresDescription(It, 1) = toc; 79 | end 80 | 81 | % save the variables if not they not allready exist 82 | if params.visualData.save 83 | save('results/visualData', 'visualData', 'timer', '-v7.3'); 84 | end 85 | 86 | end 87 | end 88 | -------------------------------------------------------------------------------- /matlab/initializationBoTW.m: -------------------------------------------------------------------------------- 1 | % 2 | 3 | % Copyright 2020, Konstantinos A. Tsintotas 4 | % ktsintot@pme.duth.gr 5 | % 6 | % This file is part of BoTW-LCD framework for visual loop closure detection 7 | % 8 | % BoTW-LCD framework is free software: you can redistribute 9 | % it and/or modify it under the terms of the MIT License as 10 | % published by the corresponding authors. 11 | % 12 | % BoTW-LCD pipeline is distributed in the hope that it will be 13 | % useful, but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | % MIT License for more details. 16 | 17 | function BoTW = initializationBoTW(visualData, params) 18 | 19 | % the visual dictionary 20 | BoTW.bagOfTrackedWords = single(zeros(100000, params.incomingVisualData.descriptorDimension)); 21 | % tracked words' indexing 22 | BoTW.twIndex = uint16(zeros(100000, 3)); 23 | % tracked words' location indexing 24 | BoTW.twLocationIndex = false(100000, size(visualData.inputImage, 2)); 25 | % the deteled words 26 | BoTW.deleted = uint16(0); 27 | % lamda is counting the number of tracked words belonging to the traversed location 28 | BoTW.lamda = zeros(1, size(visualData.inputImage, 2), 'uint16'); 29 | % maximum active point variable for the query process 30 | BoTW.maximumActivePoint = zeros(1, size(visualData.inputImage, 2), 'uint16'); 31 | % query points 32 | BoTW.queryPoints = cell(1, visualData.imagesLoaded); 33 | % query descriptors 34 | BoTW.queryDescriptors = cell(1, visualData.imagesLoaded); 35 | 36 | end 37 | -------------------------------------------------------------------------------- /matlab/locationDefinition.m: -------------------------------------------------------------------------------- 1 | % 2 | 3 | % Copyright 2020, Konstantinos A. Tsintotas 4 | % ktsintot@pme.duth.gr 5 | % 6 | % This file is part of BoTW-LCD framework for visual loop closure detection 7 | % 8 | % BoTW-LCD framework is free software: you can redistribute 9 | % it and/or modify it under the terms of the MIT License as 10 | % published by the corresponding authors. 11 | % 12 | % BoTW-LCD pipeline is distributed in the hope that it will be 13 | % useful, but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | % MIT License for more details. 16 | 17 | function [properImage, inliersTotal, HMMresults, timer] = locationDefinition(params, HMMresults, matches, candidateLocationsScores, It, iBoTW, visualData, timer) 18 | 19 | 20 | properImage = uint16(0); 21 | inliersTotal = uint16(0); 22 | 23 | % HMM observation == 2 24 | if HMMresults.observations(It) == 2 25 | candidates = find(candidateLocationsScores); 26 | if length(candidates) < 10 27 | [~,idxx] = sort(candidateLocationsScores(candidates), 'ascend'); 28 | candidates = candidates(idxx(1)); 29 | firstImg = max(1, candidates - params.queryingDatabase.locationRange); 30 | lastImg = candidates + params.queryingDatabase.locationRange; 31 | candidateLocationsScores = matches.binomialMatrix(It, firstImg : lastImg); 32 | [~,idxx] = sort(candidateLocationsScores, 'ascend'); 33 | candidates = firstImg : lastImg; 34 | candidates = candidates(idxx); 35 | c = matches.binomialMatrix(It, (candidates))>0; 36 | candidates = candidates(c ==true); 37 | else 38 | [~,idxx] = sort(candidateLocationsScores(candidates), 'ascend'); 39 | candidates = candidates(idxx); 40 | end 41 | if ~isempty(candidates) 42 | % start the timer for the geometrical verification 43 | tic 44 | [properImage, inliersTotal] = geometricalCheck(It, iBoTW, params, candidates, visualData); 45 | % stop the timer for the geometrical verification 46 | timer.geometricalVerification(It, 1) = toc; 47 | end 48 | 49 | % HMM observation == 1 50 | elseif HMMresults.observations(It) == 1 && matches.matches(It-1) ~= 0 51 | firstImg = max(1, matches.matches(It-1) - params.queryingDatabase.locationRange); 52 | lastImg = matches.matches(It-1) + params.queryingDatabase.locationRange; 53 | candidateLocationsScores = matches.binomialMatrix(It, firstImg : lastImg); 54 | [~,idxx] = sort(candidateLocationsScores, 'ascend'); 55 | candidates = firstImg : lastImg; 56 | candidates = candidates(idxx); 57 | c = matches.binomialMatrix(It, (candidates))>0; 58 | candidates = candidates(c ==true); 59 | % start the timer for the geometrical verification 60 | if ~isempty(candidates) 61 | tic 62 | [properImage, inliersTotal] = geometricalCheck(It, iBoTW, params, candidates, visualData); 63 | % stop the timer for the geometrical verification 64 | timer.geometricalVerification(It, 1) = toc; 65 | end 66 | end 67 | end 68 | -------------------------------------------------------------------------------- /matlab/main.m: -------------------------------------------------------------------------------- 1 | % 2 | 3 | % Copyright 2020, Konstantinos A. Tsintotas 4 | % ktsintot@pme.duth.gr 5 | % 6 | % This file is part of BoTW-LCD framework for visual loop closure detection 7 | % 8 | % BoTW-LCD framework is free software: you can redistribute 9 | % it and/or modify it under the terms of the MIT License as 10 | % published by the corresponding authors. 11 | % 12 | % BoTW-LCD pipeline is distributed in the hope that it will be 13 | % useful, but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | % MIT License for more details. 16 | 17 | clear all; close all; 18 | 19 | dataPath = ('images path\'); % e.g., myDatasetImages\ 20 | dataFormat = '*.png'; % e.g., for png input data 21 | 22 | % parameters' definitions 23 | params = parametersDefinition(); 24 | % extraction of visual sensory information 25 | [visualData, timer] = incomingVisualData(params, dataPath, dataFormat); 26 | % dataset's frame rate definition 27 | visualData.frameRate = %; 28 | % timers memory allocation 29 | timer = timersInitialization(visualData, timer); 30 | % 1) trajectory mapping 31 | [BoTW, timer] = buildingDatabase(visualData, params, timer); 32 | % 2) the query procedure 33 | [matches, HMMresults, iBoTW, timer] = queryingDatabaseHMM(params, visualData, BoTW, timer); 34 | % method's evaluation 35 | close all; 36 | results = methodEvaluation(params, matches, groundTruth); 37 | -------------------------------------------------------------------------------- /matlab/matchesInitialization.m: -------------------------------------------------------------------------------- 1 | % 2 | 3 | % Copyright 2020, Konstantinos A. Tsintotas 4 | % ktsintot@pme.duth.gr 5 | % 6 | % This file is part of BoTW-LCD framework for visual loop closure detection 7 | % 8 | % BoTW-LCD framework is free software: you can redistribute 9 | % it and/or modify it under the terms of the MIT License as 10 | % published by the corresponding authors. 11 | % 12 | % BoTW-LCD pipeline is distributed in the hope that it will be 13 | % useful, but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | % MIT License for more details. 16 | 17 | function matches = matchesInitialization(visualData, params) 18 | 19 | % votes' matrix to visualize the voting procedure 20 | matches.votesMatrix = zeros(visualData.imagesLoaded, visualData.imagesLoaded, 'uint16'); 21 | % binomial matrix to visualize the generated probabilities 22 | matches.binomialMatrix = zeros(visualData.imagesLoaded, visualData.imagesLoaded); 23 | % generated loop closure matrix by the framework 24 | matches.loopClosureMatrix = false(visualData.imagesLoaded, visualData.imagesLoaded); 25 | % image to image correspondence 26 | matches.matches = zeros(visualData.imagesLoaded, 1, 'single'); 27 | % inliers from geometrical check of the selected image 28 | matches.inliers = zeros(visualData.imagesLoaded, 1, 'uint16'); 29 | % hold the nearest neighbor of each query descriptor to the vocabulary 30 | matches.knnIDx = zeros(params.buildingDatabase.numPointsToTrack, visualData.imagesLoaded, 'uint16'); 31 | 32 | end 33 | -------------------------------------------------------------------------------- /matlab/methodEvaluation.m: -------------------------------------------------------------------------------- 1 | % 2 | 3 | % Copyright 2020, Konstantinos A. Tsintotas 4 | % ktsintot@pme.duth.gr 5 | % 6 | % This file is part of BoTW-LCD framework for visual loop closure detection 7 | % 8 | % BoTW-LCD framework is free software: you can redistribute 9 | % it and/or modify it under the terms of the MIT License as 10 | % published by the corresponding authors. 11 | % 12 | % BoTW-LCD pipeline is distributed in the hope that it will be 13 | % useful, but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | % MIT License for more details. 16 | 17 | function results = methodEvaluation(params, matches, groundTruthMatrix) 18 | 19 | results = zeros(1, 6); 20 | 21 | loopClosureMatrix = matches.loopClosureMatrix; 22 | 23 | if params.visualizationResults == true 24 | figure('IntegerHandle','on','Name','Loop Closure Matrix'); 25 | spy(loopClosureMatrix); 26 | end 27 | 28 | if params.visualizationResults == true 29 | figure('IntegerHandle','on','Name','Ground Truth Matrix'); 30 | spy(groundTruthMatrix); 31 | end 32 | 33 | % calculation the sum of true positives 34 | groundTruthNumber = sum(sum(groundTruthMatrix')>0); 35 | 36 | % calculation of True Positives 37 | % logical AND between ground truth and loop closure matrix for true positives 38 | tempTP = logical (groundTruthMatrix.*loopClosureMatrix); 39 | tempSumTP = sum(tempTP, 2); 40 | truePositives = sum(tempSumTP, 1); 41 | if params.visualizationResults == true 42 | figure('IntegerHandle','on','Name','True Positives'); 43 | spy(tempTP); 44 | end 45 | 46 | % calculation of False Positives 47 | % logical AND between opposite ground truth and loop closure matrix for false positives 48 | tempFP = logical (loopClosureMatrix.* not(groundTruthMatrix)); 49 | tempSumFP = sum(tempFP, 2); 50 | falsePositives = sum(tempSumFP, 1); 51 | if params.visualizationResults == true 52 | figure('IntegerHandle','on','Name','False Positives'); 53 | spy(tempFP); 54 | end 55 | 56 | % calculation of Precision - Recall 57 | precisionScore = truePositives / (truePositives + falsePositives); 58 | recallScore = truePositives / groundTruthNumber; 59 | 60 | % Results 61 | results(1, 1) = precisionScore; 62 | results(1, 2) = recallScore; 63 | results(1, 3) = int16(truePositives); 64 | results(1, 4) = int16(falsePositives); 65 | results(1, 5) = params.queryingDatabase.observationThreshold; 66 | results(1, 6) = groundTruthNumber; 67 | 68 | end 69 | -------------------------------------------------------------------------------- /matlab/parametersDefinition.m: -------------------------------------------------------------------------------- 1 | % 2 | 3 | % Copyright 2020, Konstantinos A. Tsintotas 4 | % ktsintot@pme.duth.gr 5 | % 6 | % This file is part of BoTW-LCD framework for visual loop closure detection 7 | % 8 | % BoTW-LCD framework is free software: you can redistribute 9 | % it and/or modify it under the terms of the MIT License as 10 | % published by the corresponding authors. 11 | % 12 | % BoTW-LCD pipeline is distributed in the hope that it will be 13 | % useful, but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | % MIT License for more details. 16 | 17 | function params = parametersDefinition() 18 | 19 | % incomingVisualData 20 | params.visualData.load = true; 21 | params.visualData.save = true; 22 | % points' response, Phi (incomingVisualData) 23 | params.incomingVisualData.featuresResponse = 400.0; 24 | % strongest points to hold, Phi (incomingVisualData) 25 | params.incomingVisualData.strongest = 1000; 26 | % descriptor dimension (incomingVisualData) 27 | params.incomingVisualData.descriptorDimension = uint8(64); 28 | 29 | % buildingDatabase 30 | params.buildingDatabase.load = true; 31 | params.buildingDatabase.save = true; 32 | % number of maximum points fed into the tracker, ni (buildingDatabase) 33 | params.buildingDatabase.numPointsToTrack = uint16(150); 34 | % minimum track word length, rho (buildingDatabase) 35 | params.buildingDatabase.trackLength = uint8(4); 36 | % minimum pointss distance, alpha (buildingDatabase) 37 | params.buildingDatabase.pointsDist = single(5); 38 | % minimum descriptors distance, vita (buildingDatabase) 39 | params.buildingDatabase.descriptorsDist = single(0.6); 40 | % minimum words sampling ratio (buildingDatabase) 41 | params.buildingDatabase.wordsRatio = single(0.5); 42 | 43 | % queryingDatabase 44 | params.queryingDatabase.load = true; 45 | params.queryingDatabase.save = true; 46 | % loop closure threshold, th (queryingDatabase) 47 | params.queryingDatabase.observationThreshold = 2e-9; 48 | % transition matrix states (queryingDatabase) 49 | params.queryingDatabase.numStates = 2; 50 | % hidden markov model transition matrix (queryingDatabase) 51 | params.queryingDatabase.HMM.TRANS = [ 0.975 0.025 ; 0.025 0.975]; 52 | % hidden markov model emission matrix (queryingDatabase) 53 | params.queryingDatabase.HMM.EMIS = [1 0 ; 0.46 0.54]; 54 | % temporal consistency locations' range (queryingDatabase) 55 | params.queryingDatabase.locationRange = 5; 56 | 57 | % feature matching max ration (queryingDatabase) 58 | params.queryingDatabase.maxRatio = 0.4; 59 | % RANSAC inliers, phi (queryingDatabase) 60 | params.queryingDatabase.inliersTheshold = uint16(8); 61 | 62 | % tracked words' maximum distance (queryingDatabase) 63 | params.queryingDatabase.wordsDist = 0.4; 64 | % tracked words' correspondence (queryingDatabase) 65 | params.queryingDatabase.wordsCorrespondence = 0.5; 66 | 67 | % evaluation visualization results 68 | params.visualizationResults = true; 69 | 70 | end 71 | -------------------------------------------------------------------------------- /matlab/queryingDatabaseHMM.m: -------------------------------------------------------------------------------- 1 | % 2 | 3 | % 4 | 5 | % Copyright 2020, Konstantinos A. Tsintotas 6 | % ktsintot@pme.duth.gr 7 | % 8 | % This file is part of BoTW-LCD framework for visual loop closure detection 9 | % 10 | % BoTW-LCD framework is free software: you can redistribute 11 | % it and/or modify it under the terms of the MIT License as 12 | % published by the corresponding authors. 13 | % 14 | % BoTW-LCD pipeline is distributed in the hope that it will be 15 | % useful, but WITHOUT ANY WARRANTY; without even the implied warranty of 16 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 17 | % MIT License for more details. 18 | 19 | function [matches, HMMresults, BoTWnew, timer] = queryingDatabaseProposed(params, visualData, BoTW, timer) 20 | 21 | if params.queryingDatabase.load == true && exist('results/queryingDatabaseHMM.mat', 'file') 22 | load('results/queryingDatabaseHMM.mat'); 23 | 24 | else 25 | 26 | % copy the visual dictionary in order a comparison to be permitted 27 | BoTWnew = BoTW; 28 | % memory allocation for system's outputs 29 | matches = matchesInitialization(visualData, params); 30 | % memory allocation for system's results 31 | HMMresults = HMMinitialization(visualData); 32 | 33 | for It = uint16(1 : visualData.imagesLoaded) 34 | 35 | disp(It); 36 | 37 | % SEARCHING THE DATABASE 38 | % excluding the vocabulary area which would be avoided 39 | if BoTWnew.maximumActivePoint(It) < visualData.frameRate 40 | lastDatabaseLocation = It - ceil(4 * visualData.frameRate); 41 | else 42 | lastDatabaseLocation = It - ceil(4 * BoTWnew.maximumActivePoint(It) ); 43 | end 44 | if lastDatabaseLocation > 0 45 | databaseIndexTemp = find(BoTWnew.twIndex(:, 2) <= lastDatabaseLocation); 46 | if ~isempty(databaseIndexTemp) 47 | databaseIndexTemp = databaseIndexTemp(end); 48 | % visual vocabulary to be searched 49 | database = BoTWnew.bagOfTrackedWords(1 : databaseIndexTemp, :); 50 | else 51 | database = single([]); 52 | end 53 | else 54 | database = single([]); 55 | end 56 | 57 | % vote aggregation 58 | if ~isempty(database) && size(BoTWnew.queryDescriptors{It}, 1) > params.queryingDatabase.inliersTheshold 59 | % k-NN search using only the CPU 60 | 61 | % start the timer for the database search 62 | tic 63 | queryIdxNN = single(knnsearch(database, BoTWnew.queryDescriptors{It}, 'K', 1, 'NSMethod', 'exhaustive')); 64 | % stop the timer for the database search 65 | timer.databaseSearch(It, 1) = toc; 66 | 67 | % knn Indexing for the correspondance at visual vocabulary management 68 | matches.knnIDx(1 : length(queryIdxNN), It) = queryIdxNN; 69 | 70 | % start the timer for the votes' distribution 71 | tic 72 | % votes distribution through the Nearest Neighbor procedure 73 | for v = uint16(1 : length(queryIdxNN)) 74 | votedLocations = uint16(find(BoTWnew.twLocationIndex(queryIdxNN(v), 1 : lastDatabaseLocation) == true)); 75 | matches.votesMatrix(It, votedLocations) = matches.votesMatrix(It, votedLocations) + 1; 76 | end 77 | % stop the timer for the votes' distribution 78 | timer.votesDistribution(It, 1) = toc; 79 | 80 | % NAVIGATION USING PROBABILISTIC SCORING 81 | % images which gather votes 82 | imagesForBinomial = uint16(find(matches.votesMatrix(It, 1 : lastDatabaseLocation) >= 0.01 * size(BoTWnew.queryDescriptors{It}, 1))); 83 | % start the timer for the binomial scoring 84 | tic 85 | % locations which pass the two conditions 86 | candidateLocationsObservation = zeros(1, lastDatabaseLocation, 'double'); 87 | % number of Tracked Words within the searching area 88 | LAMDA = databaseIndexTemp; 89 | % number of query’s Tracked Points (number of points after the guided feature-detection) 90 | N = size(BoTWnew.queryDescriptors{It}, 1); 91 | % number of accumulated votes of database location l 92 | xl = double(matches.votesMatrix(It, imagesForBinomial)); 93 | % number of TWs members in l over the size of the BoTW list (without the rejected locations) 94 | p = double(BoTWnew.lamda(imagesForBinomial)) / LAMDA; 95 | % distribution’s expected value 96 | expectedValue = N*p; 97 | % probability computation for the selected images in the database 98 | locationProbability = binopdf(xl, N , p); 99 | % binomial Matrix completion 100 | matches.binomialMatrix(It, imagesForBinomial) = locationProbability; 101 | % the binomial expected value on each location has to 102 | % satisfy two conditions, (1) loop closure threshold and (2) over expected value xl(t) > E [Xi(t)] 103 | Condition2Locations = uint16(find(xl > expectedValue)); 104 | % locations which satisfy condition 2 and condition 1 - observation 3 105 | if ~isempty(Condition2Locations) ... 106 | && ~isempty(find(locationProbability(Condition2Locations) < params.queryingDatabase.observationThreshold, 1)) 107 | candidateLocations = imagesForBinomial(Condition2Locations(locationProbability(Condition2Locations) < params.queryingDatabase.observationThreshold)); 108 | candidateLocationsObservation(candidateLocations) = matches.binomialMatrix(It, candidateLocations); 109 | HMMresults.observations(It) = 2; 110 | end 111 | % stop the timer for the binomial scoring 112 | timer.binomialScoring(It, 1) = toc; 113 | 114 | % MATCHING PROCEDURE 115 | % filtering the binomial through Bayes estimation 116 | 117 | % start the timer for the Bayesian filtering 118 | tic 119 | [HMMresults] = forwardHMM(HMMresults, params, It); 120 | % stop the timer for the binomial scoring 121 | timer.bayesianFiltering(It, 1) = toc; 122 | 123 | % observation 2 and loop closure detection 124 | if HMMresults.loopStates(It) == 2 125 | [properImage, inliersTotal, HMMresults, timer] = ... 126 | locationDefinition(params, HMMresults, matches, candidateLocationsObservation, It, BoTWnew, visualData, timer); 127 | if properImage ~= 0 128 | matches.loopClosureMatrix(It, properImage) = true; 129 | matches.matches(It, 1) = properImage; 130 | matches.matches(It, 2) = matches.binomialMatrix(It, properImage); 131 | matches.inliers(It) = inliersTotal; 132 | end 133 | % vocabulary management 134 | if properImage ~= 0 && matches.loopClosureMatrix(It, properImage) == true 135 | wordsToManage = single(find(BoTWnew.twIndex(:, 2) == It)); 136 | if ~isempty(wordsToManage) 137 | [BoTWnew, timer] = vocabularyManagement(BoTWnew, wordsToManage, It, properImage, matches, params, timer); 138 | end 139 | end 140 | end 141 | end 142 | end 143 | 144 | if params.queryingDatabase.save 145 | % save variables with real valued mapping and no GPU for searching 146 | save('results/queryingDatabaseHMM', 'matches', 'HMMresults', 'BoTWnew','timer'); 147 | end 148 | 149 | end 150 | end 151 | -------------------------------------------------------------------------------- /matlab/timersInitialization.m: -------------------------------------------------------------------------------- 1 | % 2 | 3 | % Copyright 2020, Konstantinos A. Tsintotas 4 | % ktsintot@pme.duth.gr 5 | % 6 | % This file is part of BoTW-LCD framework for visual loop closure detection 7 | % 8 | % BoTW-LCD framework is free software: you can redistribute 9 | % it and/or modify it under the terms of the MIT License as 10 | % published by the corresponding authors. 11 | % 12 | % BoTW-LCD pipeline is distributed in the hope that it will be 13 | % useful, but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | % MIT License for more details. 16 | 17 | function timer = timersInitialization(visualData, timer) 18 | 19 | % memory allocation for timer for feature tracking 20 | timer.trackingPoints = zeros(visualData.imagesLoaded, 1,'single'); 21 | % memory allocation for timer for guided feature selection 22 | timer.guidedFeatureSelection = zeros(visualData.imagesLoaded, 1,'single'); 23 | % memory allocation for timer for brute force database search 24 | timer.databaseSearch = zeros(visualData.imagesLoaded, 1,'single'); 25 | % memory allocation for timer for votes distribution 26 | timer.votesDistribution = zeros(visualData.imagesLoaded, 1,'single'); 27 | % memory allocation for timer for binomial scoring 28 | timer.binomialScoring = zeros(visualData.imagesLoaded, 1,'single'); 29 | % memory allocation for timer for Bayesian filtering 30 | timer.bayesianFiltering = zeros(visualData.imagesLoaded, 1,'single'); 31 | % memory allocation for timer for geometrical verification 32 | timer.geometricalVerification = zeros(visualData.imagesLoaded, 1,'single'); 33 | 34 | end 35 | -------------------------------------------------------------------------------- /matlab/timesBoTW_LCD.m: -------------------------------------------------------------------------------- 1 | sumTiming = zeros(visualData.imagesLoaded, 1,'single'); 2 | 3 | for It = 1 : visualData.imagesLoaded 4 | 5 | sumTiming(It, 1) = timer.featuresDetection(It, 1) + timer.featuresDescription(It, 1) + timer.trackingPoints(It, 1) + timer.guidedFeatureSelection(It, 1) + timer.databaseSearch(It, 1) + ... 6 | timer.binomialScoring(It, 1) + timer.geometricalVerification(It, 1); 7 | 8 | end 9 | 10 | % average time for feature detection %%%%%%%% 11 | candidatesFeaturesDetection = single(find(timer.featuresDetection(:, 1) >= 0)); 12 | avgFeaturesDetection = mean(timer.featuresDetection(candidatesFeaturesDetection, 1)); 13 | standardDeviationFeaturesDetection = std(timer.featuresDetection(candidatesFeaturesDetection, 1)); 14 | % clear vars candidatesFeaturesDetection avgFeaturesDetection standardDeviationFeaturesDetection 15 | 16 | % average time for feature description %%%%%%%% 17 | candidatesFeaturesDescription = single(find(timer.featuresDescription(:, 1) >= 0)); 18 | avgFeaturesDescription = mean(timer.featuresDescription(candidatesFeaturesDescription, 1)); 19 | standardDeviationFeaturesDescription = std(timer.featuresDescription(candidatesFeaturesDescription, 1)); 20 | % clear vars candidatesFeaturesDescription avgFeaturesDescription standardDeviationFeaturesDescription 21 | 22 | % average time for feature tracking %%%%%%%% 23 | candidatesTrackingPoints = single(find(timer.trackingPoints(:, 1) >= 0)); 24 | avgTrackingPoints = mean(timer.trackingPoints(candidatesTrackingPoints, 1)); 25 | standardDeviationTrackingPoints = std(timer.trackingPoints(candidatesTrackingPoints, 1)); 26 | % clear vars candidatesTrackingPoints avgTrackingPoints standardDeviationTrackingPoints 27 | 28 | % average time for guided feature selection %%%%%%%% 29 | candidatesGuidedFeatureSelection = single(find(timer.guidedFeatureSelection(:, 1) >= 0)); 30 | avgGuidedFeatureSelection = mean(timer.guidedFeatureSelection(candidatesGuidedFeatureSelection, 1)); 31 | standardDeviationGuidedFeatureSelection = std(timer.guidedFeatureSelection(candidatesGuidedFeatureSelection, 1)); 32 | % clear vars candidatesGuidedFeatureSelection avgGuidedFeatureSelection standardDeviationGuidedFeatureSelection 33 | 34 | % average time for database search %%%%%%%% 35 | candidatesDatabaseSearch = single(find(timer.databaseSearch(:, 1) >= 0)); 36 | avgDatabaseSearch = mean(timer.databaseSearch(candidatesDatabaseSearch, 1)); 37 | standardDeviationDatabaseSearch = std(timer.databaseSearch(candidatesDatabaseSearch, 1)); 38 | % clear vars candidatesDatabaseSearch avgDatabaseSearch standardDeviationDatabaseSearch 39 | 40 | % average time for binomial scoring %%%%%%%% 41 | candidatesBinomialScoring = single(find(timer.binomialScoring(:, 1)>= 0)); 42 | avgBinomialScoring = mean(timer.binomialScoring(candidatesBinomialScoring, 1)); 43 | standardDeviation = std(timer.binomialScoring(candidatesBinomialScoring, 1)); 44 | % clear vars candidatesBinomialScoring avgBinomialScoring standardDeviation 45 | 46 | % average time for geometrical verification %%%%%%%% 47 | candidatesGeometricalVerification = single(find(timer.geometricalVerification(:, 1) >= 0)); 48 | avgGeometricalVerification = mean(timer.geometricalVerification(candidatesGeometricalVerification, 1)); 49 | standardDeviationGeometricalVerification = std(timer.geometricalVerification(candidatesGeometricalVerification, 1)); 50 | % clear vars candidatesGeometricalVerification avgGeometricalVerification standardDeviationGeometricalVerification 51 | 52 | sumAvg = avgFeaturesDetection + avgFeaturesDescription + avgTrackingPoints + avgGuidedFeatureSelection +... 53 | avgDatabaseSearch + avgBinomialScoring + avgGeometricalVerification + avgWordsUpdate; 54 | -------------------------------------------------------------------------------- /matlab/vocabularyManagement.m: -------------------------------------------------------------------------------- 1 | % 2 | 3 | % Copyright 2020, Konstantinos A. Tsintotas 4 | % ktsintot@pme.duth.gr 5 | % 6 | % This file is part of BoTW-LCD framework for visual loop closure detection 7 | % 8 | % BoTW-LCD framework is free software: you can redistribute 9 | % it and/or modify it under the terms of the MIT License as 10 | % published by the corresponding authors. 11 | % 12 | % BoTW-LCD pipeline is distributed in the hope that it will be 13 | % useful, but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | % MIT License for more details. 16 | 17 | function [BoTWnew, timer] = vocabularyManagement(BoTWnew, wordsToManage, It, properImage, matches, params, timer) 18 | 19 | wordsDist = zeros(length(wordsToManage), 1); 20 | wordsToDelete = single(zeros(1, length(wordsToManage))); 21 | 22 | for w = 1 : length(wordsToManage) 23 | 24 | h = size(BoTWnew.trackedWordDescriptors{wordsToManage(w)}, 1); 25 | % find which descriptor is being tracked from the query points and subsequently is transformed into Tracked Word 26 | id = knnsearch(BoTWnew.queryDescriptors{It}, BoTWnew.trackedWordDescriptors{wordsToManage(w)}(end, :), 'K', 1); 27 | % indicate the correspondances between the generated tracked word and the database matches 28 | votedDatabaseWords = uint16(matches.knnIDx(id, It - h + 1: It)); 29 | % highlight the maximum correspondance database word 30 | votedNonZeros = nonzeros(votedDatabaseWords)'; 31 | [~, idx] = max(sum(votedNonZeros == votedNonZeros')); 32 | idx = find(votedDatabaseWords == votedNonZeros(idx)); 33 | idx = idx(1); 34 | 35 | % comparison between the tracked words 36 | wordsDist(w) = norm(BoTWnew.bagOfTrackedWords(wordsToManage(w), :) - BoTWnew.bagOfTrackedWords(votedDatabaseWords(idx), :)); 37 | 38 | if wordsDist(w) <= params.queryingDatabase.wordsDist && ... 39 | BoTWnew.twLocationIndex(votedDatabaseWords(idx), properImage) == true 40 | 41 | % renew the visual word 42 | BoTWnew.bagOfTrackedWords(votedDatabaseWords(idx), :) = median([BoTWnew.bagOfTrackedWords(votedDatabaseWords(idx), :) ; ... 43 | BoTWnew.trackedWordDescriptors{wordsToManage(w)}], 1 ); 44 | % renew indexing 45 | BoTWnew.twLocationIndex(votedDatabaseWords(idx), :) = ... 46 | or(BoTWnew.twLocationIndex(votedDatabaseWords(idx), :), BoTWnew.twLocationIndex(wordsToManage(w), :)); 47 | % increase the number of merged words 48 | wordsToDelete(w) = wordsToManage(w); 49 | end 50 | 51 | end 52 | 53 | % deleting the generated words which are very similar and are merged 54 | wordsToDelete = wordsToDelete(wordsToDelete>0)'; 55 | BoTWnew.bagOfTrackedWords(wordsToDelete, :) = []; 56 | BoTWnew.twIndex(wordsToDelete, :) = []; 57 | BoTWnew.twLocationIndex(wordsToDelete, :) = []; 58 | BoTWnew.trackedWordPoints(wordsToDelete) = []; 59 | BoTWnew.trackedWordDescriptors(wordsToDelete) = []; 60 | BoTWnew.deleted = [BoTWnew.deleted; wordsToDelete]; 61 | 62 | end 63 | --------------------------------------------------------------------------------