Optimizer.h
Go to the documentation of this file.
1 /*
2 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
3 All rights reserved.
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  * Redistributions of source code must retain the above copyright
8  notice, this list of conditions and the following disclaimer.
9  * Redistributions in binary form must reproduce the above copyright
10  notice, this list of conditions and the following disclaimer in the
11  documentation and/or other materials provided with the distribution.
12  * Neither the name of the Universite de Sherbrooke nor the
13  names of its contributors may be used to endorse or promote products
14  derived from this software without specific prior written permission.
15 
16 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
20 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27 
28 #ifndef OPTIMIZER_H_
29 #define OPTIMIZER_H_
30 
31 #include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
32 
33 #include <map>
34 #include <list>
35 #include <rtabmap/core/Link.h>
37 #include <rtabmap/core/Signature.h>
38 
39 namespace rtabmap {
40 
41 class FeatureBA
42 {
43 public:
44  FeatureBA(const cv::KeyPoint & kptIn, const float & depthIn = 0.0f, const cv::Mat & descriptorIn = cv::Mat()):
45  kpt(kptIn),
46  depth(depthIn),
47  descriptor(descriptorIn)
48  {}
49  cv::KeyPoint kpt;
50  float depth;
51  cv::Mat descriptor;
52 };
53 
55 // Graph optimizers
58 {
59 public:
60  enum Type {
61  kTypeUndef = -1,
62  kTypeTORO = 0,
63  kTypeG2O = 1,
64  kTypeGTSAM = 2,
65  kTypeCeres = 3,
66  kTypeCVSBA = 4
67  };
68  static bool isAvailable(Optimizer::Type type);
69  static Optimizer * create(const ParametersMap & parameters);
70  static Optimizer * create(Optimizer::Type type, const ParametersMap & parameters = ParametersMap());
71 
72  // Get connected poses and constraints from a set of links
73  void getConnectedGraph(
74  int fromId,
75  const std::map<int, Transform> & posesIn,
76  const std::multimap<int, Link> & linksIn,
77  std::map<int, Transform> & posesOut,
78  std::multimap<int, Link> & linksOut) const;
79 
80 public:
81  virtual ~Optimizer() {}
82 
83  virtual Type type() const = 0;
84 
85  // getters
86  int iterations() const {return iterations_;}
87  bool isSlam2d() const {return slam2d_;}
88  bool isCovarianceIgnored() const {return covarianceIgnored_;}
89  double epsilon() const {return epsilon_;}
90  bool isRobust() const {return robust_;}
91  bool priorsIgnored() const {return priorsIgnored_;}
92  bool landmarksIgnored() const {return landmarksIgnored_;}
93  float gravitySigma() const {return gravitySigma_;}
94 
95  // setters
96  void setIterations(int iterations) {iterations_ = iterations;}
97  void setSlam2d(bool enabled) {slam2d_ = enabled;}
98  void setCovarianceIgnored(bool enabled) {covarianceIgnored_ = enabled;}
99  void setEpsilon(double epsilon) {epsilon_ = epsilon;}
100  void setRobust(bool enabled) {robust_ = enabled;}
101  void setPriorsIgnored(bool enabled) {priorsIgnored_ = enabled;}
102  void setLandmarksIgnored(bool enabled) {landmarksIgnored_ = enabled;}
103  void setGravitySigma(float value) {gravitySigma_ = value;}
104 
105  virtual void parseParameters(const ParametersMap & parameters);
106 
107  std::map<int, Transform> optimizeIncremental(
108  int rootId,
109  const std::map<int, Transform> & poses,
110  const std::multimap<int, Link> & constraints,
111  std::list<std::map<int, Transform> > * intermediateGraphes = 0,
112  double * finalError = 0,
113  int * iterationsDone = 0);
114 
115  std::map<int, Transform> optimize(
116  int rootId,
117  const std::map<int, Transform> & poses,
118  const std::multimap<int, Link> & constraints,
119  std::list<std::map<int, Transform> > * intermediateGraphes = 0,
120  double * finalError = 0,
121  int * iterationsDone = 0);
122 
123  // inherited classes should implement one of these methods
124  virtual std::map<int, Transform> optimize(
125  int rootId,
126  const std::map<int, Transform> & poses,
127  const std::multimap<int, Link> & constraints,
128  cv::Mat & outputCovariance,
129  std::list<std::map<int, Transform> > * intermediateGraphes = 0,
130  double * finalError = 0,
131  int * iterationsDone = 0);
132  virtual std::map<int, Transform> optimizeBA(
133  int rootId, // if negative, all other poses are fixed
134  const std::map<int, Transform> & poses,
135  const std::multimap<int, Link> & links,
136  const std::map<int, CameraModel> & models, // in case of stereo, Tx should be set
137  std::map<int, cv::Point3f> & points3DMap,
138  const std::map<int, std::map<int, FeatureBA> > & wordReferences, // <ID words, IDs frames + keypoint/depth/descriptor>
139  std::set<int> * outliers = 0);
140 
141  std::map<int, Transform> optimizeBA(
142  int rootId,
143  const std::map<int, Transform> & poses,
144  const std::multimap<int, Link> & links,
145  const std::map<int, Signature> & signatures,
146  std::map<int, cv::Point3f> & points3DMap,
147  std::map<int, std::map<int, FeatureBA> > & wordReferences, // <ID words, IDs frames + keypoint/depth/descriptor>
148  bool rematchFeatures = false);
149 
150  std::map<int, Transform> optimizeBA(
151  int rootId,
152  const std::map<int, Transform> & poses,
153  const std::multimap<int, Link> & links,
154  const std::map<int, Signature> & signatures,
155  bool rematchFeatures = false);
156 
157  Transform optimizeBA(
158  const Link & link,
159  const CameraModel & model,
160  std::map<int, cv::Point3f> & points3DMap,
161  const std::map<int, std::map<int, FeatureBA> > & wordReferences,
162  std::set<int> * outliers = 0);
163 
164  void computeBACorrespondences(
165  const std::map<int, Transform> & poses,
166  const std::multimap<int, Link> & links,
167  const std::map<int, Signature> & signatures,
168  std::map<int, cv::Point3f> & points3DMap,
169  std::map<int, std::map<int, FeatureBA > > & wordReferences, // <ID words, IDs frames + keypoint/depth/descriptor>
170  bool rematchFeatures = false);
171 
172 protected:
173  Optimizer(
174  int iterations = Parameters::defaultOptimizerIterations(),
175  bool slam2d = Parameters::defaultRegForce3DoF(),
176  bool covarianceIgnored = Parameters::defaultOptimizerVarianceIgnored(),
177  double epsilon = Parameters::defaultOptimizerEpsilon(),
178  bool robust = Parameters::defaultOptimizerRobust(),
179  bool priorsIgnored = Parameters::defaultOptimizerPriorsIgnored(),
180  bool landmarksIgnored = Parameters::defaultOptimizerLandmarksIgnored(),
181  float gravitySigma = Parameters::defaultOptimizerGravitySigma());
182  Optimizer(const ParametersMap & parameters);
183 
184 private:
186  bool slam2d_;
188  double epsilon_;
189  bool robust_;
193 };
194 
195 } /* namespace rtabmap */
196 #endif /* OPTIMIZER_H_ */
bool priorsIgnored() const
Definition: Optimizer.h:91
FeatureBA(const cv::KeyPoint &kptIn, const float &depthIn=0.0f, const cv::Mat &descriptorIn=cv::Mat())
Definition: Optimizer.h:44
float gravitySigma() const
Definition: Optimizer.h:93
double epsilon
void setIterations(int iterations)
Definition: Optimizer.h:96
bool isSlam2d() const
Definition: Optimizer.h:87
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:43
bool isCovarianceIgnored() const
Definition: Optimizer.h:88
cv::KeyPoint kpt
Definition: Optimizer.h:49
#define RTABMAP_EXP
Definition: RtabmapExp.h:38
void setSlam2d(bool enabled)
Definition: Optimizer.h:97
bool landmarksIgnored() const
Definition: Optimizer.h:92
void setPriorsIgnored(bool enabled)
Definition: Optimizer.h:101
cv::Mat descriptor
Definition: Optimizer.h:51
void setLandmarksIgnored(bool enabled)
Definition: Optimizer.h:102
void setGravitySigma(float value)
Definition: Optimizer.h:103
double epsilon() const
Definition: Optimizer.h:89
int iterations() const
Definition: Optimizer.h:86
bool isRobust() const
Definition: Optimizer.h:90
void setRobust(bool enabled)
Definition: Optimizer.h:100
void setCovarianceIgnored(bool enabled)
Definition: Optimizer.h:98
virtual ~Optimizer()
Definition: Optimizer.h:81
void setEpsilon(double epsilon)
Definition: Optimizer.h:99


rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Dec 14 2020 03:34:59