OKVIS ROS
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Pages
DenseMatcher.hpp
Go to the documentation of this file.
1 /*********************************************************************************
2  * OKVIS - Open Keyframe-based Visual-Inertial SLAM
3  * Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich
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  * * Redistributions of source code must retain the above copyright notice,
9  * this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright notice,
11  * this list of conditions and the following disclaimer in the documentation
12  * and/or other materials provided with the distribution.
13  * * Neither the name of Autonomous Systems Lab / ETH Zurich nor the names of
14  * its contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  *
29  * Created on: 2013
30  * Author: Simon Lynen
31  * Modified: Stefan Leutenegger (s.leutenegger@imperial.ac.uk)
32  *********************************************************************************/
33 
41 #include <map>
42 
44 namespace okvis {
45 
46 // This function creates all the matching threads and assigns the best matches afterwards.
47 template<typename MATCHING_ALGORITHM_T>
49  void (DenseMatcher::*doWorkPtr)(MatchJob&, MATCHING_ALGORITHM_T*),
50  MATCHING_ALGORITHM_T& matchingAlgorithm) {
51  // create lock list
52  std::mutex* locks = new std::mutex[matchingAlgorithm.sizeB()];
53 
54  //the pairing list
55  pairing_list_t vpairs;
56  // a list with best matches for each "A" point
57  std::vector<std::vector<pairing_t> > vMyBest;
58 
59  vMyBest.resize(matchingAlgorithm.sizeA());
60 
61  // this point is not paired so far, score max
62  vpairs.resize(matchingAlgorithm.sizeB(),
63  pairing_t(-1, std::numeric_limits<distance_t>::max()));
64 
65  // prepare the jobs for the threads
66  std::vector<MatchJob> jobs(numMatcherThreads_);
67  for (int i = 0; i < numMatcherThreads_; ++i) {
68  jobs[i].iThreadID = i;
69  jobs[i].vpairs = &vpairs;
70  jobs[i].vMyBest = &vMyBest;
71  jobs[i].mutexes = locks;
72  }
73 
74  //create all threads
75  // boost::thread_group matchers;
76  for (int i = 0; i < numMatcherThreads_; ++i) {
77  matcherThreadPool_->enqueue(doWorkPtr, this, jobs[i], &matchingAlgorithm);
78  // matchers.create_thread(boost::bind(doWorkPtr, this, jobs[i], &matchingAlgorithm));
79  }
80 
81  // matchers.join_all();
82  matcherThreadPool_->waitForEmptyQueue();
83 
84  // Looks like running this in one thread is faster than creating 30+ new threads for every image.
85  //TODO(gohlp): distribute this to n threads.
86 
87  // for (int i = 0; i < _numMatcherThreads; ++i)
88  // {
89  // (this->*doWorkPtr)(jobs[i], &matchingAlgorithm);
90  // }
91 
92  matchingAlgorithm.reserveMatches(vpairs.size());
93 
94  // assemble the pairs and return
95  const distance_t& const_distratiothres = matchingAlgorithm.distanceRatioThreshold();
96  const distance_t& const_distthres = matchingAlgorithm.distanceThreshold();
97  for (size_t i = 0; i < vpairs.size(); ++i) {
98  if (useDistanceRatioThreshold_ && vpairs[i].distance < const_distthres) {
99  const std::vector<pairing_t>& best_matches_list =
100  vMyBest[vpairs[i].indexA];
101  OKVIS_ASSERT_TRUE_DBG(Exception, best_matches_list[0].indexA != -1,
102  "assertion failed");
103 
104  if (best_matches_list[1].indexA != -1) {
105  const distance_t& best_match_distance = best_matches_list[0].distance;
106  const distance_t& second_best_match_distance = best_matches_list[1]
107  .distance;
108  // Only assign if the distance ratio better than the threshold.
109  if (best_match_distance == 0
110  || second_best_match_distance / best_match_distance
111  > const_distratiothres) {
112  matchingAlgorithm.setBestMatch(vpairs[i].indexA, i,
113  vpairs[i].distance);
114  }
115  } else {
116  // If there is only one matching feature, we assign it.
117  matchingAlgorithm.setBestMatch(vpairs[i].indexA, i, vpairs[i].distance);
118  }
119  } else if (vpairs[i].distance < const_distthres) {
120  matchingAlgorithm.setBestMatch(vpairs[i].indexA, i, vpairs[i].distance);
121  }
122  }
123 
124  delete[] locks;
125 }
126 
127 // Execute a matching algorithm. This is the fast, templated version. Use this.
128 template<typename MATCHING_ALGORITHM_T>
129 void DenseMatcher::match(MATCHING_ALGORITHM_T & matchingAlgorithm) {
130  typedef MATCHING_ALGORITHM_T matching_algorithm_t;
131  matchingAlgorithm.doSetup();
132 
133  // call the matching body with the linear matching function pointer
134  matchBody(&DenseMatcher::template doWorkLinearMatching<matching_algorithm_t>,
135  matchingAlgorithm);
136 }
137 
138 // Execute a matching algorithm implementing image space matching.
139 template<typename MATCHING_ALGORITHM_T>
140 void DenseMatcher::matchInImageSpace(MATCHING_ALGORITHM_T & matchingAlgorithm) {
141  typedef MATCHING_ALGORITHM_T matching_algorithm_t;
142  matchingAlgorithm.doSetup();
143 
144  // call the matching body with the image space matching function pointer
145  matchBody(
146  &DenseMatcher::template doWorkImageSpaceMatching<matching_algorithm_t>,
147  matchingAlgorithm);
148 }
149 
150 // This calculates the distance between to keypoint descriptors. If it is better than the /e numBest_
151 // found so far, it is included in the aiBest list.
152 template<typename MATCHING_ALGORITHM_T>
154  MATCHING_ALGORITHM_T* matchingAlgorithm, std::vector<pairing_t>& aiBest,
155  size_t shortindexA, size_t i) {
156  OKVIS_ASSERT_TRUE(std::runtime_error, matchingAlgorithm != NULL,
157  "matching algorithm is NULL");
158  typename DenseMatcher::distance_t tmpdist;
159 
160  // is this better than worst found so far?
161  tmpdist = matchingAlgorithm->distance(shortindexA, i);
162  if (tmpdist < aiBest[numBest_ - 1].distance) {
163  pairing_t tmp(static_cast<int>(i), tmpdist);
164  typename std::vector<pairing_t>::iterator lb = std::lower_bound(
165  aiBest.begin(), aiBest.end(), tmp); //get position for insertion
166  typename std::vector<pairing_t>::iterator it, it_next;
167  it = it_next = aiBest.end();
168 
169  --it;
170  --it_next;
171  // Insert the new match value into the list
172  while (it_next != lb) {
173  --it;
174  *it_next = *it; //move value one position to the back
175  --it_next;
176  }
177  *lb = tmp; //insert both index and score to the correct position to keep strict weak->strong ordering
178  }
179 }
180 
181 // The threading worker. This matches a keypoint with every other keypoint to find the best match.
182 template<typename MATCHING_ALGORITHM_T>
184  MatchJob & my_job, MATCHING_ALGORITHM_T * matchingAlgorithm) {
185  OKVIS_ASSERT_TRUE(std::runtime_error, matchingAlgorithm != NULL,
186  "matching algorithm is NULL");
187  try {
188  int start = my_job.iThreadID;
189  distance_t const_distthres = matchingAlgorithm->distanceThreshold();
191  // When using the distance ratio threshold, we want to build a list of good matches
192  // independent of the threshold first and then later threshold on the ratio.
193  const_distthres = std::numeric_limits<distance_t>::max();
194  }
195 
196  size_t sizeA = matchingAlgorithm->sizeA();
197  for (size_t shortindexA = start; shortindexA < sizeA; shortindexA +=
199  if (matchingAlgorithm->skipA(shortindexA))
200  continue;
201 
202  //typename DenseMatcher::distance_t tmpdist;
203  std::vector<pairing_t> & aiBest = (*my_job.vMyBest)[shortindexA];
204 
205  // initialize the best match to be -1 (no match) and set the score to be the distance threshold
206  // No matches worse than the distance threshold will get through.
207  aiBest.resize(numBest_, pairing_t(-1, const_distthres)); //the best x matches for this feature from the long list
208 
209  size_t numElementsInListB = matchingAlgorithm->sizeB();
210  for (size_t i = 0; i < numElementsInListB; ++i) {
211  if (matchingAlgorithm->skipB(i)) {
212  continue;
213  }
214 
215  listBIteration(matchingAlgorithm, aiBest, shortindexA, i);
216 
217  }
218  assignbest(static_cast<int>(shortindexA), *(my_job.vpairs),
219  *(my_job.vMyBest), my_job.mutexes, 0); //this call assigns the match and reassigns losing matches recursively
220  }
221  } catch (const std::exception & e) {
222  // \todo Install an error handler in the matching algorithm?
223  std::cout << "\033[31mException in matching thread:\033[0m " << e.what();
224  }
225 }
226 
227 // The threading worker. This matches a keypoint with only a subset of the other keypoints
228 // to find the best match. (From matchingAlgorithm->getListBStartIterator() to
229 // MatchingAlgorithm->getListBEndIterator().
230 template<typename MATCHING_ALGORITHM_T>
232  MatchJob & my_job, MATCHING_ALGORITHM_T* matchingAlgorithm) {
233  OKVIS_ASSERT_TRUE(std::runtime_error, matchingAlgorithm != NULL,
234  "matching algorithm is NULL");
235  try {
236  int start = my_job.iThreadID;
237 
238  size_t numElementsInListB = matchingAlgorithm->sizeB();
239  size_t numElementsInListA = matchingAlgorithm->sizeA();
240 
241  distance_t const_distthres = matchingAlgorithm->distanceThreshold();
243  // When using the distance ratio threshold, we want to build a list of good matches
244  // independent of the threshold first and then later threshold on the ratio.
245  const_distthres = std::numeric_limits<distance_t>::max();
246  }
247 
248  for (size_t shortindexA = start; shortindexA < matchingAlgorithm->sizeA();
249  shortindexA += numMatcherThreads_) {
250  if (matchingAlgorithm->skipA(shortindexA))
251  continue;
252 
253  typename DenseMatcher::distance_t tmpdist;
254  std::vector<pairing_t>& aiBest = (*my_job.vMyBest)[shortindexA];
255 
256  // initialize the best match to be -1 (no match) and set the score to be the distance threshold
257  // No matches worse than the distance threshold will get through.
258  aiBest.resize(numBest_, pairing_t(-1, const_distthres)); //the best x matches for this feature from the long list
259 
260  typename MATCHING_ALGORITHM_T::listB_tree_structure_t::iterator itBegin =
261  matchingAlgorithm->getListBStartIterator(shortindexA);
262  typename MATCHING_ALGORITHM_T::listB_tree_structure_t::iterator itEnd =
263  matchingAlgorithm->getListBEndIterator(shortindexA);
264  //check all features from the long list
265  for (typename MATCHING_ALGORITHM_T::listB_tree_structure_t::iterator it =
266  itBegin; it != itEnd; ++it) {
267  size_t i = it->second;
268 
269  if (matchingAlgorithm->skipB(i)) {
270  continue;
271  }
272 
273  listBIteration(matchingAlgorithm, aiBest, shortindexA, i);
274 
275  }
276 
277  assignbest(static_cast<int>(shortindexA), *(my_job.vpairs),
278  *(my_job.vMyBest), my_job.mutexes, 0); //this call assigns the match and reassigns losing matches recursively
279  }
280 
281  } catch (const std::exception & e) {
282  // \todo Install an error handler in the matching algorithm?
283  std::cout << "\033[31mException in matching thread:\033[0m " << e.what();
284  }
285 }
286 
287 } // namespace okvis
unsigned char numBest_
The set number of best pairings to save.
Definition: DenseMatcher.hpp:208
void doWorkLinearMatching(MatchJob &my_job, MATCHING_ALGORITHM_T *matchingAlgorithm)
The threading worker. This matches a keypoint with every other keypoint to find the best match...
Definition: DenseMatcher.hpp:183
A struct to save an index and distance pair.
Definition: DenseMatcher.hpp:95
std::vector< std::vector< pairing_t > > * vMyBest
The list of best matches so far.
Definition: DenseMatcher.hpp:134
std::unique_ptr< okvis::ThreadPool > matcherThreadPool_
The threads.
Definition: DenseMatcher.hpp:211
void match(MATCHING_ALGORITHM_T &matchingAlgorithm)
Execute a matching algorithm. This is the fast, templated version. Use this.
Definition: DenseMatcher.hpp:129
void assignbest(int myIndexScored, pairing_list_t &vPairsWithScore, std::vector< std::vector< pairing_t > > &aiBestList, std::mutex *locks, int startidx)
A recursive function that reassigns weak matches, if a stronger match is found for a particular point...
Definition: DenseMatcher.cpp:69
std::mutex * mutexes
Mutexes for read/write synchronization in assignment of best match.
Definition: DenseMatcher.hpp:143
DenseMatcher::Pairing pairing_t
Definition: DenseMatcher.hpp:121
float distance_t
Definition: DenseMatcher.hpp:92
unsigned char numMatcherThreads_
The set number of threads.
Definition: DenseMatcher.hpp:207
bool useDistanceRatioThreshold_
Use ratio of best and second best match instead of absolute threshold.
Definition: DenseMatcher.hpp:209
A data struct for the worker thread.
Definition: DenseMatcher.hpp:127
void matchInImageSpace(MATCHING_ALGORITHM_T &matchingAlgorithm)
Execute a matching algorithm implementing image space matching (i.e. match landmarks with features in...
Definition: DenseMatcher.hpp:140
int iThreadID
The thread ID of this job.
Definition: DenseMatcher.hpp:137
std::vector< pairing_t > pairing_list_t
Definition: DenseMatcher.hpp:122
This class matches keypoints from two frames in parallel.
Definition: DenseMatcher.hpp:59
DenseMatcher::pairing_list_t * vpairs
The list of pairs for this thread.
Definition: DenseMatcher.hpp:140
void doWorkImageSpaceMatching(MatchJob &my_job, MATCHING_ALGORITHM_T *matchingAlgorithm)
The threading worker. This matches a keypoint with only a subset of the other keypoints to find the b...
Definition: DenseMatcher.hpp:231
void matchBody(void(DenseMatcher::*doWorkPtr)(MatchJob &, MATCHING_ALGORITHM_T *), MATCHING_ALGORITHM_T &matchingAlgorithm)
This function creates all the matching threads and assigns the best matches afterwards.
Definition: DenseMatcher.hpp:48
#define OKVIS_ASSERT_TRUE_DBG(exceptionType, condition, message)
Definition: assert_macros.hpp:211
void listBIteration(MATCHING_ALGORITHM_T *matchingAlgorithm, std::vector< pairing_t > &aiBest, size_t shortindexA, size_t i)
This calculates the distance between to keypoint descriptors. If it is better than the /e numBest_ fo...
Definition: DenseMatcher.hpp:153
#define OKVIS_ASSERT_TRUE(exceptionType, condition, message)
Definition: assert_macros.hpp:111