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

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