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/optimizer/OptimizerTORO.h>
40 #include <rtabmap/core/optimizer/OptimizerG2O.h>
41 #include <rtabmap/core/optimizer/OptimizerGTSAM.h>
42 #include <rtabmap/core/optimizer/OptimizerCVSBA.h>
43 #include <rtabmap/core/optimizer/OptimizerCeres.h>
44 
45 namespace rtabmap {
46 
47 bool Optimizer::isAvailable(Optimizer::Type type)
48 {
49  if(type == Optimizer::kTypeG2O)
50  {
51  return OptimizerG2O::available();
52  }
53  else if(type == Optimizer::kTypeGTSAM)
54  {
55  return OptimizerGTSAM::available();
56  }
57  else if(type == Optimizer::kTypeCVSBA)
58  {
59  return OptimizerCVSBA::available();
60  }
61  else if(type == Optimizer::kTypeTORO)
62  {
63  return OptimizerTORO::available();
64  }
65  else if(type == Optimizer::kTypeCeres)
66  {
67  return OptimizerCeres::available();
68  }
69  return false;
70 }
71 
72 Optimizer * Optimizer::create(const ParametersMap & parameters)
73 {
74  int optimizerTypeInt = Parameters::defaultOptimizerStrategy();
75  Parameters::parse(parameters, Parameters::kOptimizerStrategy(), optimizerTypeInt);
76  return create((Optimizer::Type)optimizerTypeInt, parameters);
77 }
78 
79 Optimizer * Optimizer::create(Optimizer::Type type, const ParametersMap & parameters)
80 {
81  UASSERT_MSG(OptimizerG2O::available() || OptimizerGTSAM::available() || OptimizerTORO::available() || OptimizerCeres::available(),
82  "RTAB-Map is not built with any graph optimization approach!");
83 
84  if(!OptimizerTORO::available() && type == Optimizer::kTypeTORO)
85  {
86  if(OptimizerGTSAM::available())
87  {
88  UWARN("TORO optimizer not available. GTSAM will be used instead.");
89  type = Optimizer::kTypeGTSAM;
90  }
91  else if(OptimizerG2O::available())
92  {
93  UWARN("TORO optimizer not available. g2o will be used instead.");
94  type = Optimizer::kTypeG2O;
95  }
96  else if(OptimizerCeres::available())
97  {
98  UWARN("TORO optimizer not available. ceres will be used instead.");
99  type = Optimizer::kTypeCeres;
100  }
101  }
102  if(!OptimizerG2O::available() && type == Optimizer::kTypeG2O)
103  {
104  if(OptimizerGTSAM::available())
105  {
106  UWARN("g2o optimizer not available. GTSAM will be used instead.");
107  type = Optimizer::kTypeGTSAM;
108  }
109  else if(OptimizerTORO::available())
110  {
111  UWARN("g2o optimizer not available. TORO will be used instead.");
112  type = Optimizer::kTypeTORO;
113  }
114  else if(OptimizerCeres::available())
115  {
116  UWARN("g2o optimizer not available. ceres will be used instead.");
117  type = Optimizer::kTypeCeres;
118  }
119  }
120  if(!OptimizerGTSAM::available() && type == Optimizer::kTypeGTSAM)
121  {
122  if(OptimizerG2O::available())
123  {
124  UWARN("GTSAM optimizer not available. g2o will be used instead.");
125  type = Optimizer::kTypeG2O;
126  }
127  else if(OptimizerTORO::available())
128  {
129  UWARN("GTSAM optimizer not available. TORO will be used instead.");
130  type = Optimizer::kTypeTORO;
131  }
132  else if(OptimizerCeres::available())
133  {
134  UWARN("GTSAM optimizer not available. ceres will be used instead.");
135  type = Optimizer::kTypeCeres;
136  }
137  }
138  if(!OptimizerCVSBA::available() && type == Optimizer::kTypeCVSBA)
139  {
140  if(OptimizerG2O::available())
141  {
142  UWARN("CVSBA optimizer not available. g2o will be used instead.");
143  type = Optimizer::kTypeG2O;
144  }
145  }
146  if(!OptimizerCeres::available() && type == Optimizer::kTypeCeres)
147  {
148  if(OptimizerGTSAM::available())
149  {
150  UWARN("Ceres optimizer not available. gtsam will be used instead.");
151  type = Optimizer::kTypeGTSAM;
152  }
153  else if(OptimizerG2O::available())
154  {
155  UWARN("Ceres optimizer not available. g2o will be used instead.");
156  type = Optimizer::kTypeG2O;
157  }
158  else if(OptimizerTORO::available())
159  {
160  UWARN("Ceres optimizer not available. TORO will be used instead.");
161  type = Optimizer::kTypeTORO;
162  }
163  }
164  Optimizer * optimizer = 0;
165  switch(type)
166  {
167  case Optimizer::kTypeGTSAM:
168  optimizer = new OptimizerGTSAM(parameters);
169  break;
170  case Optimizer::kTypeG2O:
171  optimizer = new OptimizerG2O(parameters);
172  break;
173  case Optimizer::kTypeCVSBA:
174  optimizer = new OptimizerCVSBA(parameters);
175  break;
176  case Optimizer::kTypeCeres:
177  optimizer = new OptimizerCeres(parameters);
178  break;
179  case Optimizer::kTypeTORO:
180  default:
181  optimizer = new OptimizerTORO(parameters);
182  break;
183 
184  }
185  return optimizer;
186 }
187 
188 void Optimizer::getConnectedGraph(
189  int fromId,
190  const std::map<int, Transform> & posesIn,
191  const std::multimap<int, Link> & linksIn,
192  std::map<int, Transform> & posesOut,
193  std::multimap<int, Link> & linksOut) const
194 {
195  UDEBUG("IN: fromId=%d poses=%d links=%d priorsIgnored=%d landmarksIgnored=%d", fromId, (int)posesIn.size(), (int)linksIn.size(), priorsIgnored()?1:0, landmarksIgnored()?1:0);
196  UASSERT(fromId>0);
197  UASSERT(uContains(posesIn, fromId));
198 
199  posesOut.clear();
200  linksOut.clear();
201 
202  std::set<int> nextPoses;
203  nextPoses.insert(fromId);
204  std::multimap<int, int> biLinks;
205  for(std::multimap<int, Link>::const_iterator iter=linksIn.begin(); iter!=linksIn.end(); ++iter)
206  {
207  if(iter->second.from() != iter->second.to())
208  {
209  if(graph::findLink(biLinks, iter->second.from(), iter->second.to()) == biLinks.end())
210  {
211  biLinks.insert(std::make_pair(iter->second.from(), iter->second.to()));
212  biLinks.insert(std::make_pair(iter->second.to(), iter->second.from()));
213  }
214  }
215  }
216 
217  while(nextPoses.size())
218  {
219  int fromId = *nextPoses.rbegin(); // fill up all nodes before landmarks
220  nextPoses.erase(*nextPoses.rbegin());
221 
222  if(posesOut.empty())
223  {
224  posesOut.insert(std::make_pair(fromId, posesIn.find(fromId)->second));
225 
226  // add prior links
227  for(std::multimap<int, Link>::const_iterator pter=linksIn.find(fromId); pter!=linksIn.end() && pter->first==fromId; ++pter)
228  {
229  if(pter->second.from() == pter->second.to() && (!priorsIgnored() || pter->second.type() != Link::kPosePrior))
230  {
231  linksOut.insert(*pter);
232  }
233  }
234  }
235 
236  for(std::multimap<int, int>::const_iterator iter=biLinks.find(fromId); iter!=biLinks.end() && iter->first==fromId; ++iter)
237  {
238  int toId = iter->second;
239  if(posesIn.find(toId) != posesIn.end() && (!landmarksIgnored() || toId>0))
240  {
241  std::multimap<int, Link>::const_iterator kter = graph::findLink(linksIn, fromId, toId);
242  if(nextPoses.find(toId) == nextPoses.end())
243  {
244  if(!uContains(posesOut, toId))
245  {
246  if(isSlam2d() && kter->second.type() == Link::kLandmark && toId>0)
247  {
248  Transform t;
249  if(kter->second.from()==fromId)
250  {
251  t = kter->second.transform();
252  }
253  else
254  {
255  t = kter->second.transform().inverse();
256  }
257  posesOut.insert(std::make_pair(toId, (posesOut.at(fromId) * t).to3DoF()));
258  }
259  else
260  {
261  Transform t = posesOut.at(fromId) * (kter->second.from()==fromId?kter->second.transform():kter->second.transform().inverse());
262  posesOut.insert(std::make_pair(toId, t));
263  }
264  // add prior links
265  for(std::multimap<int, Link>::const_iterator pter=linksIn.find(toId); pter!=linksIn.end() && pter->first==toId; ++pter)
266  {
267  if(pter->second.from() == pter->second.to() && (!priorsIgnored() || pter->second.type() != Link::kPosePrior))
268  {
269  linksOut.insert(*pter);
270  }
271  }
272 
273  nextPoses.insert(toId);
274  }
275 
276  // only add unique links
277  if(graph::findLink(linksOut, fromId, toId) == linksOut.end())
278  {
279  if(kter->second.to() < 0)
280  {
281  // For landmarks, make sure fromId is the landmark
282  linksOut.insert(std::make_pair(kter->second.to(), kter->second.inverse()));
283  }
284  else
285  {
286  linksOut.insert(*kter);
287  }
288  }
289  }
290  }
291  }
292  }
293  UDEBUG("OUT: poses=%d links=%d", (int)posesOut.size(), (int)linksOut.size());
294 }
295 
296 Optimizer::Optimizer(int iterations, bool slam2d, bool covarianceIgnored, double epsilon, bool robust, bool priorsIgnored, bool landmarksIgnored, float gravitySigma) :
297  iterations_(iterations),
298  slam2d_(slam2d),
299  covarianceIgnored_(covarianceIgnored),
300  epsilon_(epsilon),
301  robust_(robust),
302  priorsIgnored_(priorsIgnored),
303  landmarksIgnored_(landmarksIgnored),
304  gravitySigma_(gravitySigma)
305 {
306 }
307 
308 Optimizer::Optimizer(const ParametersMap & parameters) :
309  iterations_(Parameters::defaultOptimizerIterations()),
310  slam2d_(Parameters::defaultRegForce3DoF()),
311  covarianceIgnored_(Parameters::defaultOptimizerVarianceIgnored()),
312  epsilon_(Parameters::defaultOptimizerEpsilon()),
313  robust_(Parameters::defaultOptimizerRobust()),
314  priorsIgnored_(Parameters::defaultOptimizerPriorsIgnored()),
315  landmarksIgnored_(Parameters::defaultOptimizerLandmarksIgnored()),
316  gravitySigma_(Parameters::defaultOptimizerGravitySigma())
317 {
318  parseParameters(parameters);
319 }
320 
321 void Optimizer::parseParameters(const ParametersMap & parameters)
322 {
323  Parameters::parse(parameters, Parameters::kOptimizerIterations(), iterations_);
324  Parameters::parse(parameters, Parameters::kOptimizerVarianceIgnored(), covarianceIgnored_);
325  Parameters::parse(parameters, Parameters::kRegForce3DoF(), slam2d_);
326  Parameters::parse(parameters, Parameters::kOptimizerEpsilon(), epsilon_);
327  Parameters::parse(parameters, Parameters::kOptimizerRobust(), robust_);
328  Parameters::parse(parameters, Parameters::kOptimizerPriorsIgnored(), priorsIgnored_);
329  Parameters::parse(parameters, Parameters::kOptimizerLandmarksIgnored(), landmarksIgnored_);
330  Parameters::parse(parameters, Parameters::kOptimizerGravitySigma(), gravitySigma_);
331 }
332 
333 std::map<int, Transform> Optimizer::optimizeIncremental(
334  int rootId,
335  const std::map<int, Transform> & poses,
336  const std::multimap<int, Link> & constraints,
337  std::list<std::map<int, Transform> > * intermediateGraphes,
338  double * finalError,
339  int * iterationsDone)
340 {
341  std::map<int, Transform> incGraph;
342  std::multimap<int, Link> incGraphLinks;
343  incGraph.insert(*poses.begin());
344  int i=0;
345  std::multimap<int, Link> constraintsCpy = constraints;
346  UDEBUG("Incremental optimization... poses=%d constraints=%d", (int)poses.size(), (int)constraints.size());
347  for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
348  {
349  incGraph.insert(*iter);
350  bool hasLoopClosure = false;
351  for(std::multimap<int, Link>::iterator jter=constraintsCpy.lower_bound(iter->first); jter!=constraintsCpy.end() && jter->first==iter->first; ++jter)
352  {
353  UDEBUG("%d: %d -> %d type=%d", iter->first, jter->second.from(), jter->second.to(), jter->second.type());
354  if(jter->second.type() == Link::kNeighbor || jter->second.type() == Link::kNeighborMerged)
355  {
356  UASSERT(uContains(incGraph, iter->first));
357  incGraph.insert(std::make_pair(jter->second.to(), incGraph.at(iter->first) * jter->second.transform()));
358  incGraphLinks.insert(*jter);
359  }
360  else
361  {
362  if(!uContains(incGraph, jter->second.to()) && jter->second.to() > iter->first)
363  {
364  // node not yet in graph, switch link direction
365  constraintsCpy.insert(std::make_pair(jter->second.to(), jter->second.inverse()));
366  }
367  else
368  {
369  UASSERT(uContains(incGraph, jter->second.to()));
370  incGraphLinks.insert(*jter);
371  hasLoopClosure = true;
372  }
373  }
374  }
375  if(hasLoopClosure)
376  {
377  incGraph = this->optimize(incGraph.begin()->first, incGraph, incGraphLinks);
378  if(incGraph.empty())
379  {
380  UWARN("Failed incremental optimization... last pose added is %d", iter->first);
381  break;
382  }
383  }
384  UDEBUG("Iteration %d/%d %s", ++i, (int)poses.size(), hasLoopClosure?"*":"");
385  }
386  if(!incGraph.empty() && incGraph.size() == poses.size())
387  {
388  UASSERT(incGraphLinks.size() == constraints.size());
389  UASSERT(uContains(poses, rootId) && uContains(incGraph, rootId));
390  incGraph.at(rootId) = poses.at(rootId);
391  return this->optimize(rootId, incGraph, incGraphLinks, intermediateGraphes, finalError, iterationsDone);
392  }
393 
394  UDEBUG("Failed incremental optimization");
395  return std::map<int, Transform>();
396 }
397 
398 std::map<int, Transform> Optimizer::optimize(
399  int rootId,
400  const std::map<int, Transform> & poses,
401  const std::multimap<int, Link> & edgeConstraints,
402  std::list<std::map<int, Transform> > * intermediateGraphes,
403  double * finalError,
404  int * iterationsDone)
405 {
406  cv::Mat covariance;
407  return optimize(rootId,
408  poses,
409  edgeConstraints,
410  covariance,
411  intermediateGraphes,
412  finalError,
413  iterationsDone);
414 }
415 
416 std::map<int, Transform> Optimizer::optimize(
417  int rootId,
418  const std::map<int, Transform> & poses,
419  const std::multimap<int, Link> & constraints,
420  cv::Mat & outputCovariance,
421  std::list<std::map<int, Transform> > * intermediateGraphes,
422  double * finalError,
423  int * iterationsDone)
424 {
425  UERROR("Optimizer %d doesn't implement optimize() method.", (int)this->type());
426  return std::map<int, Transform>();
427 }
428 
429 std::map<int, Transform> Optimizer::optimizeBA(
430  int rootId,
431  const std::map<int, Transform> & poses,
432  const std::multimap<int, Link> & links,
433  const std::map<int, CameraModel> & models,
434  std::map<int, cv::Point3f> & points3DMap,
435  const std::map<int, std::map<int, FeatureBA> > & wordReferences,
436  std::set<int> * outliers)
437 {
438  UERROR("Optimizer %d doesn't implement optimizeBA() method.", (int)this->type());
439  return std::map<int, Transform>();
440 }
441 
442 std::map<int, Transform> Optimizer::optimizeBA(
443  int rootId,
444  const std::map<int, Transform> & posesIn,
445  const std::multimap<int, Link> & links,
446  const std::map<int, Signature> & signatures,
447  std::map<int, cv::Point3f> & points3DMap,
448  std::map<int, std::map<int, FeatureBA> > & wordReferences,
449  bool rematchFeatures)
450 {
451  UDEBUG("");
452  std::map<int, CameraModel> models;
453  std::map<int, Transform> poses;
454  for(std::map<int, Transform>::const_iterator iter=posesIn.lower_bound(1); iter!=posesIn.end(); ++iter)
455  {
456  // Get camera model
457  CameraModel model;
458  if(uContains(signatures, iter->first))
459  {
460  if(signatures.at(iter->first).sensorData().cameraModels().size() == 1 && signatures.at(iter->first).sensorData().cameraModels().at(0).isValidForProjection())
461  {
462  model = signatures.at(iter->first).sensorData().cameraModels()[0];
463  }
464  else if(signatures.at(iter->first).sensorData().stereoCameraModel().isValidForProjection())
465  {
466  model = signatures.at(iter->first).sensorData().stereoCameraModel().left();
467 
468  // Set Tx = -baseline*fx for stereo BA
469  model = CameraModel(
470  model.fx(),
471  model.fy(),
472  model.cx(),
473  model.cy(),
474  model.localTransform(),
475  -signatures.at(iter->first).sensorData().stereoCameraModel().baseline()*model.fx());
476  }
477  else if(signatures.at(iter->first).sensorData().cameraModels().size() > 1)
478  {
479  UERROR("Multi-cameras (%d) is not supported (id=%d).",
480  signatures.at(iter->first).sensorData().cameraModels().size(),
481  iter->first);
482  return std::map<int, Transform>();
483  }
484  else
485  {
486  UERROR("Missing calibration for node %d", iter->first);
487  return std::map<int, Transform>();
488  }
489  }
490  else
491  {
492  UERROR("Did not find node %d in cache", iter->first);
493  return std::map<int, Transform>();
494  }
495 
496  UASSERT(model.isValidForProjection());
497 
498  models.insert(std::make_pair(iter->first, model));
499  poses.insert(*iter);
500  }
501 
502  // compute correspondences
503  this->computeBACorrespondences(poses, links, signatures, points3DMap, wordReferences, rematchFeatures);
504 
505  return optimizeBA(rootId, poses, links, models, points3DMap, wordReferences);
506 }
507 
508 std::map<int, Transform> Optimizer::optimizeBA(
509  int rootId,
510  const std::map<int, Transform> & poses,
511  const std::multimap<int, Link> & links,
512  const std::map<int, Signature> & signatures,
513  bool rematchFeatures)
514 {
515  std::map<int, cv::Point3f> points3DMap;
516  std::map<int, std::map<int, FeatureBA> > wordReferences;
517  return optimizeBA(rootId, poses, links, signatures, points3DMap, wordReferences, rematchFeatures);
518 }
519 
520 Transform Optimizer::optimizeBA(
521  const Link & link,
522  const CameraModel & model,
523  std::map<int, cv::Point3f> & points3DMap,
524  const std::map<int, std::map<int, FeatureBA> > & wordReferences,
525  std::set<int> * outliers)
526 {
527  std::map<int, Transform> poses;
528  poses.insert(std::make_pair(link.from(), Transform::getIdentity()));
529  poses.insert(std::make_pair(link.to(), link.transform()));
530  std::multimap<int, Link> links;
531  links.insert(std::make_pair(link.from(), link));
532  std::map<int, CameraModel> models;
533  models.insert(std::make_pair(link.from(), model));
534  models.insert(std::make_pair(link.to(), model));
535  poses = optimizeBA(link.from(), poses, links, models, points3DMap, wordReferences, outliers);
536  if(poses.size() == 2)
537  {
538  return poses.rbegin()->second;
539  }
540  else
541  {
542  return link.transform();
543  }
544 }
545 
546 struct KeyPointCompare
547 {
548  bool operator() (const cv::KeyPoint& lhs, const cv::KeyPoint& rhs) const
549  {
550  return lhs.pt.x < rhs.pt.x || (lhs.pt.x == rhs.pt.x && lhs.pt.y < rhs.pt.y);
551  }
552 };
553 
554 void Optimizer::computeBACorrespondences(
555  const std::map<int, Transform> & poses,
556  const std::multimap<int, Link> & links,
557  const std::map<int, Signature> & signatures,
558  std::map<int, cv::Point3f> & points3DMap,
559  std::map<int, std::map<int, FeatureBA> > & wordReferences,
560  bool rematchFeatures)
561 {
562  UDEBUG("");
563  int wordCount = 0;
564  int edgeWithWordsAdded = 0;
565  std::map<int, std::map<cv::KeyPoint, int, KeyPointCompare> > frameToWordMap; // <FrameId, <Keypoint, wordId> >
566  for(std::multimap<int, Link>::const_iterator iter=links.lower_bound(1); iter!=links.end(); ++iter)
567  {
568  Link link = iter->second;
569  if(link.to() < link.from())
570  {
571  link = link.inverse();
572  }
573  if(link.to() != link.from() &&
574  uContains(signatures, link.from()) &&
575  uContains(signatures, link.to()) &&
576  uContains(poses, link.from()))
577  {
578  Signature sFrom = signatures.at(link.from());
579  if(sFrom.getWeight() >= 0) // ignore intermediate links
580  {
581  Signature sTo = signatures.at(link.to());
582  if(sTo.getWeight() < 0)
583  {
584  for(std::multimap<int, Link>::const_iterator jter=links.find(sTo.id());
585  sTo.getWeight() < 0 && jter!=links.end() && uContains(signatures, jter->second.to());
586  ++jter)
587  {
588  sTo = signatures.at(jter->second.to());
589  }
590  }
591 
592  if(sFrom.getWords().size() &&
593  sTo.getWords().size() &&
594  sFrom.getWords3().size())
595  {
596  ParametersMap regParam;
597  regParam.insert(ParametersPair(Parameters::kVisEstimationType(), "1"));
598  regParam.insert(ParametersPair(Parameters::kVisPnPReprojError(), "5"));
599  regParam.insert(ParametersPair(Parameters::kVisMinInliers(), "6"));
600  regParam.insert(ParametersPair(Parameters::kVisCorNNDR(), "0.6"));
601  RegistrationVis reg(regParam);
602 
603  if(!rematchFeatures)
604  {
605  sFrom.setWordsDescriptors(cv::Mat());
606  sTo.setWordsDescriptors(cv::Mat());
607  }
608 
609  RegistrationInfo info;
610  Transform t = reg.computeTransformationMod(sFrom, sTo, Transform(), &info);
611  //Transform t = reg.computeTransformationMod(sFrom, sTo, iter->second.transform(), &info);
612  UDEBUG("%d->%d, inliers=%d",sFrom.id(), sTo.id(), (int)info.inliersIDs.size());
613 
614  if(!t.isNull())
615  {
616  if(!rematchFeatures)
617  {
618  // set descriptors for the output
619  if(sFrom.getWords().size() &&
620  sFrom.getWordsDescriptors().empty() &&
621  (int)sFrom.getWords().size() == signatures.at(link.from()).getWordsDescriptors().rows)
622  {
623  sFrom.setWordsDescriptors(signatures.at(link.from()).getWordsDescriptors());
624  }
625  if(sTo.getWords().size() &&
626  sTo.getWordsDescriptors().empty() &&
627  (int)sTo.getWords().size() == signatures.at(link.to()).getWordsDescriptors().rows)
628  {
629  sTo.setWordsDescriptors(signatures.at(link.to()).getWordsDescriptors());
630  }
631  }
632 
633  Transform pose = poses.at(sFrom.id());
634  UASSERT(!pose.isNull());
635  for(unsigned int i=0; i<info.inliersIDs.size(); ++i)
636  {
637  int indexFrom = sFrom.getWords().lower_bound(info.inliersIDs[i])->second;
638  cv::Point3f p = sFrom.getWords3()[indexFrom];
639  if(p.x > 0.0f) // make sure the point is valid
640  {
641  cv::KeyPoint ptFrom = sFrom.getWordsKpts()[indexFrom];
642  int indexTo = sTo.getWords().lower_bound(info.inliersIDs[i])->second;
643  cv::KeyPoint ptTo = sTo.getWordsKpts()[indexTo];
644 
645  int wordId = -1;
646 
647  // find if the word is already added
648  std::map<int, std::map<cv::KeyPoint, int, KeyPointCompare> >::iterator fromIter = frameToWordMap.find(sFrom.id());
649  std::map<int, std::map<cv::KeyPoint, int, KeyPointCompare> >::iterator toIter = frameToWordMap.find(sTo.id());
650  bool fromAlreadyAdded = false;
651  bool toAlreadyAdded = false;
652  if( fromIter != frameToWordMap.end() &&
653  fromIter->second.find(ptFrom) != fromIter->second.end())
654  {
655  wordId = fromIter->second.at(ptFrom);
656  fromAlreadyAdded = true;
657  }
658  if( toIter != frameToWordMap.end() &&
659  toIter->second.find(ptTo) != toIter->second.end())
660  {
661  wordId = toIter->second.at(ptTo);
662  toAlreadyAdded = true;
663  }
664 
665  if(wordId == -1)
666  {
667  wordId = ++wordCount;
668  wordReferences.insert(std::make_pair(wordId, std::map<int, FeatureBA>()));
669 
670  p = util3d::transformPoint(p, pose);
671  points3DMap.insert(std::make_pair(wordId, p));
672  }
673  else
674  {
675  UASSERT(wordReferences.find(wordId) != wordReferences.end());
676  UASSERT(points3DMap.find(wordId) != points3DMap.end());
677  }
678 
679  if(!fromAlreadyAdded)
680  {
681  cv::Mat descriptorFrom;
682  if(!sFrom.getWordsDescriptors().empty())
683  {
684  UASSERT(indexFrom < sFrom.getWordsDescriptors().rows);
685  descriptorFrom = sFrom.getWordsDescriptors().row(indexFrom);
686  }
687  wordReferences.at(wordId).insert(std::make_pair(sFrom.id(), FeatureBA(ptFrom, p.x, descriptorFrom)));
688  frameToWordMap.insert(std::make_pair(sFrom.id(), std::map<cv::KeyPoint, int, KeyPointCompare>()));
689  frameToWordMap.at(sFrom.id()).insert(std::make_pair(ptFrom, wordId));
690  }
691 
692  if(!toAlreadyAdded)
693  {
694  cv::Mat descriptorTo;
695  if(!sTo.getWordsDescriptors().empty())
696  {
697  UASSERT(indexTo < sTo.getWordsDescriptors().rows);
698  descriptorTo = sTo.getWordsDescriptors().row(indexTo);
699  }
700  float depth = 0.0f;
701  if(!sTo.getWords3().empty())
702  {
703  UASSERT(indexTo < (int)sTo.getWords3().size());
704  const cv::Point3f & pt = sTo.getWords3()[indexTo];
705  if( pt.x > 0)
706  {
707  depth = pt.x;
708  }
709  }
710  wordReferences.at(wordId).insert(std::make_pair(sTo.id(), FeatureBA(ptTo, depth, descriptorTo)));
711  frameToWordMap.insert(std::make_pair(sTo.id(), std::map<cv::KeyPoint, int, KeyPointCompare>()));
712  frameToWordMap.at(sTo.id()).insert(std::make_pair(ptTo, wordId));
713  }
714  }
715  }
716  ++edgeWithWordsAdded;
717  }
718  else
719  {
720  UWARN("Not enough inliers (%d) between %d and %d", info.inliersIDs.size(), sFrom.id(), sTo.id());
721  }
722  }
723  }
724  }
725  }
726  UDEBUG("Added %d words (edges with words=%d/%d)", wordCount, edgeWithWordsAdded, links.size());
727 }
728 
729 } /* namespace rtabmap */
rtabmap::Optimizer::priorsIgnored
bool priorsIgnored() const
Definition: Optimizer.h:91
rtabmap::Parameters::parse
static bool parse(const ParametersMap &parameters, const std::string &key, bool &value)
Definition: Parameters.cpp:476
rtabmap::Optimizer::gravitySigma
float gravitySigma() const
Definition: Optimizer.h:93
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(), bool landmarksIgnored=Parameters::defaultOptimizerLandmarksIgnored(), float gravitySigma=Parameters::defaultOptimizerGravitySigma())
Definition: Optimizer.cpp:296
rtabmap::util3d::transformPoint
cv::Point3f RTABMAP_EXP transformPoint(const cv::Point3f &pt, const Transform &transform)
Definition: util3d_transforms.cpp:211
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, FeatureBA > > &wordReferences, std::set< int > *outliers=0)
Definition: Optimizer.cpp:429
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:44
rtabmap::Transform::getIdentity
static Transform getIdentity()
Definition: Transform.cpp:380
rtabmap::Optimizer::isSlam2d
bool isSlam2d() const
Definition: Optimizer.h:87
rtabmap::ParametersMap
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:43
UMath.h
Basic mathematics functions.
rtabmap::RegistrationInfo::inliersIDs
std::vector< int > inliersIDs
Definition: RegistrationInfo.h:81
UConversion.h
Some conversion functions.
rtabmap::Optimizer::isAvailable
static bool isAvailable(Optimizer::Type type)
Definition: Optimizer.cpp:47
rtabmap::Signature::getWordsKpts
const std::vector< cv::KeyPoint > & getWordsKpts() const
Definition: Signature.h:112
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:333
rtabmap::CameraModel::cx
double cx() const
Definition: CameraModel.h:104
rtabmap::Optimizer::landmarksIgnored
bool landmarksIgnored() const
Definition: Optimizer.h:92
rtabmap::Optimizer::parseParameters
virtual void parseParameters(const ParametersMap &parameters)
Definition: Optimizer.cpp:321
rtabmap::Signature::getWords
const std::multimap< int, int > & getWords() const
Definition: Signature.h:111
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:103
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:398
rtabmap::Signature::setWordsDescriptors
void setWordsDescriptors(const cv::Mat &descriptors)
Definition: Signature.cpp:304
rtabmap::Signature::getWordsDescriptors
const cv::Mat & getWordsDescriptors() const
Definition: Signature.h:115
rtabmap::Signature::id
int id() const
Definition: Signature.h:70
rtabmap::Optimizer::epsilon
double epsilon() const
Definition: Optimizer.h:89
rtabmap::Optimizer::iterations
int iterations() const
Definition: Optimizer.h:86
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:105
rtabmap::OptimizerG2O::available
static bool available()
Definition: OptimizerG2O.cpp:107
rtabmap::Optimizer::getConnectedGraph
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) const
Definition: Optimizer.cpp:188
rtabmap::CameraModel::isValidForProjection
bool isValidForProjection() const
Definition: CameraModel.h:87
rtabmap::Optimizer::type
virtual Type type() const =0
rtabmap::Signature::getWords3
const std::vector< cv::Point3f > & getWords3() const
Definition: Signature.h:131
UDEBUG
#define UDEBUG(...)
rtabmap::CameraModel::fx
double fx() const
Definition: CameraModel.h:102
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:969
UERROR
#define UERROR(...)
ULogger.h
ULogger class and convenient macros.
UWARN
#define UWARN(...)
rtabmap::Transform::inverse
Transform inverse() const
Definition: Transform.cpp:178
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, FeatureBA > > &wordReferences, bool rematchFeatures=false)
Definition: Optimizer.cpp:554
lhs
YYCODETYPE lhs
Definition: sqlite3.c:116035
rtabmap::Optimizer::create
static Optimizer * create(const ParametersMap &parameters)
Definition: Optimizer.cpp:72
rtabmap::CameraModel::localTransform
const Transform & localTransform() const
Definition: CameraModel.h:116

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