Optimizer.cpp
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 #include <rtabmap/utilite/ULogger.h>
29 #include <rtabmap/utilite/UStl.h>
30 #include <rtabmap/utilite/UMath.h>
31 #include <rtabmap/utilite/UConversion.h>
32 #include <rtabmap/core/Optimizer.h>
33 #include <rtabmap/core/Graph.h>
34 #include <rtabmap/core/util3d_transforms.h>
35 #include <rtabmap/core/RegistrationVis.h>
36 #include <set>
37 #include <queue>
38 
39 #include <rtabmap/core/OptimizerTORO.h>
40 #include <rtabmap/core/OptimizerG2O.h>
41 #include <rtabmap/core/OptimizerGTSAM.h>
42 #include <rtabmap/core/OptimizerCVSBA.h>
43 
44 namespace rtabmap {
45 
46 bool Optimizer::isAvailable(Optimizer::Type type)
47 {
48  if(type == Optimizer::kTypeG2O)
49  {
50  return OptimizerG2O::available();
51  }
52  else if(type == Optimizer::kTypeGTSAM)
53  {
54  return OptimizerGTSAM::available();
55  }
56  else if(type == Optimizer::kTypeCVSBA)
57  {
58  return OptimizerCVSBA::available();
59  }
60  else if(type == Optimizer::kTypeTORO)
61  {
62  return OptimizerTORO::available();
63  }
64  return false;
65 }
66 
67 Optimizer * Optimizer::create(const ParametersMap & parameters)
68 {
69  int optimizerTypeInt = Parameters::defaultOptimizerStrategy();
70  Parameters::parse(parameters, Parameters::kOptimizerStrategy(), optimizerTypeInt);
71  return create((Optimizer::Type)optimizerTypeInt, parameters);
72 }
73 
74 Optimizer * Optimizer::create(Optimizer::Type type, const ParametersMap & parameters)
75 {
76  UASSERT_MSG(OptimizerG2O::available() || OptimizerGTSAM::available() || OptimizerTORO::available(),
77  "RTAB-Map is not built with any graph optimization approach!");
78 
79  if(!OptimizerTORO::available() && type == Optimizer::kTypeTORO)
80  {
81  if(OptimizerGTSAM::available())
82  {
83  UWARN("TORO optimizer not available. GTSAM will be used instead.");
84  type = Optimizer::kTypeGTSAM;
85  }
86  else if(OptimizerG2O::available())
87  {
88  UWARN("TORO optimizer not available. g2o will be used instead.");
89  type = Optimizer::kTypeG2O;
90  }
91  }
92  if(!OptimizerG2O::available() && type == Optimizer::kTypeG2O)
93  {
94  if(OptimizerTORO::available())
95  {
96  UWARN("g2o optimizer not available. TORO will be used instead.");
97  type = Optimizer::kTypeTORO;
98  }
99  else if(OptimizerGTSAM::available())
100  {
101  UWARN("g2o optimizer not available. GTSAM will be used instead.");
102  type = Optimizer::kTypeGTSAM;
103  }
104  }
105  if(!OptimizerGTSAM::available() && type == Optimizer::kTypeGTSAM)
106  {
107  if(OptimizerTORO::available())
108  {
109  UWARN("GTSAM optimizer not available. TORO will be used instead.");
110  type = Optimizer::kTypeTORO;
111  }
112  else if(OptimizerG2O::available())
113  {
114  UWARN("GTSAM optimizer not available. g2o will be used instead.");
115  type = Optimizer::kTypeG2O;
116  }
117  }
118  if(!OptimizerCVSBA::available() && type == Optimizer::kTypeCVSBA)
119  {
120  if(OptimizerTORO::available())
121  {
122  UWARN("CVSBA optimizer not available. TORO will be used instead.");
123  type = Optimizer::kTypeTORO;
124  }
125  else if(OptimizerGTSAM::available())
126  {
127  UWARN("CVSBA optimizer not available. GTSAM will be used instead.");
128  type = Optimizer::kTypeGTSAM;
129  }
130  else if(OptimizerG2O::available())
131  {
132  UWARN("CVSBA optimizer not available. g2o will be used instead.");
133  type = Optimizer::kTypeG2O;
134  }
135  }
136  Optimizer * optimizer = 0;
137  switch(type)
138  {
139  case Optimizer::kTypeGTSAM:
140  optimizer = new OptimizerGTSAM(parameters);
141  break;
142  case Optimizer::kTypeG2O:
143  optimizer = new OptimizerG2O(parameters);
144  break;
145  case Optimizer::kTypeCVSBA:
146  optimizer = new OptimizerCVSBA(parameters);
147  break;
148  case Optimizer::kTypeTORO:
149  default:
150  optimizer = new OptimizerTORO(parameters);
151  break;
152 
153  }
154  return optimizer;
155 }
156 
157 void Optimizer::getConnectedGraph(
158  int fromId,
159  const std::map<int, Transform> & posesIn,
160  const std::multimap<int, Link> & linksIn,
161  std::map<int, Transform> & posesOut,
162  std::multimap<int, Link> & linksOut,
163  int depth)
164 {
165  UASSERT(depth >= 0);
166  UASSERT(fromId>0);
167  UASSERT(uContains(posesIn, fromId));
168 
169  posesOut.clear();
170  linksOut.clear();
171 
172  std::set<int> ids;
173  std::set<int> curentDepth;
174  std::set<int> nextDepth;
175  nextDepth.insert(fromId);
176  int d = 0;
177  std::multimap<int, int> biLinks;
178  for(std::multimap<int, Link>::const_iterator iter=linksIn.begin(); iter!=linksIn.end(); ++iter)
179  {
180  UASSERT_MSG(graph::findLink(biLinks, iter->second.from(), iter->second.to()) == biLinks.end(),
181  uFormat("Input links should be unique between two poses (%d->%d).",
182  iter->second.from(), iter->second.to()).c_str());
183  biLinks.insert(std::make_pair(iter->second.from(), iter->second.to()));
184  if(iter->second.from() != iter->second.to())
185  {
186  biLinks.insert(std::make_pair(iter->second.to(), iter->second.from()));
187  }
188  }
189 
190  while((depth == 0 || d < depth) && nextDepth.size())
191  {
192  curentDepth = nextDepth;
193  nextDepth.clear();
194 
195  for(std::set<int>::iterator jter = curentDepth.begin(); jter!=curentDepth.end(); ++jter)
196  {
197  if(ids.find(*jter) == ids.end())
198  {
199  ids.insert(*jter);
200  posesOut.insert(*posesIn.find(*jter));
201 
202  for(std::multimap<int, int>::const_iterator iter=biLinks.find(*jter); iter!=biLinks.end() && iter->first==*jter; ++iter)
203  {
204  int nextId = iter->second;
205  if(uContains(posesIn, nextId))
206  {
207  if(ids.find(nextId) == ids.end())
208  {
209  nextDepth.insert(nextId);
210 
211  std::multimap<int, Link>::const_iterator kter = graph::findLink(linksIn, *jter, nextId);
212  if(depth == 0 || d < depth-1)
213  {
214  linksOut.insert(*kter);
215  }
216  else if(curentDepth.find(nextId) != curentDepth.end() ||
217  ids.find(nextId) != ids.end())
218  {
219  linksOut.insert(*kter);
220  }
221  }
222  else if(*jter == nextId)
223  {
224  std::multimap<int, Link>::const_iterator kter = graph::findLink(linksIn, *jter, nextId);
225  linksOut.insert(*kter);
226  }
227  }
228  }
229  }
230  }
231  ++d;
232  }
233 }
234 
235 Optimizer::Optimizer(int iterations, bool slam2d, bool covarianceIgnored, double epsilon, bool robust, bool priorsIgnored) :
236  iterations_(iterations),
237  slam2d_(slam2d),
238  covarianceIgnored_(covarianceIgnored),
239  epsilon_(epsilon),
240  robust_(robust),
241  priorsIgnored_(priorsIgnored)
242 {
243 }
244 
245 Optimizer::Optimizer(const ParametersMap & parameters) :
246  iterations_(Parameters::defaultOptimizerIterations()),
247  slam2d_(Parameters::defaultRegForce3DoF()),
248  covarianceIgnored_(Parameters::defaultOptimizerVarianceIgnored()),
249  epsilon_(Parameters::defaultOptimizerEpsilon()),
250  robust_(Parameters::defaultOptimizerRobust()),
251  priorsIgnored_(Parameters::defaultOptimizerPriorsIgnored())
252 {
253  parseParameters(parameters);
254 }
255 
256 void Optimizer::parseParameters(const ParametersMap & parameters)
257 {
258  Parameters::parse(parameters, Parameters::kOptimizerIterations(), iterations_);
259  Parameters::parse(parameters, Parameters::kOptimizerVarianceIgnored(), covarianceIgnored_);
260  Parameters::parse(parameters, Parameters::kRegForce3DoF(), slam2d_);
261  Parameters::parse(parameters, Parameters::kOptimizerEpsilon(), epsilon_);
262  Parameters::parse(parameters, Parameters::kOptimizerRobust(), robust_);
263  Parameters::parse(parameters, Parameters::kOptimizerPriorsIgnored(), priorsIgnored_);
264 }
265 
266 std::map<int, Transform> Optimizer::optimizeIncremental(
267  int rootId,
268  const std::map<int, Transform> & poses,
269  const std::multimap<int, Link> & constraints,
270  std::list<std::map<int, Transform> > * intermediateGraphes,
271  double * finalError,
272  int * iterationsDone)
273 {
274  std::map<int, Transform> incGraph;
275  std::multimap<int, Link> incGraphLinks;
276  incGraph.insert(*poses.begin());
277  int i=0;
278  std::multimap<int, Link> constraintsCpy = constraints;
279  UDEBUG("Incremental optimization... poses=%d comstraints=%d", (int)poses.size(), (int)constraints.size());
280  for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
281  {
282  incGraph.insert(*iter);
283  bool hasLoopClosure = false;
284  for(std::multimap<int, Link>::iterator jter=constraintsCpy.lower_bound(iter->first); jter!=constraintsCpy.end() && jter->first==iter->first; ++jter)
285  {
286  UDEBUG("%d: %d -> %d type=%d", iter->first, jter->second.from(), jter->second.to(), jter->second.type());
287  if(jter->second.type() == Link::kNeighbor || jter->second.type() == Link::kNeighborMerged)
288  {
289  UASSERT(uContains(incGraph, iter->first));
290  incGraph.insert(std::make_pair(jter->second.to(), incGraph.at(iter->first) * jter->second.transform()));
291  incGraphLinks.insert(*jter);
292  }
293  else
294  {
295  if(!uContains(incGraph, jter->second.to()) && jter->second.to() > iter->first)
296  {
297  // node not yet in graph, switch link direction
298  constraintsCpy.insert(std::make_pair(jter->second.to(), jter->second.inverse()));
299  }
300  else
301  {
302  UASSERT(uContains(incGraph, jter->second.to()));
303  incGraphLinks.insert(*jter);
304  hasLoopClosure = true;
305  }
306  }
307  }
308  if(hasLoopClosure)
309  {
310  incGraph = this->optimize(incGraph.begin()->first, incGraph, incGraphLinks);
311  if(incGraph.empty())
312  {
313  UWARN("Failed incremental optimization...");
314  break;
315  }
316  }
317  UDEBUG("Iteration %d/%d %s", ++i, (int)poses.size(), hasLoopClosure?"*":"");
318  }
319  if(!incGraph.empty() && incGraph.size() == poses.size())
320  {
321  UASSERT(incGraphLinks.size() == constraints.size());
322  UASSERT(uContains(poses, rootId) && uContains(incGraph, rootId));
323  incGraph.at(rootId) = poses.at(rootId);
324  return this->optimize(rootId, incGraph, incGraphLinks, intermediateGraphes, finalError, iterationsDone);
325  }
326 
327  UDEBUG("Failed incremental optimization");
328  return std::map<int, Transform>();
329 }
330 
331 std::map<int, Transform> Optimizer::optimize(
332  int rootId,
333  const std::map<int, Transform> & poses,
334  const std::multimap<int, Link> & edgeConstraints,
335  std::list<std::map<int, Transform> > * intermediateGraphes,
336  double * finalError,
337  int * iterationsDone)
338 {
339  cv::Mat covariance;
340  return optimize(rootId,
341  poses,
342  edgeConstraints,
343  covariance,
344  intermediateGraphes,
345  finalError,
346  iterationsDone);
347 }
348 
349 std::map<int, Transform> Optimizer::optimize(
350  int rootId,
351  const std::map<int, Transform> & poses,
352  const std::multimap<int, Link> & constraints,
353  cv::Mat & outputCovariance,
354  std::list<std::map<int, Transform> > * intermediateGraphes,
355  double * finalError,
356  int * iterationsDone)
357 {
358  UERROR("Optimizer %d doesn't implement optimize() method.", (int)this->type());
359  return std::map<int, Transform>();
360 }
361 
362 std::map<int, Transform> Optimizer::optimizeBA(
363  int rootId,
364  const std::map<int, Transform> & poses,
365  const std::multimap<int, Link> & links,
366  const std::map<int, CameraModel> & models,
367  std::map<int, cv::Point3f> & points3DMap,
368  const std::map<int, std::map<int, cv::Point3f> > & wordReferences,
369  std::set<int> * outliers)
370 {
371  UERROR("Optimizer %d doesn't implement optimizeBA() method.", (int)this->type());
372  return std::map<int, Transform>();
373 }
374 
375 std::map<int, Transform> Optimizer::optimizeBA(
376  int rootId,
377  const std::map<int, Transform> & poses,
378  const std::multimap<int, Link> & links,
379  const std::map<int, Signature> & signatures)
380 {
381  UDEBUG("");
382  std::map<int, CameraModel> models;
383  for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
384  {
385  // Get camera model
386  CameraModel model;
387  if(uContains(signatures, iter->first))
388  {
389  if(signatures.at(iter->first).sensorData().cameraModels().size() == 1 && signatures.at(iter->first).sensorData().cameraModels().at(0).isValidForProjection())
390  {
391  model = signatures.at(iter->first).sensorData().cameraModels()[0];
392  }
393  else if(signatures.at(iter->first).sensorData().stereoCameraModel().isValidForProjection())
394  {
395  model = signatures.at(iter->first).sensorData().stereoCameraModel().left();
396 
397  // Set Tx = -baseline*fx for stereo BA
398  model = CameraModel(
399  model.fx(),
400  model.fy(),
401  model.cx(),
402  model.cy(),
403  model.localTransform(),
404  -signatures.at(iter->first).sensorData().stereoCameraModel().baseline()*model.fx());
405  }
406  else
407  {
408  UERROR("Missing calibration for node %d", iter->first);
409  return std::map<int, Transform>();
410  }
411  }
412  else
413  {
414  UERROR("Did not find node %d in cache", iter->first);
415  return std::map<int, Transform>();
416  }
417 
418  UASSERT(model.isValidForProjection());
419 
420  models.insert(std::make_pair(iter->first, model));
421  }
422 
423  // compute correspondences
424  std::map<int, cv::Point3f> points3DMap;
425  std::map<int, std::map<int, cv::Point3f> > wordReferences;
426  this->computeBACorrespondences(poses, links, signatures, points3DMap, wordReferences);
427 
428  return optimizeBA(rootId, poses, links, models, points3DMap, wordReferences);
429 }
430 
431 Transform Optimizer::optimizeBA(
432  const Link & link,
433  const CameraModel & model,
434  std::map<int, cv::Point3f> & points3DMap,
435  const std::map<int, std::map<int, cv::Point3f> > & wordReferences,
436  std::set<int> * outliers)
437 {
438  std::map<int, Transform> poses;
439  poses.insert(std::make_pair(link.from(), Transform::getIdentity()));
440  poses.insert(std::make_pair(link.to(), link.transform()));
441  std::multimap<int, Link> links;
442  links.insert(std::make_pair(link.from(), link));
443  std::map<int, CameraModel> models;
444  models.insert(std::make_pair(link.from(), model));
445  models.insert(std::make_pair(link.to(), model));
446  poses = optimizeBA(link.from(), poses, links, models, points3DMap, wordReferences, outliers);
447  if(poses.size() == 2)
448  {
449  return poses.rbegin()->second;
450  }
451  else
452  {
453  return link.transform();
454  }
455 }
456 
457 void Optimizer::computeBACorrespondences(
458  const std::map<int, Transform> & poses,
459  const std::multimap<int, Link> & links,
460  const std::map<int, Signature> & signatures,
461  std::map<int, cv::Point3f> & points3DMap,
462  std::map<int, std::map<int, cv::Point3f> > & wordReferences) // <ID words, IDs frames + keypoint/depth>
463 {
464  UDEBUG("");
465  int wordCount = 0;
466  int edgeWithWordsAdded = 0;
467  for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
468  {
469  Link link = iter->second;
470  if(link.to() < link.from())
471  {
472  link = link.inverse();
473  }
474  if(uContains(signatures, link.from()) &&
475  uContains(signatures, link.to()) &&
476  uContains(poses, link.from()))
477  {
478  Signature sFrom = signatures.at(link.from());
479  if(sFrom.getWeight() >= 0) // ignore intermediate links
480  {
481  Signature sTo = signatures.at(link.to());
482  if(sTo.getWeight() < 0)
483  {
484  for(std::multimap<int, Link>::const_iterator jter=links.find(sTo.id());
485  sTo.getWeight() < 0 && jter!=links.end() && uContains(signatures, jter->second.to());
486  ++jter)
487  {
488  sTo = signatures.at(jter->second.to());
489  }
490  }
491 
492  if(sFrom.getWords().size() &&
493  sTo.getWords().size() &&
494  sFrom.getWords3().size())
495  {
496  ParametersMap regParam;
497  regParam.insert(ParametersPair(Parameters::kVisEstimationType(), "1"));
498  regParam.insert(ParametersPair(Parameters::kVisPnPReprojError(), "5"));
499  regParam.insert(ParametersPair(Parameters::kVisMinInliers(), "5"));
500  regParam.insert(ParametersPair(Parameters::kVisCorNNDR(), "0.6"));
501  RegistrationVis reg(regParam);
502 
503  //sFrom.setWordsDescriptors(std::multimap<int, cv::Mat>());
504  //sTo.setWordsDescriptors(std::multimap<int, cv::Mat>());
505 
506  RegistrationInfo info;
507  Transform t = reg.computeTransformationMod(sFrom, sTo, Transform(), &info);
508  //Transform t = reg.computeTransformationMod(sFrom, sTo, iter->second.transform(), &info);
509  UDEBUG("%d->%d, inliers=%d",sFrom.id(), sTo.id(), (int)info.inliersIDs.size());
510 
511  if(!t.isNull())
512  {
513  Transform pose = poses.at(sFrom.id());
514  UASSERT(!pose.isNull());
515  for(unsigned int i=0; i<info.inliersIDs.size(); ++i)
516  {
517  cv::Point3f p = sFrom.getWords3().lower_bound(info.inliersIDs[i])->second;
518  if(p.x > 0.0f) // make sure the point is valid
519  {
520  int wordId = ++wordCount;
521 
522  wordReferences.insert(std::make_pair(wordId, std::map<int, cv::Point3f>()));
523 
524  cv::Point2f pt = sFrom.getWords().lower_bound(info.inliersIDs[i])->second.pt;
525  wordReferences.at(wordId).insert(std::make_pair(sFrom.id(), cv::Point3f(pt.x, pt.y, p.x)));
526 
527 
528  pt = sTo.getWords().lower_bound(info.inliersIDs[i])->second.pt;
529  float depth = 0.0f;
530  std::multimap<int, cv::Point3f>::const_iterator iterTo = sTo.getWords3().lower_bound(info.inliersIDs[i]);
531  if( iterTo!=sTo.getWords3().end() &&
532  iterTo->second.x > 0)
533  {
534  depth = iterTo->second.x;
535  }
536  wordReferences.at(wordId).insert(std::make_pair(sTo.id(), cv::Point3f(pt.x, pt.y, depth)));
537 
538  p = util3d::transformPoint(p, pose);
539  points3DMap.insert(std::make_pair(wordId, p));
540  }
541  }
542  ++edgeWithWordsAdded;
543  }
544  else
545  {
546  UWARN("Not enough inliers (%d) between %d and %d", info.inliersIDs.size(), sFrom.id(), sTo.id());
547  }
548  }
549  }
550  }
551  }
552  UDEBUG("Added %d words (edges with words=%d/%d)", wordCount, edgeWithWordsAdded, links.size());
553 }
554 
555 } /* namespace rtabmap */
rtabmap::Signature::getWords3
const std::multimap< int, cv::Point3f > & getWords3() const
Definition: Signature.h:128
d
d
rtabmap::Optimizer::priorsIgnored
bool priorsIgnored() const
Definition: Optimizer.h:78
rtabmap::Parameters::parse
static bool parse(const ParametersMap &parameters, const std::string &key, bool &value)
Definition: Parameters.cpp:446
rtabmap::Optimizer::computeBACorrespondences
void computeBACorrespondences(const std::map< int, Transform > &poses, const std::multimap< int, Link > &links, const std::map< int, Signature > &signatures, std::map< int, cv::Point3f > &points3DMap, std::map< int, std::map< int, cv::Point3f > > &wordReferences)
Definition: Optimizer.cpp:457
rtabmap::util3d::transformPoint
cv::Point3f RTABMAP_EXP transformPoint(const cv::Point3f &pt, const Transform &transform)
Definition: util3d_transforms.cpp:193
rtabmap::Signature::getWords
const std::multimap< int, cv::KeyPoint > & getWords() const
Definition: Signature.h:108
rtabmap::Registration::computeTransformationMod
Transform computeTransformationMod(Signature &from, Signature &to, Transform guess=Transform::getIdentity(), RegistrationInfo *info=0) const
Definition: Registration.cpp:188
rtabmap::ParametersPair
std::pair< std::string, std::string > ParametersPair
Definition: Parameters.h:42
rtabmap::Transform::getIdentity
static Transform getIdentity()
Definition: Transform.cpp:364
rtabmap::ParametersMap
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:41
UMath.h
Basic mathematics functions.
rtabmap::RegistrationInfo::inliersIDs
std::vector< int > inliersIDs
Definition: RegistrationInfo.h:70
UConversion.h
Some conversion functions.
rtabmap::Optimizer::isAvailable
static bool isAvailable(Optimizer::Type type)
Definition: Optimizer.cpp:46
UASSERT
#define UASSERT(condition)
UStl.h
Wrappers of STL for convenient functions.
rtabmap::Optimizer::optimizeIncremental
std::map< int, Transform > optimizeIncremental(int rootId, const std::map< int, Transform > &poses, const std::multimap< int, Link > &constraints, std::list< std::map< int, Transform > > *intermediateGraphes=0, double *finalError=0, int *iterationsDone=0)
Definition: Optimizer.cpp:266
rtabmap::CameraModel::cx
double cx() const
Definition: CameraModel.h:97
rtabmap::Optimizer::Optimizer
Optimizer(int iterations=Parameters::defaultOptimizerIterations(), bool slam2d=Parameters::defaultRegForce3DoF(), bool covarianceIgnored=Parameters::defaultOptimizerVarianceIgnored(), double epsilon=Parameters::defaultOptimizerEpsilon(), bool robust=Parameters::defaultOptimizerRobust(), bool priorsIgnored=Parameters::defaultOptimizerPriorsIgnored())
Definition: Optimizer.cpp:235
rtabmap::Optimizer::parseParameters
virtual void parseParameters(const ParametersMap &parameters)
Definition: Optimizer.cpp:256
rtabmap::Transform::isNull
bool isNull() const
Definition: Transform.cpp:107
UASSERT_MSG
#define UASSERT_MSG(condition, msg_str)
Definition: ULogger.h:67
rtabmap::CameraModel::fy
double fy() const
Definition: CameraModel.h:96
rtabmap::Optimizer::optimize
std::map< int, Transform > optimize(int rootId, const std::map< int, Transform > &poses, const std::multimap< int, Link > &constraints, std::list< std::map< int, Transform > > *intermediateGraphes=0, double *finalError=0, int *iterationsDone=0)
Definition: Optimizer.cpp:331
rtabmap::Signature::id
int id() const
Definition: Signature.h:70
rtabmap::Optimizer::epsilon
double epsilon() const
Definition: Optimizer.h:76
rtabmap::Optimizer::iterations
int iterations() const
Definition: Optimizer.h:73
rtabmap::Signature::getWeight
int getWeight() const
Definition: Signature.h:74
util3d_transforms.h
uContains
bool uContains(const std::list< V > &list, const V &value)
Definition: UStl.h:409
rtabmap::CameraModel::cy
double cy() const
Definition: CameraModel.h:98
rtabmap::OptimizerG2O::available
static bool available()
Definition: OptimizerG2O.cpp:101
rtabmap::CameraModel::isValidForProjection
bool isValidForProjection() const
Definition: CameraModel.h:80
rtabmap::Optimizer::type
virtual Type type() const =0
rtabmap::Optimizer::optimizeBA
virtual std::map< int, Transform > optimizeBA(int rootId, const std::map< int, Transform > &poses, const std::multimap< int, Link > &links, const std::map< int, CameraModel > &models, std::map< int, cv::Point3f > &points3DMap, const std::map< int, std::map< int, cv::Point3f > > &wordReferences, std::set< int > *outliers=0)
Definition: Optimizer.cpp:362
UDEBUG
#define UDEBUG(...)
rtabmap::CameraModel::fx
double fx() const
Definition: CameraModel.h:95
rtabmap::graph::findLink
std::multimap< int, Link >::iterator RTABMAP_EXP findLink(std::multimap< int, Link > &links, int from, int to, bool checkBothWays=true)
Definition: Graph.cpp:825
UERROR
#define UERROR(...)
ULogger.h
ULogger class and convenient macros.
UWARN
#define UWARN(...)
rtabmap::Optimizer::getConnectedGraph
static void getConnectedGraph(int fromId, const std::map< int, Transform > &posesIn, const std::multimap< int, Link > &linksIn, std::map< int, Transform > &posesOut, std::multimap< int, Link > &linksOut, int depth=0)
Definition: Optimizer.cpp:157
uFormat
std::string UTILITE_EXP uFormat(const char *fmt,...)
Definition: UConversion.cpp:350
rtabmap::Optimizer::create
static Optimizer * create(const ParametersMap &parameters)
Definition: Optimizer.cpp:67
rtabmap::CameraModel::localTransform
const Transform & localTransform() const
Definition: CameraModel.h:109

rtabmap
Author(s): Mathieu Labbe
autogenerated on Wed Jun 5 2019 22:41:32