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,
194  bool adjustPosesWithConstraints) const
195 {
196  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);
197  UASSERT(fromId>0);
198  UASSERT(uContains(posesIn, fromId));
199 
200  posesOut.clear();
201  linksOut.clear();
202 
203  std::set<int> nextPoses;
204  nextPoses.insert(fromId);
205  std::multimap<int, int> biLinks;
206  for(std::multimap<int, Link>::const_iterator iter=linksIn.begin(); iter!=linksIn.end(); ++iter)
207  {
208  if(iter->second.from() != iter->second.to())
209  {
210  if(graph::findLink(biLinks, iter->second.from(), iter->second.to()) == biLinks.end())
211  {
212  biLinks.insert(std::make_pair(iter->second.from(), iter->second.to()));
213  biLinks.insert(std::make_pair(iter->second.to(), iter->second.from()));
214  }
215  }
216  }
217 
218  while(nextPoses.size())
219  {
220  int currentId = *nextPoses.rbegin(); // fill up all nodes before landmarks
221  nextPoses.erase(*nextPoses.rbegin());
222 
223  if(posesOut.empty())
224  {
225  posesOut.insert(std::make_pair(currentId, posesIn.find(currentId)->second));
226 
227  // add prior links
228  for(std::multimap<int, Link>::const_iterator pter=linksIn.find(currentId); pter!=linksIn.end() && pter->first==currentId; ++pter)
229  {
230  if(pter->second.from() == pter->second.to() && (!priorsIgnored() || pter->second.type() != Link::kPosePrior))
231  {
232  linksOut.insert(*pter);
233  }
234  }
235  }
236 
237  for(std::multimap<int, int>::const_iterator iter=biLinks.find(currentId); iter!=biLinks.end() && iter->first==currentId; ++iter)
238  {
239  int toId = iter->second;
240  if(posesIn.find(toId) != posesIn.end() && (!landmarksIgnored() || toId>0))
241  {
242  std::multimap<int, Link>::const_iterator kter = graph::findLink(linksIn, currentId, toId);
243  if(nextPoses.find(toId) == nextPoses.end())
244  {
245  if(!uContains(posesOut, toId))
246  {
247  if(adjustPosesWithConstraints)
248  {
249  if(isSlam2d() && kter->second.type() == Link::kLandmark && toId>0)
250  {
251  Transform t;
252  if(kter->second.from()==currentId)
253  {
254  t = kter->second.transform();
255  }
256  else
257  {
258  t = kter->second.transform().inverse();
259  }
260  posesOut.insert(std::make_pair(toId, (posesOut.at(currentId) * t).to3DoF()));
261  }
262  else
263  {
264  Transform t = posesOut.at(currentId) * (kter->second.from()==currentId?kter->second.transform():kter->second.transform().inverse());
265  posesOut.insert(std::make_pair(toId, t));
266  }
267  }
268  else
269  {
270  posesOut.insert(*posesIn.find(toId));
271  }
272  // add prior links
273  for(std::multimap<int, Link>::const_iterator pter=linksIn.find(toId); pter!=linksIn.end() && pter->first==toId; ++pter)
274  {
275  if(pter->second.from() == pter->second.to() && (!priorsIgnored() || pter->second.type() != Link::kPosePrior))
276  {
277  linksOut.insert(*pter);
278  }
279  }
280 
281  nextPoses.insert(toId);
282  }
283 
284  // only add unique links
285  if(graph::findLink(linksOut, currentId, toId) == linksOut.end())
286  {
287  if(kter->second.to() < 0)
288  {
289  // For landmarks, make sure fromId is the landmark
290  linksOut.insert(std::make_pair(kter->second.to(), kter->second.inverse()));
291  }
292  else
293  {
294  linksOut.insert(*kter);
295  }
296  }
297  }
298  }
299  }
300  }
301  UDEBUG("OUT: poses=%d links=%d", (int)posesOut.size(), (int)linksOut.size());
302 }
303 
304 Optimizer::Optimizer(int iterations, bool slam2d, bool covarianceIgnored, double epsilon, bool robust, bool priorsIgnored, bool landmarksIgnored, float gravitySigma) :
305  iterations_(iterations),
306  slam2d_(slam2d),
307  covarianceIgnored_(covarianceIgnored),
308  epsilon_(epsilon),
309  robust_(robust),
310  priorsIgnored_(priorsIgnored),
311  landmarksIgnored_(landmarksIgnored),
312  gravitySigma_(gravitySigma)
313 {
314 }
315 
316 Optimizer::Optimizer(const ParametersMap & parameters) :
317  iterations_(Parameters::defaultOptimizerIterations()),
318  slam2d_(Parameters::defaultRegForce3DoF()),
319  covarianceIgnored_(Parameters::defaultOptimizerVarianceIgnored()),
320  epsilon_(Parameters::defaultOptimizerEpsilon()),
321  robust_(Parameters::defaultOptimizerRobust()),
322  priorsIgnored_(Parameters::defaultOptimizerPriorsIgnored()),
323  landmarksIgnored_(Parameters::defaultOptimizerLandmarksIgnored()),
324  gravitySigma_(Parameters::defaultOptimizerGravitySigma())
325 {
326  parseParameters(parameters);
327 }
328 
329 void Optimizer::parseParameters(const ParametersMap & parameters)
330 {
331  Parameters::parse(parameters, Parameters::kOptimizerIterations(), iterations_);
332  Parameters::parse(parameters, Parameters::kOptimizerVarianceIgnored(), covarianceIgnored_);
333  Parameters::parse(parameters, Parameters::kRegForce3DoF(), slam2d_);
334  Parameters::parse(parameters, Parameters::kOptimizerEpsilon(), epsilon_);
335  Parameters::parse(parameters, Parameters::kOptimizerRobust(), robust_);
336  Parameters::parse(parameters, Parameters::kOptimizerPriorsIgnored(), priorsIgnored_);
337  Parameters::parse(parameters, Parameters::kOptimizerLandmarksIgnored(), landmarksIgnored_);
338  Parameters::parse(parameters, Parameters::kOptimizerGravitySigma(), gravitySigma_);
339 }
340 
341 std::map<int, Transform> Optimizer::optimizeIncremental(
342  int rootId,
343  const std::map<int, Transform> & poses,
344  const std::multimap<int, Link> & constraints,
345  std::list<std::map<int, Transform> > * intermediateGraphes,
346  double * finalError,
347  int * iterationsDone)
348 {
349  std::map<int, Transform> incGraph;
350  std::multimap<int, Link> incGraphLinks;
351  incGraph.insert(*poses.begin());
352  int i=0;
353  std::multimap<int, Link> constraintsCpy = constraints;
354  UDEBUG("Incremental optimization... poses=%d constraints=%d", (int)poses.size(), (int)constraints.size());
355  for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
356  {
357  incGraph.insert(*iter);
358  bool hasLoopClosure = false;
359  for(std::multimap<int, Link>::iterator jter=constraintsCpy.lower_bound(iter->first); jter!=constraintsCpy.end() && jter->first==iter->first; ++jter)
360  {
361  UDEBUG("%d: %d -> %d type=%d", iter->first, jter->second.from(), jter->second.to(), jter->second.type());
362  if(jter->second.type() == Link::kNeighbor || jter->second.type() == Link::kNeighborMerged)
363  {
364  UASSERT(uContains(incGraph, iter->first));
365  incGraph.insert(std::make_pair(jter->second.to(), incGraph.at(iter->first) * jter->second.transform()));
366  incGraphLinks.insert(*jter);
367  }
368  else
369  {
370  if(!uContains(incGraph, jter->second.to()) && jter->second.to() > iter->first)
371  {
372  // node not yet in graph, switch link direction
373  constraintsCpy.insert(std::make_pair(jter->second.to(), jter->second.inverse()));
374  }
375  else
376  {
377  UASSERT(uContains(incGraph, jter->second.to()));
378  incGraphLinks.insert(*jter);
379  hasLoopClosure = true;
380  }
381  }
382  }
383  if(hasLoopClosure)
384  {
385  incGraph = this->optimize(incGraph.begin()->first, incGraph, incGraphLinks);
386  if(incGraph.empty())
387  {
388  UWARN("Failed incremental optimization... last pose added is %d", iter->first);
389  break;
390  }
391  }
392  UDEBUG("Iteration %d/%d %s", ++i, (int)poses.size(), hasLoopClosure?"*":"");
393  }
394  if(!incGraph.empty() && incGraph.size() == poses.size())
395  {
396  UASSERT(incGraphLinks.size() == constraints.size());
397  UASSERT(uContains(poses, rootId) && uContains(incGraph, rootId));
398  incGraph.at(rootId) = poses.at(rootId);
399  return this->optimize(rootId, incGraph, incGraphLinks, intermediateGraphes, finalError, iterationsDone);
400  }
401 
402  UDEBUG("Failed incremental optimization");
403  return std::map<int, Transform>();
404 }
405 
406 std::map<int, Transform> Optimizer::optimize(
407  int rootId,
408  const std::map<int, Transform> & poses,
409  const std::multimap<int, Link> & edgeConstraints,
410  std::list<std::map<int, Transform> > * intermediateGraphes,
411  double * finalError,
412  int * iterationsDone)
413 {
414  cv::Mat covariance;
415  return optimize(rootId,
416  poses,
417  edgeConstraints,
418  covariance,
419  intermediateGraphes,
420  finalError,
421  iterationsDone);
422 }
423 
424 std::map<int, Transform> Optimizer::optimize(
425  int rootId,
426  const std::map<int, Transform> & poses,
427  const std::multimap<int, Link> & constraints,
428  cv::Mat & outputCovariance,
429  std::list<std::map<int, Transform> > * intermediateGraphes,
430  double * finalError,
431  int * iterationsDone)
432 {
433  UERROR("Optimizer %d doesn't implement optimize() method.", (int)this->type());
434  return std::map<int, Transform>();
435 }
436 
437 std::map<int, Transform> Optimizer::optimizeBA(
438  int rootId,
439  const std::map<int, Transform> & poses,
440  const std::multimap<int, Link> & links,
441  const std::map<int, std::vector<CameraModel> > & models,
442  std::map<int, cv::Point3f> & points3DMap,
443  const std::map<int, std::map<int, FeatureBA> > & wordReferences,
444  std::set<int> * outliers)
445 {
446  UERROR("Optimizer %d doesn't implement optimizeBA() method.", (int)this->type());
447  return std::map<int, Transform>();
448 }
449 
450 std::map<int, Transform> Optimizer::optimizeBA(
451  int rootId,
452  const std::map<int, Transform> & posesIn,
453  const std::multimap<int, Link> & links,
454  const std::map<int, Signature> & signatures,
455  std::map<int, cv::Point3f> & points3DMap,
456  std::map<int, std::map<int, FeatureBA> > & wordReferences,
457  bool rematchFeatures)
458 {
459  UDEBUG("");
460  std::map<int, std::vector<CameraModel> > multiModels;
461  std::map<int, Transform> poses;
462  for(std::map<int, Transform>::const_iterator iter=posesIn.lower_bound(1); iter!=posesIn.end(); ++iter)
463  {
464  // Get camera model
465  std::vector<CameraModel> models;
466  if(uContains(signatures, iter->first))
467  {
468  const SensorData & s = signatures.at(iter->first).sensorData();
469  if(s.cameraModels().size() >= 1 && s.cameraModels().at(0).isValidForProjection())
470  {
471  models = s.cameraModels();
472  }
473  else if(!s.stereoCameraModels().empty() && s.stereoCameraModels()[0].isValidForProjection())
474  {
475  for(size_t i=0; i<s.stereoCameraModels().size(); ++i)
476  {
477  CameraModel model = s.stereoCameraModels()[i].left();
478 
479  // Set Tx = -baseline*fx for stereo BA
480  models.push_back(CameraModel(
481  model.fx(),
482  model.fy(),
483  model.cx(),
484  model.cy(),
485  model.localTransform(),
486  -s.stereoCameraModels()[i].baseline()*model.fx(),
487  model.imageSize()));
488  }
489  }
490  else
491  {
492  UERROR("Missing calibration for node %d", iter->first);
493  return std::map<int, Transform>();
494  }
495  }
496  else
497  {
498  UERROR("Did not find node %d in cache", iter->first);
499  return std::map<int, Transform>();
500  }
501 
502  multiModels.insert(std::make_pair(iter->first, models));
503  poses.insert(*iter);
504  }
505 
506  // compute correspondences
507  this->computeBACorrespondences(poses, links, signatures, points3DMap, wordReferences, rematchFeatures);
508 
509  return optimizeBA(rootId, poses, links, multiModels, points3DMap, wordReferences);
510 }
511 
512 std::map<int, Transform> Optimizer::optimizeBA(
513  int rootId,
514  const std::map<int, Transform> & poses,
515  const std::multimap<int, Link> & links,
516  const std::map<int, Signature> & signatures,
517  bool rematchFeatures)
518 {
519  std::map<int, cv::Point3f> points3DMap;
520  std::map<int, std::map<int, FeatureBA> > wordReferences;
521  return optimizeBA(rootId, poses, links, signatures, points3DMap, wordReferences, rematchFeatures);
522 }
523 
524 Transform Optimizer::optimizeBA(
525  const Link & link,
526  const CameraModel & model,
527  std::map<int, cv::Point3f> & points3DMap,
528  const std::map<int, std::map<int, FeatureBA> > & wordReferences,
529  std::set<int> * outliers)
530 {
531  std::map<int, Transform> poses;
532  poses.insert(std::make_pair(link.from(), Transform::getIdentity()));
533  poses.insert(std::make_pair(link.to(), link.transform()));
534  std::multimap<int, Link> links;
535  links.insert(std::make_pair(link.from(), link));
536  std::map<int, std::vector<CameraModel> > models;
537  std::vector<CameraModel> tmp;
538  tmp.push_back(model);
539  models.insert(std::make_pair(link.from(), tmp));
540  models.insert(std::make_pair(link.to(), tmp));
541  poses = optimizeBA(link.from(), poses, links, models, points3DMap, wordReferences, outliers);
542  if(poses.size() == 2)
543  {
544  return poses.rbegin()->second;
545  }
546  else
547  {
548  return link.transform();
549  }
550 }
551 
552 struct KeyPointCompare
553 {
554  bool operator() (const cv::KeyPoint& lhs, const cv::KeyPoint& rhs) const
555  {
556  return lhs.pt.x < rhs.pt.x || (lhs.pt.x == rhs.pt.x && lhs.pt.y < rhs.pt.y);
557  }
558 };
559 
560 void Optimizer::computeBACorrespondences(
561  const std::map<int, Transform> & poses,
562  const std::multimap<int, Link> & links,
563  const std::map<int, Signature> & signatures,
564  std::map<int, cv::Point3f> & points3DMap,
565  std::map<int, std::map<int, FeatureBA> > & wordReferences,
566  bool rematchFeatures)
567 {
568  UDEBUG("rematchFeatures=%d", rematchFeatures?1:0);
569  int wordCount = 0;
570  int edgeWithWordsAdded = 0;
571  std::map<int, std::map<cv::KeyPoint, int, KeyPointCompare> > frameToWordMap; // <FrameId, <Keypoint, wordId> >
572  for(std::multimap<int, Link>::const_iterator iter=links.lower_bound(1); iter!=links.end(); ++iter)
573  {
574  Link link = iter->second;
575  if(link.to() < link.from())
576  {
577  link = link.inverse();
578  }
579  if(link.to() != link.from() &&
580  uContains(signatures, link.from()) &&
581  uContains(signatures, link.to()) &&
582  uContains(poses, link.from()))
583  {
584  Signature sFrom = signatures.at(link.from());
585  if(sFrom.getWeight() >= 0) // ignore intermediate links
586  {
587  Signature sTo = signatures.at(link.to());
588 
589  if((sFrom.sensorData().cameraModels().empty() && sFrom.sensorData().stereoCameraModels().empty()) ||
590  (sTo.sensorData().cameraModels().empty() && sTo.sensorData().stereoCameraModels().empty()))
591  {
592  UERROR("No camera models found");
593  continue;
594  }
595 
596  if(sTo.getWeight() < 0)
597  {
598  for(std::multimap<int, Link>::const_iterator jter=links.find(sTo.id());
599  sTo.getWeight() < 0 && jter!=links.end() && uContains(signatures, jter->second.to());
600  ++jter)
601  {
602  sTo = signatures.at(jter->second.to());
603  }
604  }
605 
606  if(sFrom.getWords().size() &&
607  sTo.getWords().size() &&
608  sFrom.getWords3().size())
609  {
610  ParametersMap regParam;
611  regParam.insert(ParametersPair(Parameters::kVisEstimationType(), "1"));
612  regParam.insert(ParametersPair(Parameters::kVisPnPReprojError(), "5"));
613  regParam.insert(ParametersPair(Parameters::kVisMinInliers(), "6"));
614  regParam.insert(ParametersPair(Parameters::kVisCorNNDR(), "0.6"));
615  RegistrationVis reg(regParam);
616 
617  if(!rematchFeatures)
618  {
619  sFrom.setWordsDescriptors(cv::Mat());
620  sTo.setWordsDescriptors(cv::Mat());
621  }
622 
623  RegistrationInfo info;
624  Transform t = reg.computeTransformationMod(sFrom, sTo, Transform(), &info);
625  //Transform t = reg.computeTransformationMod(sFrom, sTo, iter->second.transform(), &info);
626  UDEBUG("%d->%d, inliers=%d",sFrom.id(), sTo.id(), (int)info.inliersIDs.size());
627 
628  if(!t.isNull())
629  {
630  if(!rematchFeatures)
631  {
632  // set descriptors for the output
633  if(sFrom.getWords().size() &&
634  sFrom.getWordsDescriptors().empty() &&
635  (int)sFrom.getWords().size() == signatures.at(link.from()).getWordsDescriptors().rows)
636  {
637  sFrom.setWordsDescriptors(signatures.at(link.from()).getWordsDescriptors());
638  }
639  if(sTo.getWords().size() &&
640  sTo.getWordsDescriptors().empty() &&
641  (int)sTo.getWords().size() == signatures.at(link.to()).getWordsDescriptors().rows)
642  {
643  sTo.setWordsDescriptors(signatures.at(link.to()).getWordsDescriptors());
644  }
645  }
646 
647  Transform pose = poses.at(sFrom.id());
648  UASSERT(!pose.isNull());
649  for(unsigned int i=0; i<info.inliersIDs.size(); ++i)
650  {
651  int indexFrom = sFrom.getWords().lower_bound(info.inliersIDs[i])->second;
652  cv::Point3f p = sFrom.getWords3()[indexFrom];
653  if(p.x > 0.0f) // make sure the point is valid
654  {
655  cv::KeyPoint ptFrom = sFrom.getWordsKpts()[indexFrom];
656  int indexTo = sTo.getWords().lower_bound(info.inliersIDs[i])->second;
657  cv::KeyPoint ptTo = sTo.getWordsKpts()[indexTo];
658 
659  int wordId = -1;
660 
661  // find if the word is already added
662  std::map<int, std::map<cv::KeyPoint, int, KeyPointCompare> >::iterator fromIter = frameToWordMap.find(sFrom.id());
663  std::map<int, std::map<cv::KeyPoint, int, KeyPointCompare> >::iterator toIter = frameToWordMap.find(sTo.id());
664  bool fromAlreadyAdded = false;
665  bool toAlreadyAdded = false;
666  if( fromIter != frameToWordMap.end() &&
667  fromIter->second.find(ptFrom) != fromIter->second.end())
668  {
669  wordId = fromIter->second.at(ptFrom);
670  fromAlreadyAdded = true;
671  }
672  if( toIter != frameToWordMap.end() &&
673  toIter->second.find(ptTo) != toIter->second.end())
674  {
675  wordId = toIter->second.at(ptTo);
676  toAlreadyAdded = true;
677  }
678 
679  if(wordId == -1)
680  {
681  wordId = ++wordCount;
682  wordReferences.insert(std::make_pair(wordId, std::map<int, FeatureBA>()));
683 
684  points3DMap.insert(std::make_pair(wordId, util3d::transformPoint(p, pose)));
685  }
686  else
687  {
688  UASSERT(wordReferences.find(wordId) != wordReferences.end());
689  UASSERT(points3DMap.find(wordId) != points3DMap.end());
690  }
691 
692  if(!fromAlreadyAdded)
693  {
694  cv::Mat descriptorFrom;
695  if(!sFrom.getWordsDescriptors().empty())
696  {
697  UASSERT(indexFrom < sFrom.getWordsDescriptors().rows);
698  descriptorFrom = sFrom.getWordsDescriptors().row(indexFrom);
699  }
700  int cameraIndex = 0;
701  if(sFrom.sensorData().cameraModels().size()>1 || sFrom.sensorData().stereoCameraModels().size()>1)
702  {
703  float subImageWidth = sFrom.sensorData().cameraModels().size()>1?sFrom.sensorData().cameraModels()[0].imageWidth():sFrom.sensorData().stereoCameraModels()[0].left().imageWidth();
704  cameraIndex = int(ptFrom.pt.x / subImageWidth);
705  ptFrom.pt.x = ptFrom.pt.x - (subImageWidth*float(cameraIndex));
706  }
707 
708  float depth = 0.0f;
709  if(!sFrom.sensorData().cameraModels().empty())
710  {
711  depth = util3d::transformPoint(p, sFrom.sensorData().cameraModels()[cameraIndex].localTransform().inverse()).z;
712  }
713  else
714  {
715  UASSERT(!sFrom.sensorData().stereoCameraModels().empty());
716  depth = util3d::transformPoint(p, sFrom.sensorData().stereoCameraModels()[cameraIndex].localTransform().inverse()).z;
717  }
718 
719 
720  wordReferences.at(wordId).insert(std::make_pair(sFrom.id(), FeatureBA(ptFrom, depth, descriptorFrom, cameraIndex)));
721  frameToWordMap.insert(std::make_pair(sFrom.id(), std::map<cv::KeyPoint, int, KeyPointCompare>()));
722  frameToWordMap.at(sFrom.id()).insert(std::make_pair(ptFrom, wordId));
723  }
724 
725  if(!toAlreadyAdded)
726  {
727  cv::Mat descriptorTo;
728  if(!sTo.getWordsDescriptors().empty())
729  {
730  UASSERT(indexTo < sTo.getWordsDescriptors().rows);
731  descriptorTo = sTo.getWordsDescriptors().row(indexTo);
732  }
733 
734  int cameraIndex = 0;
735  if(sTo.sensorData().cameraModels().size()>1 || sTo.sensorData().stereoCameraModels().size()>1)
736  {
737  float subImageWidth = sTo.sensorData().cameraModels().size()>1?sTo.sensorData().cameraModels()[0].imageWidth():sTo.sensorData().stereoCameraModels()[0].left().imageWidth();
738  cameraIndex = int(ptTo.pt.x / subImageWidth);
739  ptTo.pt.x = ptTo.pt.x - (subImageWidth*float(cameraIndex));
740  }
741 
742  float depth = 0.0f;
743  if(!sTo.getWords3().empty())
744  {
745  UASSERT(indexTo < (int)sTo.getWords3().size());
746  const cv::Point3f & pt = sTo.getWords3()[indexTo];
747  if(!sTo.sensorData().cameraModels().empty())
748  {
749  depth = util3d::transformPoint(pt, sTo.sensorData().cameraModels()[cameraIndex].localTransform().inverse()).z;
750  }
751  else
752  {
753  UASSERT(!sTo.sensorData().stereoCameraModels().empty());
754  depth = util3d::transformPoint(pt, sTo.sensorData().stereoCameraModels()[cameraIndex].localTransform().inverse()).z;
755  }
756  }
757 
758  wordReferences.at(wordId).insert(std::make_pair(sTo.id(), FeatureBA(ptTo, depth, descriptorTo, cameraIndex)));
759  frameToWordMap.insert(std::make_pair(sTo.id(), std::map<cv::KeyPoint, int, KeyPointCompare>()));
760  frameToWordMap.at(sTo.id()).insert(std::make_pair(ptTo, wordId));
761  }
762  }
763  }
764  ++edgeWithWordsAdded;
765  }
766  else
767  {
768  UWARN("Not enough inliers (%d) between %d and %d", info.inliersIDs.size(), sFrom.id(), sTo.id());
769  }
770  }
771  }
772  }
773  }
774  UDEBUG("Added %d words (edges with words=%d/%d)", wordCount, edgeWithWordsAdded, links.size());
775  if(links.empty())
776  {
777  UERROR("No links found for BA?!");
778  }
779  else if(wordCount == 0)
780  {
781  UERROR("No words added for BA?!");
782  }
783 }
784 
785 } /* namespace rtabmap */
rtabmap::Parameters::parse
static bool parse(const ParametersMap &parameters, const std::string &key, bool &value)
Definition: Parameters.cpp:497
rtabmap::Registration::computeTransformationMod
Transform computeTransformationMod(Signature &from, Signature &to, Transform guess=Transform::getIdentity(), RegistrationInfo *info=0) const
Definition: Registration.cpp:189
rtabmap::CameraModel::imageSize
const cv::Size & imageSize() const
Definition: CameraModel.h:119
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:304
rtabmap::graph::findLink
std::multimap< int, Link >::iterator RTABMAP_EXP findLink(std::multimap< int, Link > &links, int from, int to, bool checkBothWays=true, Link::Type type=Link::kUndef)
Definition: Graph.cpp:992
rtabmap::util3d::transformPoint
cv::Point3f RTABMAP_EXP transformPoint(const cv::Point3f &pt, const Transform &transform)
Definition: util3d_transforms.cpp:211
rtabmap::ParametersPair
std::pair< std::string, std::string > ParametersPair
Definition: Parameters.h:44
rtabmap::Transform::getIdentity
static Transform getIdentity()
Definition: Transform.cpp:401
rtabmap::Signature::getWordsDescriptors
const cv::Mat & getWordsDescriptors() const
Definition: Signature.h:115
rtabmap::Optimizer::isSlam2d
bool isSlam2d() const
Definition: Optimizer.h:92
rtabmap::SensorData::stereoCameraModels
const std::vector< StereoCameraModel > & stereoCameraModels() const
Definition: SensorData.h:237
rtabmap::ParametersMap
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:43
rtabmap::Signature::getWords3
const std::vector< cv::Point3f > & getWords3() const
Definition: Signature.h:131
rtabmap::Optimizer::epsilon
double epsilon() const
Definition: Optimizer.h:94
rtabmap::CameraModel::fx
double fx() const
Definition: CameraModel.h:102
UMath.h
Basic mathematics functions.
rtabmap::RegistrationInfo::inliersIDs
std::vector< int > inliersIDs
Definition: RegistrationInfo.h:84
UConversion.h
Some conversion functions.
rtabmap::Signature::getWeight
int getWeight() const
Definition: Signature.h:74
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, std::vector< 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:437
rtabmap::Optimizer::isAvailable
static bool isAvailable(Optimizer::Type type)
Definition: Optimizer.cpp:47
rtabmap::Optimizer::gravitySigma
float gravitySigma() const
Definition: Optimizer.h:98
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:341
rtabmap::SensorData::cameraModels
const std::vector< CameraModel > & cameraModels() const
Definition: SensorData.h:236
rtabmap::Optimizer::parseParameters
virtual void parseParameters(const ParametersMap &parameters)
Definition: Optimizer.cpp:329
rtabmap::Optimizer::landmarksIgnored
bool landmarksIgnored() const
Definition: Optimizer.h:97
UASSERT_MSG
#define UASSERT_MSG(condition, msg_str)
Definition: ULogger.h:67
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:406
rtabmap::Optimizer::iterations
int iterations() const
Definition: Optimizer.h:91
rtabmap::Signature::setWordsDescriptors
void setWordsDescriptors(const cv::Mat &descriptors)
Definition: Signature.cpp:304
rtabmap::CameraModel::localTransform
const Transform & localTransform() const
Definition: CameraModel.h:116
util3d_transforms.h
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, bool adjustPosesWithConstraints=true) const
Definition: Optimizer.cpp:188
uContains
bool uContains(const std::list< V > &list, const V &value)
Definition: UStl.h:409
rtabmap::OptimizerG2O::available
static bool available()
Definition: OptimizerG2O.cpp:117
rtabmap::Transform::isNull
bool isNull() const
Definition: Transform.cpp:107
rtabmap::CameraModel::cx
double cx() const
Definition: CameraModel.h:104
rtabmap::CameraModel::cy
double cy() const
Definition: CameraModel.h:105
rtabmap::Optimizer::type
virtual Type type() const =0
UDEBUG
#define UDEBUG(...)
rtabmap::Signature::sensorData
SensorData & sensorData()
Definition: Signature.h:137
UERROR
#define UERROR(...)
ULogger.h
ULogger class and convenient macros.
UWARN
#define UWARN(...)
rtabmap::Signature::id
int id() const
Definition: Signature.h:70
rtabmap::CameraModel::fy
double fy() const
Definition: CameraModel.h:103
trace.model
model
Definition: trace.py:4
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:560
rtabmap::Optimizer::priorsIgnored
bool priorsIgnored() const
Definition: Optimizer.h:96
lhs
YYCODETYPE lhs
Definition: sqlite3.c:116035
rtabmap::Optimizer::create
static Optimizer * create(const ParametersMap &parameters)
Definition: Optimizer.cpp:72
rtabmap::Transform::inverse
Transform inverse() const
Definition: Transform.cpp:178
rtabmap::Signature::getWords
const std::multimap< int, int > & getWords() const
Definition: Signature.h:111

rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Jan 23 2023 03:37:29