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/rtabmap_core_export.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(), int cameraIndexIn = 0):
45  kpt(kptIn),
46  depth(depthIn),
47  descriptor(descriptorIn),
48  cameraIndex(cameraIndexIn)
49  {
50  //UDEBUG("kpt=(%f,%f) depth=%f, camIndex=%d", kpt.pt.x, kpt.pt.y, depth, cameraIndex);
51  }
52  cv::KeyPoint kpt;
53  float depth;
54  cv::Mat descriptor;
56 };
57 
59 // Graph optimizers
61 class RTABMAP_CORE_EXPORT Optimizer
62 {
63 public:
64  enum Type {
65  kTypeUndef = -1,
66  kTypeTORO = 0,
67  kTypeG2O = 1,
68  kTypeGTSAM = 2,
69  kTypeCeres = 3,
70  kTypeCVSBA = 4
71  };
72  static bool isAvailable(Optimizer::Type type);
73  static Optimizer * create(const ParametersMap & parameters);
74  static Optimizer * create(Optimizer::Type type, const ParametersMap & parameters = ParametersMap());
75 
76  // Get connected poses and constraints from a set of links
77  void getConnectedGraph(
78  int fromId,
79  const std::map<int, Transform> & posesIn,
80  const std::multimap<int, Link> & linksIn,
81  std::map<int, Transform> & posesOut,
82  std::multimap<int, Link> & linksOut) const;
83 
84 public:
85  virtual ~Optimizer() {}
86 
87  virtual Type type() const = 0;
88 
89  // getters
90  int iterations() const {return iterations_;}
91  bool isSlam2d() const {return slam2d_;}
92  bool isCovarianceIgnored() const {return covarianceIgnored_;}
93  double epsilon() const {return epsilon_;}
94  bool isRobust() const {return robust_;}
95  bool priorsIgnored() const {return priorsIgnored_;}
96  bool landmarksIgnored() const {return landmarksIgnored_;}
97  float gravitySigma() const {return gravitySigma_;}
98 
99  // setters
100  void setIterations(int iterations) {iterations_ = iterations;}
101  void setSlam2d(bool enabled) {slam2d_ = enabled;}
102  void setCovarianceIgnored(bool enabled) {covarianceIgnored_ = enabled;}
103  void setEpsilon(double epsilon) {epsilon_ = epsilon;}
104  void setRobust(bool enabled) {robust_ = enabled;}
105  void setPriorsIgnored(bool enabled) {priorsIgnored_ = enabled;}
106  void setLandmarksIgnored(bool enabled) {landmarksIgnored_ = enabled;}
107  void setGravitySigma(float value) {gravitySigma_ = value;}
108 
109  virtual void parseParameters(const ParametersMap & parameters);
110 
111  std::map<int, Transform> optimizeIncremental(
112  int rootId,
113  const std::map<int, Transform> & poses,
114  const std::multimap<int, Link> & constraints,
115  std::list<std::map<int, Transform> > * intermediateGraphes = 0,
116  double * finalError = 0,
117  int * iterationsDone = 0);
118 
119  std::map<int, Transform> optimize(
120  int rootId,
121  const std::map<int, Transform> & poses,
122  const std::multimap<int, Link> & constraints,
123  std::list<std::map<int, Transform> > * intermediateGraphes = 0,
124  double * finalError = 0,
125  int * iterationsDone = 0);
126 
127  // inherited classes should implement one of these methods
128  virtual std::map<int, Transform> optimize(
129  int rootId,
130  const std::map<int, Transform> & poses,
131  const std::multimap<int, Link> & constraints,
132  cv::Mat & outputCovariance,
133  std::list<std::map<int, Transform> > * intermediateGraphes = 0,
134  double * finalError = 0,
135  int * iterationsDone = 0);
136  virtual std::map<int, Transform> optimizeBA(
137  int rootId, // if negative, all other poses are fixed
138  const std::map<int, Transform> & poses,
139  const std::multimap<int, Link> & links,
140  const std::map<int, std::vector<CameraModel> > & models, // in case of stereo, Tx should be set
141  std::map<int, cv::Point3f> & points3DMap,
142  const std::map<int, std::map<int, FeatureBA> > & wordReferences, // <ID words, IDs frames + keypoint/depth/descriptor>
143  std::set<int> * outliers = 0);
144 
145  std::map<int, Transform> optimizeBA(
146  int rootId,
147  const std::map<int, Transform> & poses,
148  const std::multimap<int, Link> & links,
149  const std::map<int, Signature> & signatures,
150  std::map<int, cv::Point3f> & points3DMap,
151  std::map<int, std::map<int, FeatureBA> > & wordReferences, // <ID words, IDs frames + keypoint/depth/descriptor>
152  bool rematchFeatures = false);
153 
154  std::map<int, Transform> optimizeBA(
155  int rootId,
156  const std::map<int, Transform> & poses,
157  const std::multimap<int, Link> & links,
158  const std::map<int, Signature> & signatures,
159  bool rematchFeatures = false);
160 
161  Transform optimizeBA(
162  const Link & link,
163  const CameraModel & model,
164  std::map<int, cv::Point3f> & points3DMap,
165  const std::map<int, std::map<int, FeatureBA> > & wordReferences,
166  std::set<int> * outliers = 0);
167 
168  void computeBACorrespondences(
169  const std::map<int, Transform> & poses,
170  const std::multimap<int, Link> & links,
171  const std::map<int, Signature> & signatures,
172  std::map<int, cv::Point3f> & points3DMap,
173  std::map<int, std::map<int, FeatureBA > > & wordReferences, // <ID words, IDs frames + keypoint/depth/descriptor>
174  bool rematchFeatures = false);
175 
176 protected:
177  Optimizer(
178  int iterations = Parameters::defaultOptimizerIterations(),
179  bool slam2d = Parameters::defaultRegForce3DoF(),
180  bool covarianceIgnored = Parameters::defaultOptimizerVarianceIgnored(),
181  double epsilon = Parameters::defaultOptimizerEpsilon(),
182  bool robust = Parameters::defaultOptimizerRobust(),
183  bool priorsIgnored = Parameters::defaultOptimizerPriorsIgnored(),
184  bool landmarksIgnored = Parameters::defaultOptimizerLandmarksIgnored(),
185  float gravitySigma = Parameters::defaultOptimizerGravitySigma());
186  Optimizer(const ParametersMap & parameters);
187 
188 private:
190  bool slam2d_;
192  double epsilon_;
193  bool robust_;
197 };
198 
199 } /* namespace rtabmap */
200 #endif /* OPTIMIZER_H_ */
create
ADT create(const Signature &signature)
rtabmap::Optimizer::~Optimizer
virtual ~Optimizer()
Definition: Optimizer.h:85
rtabmap::FeatureBA::cameraIndex
int cameraIndex
Definition: Optimizer.h:55
rtabmap::FeatureBA::kpt
cv::KeyPoint kpt
Definition: Optimizer.h:52
rtabmap::Optimizer::priorsIgnored
bool priorsIgnored() const
Definition: Optimizer.h:95
rtabmap::Optimizer::slam2d_
bool slam2d_
Definition: Optimizer.h:190
rtabmap::Optimizer::setSlam2d
void setSlam2d(bool enabled)
Definition: Optimizer.h:101
rtabmap::CameraModel
Definition: CameraModel.h:38
rtabmap::Optimizer
Definition: Optimizer.h:61
rtabmap::Optimizer::landmarksIgnored
bool landmarksIgnored() const
Definition: Optimizer.h:96
type
rtabmap::Optimizer::epsilon_
double epsilon_
Definition: Optimizer.h:192
rtabmap::ParametersMap
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:43
rtabmap::Optimizer::iterations_
int iterations_
Definition: Optimizer.h:189
rtabmap::Optimizer::setCovarianceIgnored
void setCovarianceIgnored(bool enabled)
Definition: Optimizer.h:102
rtabmap::Optimizer::isSlam2d
bool isSlam2d() const
Definition: Optimizer.h:91
Parameters.h
rtabmap::Optimizer::epsilon
double epsilon() const
Definition: Optimizer.h:93
optimize
gtsam.ISAM2 optimize(List[GpsMeasurement] gps_measurements, List[ImuMeasurement] imu_measurements, gtsam.noiseModel.Diagonal sigma_init_x, gtsam.noiseModel.Diagonal sigma_init_v, gtsam.noiseModel.Diagonal sigma_init_b, gtsam.noiseModel.Diagonal noise_model_gps, KittiCalibration kitti_calibration, int first_gps_pose, int gps_skip)
rtabmap::Optimizer::setLandmarksIgnored
void setLandmarksIgnored(bool enabled)
Definition: Optimizer.h:106
Type
rtabmap::FeatureBA::FeatureBA
FeatureBA(const cv::KeyPoint &kptIn, const float &depthIn=0.0f, const cv::Mat &descriptorIn=cv::Mat(), int cameraIndexIn=0)
Definition: Optimizer.h:44
rtabmap::Optimizer::Type
Type
Definition: Optimizer.h:64
Signature.h
rtabmap::Optimizer::setEpsilon
void setEpsilon(double epsilon)
Definition: Optimizer.h:103
rtabmap::FeatureBA::depth
float depth
Definition: Optimizer.h:53
rtabmap::Optimizer::robust_
bool robust_
Definition: Optimizer.h:193
rtabmap::Optimizer::gravitySigma
float gravitySigma() const
Definition: Optimizer.h:97
rtabmap::FeatureBA
Definition: Optimizer.h:41
rtabmap::Optimizer::setGravitySigma
void setGravitySigma(float value)
Definition: Optimizer.h:107
rtabmap::Optimizer::gravitySigma_
float gravitySigma_
Definition: Optimizer.h:196
rtabmap::Transform
Definition: Transform.h:41
rtabmap::Optimizer::setRobust
void setRobust(bool enabled)
Definition: Optimizer.h:104
rtabmap::Optimizer::covarianceIgnored_
bool covarianceIgnored_
Definition: Optimizer.h:191
rtabmap::Optimizer::landmarksIgnored_
bool landmarksIgnored_
Definition: Optimizer.h:195
epsilon
double epsilon
rtabmap::Optimizer::priorsIgnored_
bool priorsIgnored_
Definition: Optimizer.h:194
rtabmap::Optimizer::iterations
int iterations() const
Definition: Optimizer.h:90
rtabmap::Optimizer::isRobust
bool isRobust() const
Definition: Optimizer.h:94
rtabmap::Optimizer::isCovarianceIgnored
bool isCovarianceIgnored() const
Definition: Optimizer.h:92
rtabmap::Optimizer::setPriorsIgnored
void setPriorsIgnored(bool enabled)
Definition: Optimizer.h:105
rtabmap
Definition: CameraARCore.cpp:35
rtabmap::FeatureBA::descriptor
cv::Mat descriptor
Definition: Optimizer.h:54
rtabmap::Optimizer::setIterations
void setIterations(int iterations)
Definition: Optimizer.h:100
trace.model
model
Definition: trace.py:4
value
value


rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Jul 25 2024 02:50:13