OKVIS ROS
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Pages
Frontend.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: Mar 27, 2015
30  * Author: Andreas Forster (an.forster@gmail.com)
31  * Modified: Stefan Leutenegger (s.leutenegger@imperial.ac.uk)
32  *********************************************************************************/
33 
41 #ifndef INCLUDE_OKVIS_FRONTEND_HPP_
42 #define INCLUDE_OKVIS_FRONTEND_HPP_
43 
44 #include <mutex>
45 #include <okvis/assert_macros.hpp>
46 #include <okvis/Estimator.hpp>
48 #include <okvis/timing/Timer.hpp>
49 #include <okvis/DenseMatcher.hpp>
50 
52 namespace okvis {
53 
58  public:
59  OKVIS_DEFINE_EXCEPTION(Exception, std::runtime_error)
60 
61 #ifdef DEACTIVATE_TIMERS
63 #else
65 #endif
66 
71  Frontend(size_t numCameras);
72  virtual ~Frontend() {
73  }
74 
76 
88  virtual bool detectAndDescribe(size_t cameraIndex,
89  std::shared_ptr<okvis::MultiFrame> frameOut,
91  const std::vector<cv::KeyPoint> * keypoints);
92 
106  okvis::Estimator& estimator,
107  okvis::kinematics::Transformation& T_WS_propagated,
108  const okvis::VioParameters & params,
109  const std::shared_ptr<okvis::MapPointVector> map,
110  std::shared_ptr<okvis::MultiFrame> framesInOut, bool* asKeyframe);
111 
126  virtual bool propagation(const okvis::ImuMeasurementDeque & imuMeasurements,
127  const okvis::ImuParameters & imuParams,
128  okvis::kinematics::Transformation& T_WS_propagated,
129  okvis::SpeedAndBias & speedAndBiases,
130  const okvis::Time& t_start, const okvis::Time& t_end,
131  Eigen::Matrix<double, 15, 15>* covariance,
132  Eigen::Matrix<double, 15, 15>* jacobian) const;
133 
137 
139  size_t getBriskDetectionOctaves() const {
140  return briskDetectionOctaves_;
141  }
142 
144  double getBriskDetectionThreshold() const {
146  }
147 
151  }
152 
156  }
157 
161 
165  }
166 
170  }
171 
175 
177  double getBriskMatchingThreshold() const {
179  }
180 
184  }
185 
189  }
190 
192  bool isInitialized() {
193  return isInitialized_;
194  }
195 
199 
201  void setBriskDetectionOctaves(size_t octaves) {
202  briskDetectionOctaves_ = octaves;
204  }
205 
207  void setBriskDetectionThreshold(double threshold) {
208  briskDetectionThreshold_ = threshold;
210  }
211 
213  void setBriskDetectionAbsoluteThreshold(double threshold) {
216  }
217 
219  void setBriskDetectionMaximumKeypoints(size_t maxKeypoints) {
220  briskDetectionMaximumKeypoints_ = maxKeypoints;
222  }
223 
227 
232  }
233 
235  void setBriskDescriptionScaleInvariance(bool invariance) {
238  }
239 
243 
245  void setBriskMatchingThreshold(double threshold) {
246  briskMatchingThreshold_ = threshold;
247  }
248 
250  void setKeyframeInsertionOverlapThreshold(float threshold) {
252  }
253 
257  }
258 
260 
261  private:
262 
268  std::vector<std::shared_ptr<cv::FeatureDetector> > featureDetectors_;
274  std::vector<std::shared_ptr<cv::DescriptorExtractor> > descriptorExtractors_;
276  std::vector<std::unique_ptr<std::mutex> > featureDetectorMutexes_;
277 
279  const size_t numCameras_;
280 
283 
288 
292 
295 
299 
301 
303 
304  std::unique_ptr<okvis::DenseMatcher> matcher_;
305 
320 
327  bool doWeNeedANewKeyframe(const okvis::Estimator& estimator,
328  std::shared_ptr<okvis::MultiFrame> currentFrame); // based on some overlap area heuristics
329 
344  template<class MATCHING_ALGORITHM>
345  int matchToKeyframes(okvis::Estimator& estimator,
346  const okvis::VioParameters& params,
347  const uint64_t currentFrameId, bool& rotationOnly,
348  bool usePoseUncertainty = true,
349  double* uncertainMatchFraction = 0,
350  bool removeOutliers = true); // for wide-baseline matches (good initial guess)
351 
363  template<class MATCHING_ALGORITHM>
364  int matchToLastFrame(okvis::Estimator& estimator,
365  const okvis::VioParameters& params,
366  const uint64_t currentFrameId,
367  bool usePoseUncertainty = true,
368  bool removeOutliers = true);
369 
377  template<class MATCHING_ALGORITHM>
378  void matchStereo(okvis::Estimator& estimator,
379  std::shared_ptr<okvis::MultiFrame> multiFrame);
380 
390  int runRansac3d2d(okvis::Estimator& estimator,
391  const okvis::cameras::NCameraSystem &nCameraSystem,
392  std::shared_ptr<okvis::MultiFrame> currentFrame,
393  bool removeOutliers);
394 
407  int runRansac2d2d(okvis::Estimator& estimator,
408  const okvis::VioParameters& params, uint64_t currentFrameId,
409  uint64_t olderFrameId, bool initializePose,
410  bool removeOutliers, bool &rotationOnly);
411 
414 
415 };
416 
417 } // namespace okvis
418 
419 #endif // INCLUDE_OKVIS_FRONTEND_HPP_
void setBriskDescriptionScaleInvariance(bool invariance)
Set the scale invariance setting of the BRISK descriptor.
Definition: Frontend.hpp:235
The estimator class.
Definition: Estimator.hpp:77
const size_t numCameras_
Number of cameras in the configuration.
Definition: Frontend.hpp:279
int runRansac2d2d(okvis::Estimator &estimator, const okvis::VioParameters &params, uint64_t currentFrameId, uint64_t olderFrameId, bool initializePose, bool removeOutliers, bool &rotationOnly)
Perform 2D/2D RANSAC.
Definition: Frontend.cpp:645
void initialiseBriskFeatureDetectors()
(re)instantiates feature detectors and descriptor extractors. Used after settings changed or at start...
Definition: Frontend.cpp:813
bool briskDescriptionRotationInvariance_
The set rotation invariance setting.
Definition: Frontend.hpp:293
std::vector< std::shared_ptr< cv::FeatureDetector > > featureDetectors_
feature detectors with the current settings. The vector contains one for each camera to ensure that t...
Definition: Frontend.hpp:268
void setKeyframeInsertionOverlapThreshold(float threshold)
Set the area overlap threshold under which a new keyframe is inserted.
Definition: Frontend.hpp:250
void setBriskDetectionThreshold(double threshold)
Set the detection threshold of the BRISK detector.
Definition: Frontend.hpp:207
bool isInitialized()
Returns true if the initialization has been completed (RANSAC with actual translation) ...
Definition: Frontend.hpp:192
std::deque< ImuMeasurement, Eigen::aligned_allocator< ImuMeasurement > > ImuMeasurementDeque
Definition: Measurements.hpp:163
double briskDetectionThreshold_
The set BRISK detection threshold.
Definition: Frontend.hpp:285
okvis::timing::Timer TimerSwitchable
Definition: Frontend.hpp:64
bool getBriskDescriptionRotationInvariance() const
Get the rotation invariance setting of the BRISK descriptor.
Definition: Frontend.hpp:163
virtual bool propagation(const okvis::ImuMeasurementDeque &imuMeasurements, const okvis::ImuParameters &imuParams, okvis::kinematics::Transformation &T_WS_propagated, okvis::SpeedAndBias &speedAndBiases, const okvis::Time &t_start, const okvis::Time &t_end, Eigen::Matrix< double, 15, 15 > *covariance, Eigen::Matrix< double, 15, 15 > *jacobian) const
Propagates pose, speeds and biases with given IMU measurements.
Definition: Frontend.cpp:274
void matchStereo(okvis::Estimator &estimator, std::shared_ptr< okvis::MultiFrame > multiFrame)
Match the frames inside the multiframe to each other to initialise new landmarks. ...
Definition: Frontend.cpp:522
std::vector< std::shared_ptr< cv::DescriptorExtractor > > descriptorExtractors_
feature descriptors with the current settings. The vector contains one for each camera to ensure that...
Definition: Frontend.hpp:274
A frontend using BRISK features.
Definition: Frontend.hpp:57
double getBriskDetectionAbsoluteThreshold() const
Get the absolute threshold of the BRISK detector.
Definition: Frontend.hpp:149
virtual ~Frontend()
Definition: Frontend.hpp:72
void setBriskDetectionAbsoluteThreshold(double threshold)
Set the absolute threshold of the BRISK detector.
Definition: Frontend.hpp:213
void setBriskDescriptionRotationInvariance(bool invariance)
Set the rotation invariance setting of the BRISK descriptor.
Definition: Frontend.hpp:229
size_t getBriskDetectionOctaves() const
Get the number of octaves of the BRISK detector.
Definition: Frontend.hpp:139
Eigen::Matrix< double, 9, 1 > SpeedAndBias
Definition: FrameTypedefs.hpp:234
virtual bool dataAssociationAndInitialization(okvis::Estimator &estimator, okvis::kinematics::Transformation &T_WS_propagated, const okvis::VioParameters &params, const std::shared_ptr< okvis::MapPointVector > map, std::shared_ptr< okvis::MultiFrame > framesInOut, bool *asKeyframe)
Matching as well as initialization of landmarks and state.
Definition: Frontend.cpp:117
Definition: Timer.hpp:104
size_t briskDetectionOctaves_
The set number of brisk octaves.
Definition: Frontend.hpp:284
virtual bool detectAndDescribe(size_t cameraIndex, std::shared_ptr< okvis::MultiFrame > frameOut, const okvis::kinematics::Transformation &T_WC, const std::vector< cv::KeyPoint > *keypoints)
Detection and descriptor extraction on a per image basis.
Definition: Frontend.cpp:92
A class that does homogeneous transformations. This relates a frame A and B: T_AB; it consists of tra...
Definition: Transformation.hpp:76
float getKeyframeInsertionOverlapThershold() const
Get the area overlap threshold under which a new keyframe is inserted.
Definition: Frontend.hpp:182
A class that assembles multiple cameras into a system of (potentially different) cameras.
Definition: NCameraSystem.hpp:61
Definition: Timer.hpp:90
void setBriskMatchingThreshold(double threshold)
Set the matching threshold.
Definition: Frontend.hpp:245
int matchToLastFrame(okvis::Estimator &estimator, const okvis::VioParameters &params, const uint64_t currentFrameId, bool usePoseUncertainty=true, bool removeOutliers=true)
Match a new multiframe to the last frame.
Definition: Frontend.cpp:464
int runRansac3d2d(okvis::Estimator &estimator, const okvis::cameras::NCameraSystem &nCameraSystem, std::shared_ptr< okvis::MultiFrame > currentFrame, bool removeOutliers)
Perform 3D/2D RANSAC.
Definition: Frontend.cpp:575
The VioFrontendInterface class is an interface for frontends.
Definition: VioFrontendInterface.hpp:69
#define OKVIS_DEFINE_EXCEPTION(exceptionName, exceptionParent)
Macro for defining an exception with a given parent.
Definition: assert_macros.hpp:52
This file contains some useful assert macros.
Header file for the DenseMatcher class.
void setBriskDetectionMaximumKeypoints(size_t maxKeypoints)
Set the maximum number of keypoints of the BRISK detector.
Definition: Frontend.hpp:219
bool doWeNeedANewKeyframe(const okvis::Estimator &estimator, std::shared_ptr< okvis::MultiFrame > currentFrame)
Decision whether a new frame should be keyframe or not.
Definition: Frontend.cpp:295
void setKeyframeInsertionMatchingRatioThreshold(float threshold)
Set the matching ratio threshold under which a new keyframe is inserted.
Definition: Frontend.hpp:255
std::unique_ptr< okvis::DenseMatcher > matcher_
Matcher object.
Definition: Frontend.hpp:304
Struct to combine all parameters and settings.
Definition: Parameters.hpp:280
size_t getBriskDetectionMaximumKeypoints() const
Get the maximum amount of keypoints of the BRISK detector.
Definition: Frontend.hpp:154
size_t briskDetectionMaximumKeypoints_
The set maximum number of keypoints.
Definition: Frontend.hpp:287
double briskMatchingThreshold_
The set BRISK matching threshold.
Definition: Frontend.hpp:300
float getKeyframeInsertionMatchingRatioThreshold() const
Get the matching ratio threshold under which a new keyframe is inserted.
Definition: Frontend.hpp:187
int matchToKeyframes(okvis::Estimator &estimator, const okvis::VioParameters &params, const uint64_t currentFrameId, bool &rotationOnly, bool usePoseUncertainty=true, double *uncertainMatchFraction=0, bool removeOutliers=true)
Match a new multiframe to existing keyframes.
Definition: Frontend.cpp:373
float keyframeInsertionMatchingRatioThreshold_
If the number of matched keypoints of the current frame with an older frame divided by the amount of ...
Definition: Frontend.hpp:319
bool getBriskDescriptionScaleInvariance() const
Get the scale invariance setting of the BRISK descriptor.
Definition: Frontend.hpp:168
Header file for the Estimator class. This does all the backend work.
IMU parameters.
Definition: Parameters.hpp:105
Frontend(size_t numCameras)
Constructor.
Definition: Frontend.cpp:69
Time representation. May either represent wall clock time or ROS clock time.
Definition: Time.hpp:187
double getBriskMatchingThreshold() const
Get the matching threshold.
Definition: Frontend.hpp:177
bool briskDescriptionScaleInvariance_
The set scale invariance setting.
Definition: Frontend.hpp:294
std::vector< std::unique_ptr< std::mutex > > featureDetectorMutexes_
Mutexes for feature detectors and descriptors.
Definition: Frontend.hpp:276
double getBriskDetectionThreshold() const
Get the detection threshold of the BRISK detector.
Definition: Frontend.hpp:144
float keyframeInsertionOverlapThreshold_
If the hull-area around all matched keypoints of the current frame (with existing landmarks) divided ...
Definition: Frontend.hpp:312
Header file for the VioFrontendInterface class.
double briskDetectionAbsoluteThreshold_
The set BRISK absolute detection threshold.
Definition: Frontend.hpp:286
void setBriskDetectionOctaves(size_t octaves)
Set the number of octaves of the BRISK detector.
Definition: Frontend.hpp:201
bool isInitialized_
Is the pose initialised?
Definition: Frontend.hpp:278