OptimizerTORO.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 #include "rtabmap/core/Graph.h"
28 
29 #include <rtabmap/utilite/ULogger.h>
30 #include <rtabmap/utilite/UStl.h>
31 #include <rtabmap/utilite/UMath.h>
32 #include <rtabmap/utilite/UConversion.h>
33 #include <rtabmap/utilite/UTimer.h>
34 #include <set>
35 
36 #include <rtabmap/core/optimizer/OptimizerTORO.h>
37 
38 #ifdef RTABMAP_TORO
39 #include "toro3d/treeoptimizer3.hh"
40 #include "toro3d/treeoptimizer2.hh"
41 #endif
42 
43 namespace rtabmap {
44 
45 bool OptimizerTORO::available()
46 {
47 #ifdef RTABMAP_TORO
48  return true;
49 #else
50  return false;
51 #endif
52 }
53 
54 std::map<int, Transform> OptimizerTORO::optimize(
55  int rootId,
56  const std::map<int, Transform> & poses,
57  const std::multimap<int, Link> & edgeConstraints,
58  cv::Mat & outputCovariance,
59  std::list<std::map<int, Transform> > * intermediateGraphes, // contains poses after tree init to last one before the end
60  double * finalError,
61  int * iterationsDone)
62 {
63  outputCovariance = cv::Mat::eye(6,6,CV_64FC1);
64  std::map<int, Transform> optimizedPoses;
65 #ifdef RTABMAP_TORO
66  UDEBUG("Optimizing graph (pose=%d constraints=%d)...", (int)poses.size(), (int)edgeConstraints.size());
67  if(edgeConstraints.size()>=1 && poses.size()>=2 && iterations() > 0)
68  {
69  // Apply TORO optimization
70  AISNavigation::TreeOptimizer2 pg2;
71  AISNavigation::TreeOptimizer3 pg3;
72  pg2.verboseLevel = 0;
73  pg3.verboseLevel = 0;
74 
75  UDEBUG("fill poses to TORO...");
76  if(isSlam2d())
77  {
78  for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
79  {
80  if(iter->first > 0)
81  {
82  UASSERT(!iter->second.isNull());
83  AISNavigation::TreePoseGraph2::Pose p(iter->second.x(), iter->second.y(), iter->second.theta());
84  AISNavigation::TreePoseGraph2::Vertex* v = pg2.addVertex(iter->first, p);
85  UASSERT_MSG(v != 0, uFormat("cannot insert vertex %d!?", iter->first).c_str());
86  }
87  }
88  }
89  else
90  {
91  for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
92  {
93  if(iter->first > 0)
94  {
95  UASSERT(!iter->second.isNull());
96  float x,y,z, roll,pitch,yaw;
97  iter->second.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
98  AISNavigation::TreePoseGraph3::Pose p(x, y, z, roll, pitch, yaw);
99  AISNavigation::TreePoseGraph3::Vertex* v = pg3.addVertex(iter->first, p);
100  UASSERT_MSG(v != 0, uFormat("cannot insert vertex %d!?", iter->first).c_str());
101  v->transformation=AISNavigation::TreePoseGraph3::Transformation(p);
102  }
103  }
104 
105  }
106 
107  UDEBUG("fill edges to TORO...");
108  if(isSlam2d())
109  {
110  for(std::multimap<int, Link>::const_iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
111  {
112  UASSERT(!iter->second.transform().isNull());
113  AISNavigation::TreePoseGraph2::Pose p(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta());
114  AISNavigation::TreePoseGraph2::InformationMatrix inf;
115  //Identity:
116  if(isCovarianceIgnored())
117  {
118  inf.values[0][0] = 1.0; inf.values[0][1] = 0.0; inf.values[0][2] = 0.0; // x
119  inf.values[1][0] = 0.0; inf.values[1][1] = 1.0; inf.values[1][2] = 0.0; // y
120  inf.values[2][0] = 0.0; inf.values[2][1] = 0.0; inf.values[2][2] = 1.0; // theta/yaw
121  }
122  else
123  {
124  inf.values[0][0] = iter->second.infMatrix().at<double>(0,0); // x-x
125  inf.values[0][1] = iter->second.infMatrix().at<double>(0,1); // x-y
126  inf.values[0][2] = iter->second.infMatrix().at<double>(0,5); // x-theta
127  inf.values[1][0] = iter->second.infMatrix().at<double>(1,0); // y-x
128  inf.values[1][1] = iter->second.infMatrix().at<double>(1,1); // y-y
129  inf.values[1][2] = iter->second.infMatrix().at<double>(1,5); // y-theta
130  inf.values[2][0] = iter->second.infMatrix().at<double>(5,0); // theta-x
131  inf.values[2][1] = iter->second.infMatrix().at<double>(5,1); // theta-y
132  inf.values[2][2] = iter->second.infMatrix().at<double>(5,5); // theta-theta
133  }
134 
135  int id1 = iter->second.from();
136  int id2 = iter->second.to();
137  if(id1 != id2 && id1 > 0 && id2 > 0)
138  {
139  AISNavigation::TreePoseGraph2::Vertex* v1=pg2.vertex(id1);
140  AISNavigation::TreePoseGraph2::Vertex* v2=pg2.vertex(id2);
141  UASSERT(v1 != 0);
142  UASSERT(v2 != 0);
143  AISNavigation::TreePoseGraph2::Transformation t(p);
144  if (!pg2.addEdge(v1, v2, t, inf))
145  {
146  UERROR("Map: Edge already exits between nodes %d and %d, skipping", id1, id2);
147  }
148  }
149  else if(id1 == id2)
150  {
151  UWARN("TORO optimizer doesn't support prior or gravity links, use GTSAM or g2o optimizers (see parameter %s). Link %d ignored...", Parameters::kOptimizerStrategy().c_str(), id1);
152  }
153  else if(id1 < 0 || id2 < 0)
154  {
155  UWARN("TORO optimizer doesn't support landmark links, use GTSAM or g2o optimizers (see parameter %s). Link %d->%d ignored...", Parameters::kOptimizerStrategy().c_str(), id1, id2);
156  }
157  }
158  }
159  else
160  {
161  for(std::multimap<int, Link>::const_iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
162  {
163  UASSERT(!iter->second.transform().isNull());
164  float x,y,z, roll,pitch,yaw;
165  iter->second.transform().getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
166 
167  AISNavigation::TreePoseGraph3::Pose p(x, y, z, roll, pitch, yaw);
168  AISNavigation::TreePoseGraph3::InformationMatrix inf = DMatrix<double>::I(6);
169  if(!isCovarianceIgnored())
170  {
171  memcpy(inf[0], iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double));
172  }
173 
174  int id1 = iter->second.from();
175  int id2 = iter->second.to();
176  if(id1 != id2 && id1 > 0 && id2 > 0)
177  {
178  AISNavigation::TreePoseGraph3::Vertex* v1=pg3.vertex(id1);
179  AISNavigation::TreePoseGraph3::Vertex* v2=pg3.vertex(id2);
180  UASSERT(v1 != 0);
181  UASSERT(v2 != 0);
182  AISNavigation::TreePoseGraph3::Transformation t(p);
183  if (!pg3.addEdge(v1, v2, t, inf))
184  {
185  UERROR("Map: Edge already exits between nodes %d and %d, skipping", id1, id2);
186  }
187  }
188  else if(id1 == id2)
189  {
190  UWARN("TORO optimizer doesn't support prior or gravity links, use GTSAM or g2o optimizers (see parameter %s). Link %d ignored...", Parameters::kOptimizerStrategy().c_str(), id1);
191  }
192  else if(id1 < 0 || id2 < 0)
193  {
194  UWARN("TORO optimizer doesn't support landmark links, use GTSAM or g2o optimizers (see parameter %s). Link %d->%d ignored...", Parameters::kOptimizerStrategy().c_str(), id1, id2);
195  }
196  }
197  }
198  UDEBUG("buildMST... root=%d", rootId);
199  UASSERT(uContains(poses, rootId));
200  if(isSlam2d())
201  {
202  pg2.buildMST(rootId); // pg.buildSimpleTree();
203  //UDEBUG("initializeOnTree()");
204  //pg2.initializeOnTree();
205  UDEBUG("initializeTreeParameters()");
206  pg2.initializeTreeParameters();
207  UDEBUG("Building TORO tree... (if a crash happens just after this msg, "
208  "TORO is not able to find the root of the graph!)");
209  pg2.initializeOptimization();
210  }
211  else
212  {
213  pg3.buildMST(rootId); // pg.buildSimpleTree();
214  //UDEBUG("initializeOnTree()");
215  //pg3.initializeOnTree();
216  UDEBUG("initializeTreeParameters()");
217  pg3.initializeTreeParameters();
218  UDEBUG("Building TORO tree... (if a crash happens just after this msg, "
219  "TORO is not able to find the root of the graph!)");
220  pg3.initializeOptimization();
221  }
222 
223  UINFO("Initial error = %f", pg2.error());
224  UINFO("TORO optimizing begin (iterations=%d)", iterations());
225  double lastError = 0;
226  int i=0;
227  UTimer timer;
228  for (; i<iterations(); i++)
229  {
230  if(intermediateGraphes && i>0)
231  {
232  std::map<int, Transform> tmpPoses;
233  if(isSlam2d())
234  {
235  for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
236  {
237  if(iter->first > 0)
238  {
239  AISNavigation::TreePoseGraph2::Vertex* v=pg2.vertex(iter->first);
240  float roll, pitch, yaw;
241  iter->second.getEulerAngles(roll, pitch, yaw);
242  Transform newPose(v->pose.x(), v->pose.y(), iter->second.z(), roll, pitch, v->pose.theta());
243 
244  UASSERT_MSG(!newPose.isNull(), uFormat("Optimized pose %d is null!?!?", iter->first).c_str());
245  tmpPoses.insert(std::pair<int, Transform>(iter->first, newPose));
246  }
247  }
248  }
249  else
250  {
251  for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
252  {
253  if(iter->first > 0)
254  {
255  AISNavigation::TreePoseGraph3::Vertex* v=pg3.vertex(iter->first);
256  AISNavigation::TreePoseGraph3::Pose pose=v->transformation.toPoseType();
257  Transform newPose(pose.x(), pose.y(), pose.z(), pose.roll(), pose.pitch(), pose.yaw());
258 
259  UASSERT_MSG(!newPose.isNull(), uFormat("Optimized pose %d is null!?!?", iter->first).c_str());
260  tmpPoses.insert(std::pair<int, Transform>(iter->first, newPose));
261  }
262  }
263  }
264  intermediateGraphes->push_back(tmpPoses);
265  }
266 
267  double error = 0;
268  if(isSlam2d())
269  {
270  pg2.iterate();
271 
272  // compute the error and dump it
273  error=pg2.error();
274  UDEBUG("iteration %d global error=%f error/constraint=%f", i, error, error/pg2.edges.size());
275  }
276  else
277  {
278  pg3.iterate();
279 
280  // compute the error and dump it
281  double mte, mre, are, ate;
282  error=pg3.error(&mre, &mte, &are, &ate);
283  UDEBUG("i %d RotGain=%f global error=%f error/constraint=%f",
284  i, pg3.getRotGain(), error, error/pg3.edges.size());
285  }
286 
287  // early stop condition
288  double errorDelta = lastError - error;
289  if(i>0 && errorDelta < this->epsilon())
290  {
291  if(errorDelta < 0)
292  {
293  UDEBUG("Negative improvement?! Ignore and continue optimizing... (%f < %f)", errorDelta, this->epsilon());
294  }
295  else
296  {
297  UINFO("Stop optimizing, not enough improvement (%f < %f)", errorDelta, this->epsilon());
298  break;
299  }
300  }
301  else if(i==0 && error < this->epsilon())
302  {
303  UINFO("Stop optimizing, error is already under epsilon (%f < %f)", error, this->epsilon());
304  break;
305  }
306  lastError = error;
307  }
308  if(finalError)
309  {
310  *finalError = lastError;
311  }
312  if(iterationsDone)
313  {
314  *iterationsDone = i;
315  }
316  UINFO("TORO optimizing end (%d iterations done, error=%f, time = %f s)", i, lastError, timer.ticks());
317 
318  if(isSlam2d())
319  {
320  for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
321  {
322  if(iter->first > 0)
323  {
324  AISNavigation::TreePoseGraph2::Vertex* v=pg2.vertex(iter->first);
325  float roll, pitch, yaw;
326  iter->second.getEulerAngles(roll, pitch, yaw);
327  Transform newPose(v->pose.x(), v->pose.y(), iter->second.z(), roll, pitch, v->pose.theta());
328 
329  UASSERT_MSG(!newPose.isNull(), uFormat("Optimized pose %d is null!?!?", iter->first).c_str());
330  optimizedPoses.insert(std::pair<int, Transform>(iter->first, newPose));
331  }
332  }
333  }
334  else
335  {
336  for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
337  {
338  if(iter->first > 0)
339  {
340  AISNavigation::TreePoseGraph3::Vertex* v=pg3.vertex(iter->first);
341  AISNavigation::TreePoseGraph3::Pose pose=v->transformation.toPoseType();
342  Transform newPose(pose.x(), pose.y(), pose.z(), pose.roll(), pose.pitch(), pose.yaw());
343 
344  UASSERT_MSG(!newPose.isNull(), uFormat("Optimized pose %d is null!?!?", iter->first).c_str());
345  optimizedPoses.insert(std::pair<int, Transform>(iter->first, newPose));
346  }
347  }
348  }
349 
350  // TORO doesn't compute marginals...
351  }
352  else if(poses.size() == 1 || iterations() <= 0)
353  {
354  optimizedPoses = poses;
355  }
356  else
357  {
358  UWARN("This method should be called at least with 1 pose!");
359  }
360  UDEBUG("Optimizing graph...end!");
361 #else
362  UERROR("Not built with TORO support!");
363 #endif
364  return optimizedPoses;
365 }
366 
367 bool OptimizerTORO::saveGraph(
368  const std::string & fileName,
369  const std::map<int, Transform> & poses,
370  const std::multimap<int, Link> & edgeConstraints)
371 {
372  FILE * file = 0;
373 
374 #ifdef _MSC_VER
375  fopen_s(&file, fileName.c_str(), "w");
376 #else
377  file = fopen(fileName.c_str(), "w");
378 #endif
379 
380  if(file)
381  {
382 
383  for (std::map<int, Transform>::const_iterator iter = poses.begin(); iter != poses.end(); ++iter)
384  {
385  if (isSlam2d())
386  {
387  // VERTEX2 id x y theta
388  fprintf(file, "VERTEX2 %d %f %f %f\n",
389  iter->first,
390  iter->second.x(),
391  iter->second.y(),
392  iter->second.theta());
393  }
394  else
395  {
396  // VERTEX3 id x y z phi theta psi
397  float x, y, z, yaw, pitch, roll;
398  iter->second.getTranslationAndEulerAngles(x, y, z, roll, pitch, yaw);
399  fprintf(file, "VERTEX3 %d %f %f %f %f %f %f\n",
400  iter->first,
401  x,
402  y,
403  z,
404  roll,
405  pitch,
406  yaw);
407  }
408  }
409 
410  for(std::multimap<int, Link>::const_iterator iter = edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
411  {
412  if (iter->second.type() != Link::kPosePrior && iter->second.type() != Link::kGravity)
413  {
414  if (isSlam2d())
415  {
416  //EDGE2 observed_vertex_id observing_vertex_id x y theta inf_11 inf_12 inf_13 inf_22 inf_23 inf_33
417  fprintf(file, "EDGE2 %d %d %f %f %f %f %f %f %f %f %f\n",
418  iter->second.from(),
419  iter->second.to(),
420  iter->second.transform().x(),
421  iter->second.transform().y(),
422  iter->second.transform().theta(),
423  iter->second.infMatrix().at<double>(0, 0),
424  iter->second.infMatrix().at<double>(0, 1),
425  iter->second.infMatrix().at<double>(0, 5),
426  iter->second.infMatrix().at<double>(1, 1),
427  iter->second.infMatrix().at<double>(1, 5),
428  iter->second.infMatrix().at<double>(5, 5));
429  }
430  else
431  {
432  //EDGE3 observed_vertex_id observing_vertex_id x y z roll pitch yaw inf_11 inf_12 .. inf_16 inf_22 .. inf_66
433  float x, y, z, yaw, pitch, roll;
434  iter->second.transform().getTranslationAndEulerAngles(x, y, z, roll, pitch, yaw);
435  fprintf(file, "EDGE3 %d %d %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\n",
436  iter->second.from(),
437  iter->second.to(),
438  x,
439  y,
440  z,
441  roll,
442  pitch,
443  yaw,
444  iter->second.infMatrix().at<double>(0, 0),
445  iter->second.infMatrix().at<double>(0, 1),
446  iter->second.infMatrix().at<double>(0, 2),
447  iter->second.infMatrix().at<double>(0, 3),
448  iter->second.infMatrix().at<double>(0, 4),
449  iter->second.infMatrix().at<double>(0, 5),
450  iter->second.infMatrix().at<double>(1, 1),
451  iter->second.infMatrix().at<double>(1, 2),
452  iter->second.infMatrix().at<double>(1, 3),
453  iter->second.infMatrix().at<double>(1, 4),
454  iter->second.infMatrix().at<double>(1, 5),
455  iter->second.infMatrix().at<double>(2, 2),
456  iter->second.infMatrix().at<double>(2, 3),
457  iter->second.infMatrix().at<double>(2, 4),
458  iter->second.infMatrix().at<double>(2, 5),
459  iter->second.infMatrix().at<double>(3, 3),
460  iter->second.infMatrix().at<double>(3, 4),
461  iter->second.infMatrix().at<double>(3, 5),
462  iter->second.infMatrix().at<double>(4, 4),
463  iter->second.infMatrix().at<double>(4, 5),
464  iter->second.infMatrix().at<double>(5, 5));
465  }
466  }
467  }
468  UINFO("Graph saved to %s", fileName.c_str());
469  fclose(file);
470  }
471  else
472  {
473  UERROR("Cannot save to file %s", fileName.c_str());
474  return false;
475  }
476  return true;
477 }
478 
479 bool OptimizerTORO::loadGraph(
480  const std::string & fileName,
481  std::map<int, Transform> & poses,
482  std::multimap<int, Link> & edgeConstraints)
483 {
484  FILE * file = 0;
485 #ifdef _MSC_VER
486  fopen_s(&file, fileName.c_str(), "r");
487 #else
488  file = fopen(fileName.c_str(), "r");
489 #endif
490 
491  if(file)
492  {
493  char line[400];
494  while ( fgets (line , 400 , file) != NULL )
495  {
496  std::vector<std::string> strList = uListToVector(uSplit(uReplaceChar(line, '\n', ' '), ' '));
497  if(strList.size() == 8)
498  {
499  //VERTEX3
500  int id = atoi(strList[1].c_str());
501  float x = uStr2Float(strList[2]);
502  float y = uStr2Float(strList[3]);
503  float z = uStr2Float(strList[4]);
504  float roll = uStr2Float(strList[5]);
505  float pitch = uStr2Float(strList[6]);
506  float yaw = uStr2Float(strList[7]);
507  Transform pose(x, y, z, roll, pitch, yaw);
508  if(poses.find(id) == poses.end())
509  {
510  poses.insert(std::make_pair(id, pose));
511  }
512  else
513  {
514  UFATAL("Pose %d already added", id);
515  }
516  }
517  else if(strList.size() == 30)
518  {
519  //EDGE3
520  int idFrom = atoi(strList[1].c_str());
521  int idTo = atoi(strList[2].c_str());
522  float x = uStr2Float(strList[3]);
523  float y = uStr2Float(strList[4]);
524  float z = uStr2Float(strList[5]);
525  float roll = uStr2Float(strList[6]);
526  float pitch = uStr2Float(strList[7]);
527  float yaw = uStr2Float(strList[8]);
528  cv::Mat informationMatrix(6,6,CV_64FC1);
529  informationMatrix.at<double>(3,3) = uStr2Float(strList[9]);
530  informationMatrix.at<double>(4,4) = uStr2Float(strList[15]);
531  informationMatrix.at<double>(5,5) = uStr2Float(strList[20]);
532  UASSERT_MSG(informationMatrix.at<double>(3,3) > 0.0 && informationMatrix.at<double>(4,4) > 0.0 && informationMatrix.at<double>(5,5) > 0.0, uFormat("Information matrix should not be null! line=\"%s\"", line).c_str());
533  informationMatrix.at<double>(0,0) = uStr2Float(strList[24]);
534  informationMatrix.at<double>(1,1) = uStr2Float(strList[27]);
535  informationMatrix.at<double>(2,2) = uStr2Float(strList[29]);
536  UASSERT_MSG(informationMatrix.at<double>(0,0) > 0.0 && informationMatrix.at<double>(1,1) > 0.0 && informationMatrix.at<double>(2,2) > 0.0, uFormat("Information matrix should not be null! line=\"%s\"", line).c_str());
537  Transform transform(x, y, z, roll, pitch, yaw);
538  if(poses.find(idFrom) != poses.end() && poses.find(idTo) != poses.end())
539  {
540  //Link type is unknown
541  Link link(idFrom, idTo, Link::kUndef, transform, informationMatrix);
542  edgeConstraints.insert(std::pair<int, Link>(idFrom, link));
543  }
544  else
545  {
546  UFATAL("Referred poses from the link not exist!");
547  }
548  }
549  else if(strList.size())
550  {
551  UFATAL("Error parsing graph file %s on line \"%s\" (strList.size()=%d)", fileName.c_str(), line, (int)strList.size());
552  }
553  }
554 
555  UINFO("Graph loaded from %s", fileName.c_str());
556  fclose(file);
557  }
558  else
559  {
560  UERROR("Cannot open file %s", fileName.c_str());
561  return false;
562  }
563  return true;
564 }
565 
566 } /* namespace rtabmap */
AISNavigation::Pose3::y
const T & y() const
Definition: transformation3.hh:86
glm::roll
GLM_FUNC_DECL T roll(detail::tquat< T, P > const &x)
NULL
#define NULL
UTimer
Definition: UTimer.h:46
AISNavigation::TreeOptimizer2::iterate
void iterate(TreePoseGraph2::EdgeSet *eset=0)
Definition: treeoptimizer2.cpp:278
x
x
treeoptimizer2.hh
Defines the core optimizer class for 2D graphs which is a subclass of TreePoseGraph2.
AISNavigation::TreeOptimizer2
Class that contains the core optimization algorithm.
Definition: treeoptimizer2.hh:52
rtabmap::Optimizer::isSlam2d
bool isSlam2d() const
Definition: Optimizer.h:92
AISNavigation::TreePoseGraph::vertex
Vertex * vertex(int id)
Definition: posegraph.hh:48
AISNavigation::SMatrix3
A class to represent symmetric 3x3 matrices.
Definition: transformation2.hh:292
uStr2Float
float UTILITE_EXP uStr2Float(const std::string &str)
Definition: UConversion.cpp:138
rtabmap::Optimizer::epsilon
double epsilon() const
Definition: Optimizer.h:94
AISNavigation::TreeOptimizer3
Class that contains the core optimization algorithm.
Definition: treeoptimizer3.hh:52
UMath.h
Basic mathematics functions.
treeoptimizer3.hh
Defines the core optimizer class for 3D graphs which is a subclass of TreePoseGraph3.
UConversion.h
Some conversion functions.
glm::pitch
GLM_FUNC_DECL T pitch(detail::tquat< T, P > const &x)
UFATAL
#define UFATAL(...)
AISNavigation::Pose3::pitch
const T & pitch() const
Definition: transformation3.hh:83
UASSERT
#define UASSERT(condition)
UStl.h
Wrappers of STL for convenient functions.
uSplit
std::list< std::string > uSplit(const std::string &str, char separator=' ')
Definition: UStl.h:566
UASSERT_MSG
#define UASSERT_MSG(condition, msg_str)
Definition: ULogger.h:67
AISNavigation::TreeOptimizer3::error
double error(double *mre=0, double *mte=0, double *are=0, double *ate=0, TreePoseGraph3::EdgeSet *eset=0) const
Definition: treeoptimizer3.cpp:249
AISNavigation::Pose3::yaw
const T & yaw() const
Definition: transformation3.hh:84
rtabmap::Optimizer::iterations
int iterations() const
Definition: Optimizer.h:91
uReplaceChar
std::string UTILITE_EXP uReplaceChar(const std::string &str, char before, char after)
Definition: UConversion.cpp:33
AISNavigation::Pose3::z
const T & z() const
Definition: transformation3.hh:87
uContains
bool uContains(const std::list< V > &list, const V &value)
Definition: UStl.h:409
AISNavigation::TreePoseGraph::addVertex
Vertex * addVertex(int id, const Pose &pose)
Definition: posegraph.hh:100
AISNavigation::TreePoseGraph::addEdge
Edge * addEdge(Vertex *v1, Vertex *v2, const Transformation &t, const Information &i)
Definition: posegraph.hh:133
uListToVector
std::vector< V > uListToVector(const std::list< V > &list)
Definition: UStl.h:475
rtabmap::OptimizerTORO::optimize
virtual std::map< int, Transform > optimize(int rootId, const std::map< int, Transform > &poses, const std::multimap< int, Link > &edgeConstraints, cv::Mat &outputCovariance, std::list< std::map< int, Transform > > *intermediateGraphes=0, double *finalError=0, int *iterationsDone=0)
Definition: OptimizerTORO.cpp:54
UDEBUG
#define UDEBUG(...)
UERROR
#define UERROR(...)
AISNavigation::TreeOptimizer3::initializeOptimization
void initializeOptimization(EdgeCompareMode mode=EVComparator< Edge *>::CompareLevel)
Definition: treeoptimizer3.cpp:292
ULogger.h
ULogger class and convenient macros.
UWARN
#define UWARN(...)
UTimer::ticks
double ticks()
Definition: UTimer.cpp:117
glm::yaw
GLM_FUNC_DECL T yaw(detail::tquat< T, P > const &x)
AISNavigation::TreeOptimizer3::iterate
void iterate(TreePoseGraph3::EdgeSet *eset=0, bool noPreconditioner=false)
Definition: treeoptimizer3.cpp:73
rtabmap::OptimizerTORO::saveGraph
bool saveGraph(const std::string &fileName, const std::map< int, Transform > &poses, const std::multimap< int, Link > &edgeConstraints)
Definition: OptimizerTORO.cpp:367
rtabmap::OptimizerTORO::loadGraph
static bool loadGraph(const std::string &fileName, std::map< int, Transform > &poses, std::multimap< int, Link > &edgeConstraints)
Definition: OptimizerTORO.cpp:479
rtabmap::Optimizer::isCovarianceIgnored
bool isCovarianceIgnored() const
Definition: Optimizer.h:93
AISNavigation::Pose3::x
const T & x() const
Definition: transformation3.hh:85
DMatrix::I
static DMatrix I(int)
AISNavigation::TreePoseGraph3::Transformation
Ops::TransformationType Transformation
Definition: posegraph3.hh:67
uFormat
std::string UTILITE_EXP uFormat(const char *fmt,...)
Definition: UConversion.cpp:365
AISNavigation::Pose3::roll
const T & roll() const
Definition: transformation3.hh:82
AISNavigation::Pose2
2D Point (x,y) with orientation (theta)
Definition: transformation2.hh:120
AISNavigation::Transformation2
A class to represent 2D transformations (rotation and translation)
Definition: transformation2.hh:161
UINFO
#define UINFO(...)

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