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