OKVIS ROS
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Pages
VioKeyframeWindowMatchingAlgorithm.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: Oct 17, 2013
30  * Author: Stefan Leutenegger (s.leutenegger@imperial.ac.uk)
31  * Modified: Andreas Forster (an.forster@gmail.com)
32  *********************************************************************************/
33 
41 #ifndef INCLUDE_OKVIS_VIOKEYFRAMEWINDOWMATCHINGALGORITHM_HPP_
42 #define INCLUDE_OKVIS_VIOKEYFRAMEWINDOWMATCHINGALGORITHM_HPP_
43 
44 #include <memory>
45 
46 #include <okvis/DenseMatcher.hpp>
48 #include <okvis/Estimator.hpp>
49 #include <okvis/FrameTypedefs.hpp>
51 #include <okvis/MultiFrame.hpp>
52 #include <brisk/internal/hamming.h>
53 
55 namespace okvis {
56 
61 template<class CAMERA_GEOMETRY_T>
63  public:
64  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
65  OKVIS_DEFINE_EXCEPTION(Exception, std::runtime_error)
66 
67  typedef CAMERA_GEOMETRY_T camera_geometry_t;
68 
70  Match3D2D = 1,
71  Match2D2D = 2
72  };
73 
82  int matchingType, float distanceThreshold,
83  bool usePoseUncertainty = true);
84 
86 
94  void setFrames(uint64_t mfIdA, uint64_t mfIdB, size_t camIdA, size_t camIdB);
95 
100  void setMatchingType(int matchingType);
101 
103  virtual void doSetup();
104 
106  virtual size_t sizeA() const;
108  virtual size_t sizeB() const;
109 
111  virtual float distanceThreshold() const;
114 
116  virtual bool skipA(size_t indexA) const {
117  return skipA_[indexA];
118  }
119 
121  virtual bool skipB(size_t indexB) const {
122  return skipB_[indexB];
123  }
124 
132  virtual float distance(size_t indexA, size_t indexB) const {
133  OKVIS_ASSERT_LT_DBG(MatchingAlgorithm::Exception, indexA, sizeA(), "index A out of bounds");
134  OKVIS_ASSERT_LT_DBG(MatchingAlgorithm::Exception, indexB, sizeB(), "index B out of bounds");
135  const float dist = static_cast<float>(specificDescriptorDistance(
136  frameA_->keypointDescriptor(camIdA_, indexA),
137  frameB_->keypointDescriptor(camIdB_, indexB)));
138 
139  if (dist < distanceThreshold_) {
140  if (verifyMatch(indexA, indexB))
141  return dist;
142  }
143  return std::numeric_limits<float>::max();
144  }
145 
147  bool verifyMatch(size_t indexA, size_t indexB) const;
148 
151  virtual void reserveMatches(size_t numMatches);
152 
155  virtual void setBestMatch(size_t indexA, size_t indexB, double distance);
156 
158  size_t numMatches();
160  size_t numUncertainMatches();
161 
163  const okvis::Matches & getMatches() const;
164 
168  }
169 
170  private:
173 
176  uint64_t mfIdA_ = 0;
177  uint64_t mfIdB_ = 0;
178  size_t camIdA_ = 0;
179  size_t camIdB_ = 0;
180 
181  std::shared_ptr<okvis::MultiFrame> frameA_;
182  std::shared_ptr<okvis::MultiFrame> frameB_;
184 
187 
204 
206  size_t numMatches_ = 0;
209 
211  double fA_ = 0;
213  double fB_ = 0;
214 
217 
219  Eigen::Matrix<double, Eigen::Dynamic, 2> projectionsIntoB_;
221  Eigen::Matrix<double, Eigen::Dynamic, 2> projectionsIntoBUncertainties_;
222 
224  std::vector<bool> skipA_;
226  std::vector<bool> skipB_;
227 
229  Eigen::Vector3d pA_W_;
231  Eigen::Vector3d pB_W_;
232 
234  std::vector<double> raySigmasA_;
236  std::vector<double> raySigmasB_;
237 
240 
242  bool usePoseUncertainty_ = false;
243 
245  // copy from BriskDescriptor.hpp
247  const unsigned char * descriptorA,
248  const unsigned char * descriptorB) const {
250  Exception, descriptorA != NULL && descriptorB != NULL,
251  "Trying to compare a descriptor with a null description vector");
252 
253  return brisk::Hamming::PopcntofXORed(descriptorA, descriptorB, 3/*48 / 16*/);
254  }
255 };
256 
257 }
258 
259 #endif /* INCLUDE_OKVIS_VIOKEYFRAMEWINDOWMATCHINGALGORITHM_HPP_ */
bool isRelativeUncertaintyValid()
assess the validity of the relative uncertainty computation.
Definition: VioKeyframeWindowMatchingAlgorithm.hpp:166
virtual bool skipB(size_t indexB) const
Should we skip the item in list B? This will be called many times.
Definition: VioKeyframeWindowMatchingAlgorithm.hpp:121
The estimator class.
Definition: Estimator.hpp:77
virtual size_t sizeA() const
What is the size of list A?
Definition: VioKeyframeWindowMatchingAlgorithm.cpp:280
virtual void setBestMatch(size_t indexA, size_t indexB, double distance)
At the end of the matching step, this function is called once for each pair of matches discovered...
Definition: VioKeyframeWindowMatchingAlgorithm.cpp:363
std::shared_ptr< okvis::MultiFrame > frameA_
Definition: VioKeyframeWindowMatchingAlgorithm.hpp:181
size_t camIdB_
Definition: VioKeyframeWindowMatchingAlgorithm.hpp:179
okvis::kinematics::Transformation T_SaW_
Definition: VioKeyframeWindowMatchingAlgorithm.hpp:197
okvis::kinematics::Transformation T_WCa_
Definition: VioKeyframeWindowMatchingAlgorithm.hpp:199
std::shared_ptr< okvis::MultiFrame > frameB_
Definition: VioKeyframeWindowMatchingAlgorithm.hpp:182
bool usePoseUncertainty_
Definition: VioKeyframeWindowMatchingAlgorithm.hpp:242
#define OKVIS_ASSERT_LT_DBG(exceptionType, value, upperBound, message)
Definition: assert_macros.hpp:240
std::vector< bool > skipB_
Should keypoint[index] in frame B be skipped.
Definition: VioKeyframeWindowMatchingAlgorithm.hpp:226
size_t numMatches()
Get the number of matches.
Definition: VioKeyframeWindowMatchingAlgorithm.cpp:350
okvis::kinematics::Transformation T_WCb_
Definition: VioKeyframeWindowMatchingAlgorithm.hpp:200
virtual void doSetup()
This will be called exactly once for each call to DenseMatcher::match().
Definition: VioKeyframeWindowMatchingAlgorithm.cpp:122
virtual ~VioKeyframeWindowMatchingAlgorithm()
Definition: VioKeyframeWindowMatchingAlgorithm.cpp:70
okvis::kinematics::Transformation T_CaCb_
Definition: VioKeyframeWindowMatchingAlgorithm.hpp:191
Eigen::Matrix< double, Eigen::Dynamic, 2 > projectionsIntoBUncertainties_
temporarily store all projection uncertainties
Definition: VioKeyframeWindowMatchingAlgorithm.hpp:221
okvis::kinematics::Transformation T_SaCa_
Definition: VioKeyframeWindowMatchingAlgorithm.hpp:193
okvis::kinematics::Transformation T_CbCa_
Definition: VioKeyframeWindowMatchingAlgorithm.hpp:192
Header file for the MatchingAlgorithm class.
Header file for the MultiFrame class.
uint64_t mfIdB_
Definition: VioKeyframeWindowMatchingAlgorithm.hpp:177
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef CAMERA_GEOMETRY_T camera_geometry_t
Definition: VioKeyframeWindowMatchingAlgorithm.hpp:67
okvis::kinematics::Transformation T_SbCb_
Definition: VioKeyframeWindowMatchingAlgorithm.hpp:194
okvis::Estimator * estimator_
This is essentially the map.
Definition: VioKeyframeWindowMatchingAlgorithm.hpp:172
an interface for 1-1 matching between lists of things.
Definition: MatchingAlgorithm.hpp:66
A class that does homogeneous transformations. This relates a frame A and B: T_AB; it consists of tra...
Definition: Transformation.hpp:76
double fA_
Focal length of camera used in frame A.
Definition: VioKeyframeWindowMatchingAlgorithm.hpp:211
virtual float distanceThreshold() const
Get the distance threshold for which matches exceeding it will not be returned as matches...
Definition: VioKeyframeWindowMatchingAlgorithm.cpp:298
std::vector< double > raySigmasA_
Temporarily store ray sigmas of frame A.
Definition: VioKeyframeWindowMatchingAlgorithm.hpp:234
size_t numMatches_
The number of matches.
Definition: VioKeyframeWindowMatchingAlgorithm.hpp:206
virtual bool skipA(size_t indexA) const
Should we skip the item in list A? This will be called once for each item in the list.
Definition: VioKeyframeWindowMatchingAlgorithm.hpp:116
okvis::kinematics::Transformation T_CaW_
Definition: VioKeyframeWindowMatchingAlgorithm.hpp:201
uint32_t specificDescriptorDistance(const unsigned char *descriptorA, const unsigned char *descriptorB) const
Calculates the distance between two descriptors.
Definition: VioKeyframeWindowMatchingAlgorithm.hpp:246
okvis::triangulation::ProbabilisticStereoTriangulator< camera_geometry_t > probabilisticStereoTriangulator_
Stereo triangulator.
Definition: VioKeyframeWindowMatchingAlgorithm.hpp:239
A MatchingAlgorithm implementation.
Definition: VioKeyframeWindowMatchingAlgorithm.hpp:62
virtual size_t sizeB() const
What is the size of list B?
Definition: VioKeyframeWindowMatchingAlgorithm.cpp:285
uint64_t mfIdA_
Definition: VioKeyframeWindowMatchingAlgorithm.hpp:176
Eigen::Vector3d pA_W_
Camera center of frame A.
Definition: VioKeyframeWindowMatchingAlgorithm.hpp:229
#define OKVIS_DEFINE_EXCEPTION(exceptionName, exceptionParent)
Macro for defining an exception with a given parent.
Definition: assert_macros.hpp:52
virtual void reserveMatches(size_t numMatches)
A function that tells you how many times setMatching() will be called.
Definition: VioKeyframeWindowMatchingAlgorithm.cpp:343
void setMatchingType(int matchingType)
Set the matching type.
Definition: VioKeyframeWindowMatchingAlgorithm.cpp:115
Header file for the DenseMatcher class.
okvis::kinematics::Transformation T_WSa_
Definition: VioKeyframeWindowMatchingAlgorithm.hpp:195
size_t camIdA_
Definition: VioKeyframeWindowMatchingAlgorithm.hpp:178
void setFrames(uint64_t mfIdA, uint64_t mfIdB, size_t camIdA, size_t camIdB)
Set which frames to match.
Definition: VioKeyframeWindowMatchingAlgorithm.cpp:76
size_t numUncertainMatches_
The number of uncertain matches.
Definition: VioKeyframeWindowMatchingAlgorithm.hpp:208
std::vector< bool > skipA_
Should keypoint[index] in frame A be skipped.
Definition: VioKeyframeWindowMatchingAlgorithm.hpp:224
VioKeyframeWindowMatchingAlgorithm(okvis::Estimator &estimator, int matchingType, float distanceThreshold, bool usePoseUncertainty=true)
Constructor.
Definition: VioKeyframeWindowMatchingAlgorithm.cpp:60
Header file for the ProbabilisticStereoTriangulator class.
Eigen::Vector3d pB_W_
Camera center of frame B.
Definition: VioKeyframeWindowMatchingAlgorithm.hpp:231
void setDistanceThreshold(float distanceThreshold)
Set the distance threshold for which matches exceeding it will not be returned as matches...
Definition: VioKeyframeWindowMatchingAlgorithm.cpp:291
float distanceThreshold_
Distances above this threshold will not be returned as matches.
Definition: VioKeyframeWindowMatchingAlgorithm.hpp:186
okvis::kinematics::Transformation T_CbW_
Definition: VioKeyframeWindowMatchingAlgorithm.hpp:202
okvis::kinematics::Transformation T_SbW_
Definition: VioKeyframeWindowMatchingAlgorithm.hpp:198
MatchingTypes
Definition: VioKeyframeWindowMatchingAlgorithm.hpp:69
std::vector< double > raySigmasB_
Temporarily store ray sigmas of frame B.
Definition: VioKeyframeWindowMatchingAlgorithm.hpp:236
const okvis::Matches & getMatches() const
access the matching result.
Header file for the Estimator class. This does all the backend work.
virtual float distance(size_t indexA, size_t indexB) const
Calculate the distance between two keypoints.
Definition: VioKeyframeWindowMatchingAlgorithm.hpp:132
int matchingType_
Stored the matching type. See MatchingTypes().
Definition: VioKeyframeWindowMatchingAlgorithm.hpp:216
bool validRelativeUncertainty_
Definition: VioKeyframeWindowMatchingAlgorithm.hpp:241
Eigen::Matrix< double, Eigen::Dynamic, 2 > projectionsIntoB_
temporarily store all projections
Definition: VioKeyframeWindowMatchingAlgorithm.hpp:219
std::vector< Match > Matches
Definition: FrameTypedefs.hpp:134
okvis::kinematics::Transformation T_WSb_
Definition: VioKeyframeWindowMatchingAlgorithm.hpp:196
Match 3D position of established landmarks to 2D keypoint position.
Definition: VioKeyframeWindowMatchingAlgorithm.hpp:70
#define OKVIS_ASSERT_TRUE_DBG(exceptionType, condition, message)
Definition: assert_macros.hpp:211
Match 2D position of established landmarks to 2D keypoint position.
Definition: VioKeyframeWindowMatchingAlgorithm.hpp:71
bool verifyMatch(size_t indexA, size_t indexB) const
Geometric verification of a match.
Definition: VioKeyframeWindowMatchingAlgorithm.cpp:304
double fB_
Focal length of camera used in frame B.
Definition: VioKeyframeWindowMatchingAlgorithm.hpp:213
This file contains useful typedefs and structs related to frames.
size_t numUncertainMatches()
Get the number of uncertain matches.
Definition: VioKeyframeWindowMatchingAlgorithm.cpp:356