Graph.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 <rtabmap/utilite/UFile.h>
35 #include <rtabmap/core/GeodeticCoords.h>
36 #include <rtabmap/core/Memory.h>
37 #include <rtabmap/core/util3d_filtering.h>
38 #include <rtabmap/core/util3d_registration.h>
39 #include <pcl/search/kdtree.h>
40 #include <pcl/common/eigen.h>
41 #include <pcl/common/common.h>
42 #include <pcl/common/point_tests.h>
43 #include <set>
44 #include <queue>
45 #include <fstream>
46 
47 #include <rtabmap/core/optimizer/OptimizerTORO.h>
48 #include <rtabmap/core/optimizer/OptimizerG2O.h>
49 
50 namespace rtabmap {
51 
52 namespace graph {
53 
54 bool exportPoses(
55  const std::string & filePath,
56  int format, // 0=Raw, 1=RGBD-SLAM motion capture (10=without change of coordinate frame, 11=10+ID), 2=KITTI, 3=TORO, 4=g2o
57  const std::map<int, Transform> & poses,
58  const std::multimap<int, Link> & constraints, // required for formats 3 and 4
59  const std::map<int, double> & stamps, // required for format 1, 10 and 11
60  const ParametersMap & parameters) // optional for formats 3 and 4
61 {
62  UDEBUG("%s", filePath.c_str());
63  std::string tmpPath = filePath;
64  if(format==3) // TORO
65  {
66  if(UFile::getExtension(tmpPath).empty())
67  {
68  tmpPath+=".graph";
69  }
70  OptimizerTORO toro(parameters);
71  return toro.saveGraph(tmpPath, poses, constraints);
72  }
73  else if(format == 4) // g2o
74  {
75  if(UFile::getExtension(tmpPath).empty())
76  {
77  tmpPath+=".g2o";
78  }
79  OptimizerG2O g2o(parameters);
80  return g2o.saveGraph(tmpPath, poses, constraints);
81  }
82  else
83  {
84  if(UFile::getExtension(tmpPath).empty())
85  {
86  tmpPath+=".txt";
87  }
88 
89  if(format == 1 || format == 10 || format == 11)
90  {
91  if(stamps.size() != poses.size())
92  {
93  UERROR("When exporting poses to format 1 (RGBD-SLAM), stamps and poses maps should have the same size! stamps=%d poses=%d",
94  (int)stamps.size(), (int)poses.size());
95  return false;
96  }
97  }
98 
99  FILE* fout = 0;
100 #ifdef _MSC_VER
101  fopen_s(&fout, tmpPath.c_str(), "w");
102 #else
103  fout = fopen(tmpPath.c_str(), "w");
104 #endif
105  if(fout)
106  {
107  for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
108  {
109  if(format == 1 || format == 10 || format == 11) // rgbd-slam format
110  {
111  Transform pose = iter->second;
112  if(format == 1)
113  {
114  // put the pose in rgbd-slam world reference
115  Transform t( 0, 0, 1, 0,
116  0, -1, 0, 0,
117  1, 0, 0, 0);
118  pose = t.inverse() * pose;
119  t = Transform( 0, 0, 1, 0,
120  -1, 0, 0, 0,
121  0,-1, 0, 0);
122  pose = t.inverse() * pose * t;
123  }
124 
125  // Format: stamp x y z qx qy qz qw
126  Eigen::Quaternionf q = pose.getQuaternionf();
127 
128  if(iter == poses.begin())
129  {
130  // header
131  if(format == 11)
132  {
133  fprintf(fout, "#timestamp x y z qx qy qz qw id\n");
134  }
135  else
136  {
137  fprintf(fout, "#timestamp x y z qx qy qz qw\n");
138  }
139  }
140 
141  UASSERT(uContains(stamps, iter->first));
142  fprintf(fout, "%f %f %f %f %f %f %f %f%s\n",
143  stamps.at(iter->first),
144  pose.x(),
145  pose.y(),
146  pose.z(),
147  q.x(),
148  q.y(),
149  q.z(),
150  q.w(),
151  format == 11?(" "+uNumber2Str(iter->first)).c_str():"");
152  }
153  else // default / KITTI format
154  {
155  Transform pose = iter->second;
156  if(format == 2)
157  {
158  // for KITTI, we need to remove optical rotation
159  // z pointing front, x left, y down
160  Transform t( 0, 0, 1, 0,
161  -1, 0, 0, 0,
162  0,-1, 0, 0);
163  pose = t.inverse() * pose * t;
164  }
165 
166  // Format: r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz
167  const float * p = (const float *)pose.data();
168 
169  fprintf(fout, "%f", p[0]);
170  for(int i=1; i<pose.size(); i++)
171  {
172  fprintf(fout, " %f", p[i]);
173  }
174  fprintf(fout, "\n");
175  }
176  }
177  fclose(fout);
178  return true;
179  }
180  }
181  return false;
182 }
183 
184 bool importPoses(
185  const std::string & filePath,
186  int format, // 0=Raw, 1=RGBD-SLAM motion capture (10=without change of coordinate frame, 11=10+ID), 2=KITTI, 3=TORO, 4=g2o, 5=NewCollege(t,x,y), 6=Malaga Urban GPS, 7=St Lucia INS, 8=Karlsruhe, 9=EuRoC MAV
187  std::map<int, Transform> & poses,
188  std::multimap<int, Link> * constraints, // optional for formats 3 and 4
189  std::map<int, double> * stamps) // optional for format 1 and 9
190 {
191  UDEBUG("%s format=%d", filePath.c_str(), format);
192  if(format==3) // TORO
193  {
194  std::multimap<int, Link> constraintsTmp;
195  if(OptimizerTORO::loadGraph(filePath, poses, constraintsTmp))
196  {
197  if(constraints)
198  {
199  *constraints = constraintsTmp;
200  }
201  return true;
202  }
203  return false;
204  }
205  else if(format == 4) // g2o
206  {
207  std::multimap<int, Link> constraintsTmp;
208  UERROR("Cannot import from g2o format because it is not yet supported!");
209  return false;
210  }
211  else
212  {
213  std::ifstream file;
214  file.open(filePath.c_str(), std::ifstream::in);
215  if(!file.good())
216  {
217  return false;
218  }
219  int id=1;
220  GeodeticCoords origin;
221  Transform originPose;
222  while(file.good())
223  {
224  std::string str;
225  std::getline(file, str);
226 
227  if(str.size() && str.at(str.size()-1) == '\r')
228  {
229  str = str.substr(0, str.size()-1);
230  }
231 
232  if(str.empty() || str.at(0) == '#' || str.at(0) == '%')
233  {
234  continue;
235  }
236 
237  if(format == 9) // EuRoC format
238  {
239  std::list<std::string> strList = uSplit(str, ',');
240  if(strList.size() == 17)
241  {
242  double stamp = uStr2Double(strList.front())/1000000000.0;
243  strList.pop_front();
244  std::vector<std::string> v = uListToVector(strList);
245  Transform pose(uStr2Float(v[0]), uStr2Float(v[1]), uStr2Float(v[2]), // x y z
246  uStr2Float(v[4]), uStr2Float(v[5]), uStr2Float(v[6]), uStr2Float(v[3])); // qw qx qy qz -> qx qy qz qw
247  if(pose.isNull())
248  {
249  UWARN("Null transform read!? line parsed: \"%s\"", str.c_str());
250  }
251  else
252  {
253  if(stamps)
254  {
255  stamps->insert(std::make_pair(id, stamp));
256  }
257  // we need to rotate from IMU frame to world frame
258  Transform t( 0, 0, 1, 0,
259  0, -1, 0, 0,
260  1, 0, 0, 0);
261  pose = pose * t;
262  poses.insert(std::make_pair(id, pose));
263  }
264  }
265  else
266  {
267  UERROR("Error parsing \"%s\" with EuRoC MAV format (should have 17 values: stamp x y z qw qx qy qz vx vy vz vr vp vy ax ay az)", str.c_str());
268  }
269  }
270  else if(format == 8) // Karlsruhe format
271  {
272  std::vector<std::string> strList = uListToVector(uSplit(str));
273  if(strList.size() == 10)
274  {
275  // timestamp,lat,lon,alt,x,y,z,roll,pitch,yaw
276 
277  // stamp is 1252400096825289728, transform to 1252400096.825289728
278  double stamp = uStr2Double(strList[0].insert(10, 1, '.'));
279  double x = uStr2Double(strList[4]);
280  double y = uStr2Double(strList[5]);
281  double z = uStr2Double(strList[6]);
282  double roll = uStr2Double(strList[8]);
283  double pitch = uStr2Double(strList[7]);
284  double yaw = -(uStr2Double(strList[9])-M_PI_2);
285  if(stamps)
286  {
287  stamps->insert(std::make_pair(id, stamp));
288  }
289  Transform pose = Transform(x,y,z,roll,pitch,yaw);
290  if(poses.size() == 0)
291  {
292  originPose = pose.inverse();
293  }
294  pose = originPose * pose; // transform in local coordinate where first value is the origin
295  poses.insert(std::make_pair(id, pose));
296  }
297  else
298  {
299  UERROR("Error parsing \"%s\" with Karlsruhe format (should have 10 values)", str.c_str());
300  }
301  }
302  else if(format == 7) // St Lucia format
303  {
304  std::vector<std::string> strList = uListToVector(uSplit(str));
305  if(strList.size() == 12)
306  {
307  // Data Type
308  //0=Timestamp (seconds)
309  //1=Timestamp (millisec)
310  //2=Latitude (deg)
311  //3=Longitude (deg)
312  //4=Altitude (m)
313  //5=Height AMSL (m)
314  //6=vENU X
315  //7=vENU Y
316  //8=vENU Z
317  //9=Roll (rad)
318  //10=Pitch (rad)
319  //11=Yaw (rad)
320 
321  // the millisec str needs 0-padding if size < 6
322  std::string millisecStr = strList[1];
323  while(millisecStr.size() < 6)
324  {
325  millisecStr = "0" + millisecStr;
326  }
327  double stamp = uStr2Double(strList[0] + "." + millisecStr);
328 
329  // conversion GPS to local coordinate XYZ
330  double longitude = uStr2Double(strList[2]);
331  double latitude = uStr2Double(strList[3]);
332  double altitude = uStr2Double(strList[4]);
333  if(poses.empty())
334  {
335  origin = GeodeticCoords(longitude, latitude, altitude);
336  }
337  cv::Point3d coordENU = GeodeticCoords(longitude, latitude, altitude).toENU_WGS84(origin);
338  double roll = uStr2Double(strList[10]);
339  double pitch = uStr2Double(strList[9]);
340  double yaw = -(uStr2Double(strList[11])-M_PI_2);
341  if(stamps)
342  {
343  stamps->insert(std::make_pair(id, stamp));
344  }
345  poses.insert(std::make_pair(id, Transform(coordENU.x,coordENU.y,coordENU.z, roll,pitch,yaw)));
346  }
347  else
348  {
349  UERROR("Error parsing \"%s\" with St Lucia format (should have 12 values, e.g., 101215_153851_Ins0.log)", str.c_str());
350  }
351  }
352  else if(format == 6) // MALAGA URBAN format
353  {
354  std::vector<std::string> strList = uListToVector(uSplit(str));
355  if(strList.size() == 25)
356  {
357  // 0=Time
358  // Lat Lon Alt fix #sats speed dir
359  // 8=Local_X
360  // 9=Local_Y
361  // 10=Local_Z
362  // rawlog_ID Geocen_X Geocen_Y Geocen_Z GPS_X GPS_Y GPS_Z GPS_VX GPS_VY GPS_VZ Local_VX Local_VY Local_VZ SAT_Time
363  double stamp = uStr2Double(strList[0]);
364  double x = uStr2Double(strList[8]);
365  double y = uStr2Double(strList[9]);
366  double z = uStr2Double(strList[10]);
367  if(stamps)
368  {
369  stamps->insert(std::make_pair(id, stamp));
370  }
371  float yaw = 0.0f;
372  if(uContains(poses, id-1))
373  {
374  // set yaw depending on successive poses
375  Transform & previousPose = poses.at(id-1);
376  yaw = atan2(y-previousPose.y(),x-previousPose.x());
377  previousPose = Transform(previousPose.x(), previousPose.y(), yaw);
378  }
379  poses.insert(std::make_pair(id, Transform(x,y,z,0,0,yaw)));
380  }
381  else
382  {
383  UERROR("Error parsing \"%s\" with Malaga Urban format (should have 25 values, *_GPS.txt)", str.c_str());
384  }
385  }
386  else if(format == 5) // NewCollege format
387  {
388  std::vector<std::string> strList = uListToVector(uSplit(str));
389  if(strList.size() == 3)
390  {
391  if( uIsNumber(uReplaceChar(strList[0], ' ', "")) &&
392  uIsNumber(uReplaceChar(strList[1], ' ', "")) &&
393  uIsNumber(uReplaceChar(strList[2], ' ', "")) &&
394  (strList.size()==3 || uIsNumber(uReplaceChar(strList[3], ' ', ""))))
395  {
396  double stamp = uStr2Double(uReplaceChar(strList[0], ' ', ""));
397  double x = uStr2Double(uReplaceChar(strList[1], ' ', ""));
398  double y = uStr2Double(uReplaceChar(strList[2], ' ', ""));
399 
400  if(stamps)
401  {
402  stamps->insert(std::make_pair(id, stamp));
403  }
404  float yaw = 0.0f;
405  if(uContains(poses, id-1))
406  {
407  // set yaw depending on successive poses
408  Transform & previousPose = poses.at(id-1);
409  yaw = atan2(y-previousPose.y(),x-previousPose.x());
410  previousPose = Transform(previousPose.x(), previousPose.y(), yaw);
411  }
412  Transform pose = Transform(x,y,0,0,0,yaw);
413  if(poses.size() == 0)
414  {
415  originPose = pose.inverse();
416  }
417  pose = originPose * pose; // transform in local coordinate where first value is the origin
418  poses.insert(std::make_pair(id, pose));
419  }
420  else
421  {
422  UDEBUG("Not valid values detected: \"%s\"", str.c_str());
423  }
424  }
425  else
426  {
427  UERROR("Error parsing \"%s\" with NewCollege format (should have 3 values: stamp x y, found %d)", str.c_str(), (int)strList.size());
428  }
429  }
430  else if(format == 1 || format==10 || format==11) // rgbd-slam format
431  {
432  std::list<std::string> strList = uSplit(str);
433  if((strList.size() == 8 && format!=11) || (strList.size() == 9 && format==11))
434  {
435  double stamp = uStr2Double(strList.front());
436  strList.pop_front();
437  if(format==11)
438  {
439  id = uStr2Int(strList.back());
440  strList.pop_back();
441  }
442  str = uJoin(strList, " ");
443  Transform pose = Transform::fromString(str);
444  if(pose.isNull())
445  {
446  UWARN("Null transform read!? line parsed: \"%s\"", str.c_str());
447  }
448  else
449  {
450  if(stamps)
451  {
452  stamps->insert(std::make_pair(id, stamp));
453  }
454  if(format == 1)
455  {
456  // we need to remove optical rotation
457  // z pointing front, x left, y down
458  Transform t( 0, 0, 1, 0,
459  -1, 0, 0, 0,
460  0,-1, 0, 0);
461  pose = t * pose * t.inverse();
462  t = Transform( 0, 0, 1, 0,
463  0, -1, 0, 0,
464  1, 0, 0, 0);
465  pose = t*pose;
466  }
467  poses.insert(std::make_pair(id, pose));
468  }
469  }
470  else
471  {
472  UERROR("Error parsing \"%s\" with RGBD-SLAM format (should have 8 values (or 9 with id): stamp x y z qw qx qy qz [id])", str.c_str());
473  }
474  }
475  else // default / KITTI format
476  {
477  Transform pose = Transform::fromString(str);
478  if(format == 2)
479  {
480  // for KITTI, we need to remove optical rotation
481  // z pointing front, x left, y down
482  Transform t( 0, 0, 1, 0,
483  -1, 0, 0, 0,
484  0,-1, 0, 0);
485  pose = t * pose * t.inverse();
486  }
487  poses.insert(std::make_pair(id, pose));
488  }
489  ++id;
490  }
491  file.close();
492  return true;
493  }
494  return false;
495 }
496 
497 bool exportGPS(
498  const std::string & filePath,
499  const std::map<int, GPS> & gpsValues,
500  unsigned int rgba)
501 {
502  UDEBUG("%s", filePath.c_str());
503  std::string tmpPath = filePath;
504 
505  std::string ext = UFile::getExtension(filePath);
506 
507  if(ext.compare("kml")!=0 && ext.compare("txt")!=0)
508  {
509  UERROR("Only txt and kml formats are supported!");
510  return false;
511  }
512 
513  FILE* fout = 0;
514 #ifdef _MSC_VER
515  fopen_s(&fout, tmpPath.c_str(), "w");
516 #else
517  fout = fopen(tmpPath.c_str(), "w");
518 #endif
519  if(fout)
520  {
521  if(ext.compare("kml")==0)
522  {
523  std::string values;
524  for(std::map<int, GPS>::const_iterator iter=gpsValues.begin(); iter!=gpsValues.end(); ++iter)
525  {
526  values += uFormat("%s,%s,%s ",
527  uReplaceChar(uNumber2Str(iter->second.longitude(), 8, true), ',', '.').c_str(),
528  uReplaceChar(uNumber2Str(iter->second.latitude(), 8, true), ',', '.').c_str(),
529  uReplaceChar(uNumber2Str(iter->second.altitude(), 8, true), ',', '.').c_str());
530  }
531 
532  // switch argb (Qt format) -> abgr
533  unsigned int abgr = 0xFF << 24 | (rgba & 0xFF) << 16 | (rgba & 0xFF00) | ((rgba >> 16) &0xFF);
534 
535  std::string colorHexa = uFormat("%08x", abgr);
536 
537  fprintf(fout, "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n");
538  fprintf(fout, "<kml xmlns=\"http://www.opengis.net/kml/2.2\">\n");
539  fprintf(fout, "<Document>\n"
540  " <name>%s</name>\n", tmpPath.c_str());
541  fprintf(fout, " <StyleMap id=\"msn_ylw-pushpin\">\n"
542  " <Pair>\n"
543  " <key>normal</key>\n"
544  " <styleUrl>#sn_ylw-pushpin</styleUrl>\n"
545  " </Pair>\n"
546  " <Pair>\n"
547  " <key>highlight</key>\n"
548  " <styleUrl>#sh_ylw-pushpin</styleUrl>\n"
549  " </Pair>\n"
550  " </StyleMap>\n"
551  " <Style id=\"sh_ylw-pushpin\">\n"
552  " <IconStyle>\n"
553  " <scale>1.2</scale>\n"
554  " </IconStyle>\n"
555  " <LineStyle>\n"
556  " <color>%s</color>\n"
557  " </LineStyle>\n"
558  " </Style>\n"
559  " <Style id=\"sn_ylw-pushpin\">\n"
560  " <LineStyle>\n"
561  " <color>%s</color>\n"
562  " </LineStyle>\n"
563  " </Style>\n", colorHexa.c_str(), colorHexa.c_str());
564  fprintf(fout, " <Placemark>\n"
565  " <name>%s</name>\n"
566  " <styleUrl>#msn_ylw-pushpin</styleUrl>"
567  " <LineString>\n"
568  " <coordinates>\n"
569  " %s\n"
570  " </coordinates>\n"
571  " </LineString>\n"
572  " </Placemark>\n"
573  "</Document>\n"
574  "</kml>\n",
575  uSplit(tmpPath, '.').front().c_str(),
576  values.c_str());
577  }
578  else
579  {
580  fprintf(fout, "# stamp longitude latitude altitude error bearing\n");
581  for(std::map<int, GPS>::const_iterator iter=gpsValues.begin(); iter!=gpsValues.end(); ++iter)
582  {
583  fprintf(fout, "%f %.*f %.*f %.*f %.*f %.*f\n",
584  iter->second.stamp(),
585  8, iter->second.longitude(),
586  8, iter->second.latitude(),
587  8, iter->second.altitude(),
588  8, iter->second.error(),
589  8, iter->second.bearing());
590  }
591  }
592 
593  fclose(fout);
594  return true;
595  }
596  return false;
597 }
598 
599 // KITTI evaluation
600 float lengths[] = {100,200,300,400,500,600,700,800};
601 int32_t num_lengths = 8;
602 
603 struct errors {
604  int32_t first_frame;
605  float r_err;
606  float t_err;
607  float len;
608  float speed;
609  errors (int32_t first_frame,float r_err,float t_err,float len,float speed) :
610  first_frame(first_frame),r_err(r_err),t_err(t_err),len(len),speed(speed) {}
611 };
612 
613 std::vector<float> trajectoryDistances (const std::vector<Transform> &poses) {
614  std::vector<float> dist;
615  dist.push_back(0);
616  for (unsigned int i=1; i<poses.size(); i++) {
617  Transform P1 = poses[i-1];
618  Transform P2 = poses[i];
619  float dx = P1.x()-P2.x();
620  float dy = P1.y()-P2.y();
621  float dz = P1.z()-P2.z();
622  dist.push_back(dist[i-1]+sqrt(dx*dx+dy*dy+dz*dz));
623  }
624  return dist;
625 }
626 
627 int32_t lastFrameFromSegmentLength(std::vector<float> &dist,int32_t first_frame,float len) {
628  for (unsigned int i=first_frame; i<dist.size(); i++)
629  if (dist[i]>dist[first_frame]+len)
630  return i;
631  return -1;
632 }
633 
634 inline float rotationError(const Transform &pose_error) {
635  float a = pose_error(0,0);
636  float b = pose_error(1,1);
637  float c = pose_error(2,2);
638  float d = 0.5*(a+b+c-1.0);
639  return std::acos(std::max(std::min(d,1.0f),-1.0f));
640 }
641 
642 inline float translationError(const Transform &pose_error) {
643  float dx = pose_error.x();
644  float dy = pose_error.y();
645  float dz = pose_error.z();
646  return sqrt(dx*dx+dy*dy+dz*dz);
647 }
648 
649 void calcKittiSequenceErrors (
650  const std::vector<Transform> &poses_gt,
651  const std::vector<Transform> &poses_result,
652  float & t_err,
653  float & r_err) {
654 
655  UASSERT(poses_gt.size() == poses_result.size());
656 
657  // error vector
658  std::vector<errors> err;
659 
660  // parameters
661  int32_t step_size = 10; // every second
662 
663  // pre-compute distances (from ground truth as reference)
664  std::vector<float> dist = trajectoryDistances(poses_gt);
665 
666  // for all start positions do
667  for (unsigned int first_frame=0; first_frame<poses_gt.size(); first_frame+=step_size) {
668 
669  // for all segment lengths do
670  for (int32_t i=0; i<num_lengths; i++) {
671 
672  // current length
673  float len = lengths[i];
674 
675  // compute last frame
676  int32_t last_frame = lastFrameFromSegmentLength(dist,first_frame,len);
677 
678  // continue, if sequence not long enough
679  if (last_frame==-1)
680  continue;
681 
682  // compute rotational and translational errors
683  Transform pose_delta_gt = poses_gt[first_frame].inverse()*poses_gt[last_frame];
684  Transform pose_delta_result = poses_result[first_frame].inverse()*poses_result[last_frame];
685  Transform pose_error = pose_delta_result.inverse()*pose_delta_gt;
686  float r_err = rotationError(pose_error);
687  float t_err = translationError(pose_error);
688 
689  // compute speed
690  float num_frames = (float)(last_frame-first_frame+1);
691  float speed = len/(0.1*num_frames);
692 
693  // write to file
694  err.push_back(errors(first_frame,r_err/len,t_err/len,len,speed));
695  }
696  }
697 
698  t_err = 0;
699  r_err = 0;
700 
701  // for all errors do => compute sum of t_err, r_err
702  for (std::vector<errors>::iterator it=err.begin(); it!=err.end(); it++)
703  {
704  t_err += it->t_err;
705  r_err += it->r_err;
706  }
707 
708  // save errors
709  float num = err.size();
710  t_err /= num;
711  r_err /= num;
712  t_err *= 100.0f; // Translation error (%)
713  r_err *= 180/CV_PI; // Rotation error (deg/m)
714 }
715 // KITTI evaluation end
716 
717 void calcRelativeErrors (
718  const std::vector<Transform> &poses_gt,
719  const std::vector<Transform> &poses_result,
720  float & t_err,
721  float & r_err) {
722 
723  UASSERT(poses_gt.size() == poses_result.size());
724 
725  // error vector
726  std::vector<errors> err;
727 
728  // for all start positions do
729  for (unsigned int i=0; i<poses_gt.size()-1; ++i)
730  {
731  // compute rotational and translational errors
732  Transform pose_delta_gt = poses_gt[i].inverse()*poses_gt[i+1];
733  Transform pose_delta_result = poses_result[i].inverse()*poses_result[i+1];
734  Transform pose_error = pose_delta_result.inverse()*pose_delta_gt;
735  float r_err = pose_error.getAngle();
736  float t_err = pose_error.getNorm();
737 
738  // write to file
739  err.push_back(errors(i,r_err,t_err,0,0));
740  }
741 
742  t_err = 0;
743  r_err = 0;
744 
745  // for all errors do => compute sum of t_err, r_err
746  for (std::vector<errors>::iterator it=err.begin(); it!=err.end(); it++)
747  {
748  t_err += it->t_err;
749  r_err += it->r_err;
750  }
751 
752  // save errors
753  float num = err.size();
754  t_err /= num;
755  r_err /= num;
756  r_err *= 180/CV_PI; // Rotation error (deg)
757 }
758 
759 Transform calcRMSE (
760  const std::map<int, Transform> & groundTruth,
761  const std::map<int, Transform> & poses,
762  float & translational_rmse,
763  float & translational_mean,
764  float & translational_median,
765  float & translational_std,
766  float & translational_min,
767  float & translational_max,
768  float & rotational_rmse,
769  float & rotational_mean,
770  float & rotational_median,
771  float & rotational_std,
772  float & rotational_min,
773  float & rotational_max)
774 {
775 
776  translational_rmse = 0.0f;
777  translational_mean = 0.0f;
778  translational_median = 0.0f;
779  translational_std = 0.0f;
780  translational_min = 0.0f;
781  translational_max = 0.0f;
782 
783  rotational_rmse = 0.0f;
784  rotational_mean = 0.0f;
785  rotational_median = 0.0f;
786  rotational_std = 0.0f;
787  rotational_min = 0.0f;
788  rotational_max = 0.0f;
789 
790  //align with ground truth for more meaningful results
791  pcl::PointCloud<pcl::PointXYZ> cloud1, cloud2;
792  cloud1.resize(poses.size());
793  cloud2.resize(poses.size());
794  int oi = 0;
795  int idFirst = 0;
796  for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
797  {
798  std::map<int, Transform>::const_iterator jter=groundTruth.find(iter->first);
799  if(jter != groundTruth.end())
800  {
801  if(oi==0)
802  {
803  idFirst = iter->first;
804  }
805  cloud1[oi] = pcl::PointXYZ(jter->second.x(), jter->second.y(), jter->second.z());
806  cloud2[oi++] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
807  }
808  }
809 
810  Transform t = Transform::getIdentity();
811  if(oi>5)
812  {
813  cloud1.resize(oi);
814  cloud2.resize(oi);
815 
816  t = util3d::transformFromXYZCorrespondencesSVD(cloud2, cloud1);
817  }
818  else if(idFirst)
819  {
820  t = groundTruth.at(idFirst) * poses.at(idFirst).inverse();
821  }
822 
823  std::vector<float> translationalErrors(poses.size());
824  std::vector<float> rotationalErrors(poses.size());
825  float sumTranslationalErrors = 0.0f;
826  float sumRotationalErrors = 0.0f;
827  float sumSqrdTranslationalErrors = 0.0f;
828  float sumSqrdRotationalErrors = 0.0f;
829  float radToDegree = 180.0f / M_PI;
830  oi=0;
831  for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
832  {
833  std::map<int, Transform>::const_iterator jter = groundTruth.find(iter->first);
834  if(jter!=groundTruth.end())
835  {
836  Transform pose = t * iter->second;
837  Eigen::Vector3f xAxis(1,0,0);
838  Eigen::Vector3f vA = pose.toEigen3f().linear()*xAxis;
839  Eigen::Vector3f vB = jter->second.toEigen3f().linear()*xAxis;
840  double a = pcl::getAngle3D(Eigen::Vector4f(vA[0], vA[1], vA[2], 0), Eigen::Vector4f(vB[0], vB[1], vB[2], 0));
841  rotationalErrors[oi] = a*radToDegree;
842  translationalErrors[oi] = pose.getDistance(jter->second);
843 
844  sumTranslationalErrors+=translationalErrors[oi];
845  sumSqrdTranslationalErrors+=translationalErrors[oi]*translationalErrors[oi];
846  sumRotationalErrors+=rotationalErrors[oi];
847  sumSqrdRotationalErrors+=rotationalErrors[oi]*rotationalErrors[oi];
848 
849  if(oi == 0)
850  {
851  translational_min = translational_max = translationalErrors[oi];
852  rotational_min = rotational_max = rotationalErrors[oi];
853  }
854  else
855  {
856  if(translationalErrors[oi] < translational_min)
857  {
858  translational_min = translationalErrors[oi];
859  }
860  else if(translationalErrors[oi] > translational_max)
861  {
862  translational_max = translationalErrors[oi];
863  }
864 
865  if(rotationalErrors[oi] < rotational_min)
866  {
867  rotational_min = rotationalErrors[oi];
868  }
869  else if(rotationalErrors[oi] > rotational_max)
870  {
871  rotational_max = rotationalErrors[oi];
872  }
873  }
874 
875  ++oi;
876  }
877  }
878  translationalErrors.resize(oi);
879  rotationalErrors.resize(oi);
880  if(oi)
881  {
882  float total = float(oi);
883  translational_rmse = std::sqrt(sumSqrdTranslationalErrors/total);
884  translational_mean = sumTranslationalErrors/total;
885  translational_median = translationalErrors[oi/2];
886  translational_std = std::sqrt(uVariance(translationalErrors, translational_mean));
887 
888  rotational_rmse = std::sqrt(sumSqrdRotationalErrors/total);
889  rotational_mean = sumRotationalErrors/total;
890  rotational_median = rotationalErrors[oi/2];
891  rotational_std = std::sqrt(uVariance(rotationalErrors, rotational_mean));
892  }
893  return t;
894 }
895 
896 void computeMaxGraphErrors(
897  const std::map<int, Transform> & poses,
898  const std::multimap<int, Link> & links,
899  float & maxLinearErrorRatio,
900  float & maxAngularErrorRatio,
901  float & maxLinearError,
902  float & maxAngularError,
903  const Link ** maxLinearErrorLink,
904  const Link ** maxAngularErrorLink,
905  bool for3DoF)
906 {
907  maxLinearErrorRatio = -1;
908  maxAngularErrorRatio = -1;
909  maxLinearError = -1;
910  maxAngularError = -1;
911 
912  UDEBUG("poses=%d links=%d", (int)poses.size(), (int)links.size());
913  for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
914  {
915  // ignore links with high variance, priors and landmarks
916  if(iter->second.transVariance() <= 1.0 && iter->second.from() != iter->second.to() && iter->second.type() != Link::kLandmark)
917  {
918  Transform t1 = uValue(poses, iter->second.from(), Transform());
919  Transform t2 = uValue(poses, iter->second.to(), Transform());
920  Transform t = t1.inverse()*t2;
921 
922  float linearError = uMax3(
923  fabs(iter->second.transform().x() - t.x()),
924  fabs(iter->second.transform().y() - t.y()),
925  for3DoF?0:fabs(iter->second.transform().z() - t.z()));
926  UASSERT(iter->second.transVariance(false)>0.0);
927  float stddevLinear = sqrt(iter->second.transVariance(false));
928  float linearErrorRatio = linearError/stddevLinear;
929  if(linearErrorRatio > maxLinearErrorRatio)
930  {
931  maxLinearError = linearError;
932  maxLinearErrorRatio = linearErrorRatio;
933  if(maxLinearErrorLink)
934  {
935  *maxLinearErrorLink = &iter->second;
936  }
937  }
938 
939  float opt_roll,opt_pitch,opt_yaw;
940  float link_roll,link_pitch,link_yaw;
941  t.getEulerAngles(opt_roll, opt_pitch, opt_yaw);
942  iter->second.transform().getEulerAngles(link_roll, link_pitch, link_yaw);
943  float angularError = uMax3(
944  for3DoF?0:fabs(opt_roll - link_roll),
945  for3DoF?0:fabs(opt_pitch - link_pitch),
946  fabs(opt_yaw - link_yaw));
947  angularError = angularError>M_PI?2*M_PI-angularError:angularError;
948  UASSERT(iter->second.rotVariance(false)>0.0);
949  float stddevAngular = sqrt(iter->second.rotVariance(false));
950  float angularErrorRatio = angularError/stddevAngular;
951  if(angularErrorRatio > maxAngularErrorRatio)
952  {
953  maxAngularError = angularError;
954  maxAngularErrorRatio = angularErrorRatio;
955  if(maxAngularErrorLink)
956  {
957  *maxAngularErrorLink = &iter->second;
958  }
959  }
960  }
961  }
962 }
963 
964 std::vector<double> getMaxOdomInf(const std::multimap<int, Link> & links)
965 {
966  std::vector<double> maxOdomInf(6,0.0);
967  maxOdomInf.resize(6,0.0);
968  for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
969  {
970  if(iter->second.type() == Link::kNeighbor || iter->second.type() == Link::kNeighborMerged)
971  {
972  for(int i=0; i<6; ++i)
973  {
974  const double & v = iter->second.infMatrix().at<double>(i,i);
975  if(maxOdomInf[i] < v)
976  {
977  maxOdomInf[i] = v;
978  }
979  }
980  }
981  }
982  if(maxOdomInf[0] == 0.0)
983  {
984  maxOdomInf.clear();
985  }
986  return maxOdomInf;
987 }
988 
990 // Graph utilities
992 std::multimap<int, Link>::iterator findLink(
993  std::multimap<int, Link> & links,
994  int from,
995  int to,
996  bool checkBothWays,
997  Link::Type type)
998 {
999  std::multimap<int, Link>::iterator iter = links.find(from);
1000  while(iter != links.end() && iter->first == from)
1001  {
1002  if(iter->second.to() == to && (type==Link::kUndef || type == iter->second.type()))
1003  {
1004  return iter;
1005  }
1006  ++iter;
1007  }
1008 
1009  if(checkBothWays)
1010  {
1011  // let's try to -> from
1012  iter = links.find(to);
1013  while(iter != links.end() && iter->first == to)
1014  {
1015  if(iter->second.to() == from && (type==Link::kUndef || type == iter->second.type()))
1016  {
1017  return iter;
1018  }
1019  ++iter;
1020  }
1021  }
1022  return links.end();
1023 }
1024 
1025 std::multimap<int, int>::iterator findLink(
1026  std::multimap<int, int> & links,
1027  int from,
1028  int to,
1029  bool checkBothWays)
1030 {
1031  std::multimap<int, int>::iterator iter = links.find(from);
1032  while(iter != links.end() && iter->first == from)
1033  {
1034  if(iter->second == to)
1035  {
1036  return iter;
1037  }
1038  ++iter;
1039  }
1040 
1041  if(checkBothWays)
1042  {
1043  // let's try to -> from
1044  iter = links.find(to);
1045  while(iter != links.end() && iter->first == to)
1046  {
1047  if(iter->second == from)
1048  {
1049  return iter;
1050  }
1051  ++iter;
1052  }
1053  }
1054  return links.end();
1055 }
1056 std::multimap<int, Link>::const_iterator findLink(
1057  const std::multimap<int, Link> & links,
1058  int from,
1059  int to,
1060  bool checkBothWays,
1061  Link::Type type)
1062 {
1063  std::multimap<int, Link>::const_iterator iter = links.find(from);
1064  while(iter != links.end() && iter->first == from)
1065  {
1066  if(iter->second.to() == to && (type==Link::kUndef || type == iter->second.type()))
1067  {
1068  return iter;
1069  }
1070  ++iter;
1071  }
1072 
1073  if(checkBothWays)
1074  {
1075  // let's try to -> from
1076  iter = links.find(to);
1077  while(iter != links.end() && iter->first == to)
1078  {
1079  if(iter->second.to() == from && (type==Link::kUndef || type == iter->second.type()))
1080  {
1081  return iter;
1082  }
1083  ++iter;
1084  }
1085  }
1086  return links.end();
1087 }
1088 
1089 std::multimap<int, int>::const_iterator findLink(
1090  const std::multimap<int, int> & links,
1091  int from,
1092  int to,
1093  bool checkBothWays)
1094 {
1095  std::multimap<int, int>::const_iterator iter = links.find(from);
1096  while(iter != links.end() && iter->first == from)
1097  {
1098  if(iter->second == to)
1099  {
1100  return iter;
1101  }
1102  ++iter;
1103  }
1104 
1105  if(checkBothWays)
1106  {
1107  // let's try to -> from
1108  iter = links.find(to);
1109  while(iter != links.end() && iter->first == to)
1110  {
1111  if(iter->second == from)
1112  {
1113  return iter;
1114  }
1115  ++iter;
1116  }
1117  }
1118  return links.end();
1119 }
1120 
1121 std::list<Link> findLinks(
1122  const std::multimap<int, Link> & links,
1123  int from)
1124 {
1125  std::list<Link> output;
1126  for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter != links.end(); ++iter)
1127  {
1128  if(iter->second.from() == from)
1129  {
1130  output.push_back(iter->second);
1131  }
1132  else if(iter->second.to() == from)
1133  {
1134  output.push_back(iter->second.inverse());
1135  }
1136  }
1137  return output;
1138 }
1139 
1140 std::multimap<int, Link> filterDuplicateLinks(
1141  const std::multimap<int, Link> & links)
1142 {
1143  std::multimap<int, Link> output;
1144  for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
1145  {
1146  if(graph::findLink(output, iter->second.from(), iter->second.to(), true, iter->second.type()) == output.end())
1147  {
1148  output.insert(*iter);
1149  }
1150  }
1151  return output;
1152 }
1153 
1154 std::multimap<int, Link> filterLinks(
1155  const std::multimap<int, Link> & links,
1156  Link::Type filteredType,
1157  bool inverted)
1158 {
1159  std::multimap<int, Link> output;
1160  for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
1161  {
1162  if(filteredType == Link::kSelfRefLink)
1163  {
1164  if((!inverted && iter->second.from() != iter->second.to())||
1165  (inverted && iter->second.from() == iter->second.to()))
1166  {
1167  output.insert(*iter);
1168  }
1169  }
1170  else if((!inverted && iter->second.type() != filteredType)||
1171  (inverted && iter->second.type() == filteredType))
1172  {
1173  output.insert(*iter);
1174  }
1175  }
1176  return output;
1177 }
1178 
1179 std::map<int, Link> filterLinks(
1180  const std::map<int, Link> & links,
1181  Link::Type filteredType,
1182  bool inverted)
1183 {
1184  std::map<int, Link> output;
1185  for(std::map<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
1186  {
1187  if(filteredType == Link::kSelfRefLink)
1188  {
1189  if((!inverted && iter->second.from() != iter->second.to())||
1190  (inverted && iter->second.from() == iter->second.to()))
1191  {
1192  output.insert(*iter);
1193  }
1194  }
1195  else if((!inverted && iter->second.type() != filteredType)||
1196  (inverted && iter->second.type() == filteredType))
1197  {
1198  output.insert(*iter);
1199  }
1200  }
1201  return output;
1202 }
1203 
1204 std::map<int, Transform> frustumPosesFiltering(
1205  const std::map<int, Transform> & poses,
1206  const Transform & cameraPose,
1207  float horizontalFOV, // in degrees, xfov = atan((image_width/2)/fx)*2
1208  float verticalFOV, // in degrees, yfov = atan((image_height/2)/fy)*2
1209  float nearClipPlaneDistance,
1210  float farClipPlaneDistance,
1211  bool negative)
1212 {
1213  std::map<int, Transform> output;
1214 
1215  if(poses.size())
1216  {
1217  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
1218  std::vector<int> ids(poses.size());
1219 
1220  cloud->resize(poses.size());
1221  ids.resize(poses.size());
1222  int oi=0;
1223  for(std::map<int, rtabmap::Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
1224  {
1225  if(!iter->second.isNull())
1226  {
1227  (*cloud)[oi] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
1228  ids[oi++] = iter->first;
1229  }
1230  }
1231  cloud->resize(oi);
1232  ids.resize(oi);
1233 
1234  pcl::IndicesPtr indices = util3d::frustumFiltering(
1235  cloud,
1236  pcl::IndicesPtr(new std::vector<int>),
1237  cameraPose,
1238  horizontalFOV,
1239  verticalFOV,
1240  nearClipPlaneDistance,
1241  farClipPlaneDistance,
1242  negative);
1243 
1244 
1245  for(unsigned int i=0; i<indices->size(); ++i)
1246  {
1247  output.insert(*poses.find(ids[indices->at(i)]));
1248  }
1249  }
1250  return output;
1251 }
1252 
1253 std::map<int, Transform> radiusPosesFiltering(
1254  const std::map<int, Transform> & poses,
1255  float radius,
1256  float angle,
1257  bool keepLatest)
1258 {
1259  if(poses.size() > 2 && radius > 0.0f)
1260  {
1261  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
1262  cloud->resize(poses.size());
1263  int i=0;
1264  for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter, ++i)
1265  {
1266  (*cloud)[i] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
1267  UASSERT_MSG(pcl::isFinite((*cloud)[i]), uFormat("Invalid pose (%d) %s", iter->first, iter->second.prettyPrint().c_str()).c_str());
1268  }
1269 
1270  // radius filtering
1271  std::vector<int> names = uKeys(poses);
1272  std::vector<Transform> transforms = uValues(poses);
1273 
1274  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> (false));
1275  tree->setInputCloud(cloud);
1276  std::set<int> indicesChecked;
1277  std::set<int> indicesKept;
1278 
1279  for(unsigned int i=0; i<cloud->size(); ++i)
1280  {
1281  if(indicesChecked.find(i) == indicesChecked.end())
1282  {
1283  std::vector<int> kIndices;
1284  std::vector<float> kDistances;
1285  tree->radiusSearch(cloud->at(i), radius, kIndices, kDistances);
1286 
1287  std::set<int> cloudIndices;
1288  const Transform & currentT = transforms.at(i);
1289  Eigen::Vector3f vA = currentT.toEigen3f().linear()*Eigen::Vector3f(1,0,0);
1290  for(unsigned int j=0; j<kIndices.size(); ++j)
1291  {
1292  if(indicesChecked.find(kIndices[j]) == indicesChecked.end())
1293  {
1294  if(angle > 0.0f)
1295  {
1296  const Transform & checkT = transforms.at(kIndices[j]);
1297  // same orientation?
1298  Eigen::Vector3f vB = checkT.toEigen3f().linear()*Eigen::Vector3f(1,0,0);
1299  double a = pcl::getAngle3D(Eigen::Vector4f(vA[0], vA[1], vA[2], 0), Eigen::Vector4f(vB[0], vB[1], vB[2], 0));
1300  if(a <= angle)
1301  {
1302  cloudIndices.insert(kIndices[j]);
1303  }
1304  }
1305  else
1306  {
1307  cloudIndices.insert(kIndices[j]);
1308  }
1309  }
1310  }
1311 
1312  if(keepLatest)
1313  {
1314  bool lastAdded = false;
1315  for(std::set<int>::reverse_iterator iter = cloudIndices.rbegin(); iter!=cloudIndices.rend(); ++iter)
1316  {
1317  if(!lastAdded)
1318  {
1319  indicesKept.insert(*iter);
1320  lastAdded = true;
1321  }
1322  indicesChecked.insert(*iter);
1323  }
1324  }
1325  else
1326  {
1327  bool firstAdded = false;
1328  for(std::set<int>::iterator iter = cloudIndices.begin(); iter!=cloudIndices.end(); ++iter)
1329  {
1330  if(!firstAdded)
1331  {
1332  indicesKept.insert(*iter);
1333  firstAdded = true;
1334  }
1335  indicesChecked.insert(*iter);
1336  }
1337  }
1338  }
1339  }
1340 
1341  //pcl::IndicesPtr indicesOut(new std::vector<int>);
1342  //indicesOut->insert(indicesOut->end(), indicesKept.begin(), indicesKept.end());
1343  UINFO("Cloud filtered In = %d, Out = %d (radius=%f angle=%f keepLatest=%d)", cloud->size(), indicesKept.size(), radius, angle, keepLatest?1:0);
1344  //pcl::io::savePCDFile("duplicateIn.pcd", *cloud);
1345  //pcl::io::savePCDFile("duplicateOut.pcd", *cloud, *indicesOut);
1346 
1347  std::map<int, Transform> keptPoses;
1348  for(std::set<int>::iterator iter = indicesKept.begin(); iter!=indicesKept.end(); ++iter)
1349  {
1350  keptPoses.insert(std::make_pair(names.at(*iter), transforms.at(*iter)));
1351  }
1352 
1353  // make sure the first and last poses are still here
1354  keptPoses.insert(*poses.begin());
1355  keptPoses.insert(*poses.rbegin());
1356 
1357  return keptPoses;
1358  }
1359  else
1360  {
1361  return poses;
1362  }
1363 }
1364 
1365 std::multimap<int, int> radiusPosesClustering(const std::map<int, Transform> & poses, float radius, float angle)
1366 {
1367  std::multimap<int, int> clusters;
1368  if(poses.size() > 1 && radius > 0.0f)
1369  {
1370  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
1371  cloud->resize(poses.size());
1372  int i=0;
1373  for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter, ++i)
1374  {
1375  (*cloud)[i] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
1376  UASSERT_MSG(pcl::isFinite((*cloud)[i]), uFormat("Invalid pose (%d) %s", iter->first, iter->second.prettyPrint().c_str()).c_str());
1377  }
1378 
1379  // radius clustering (nearest neighbors)
1380  std::vector<int> ids = uKeys(poses);
1381  std::vector<Transform> transforms = uValues(poses);
1382 
1383  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> (false));
1384  tree->setInputCloud(cloud);
1385 
1386  for(unsigned int i=0; i<cloud->size(); ++i)
1387  {
1388  std::vector<int> kIndices;
1389  std::vector<float> kDistances;
1390  tree->radiusSearch(cloud->at(i), radius, kIndices, kDistances);
1391 
1392  std::set<int> cloudIndices;
1393  const Transform & currentT = transforms.at(i);
1394  Eigen::Vector3f vA = currentT.toEigen3f().linear()*Eigen::Vector3f(1,0,0);
1395  for(unsigned int j=0; j<kIndices.size(); ++j)
1396  {
1397  if((int)i != kIndices[j])
1398  {
1399  if(angle > 0.0f)
1400  {
1401  const Transform & checkT = transforms.at(kIndices[j]);
1402  // same orientation?
1403  Eigen::Vector3f vB = checkT.toEigen3f().linear()*Eigen::Vector3f(1,0,0);
1404  double a = pcl::getAngle3D(Eigen::Vector4f(vA[0], vA[1], vA[2], 0), Eigen::Vector4f(vB[0], vB[1], vB[2], 0));
1405  if(a <= angle)
1406  {
1407  clusters.insert(std::make_pair(ids[i], ids[kIndices[j]]));
1408  }
1409  }
1410  else
1411  {
1412  clusters.insert(std::make_pair(ids[i], ids[kIndices[j]]));
1413  }
1414  }
1415  }
1416  }
1417  }
1418  return clusters;
1419 }
1420 
1421 void reduceGraph(
1422  const std::map<int, Transform> & poses,
1423  const std::multimap<int, Link> & links,
1424  std::multimap<int, int> & hyperNodes, //<parent ID, child ID>
1425  std::multimap<int, Link> & hyperLinks)
1426 {
1427  UINFO("Input: poses=%d links=%d", (int)poses.size(), (int)links.size());
1428  UTimer timer;
1429  std::map<int, int> posesToHyperNodes;
1430  std::map<int, std::multimap<int, Link> > clusterloopClosureLinks;
1431 
1432  {
1433  std::multimap<int, Link> bidirectionalLoopClosureLinks;
1434  for(std::multimap<int, Link>::const_iterator jter=links.begin(); jter!=links.end(); ++jter)
1435  {
1436  if(jter->second.type() != Link::kNeighbor &&
1437  jter->second.type() != Link::kNeighborMerged &&
1438  jter->second.userDataCompressed().empty())
1439  {
1440  if(uContains(poses, jter->second.from()) &&
1441  uContains(poses, jter->second.to()))
1442  {
1443  UASSERT_MSG(graph::findLink(links, jter->second.to(), jter->second.from(), false) == links.end(), "Input links should be unique!");
1444  bidirectionalLoopClosureLinks.insert(std::make_pair(jter->second.from(), jter->second));
1445  //bidirectionalLoopClosureLinks.insert(std::make_pair(jter->second.to(), jter->second.inverse()));
1446  }
1447  }
1448  }
1449 
1450  UINFO("Clustering hyper nodes...");
1451  // largest ID to smallest ID
1452  for(std::map<int, Transform>::const_reverse_iterator iter=poses.rbegin(); iter!=poses.rend(); ++iter)
1453  {
1454  if(posesToHyperNodes.find(iter->first) == posesToHyperNodes.end())
1455  {
1456  int hyperNodeId = iter->first;
1457  std::list<int> loopClosures;
1458  std::set<int> loopClosuresAdded;
1459  loopClosures.push_back(iter->first);
1460  std::multimap<int, Link> clusterLinks;
1461  while(loopClosures.size())
1462  {
1463  int id = loopClosures.front();
1464  loopClosures.pop_front();
1465 
1466  UASSERT(posesToHyperNodes.find(id) == posesToHyperNodes.end());
1467 
1468  posesToHyperNodes.insert(std::make_pair(id, hyperNodeId));
1469  hyperNodes.insert(std::make_pair(hyperNodeId, id));
1470 
1471  for(std::multimap<int, Link>::const_iterator jter=bidirectionalLoopClosureLinks.find(id); jter!=bidirectionalLoopClosureLinks.end() && jter->first==id; ++jter)
1472  {
1473  if(posesToHyperNodes.find(jter->second.to()) == posesToHyperNodes.end() &&
1474  loopClosuresAdded.find(jter->second.to()) == loopClosuresAdded.end())
1475  {
1476  loopClosures.push_back(jter->second.to());
1477  loopClosuresAdded.insert(jter->second.to());
1478  clusterLinks.insert(*jter);
1479  clusterLinks.insert(std::make_pair(jter->second.to(), jter->second.inverse()));
1480  if(jter->second.from() < jter->second.to())
1481  {
1482  UWARN("Child to Parent link? %d->%d (type=%d)",
1483  jter->second.from(),
1484  jter->second.to(),
1485  jter->second.type());
1486  }
1487  }
1488  }
1489  }
1490  UASSERT(clusterloopClosureLinks.find(hyperNodeId) == clusterloopClosureLinks.end());
1491  clusterloopClosureLinks.insert(std::make_pair(hyperNodeId, clusterLinks));
1492  UDEBUG("Created hyper node %d with %d children (%f%%)",
1493  hyperNodeId, (int)loopClosuresAdded.size(), float(posesToHyperNodes.size())/float(poses.size())*100.0f);
1494  }
1495  }
1496  UINFO("Clustering hyper nodes... done! (%f s)", timer.ticks());
1497  }
1498 
1499  UINFO("Creating hyper links...");
1500  int i=0;
1501  for(std::multimap<int, Link>::const_reverse_iterator jter=links.rbegin(); jter!=links.rend(); ++jter)
1502  {
1503  if((jter->second.type() == Link::kNeighbor ||
1504  jter->second.type() == Link::kNeighborMerged ||
1505  !jter->second.userDataCompressed().empty()) &&
1506  uContains(poses, jter->second.from()) &&
1507  uContains(poses, jter->second.to()))
1508  {
1509  UASSERT_MSG(uContains(posesToHyperNodes, jter->second.from()), uFormat("%d->%d (type=%d)", jter->second.from(), jter->second.to(), jter->second.type()).c_str());
1510  UASSERT_MSG(uContains(posesToHyperNodes, jter->second.to()), uFormat("%d->%d (type=%d)", jter->second.from(), jter->second.to(), jter->second.type()).c_str());
1511  int hyperNodeIDFrom = posesToHyperNodes.at(jter->second.from());
1512  int hyperNodeIDTo = posesToHyperNodes.at(jter->second.to());
1513 
1514  // ignore links inside a hyper node
1515  if(hyperNodeIDFrom != hyperNodeIDTo)
1516  {
1517  std::multimap<int, Link>::iterator tmpIter = graph::findLink(hyperLinks, hyperNodeIDFrom, hyperNodeIDTo);
1518  if(tmpIter!=hyperLinks.end() &&
1519  hyperNodeIDFrom == jter->second.from() &&
1520  hyperNodeIDTo == jter->second.to() &&
1521  tmpIter->second.type() > Link::kNeighbor &&
1522  jter->second.type() == Link::kNeighbor)
1523  {
1524  // neighbor links have priority, so remove the previously added link
1525  hyperLinks.erase(tmpIter);
1526  }
1527 
1528  // only add unique link between two hyper nodes (keeping only the more recent)
1529  if(graph::findLink(hyperLinks, hyperNodeIDFrom, hyperNodeIDTo) == hyperLinks.end())
1530  {
1531  UASSERT(clusterloopClosureLinks.find(hyperNodeIDFrom) != clusterloopClosureLinks.end());
1532  UASSERT(clusterloopClosureLinks.find(hyperNodeIDTo) != clusterloopClosureLinks.end());
1533  std::multimap<int, Link> tmpLinks = clusterloopClosureLinks.at(hyperNodeIDFrom);
1534  tmpLinks.insert(clusterloopClosureLinks.at(hyperNodeIDTo).begin(), clusterloopClosureLinks.at(hyperNodeIDTo).end());
1535  tmpLinks.insert(std::make_pair(jter->second.from(), jter->second));
1536 
1537  std::list<int> path = computePath(tmpLinks, hyperNodeIDFrom, hyperNodeIDTo, false, true);
1538  UASSERT_MSG(path.size()>1,
1539  uFormat("path.size()=%d, hyperNodeIDFrom=%d, hyperNodeIDTo=%d",
1540  (int)path.size(),
1541  hyperNodeIDFrom,
1542  hyperNodeIDTo).c_str());
1543 
1544  if(path.size() > 10)
1545  {
1546  UWARN("Large path! %d nodes", (int)path.size());
1547  std::stringstream stream;
1548  for(std::list<int>::const_iterator iter=path.begin(); iter!=path.end();++iter)
1549  {
1550  if(iter!=path.begin())
1551  {
1552  stream << ",";
1553  }
1554 
1555  stream << *iter;
1556  }
1557  UWARN("Path = [%s]", stream.str().c_str());
1558  }
1559 
1560  // create the hyperlink
1561  std::list<int>::iterator iter=path.begin();
1562  int from = *iter;
1563  ++iter;
1564  int to = *iter;
1565  std::multimap<int, Link>::const_iterator foundIter = graph::findLink(tmpLinks, from, to, false);
1566  UASSERT(foundIter != tmpLinks.end());
1567  Link hyperLink = foundIter->second;
1568  ++iter;
1569  from = to;
1570  for(; iter!=path.end(); ++iter)
1571  {
1572  to = *iter;
1573  std::multimap<int, Link>::const_iterator foundIter = graph::findLink(tmpLinks, from, to, false);
1574  UASSERT(foundIter != tmpLinks.end());
1575  hyperLink = hyperLink.merge(foundIter->second, jter->second.type());
1576 
1577  from = to;
1578  }
1579 
1580  UASSERT(hyperLink.from() == hyperNodeIDFrom);
1581  UASSERT(hyperLink.to() == hyperNodeIDTo);
1582  hyperLinks.insert(std::make_pair(hyperNodeIDFrom, hyperLink));
1583 
1584  UDEBUG("Created hyper link %d->%d (%f%%)",
1585  hyperLink.from(), hyperLink.to(), float(i)/float(links.size())*100.0f);
1586  if(hyperLink.transform().getNorm() > jter->second.transform().getNorm()+1)
1587  {
1588  UWARN("Large hyper link %d->%d (%f m)! original %d->%d (%f m)",
1589  hyperLink.from(),
1590  hyperLink.to(),
1591  hyperLink.transform().getNorm(),
1592  jter->second.from(),
1593  jter->second.to(),
1594  jter->second.transform().getNorm());
1595 
1596  }
1597  }
1598  }
1599  }
1600  ++i;
1601  }
1602  UINFO("Creating hyper links... done! (%f s)", timer.ticks());
1603 
1604  UINFO("Output: poses=%d links=%d", (int)uUniqueKeys(hyperNodes).size(), (int)links.size());
1605 }
1606 
1607 
1608 class Node
1609 {
1610 public:
1611  Node(int id, int fromId, const rtabmap::Transform & pose) :
1612  id_(id),
1613  costSoFar_(0.0f),
1614  distToEnd_(0.0f),
1615  fromId_(fromId),
1616  closed_(false),
1617  pose_(pose)
1618  {}
1619 
1620  int id() const {return id_;}
1621  int fromId() const {return fromId_;}
1622  bool isClosed() const {return closed_;}
1623  bool isOpened() const {return !closed_;}
1624  float costSoFar() const {return costSoFar_;} // Dijkstra cost
1625  float distToEnd() const {return distToEnd_;} // Breath-first cost
1626  float totalCost() const {return costSoFar_ + distToEnd_;} // A* cost
1627  rtabmap::Transform pose() const {return pose_;}
1628  float distFrom(const rtabmap::Transform & pose) const
1629  {
1630  return pose_.getDistance(pose); // use sqrt distance
1631  }
1632 
1633  void setClosed(bool closed) {closed_ = closed;}
1634  void setFromId(int fromId) {fromId_ = fromId;}
1635  void setCostSoFar(float costSoFar) {costSoFar_ = costSoFar;}
1636  void setDistToEnd(float distToEnd) {distToEnd_ = distToEnd;}
1637  void setPose(const Transform & pose) {pose_ = pose;}
1638 
1639 private:
1640  int id_;
1641  float costSoFar_;
1642  float distToEnd_;
1643  int fromId_;
1644  bool closed_;
1645  rtabmap::Transform pose_;
1646 };
1647 
1648 typedef std::pair<int, float> Pair; // first is id, second is cost
1649 struct Order
1650 {
1651  bool operator()(Pair const& a, Pair const& b) const
1652  {
1653  return a.second > b.second;
1654  }
1655 };
1656 
1657 // A*
1658 std::list<std::pair<int, Transform> > computePath(
1659  const std::map<int, rtabmap::Transform> & poses,
1660  const std::multimap<int, int> & links,
1661  int from,
1662  int to,
1663  bool updateNewCosts)
1664 {
1665  std::list<std::pair<int, Transform> > path;
1666 
1667  int startNode = from;
1668  int endNode = to;
1669  rtabmap::Transform endPose = poses.at(endNode);
1670  std::map<int, Node> nodes;
1671  nodes.insert(std::make_pair(startNode, Node(startNode, 0, poses.at(startNode))));
1672  std::priority_queue<Pair, std::vector<Pair>, Order> pq;
1673  std::multimap<float, int> pqmap;
1674  if(updateNewCosts)
1675  {
1676  pqmap.insert(std::make_pair(0, startNode));
1677  }
1678  else
1679  {
1680  pq.push(Pair(startNode, 0));
1681  }
1682 
1683  while((updateNewCosts && pqmap.size()) || (!updateNewCosts && pq.size()))
1684  {
1685  Node * currentNode;
1686  if(updateNewCosts)
1687  {
1688  currentNode = &nodes.find(pqmap.begin()->second)->second;
1689  pqmap.erase(pqmap.begin());
1690  }
1691  else
1692  {
1693  currentNode = &nodes.find(pq.top().first)->second;
1694  pq.pop();
1695  }
1696 
1697  currentNode->setClosed(true);
1698 
1699  if(currentNode->id() == endNode)
1700  {
1701  while(currentNode->id()!=startNode)
1702  {
1703  path.push_front(std::make_pair(currentNode->id(), currentNode->pose()));
1704  currentNode = &nodes.find(currentNode->fromId())->second;
1705  }
1706  path.push_front(std::make_pair(startNode, poses.at(startNode)));
1707  break;
1708  }
1709 
1710  // lookup neighbors
1711  for(std::multimap<int, int>::const_iterator iter = links.find(currentNode->id());
1712  iter!=links.end() && iter->first == currentNode->id();
1713  ++iter)
1714  {
1715  std::map<int, Node>::iterator nodeIter = nodes.find(iter->second);
1716  if(nodeIter == nodes.end())
1717  {
1718  std::map<int, rtabmap::Transform>::const_iterator poseIter = poses.find(iter->second);
1719  if(poseIter == poses.end())
1720  {
1721  UERROR("Next pose %d (from %d) should be found in poses! Ignoring it!", iter->second, iter->first);
1722  }
1723  else
1724  {
1725  Node n(iter->second, currentNode->id(), poseIter->second);
1726  n.setCostSoFar(currentNode->costSoFar() + currentNode->distFrom(poseIter->second));
1727  n.setDistToEnd(n.distFrom(endPose));
1728 
1729  nodes.insert(std::make_pair(iter->second, n));
1730  if(updateNewCosts)
1731  {
1732  pqmap.insert(std::make_pair(n.totalCost(), n.id()));
1733  }
1734  else
1735  {
1736  pq.push(Pair(n.id(), n.totalCost()));
1737  }
1738  }
1739  }
1740  else if(updateNewCosts && nodeIter->second.isOpened())
1741  {
1742  float newCostSoFar = currentNode->costSoFar() + currentNode->distFrom(nodeIter->second.pose());
1743  if(nodeIter->second.costSoFar() > newCostSoFar)
1744  {
1745  // update the cost in the priority queue
1746  for(std::multimap<float, int>::iterator mapIter=pqmap.begin(); mapIter!=pqmap.end(); ++mapIter)
1747  {
1748  if(mapIter->second == nodeIter->first)
1749  {
1750  pqmap.erase(mapIter);
1751  nodeIter->second.setCostSoFar(newCostSoFar);
1752  pqmap.insert(std::make_pair(nodeIter->second.totalCost(), nodeIter->first));
1753  break;
1754  }
1755  }
1756  }
1757  }
1758  }
1759  }
1760  return path;
1761 }
1762 
1763 // Dijksta
1764 std::list<int> RTABMAP_EXP computePath(
1765  const std::multimap<int, Link> & links,
1766  int from,
1767  int to,
1768  bool updateNewCosts,
1769  bool useSameCostForAllLinks)
1770 {
1771  std::list<int> path;
1772 
1773  int startNode = from;
1774  int endNode = to;
1775  std::map<int, Node> nodes;
1776  nodes.insert(std::make_pair(startNode, Node(startNode, 0, Transform())));
1777  std::priority_queue<Pair, std::vector<Pair>, Order> pq;
1778  std::multimap<float, int> pqmap;
1779  if(updateNewCosts)
1780  {
1781  pqmap.insert(std::make_pair(0, startNode));
1782  }
1783  else
1784  {
1785  pq.push(Pair(startNode, 0));
1786  }
1787 
1788  while((updateNewCosts && pqmap.size()) || (!updateNewCosts && pq.size()))
1789  {
1790  Node * currentNode;
1791  if(updateNewCosts)
1792  {
1793  currentNode = &nodes.find(pqmap.begin()->second)->second;
1794  pqmap.erase(pqmap.begin());
1795  }
1796  else
1797  {
1798  currentNode = &nodes.find(pq.top().first)->second;
1799  pq.pop();
1800  }
1801 
1802  currentNode->setClosed(true);
1803 
1804  if(currentNode->id() == endNode)
1805  {
1806  while(currentNode->id()!=startNode)
1807  {
1808  path.push_front(currentNode->id());
1809  currentNode = &nodes.find(currentNode->fromId())->second;
1810  }
1811  path.push_front(startNode);
1812  break;
1813  }
1814 
1815  // lookup neighbors
1816  for(std::multimap<int, Link>::const_iterator iter = links.find(currentNode->id());
1817  iter!=links.end() && iter->first == currentNode->id();
1818  ++iter)
1819  {
1820  std::map<int, Node>::iterator nodeIter = nodes.find(iter->second.to());
1821  float cost = 1;
1822  if(!useSameCostForAllLinks)
1823  {
1824  cost = iter->second.transform().getNorm();
1825  }
1826  if(nodeIter == nodes.end())
1827  {
1828  Node n(iter->second.to(), currentNode->id(), Transform());
1829 
1830  n.setCostSoFar(currentNode->costSoFar() + cost);
1831  nodes.insert(std::make_pair(iter->second.to(), n));
1832  if(updateNewCosts)
1833  {
1834  pqmap.insert(std::make_pair(n.totalCost(), n.id()));
1835  }
1836  else
1837  {
1838  pq.push(Pair(n.id(), n.totalCost()));
1839  }
1840  }
1841  else if(!useSameCostForAllLinks && updateNewCosts && nodeIter->second.isOpened())
1842  {
1843  float newCostSoFar = currentNode->costSoFar() + cost;
1844  if(nodeIter->second.costSoFar() > newCostSoFar)
1845  {
1846  // update the cost in the priority queue
1847  for(std::multimap<float, int>::iterator mapIter=pqmap.begin(); mapIter!=pqmap.end(); ++mapIter)
1848  {
1849  if(mapIter->second == nodeIter->first)
1850  {
1851  pqmap.erase(mapIter);
1852  nodeIter->second.setCostSoFar(newCostSoFar);
1853  pqmap.insert(std::make_pair(nodeIter->second.totalCost(), nodeIter->first));
1854  break;
1855  }
1856  }
1857  }
1858  }
1859  }
1860  }
1861  return path;
1862 }
1863 
1864 
1865 // return path starting from "fromId" (Identity pose for the first node)
1866 std::list<std::pair<int, Transform> > computePath(
1867  int fromId,
1868  int toId,
1869  const Memory * memory,
1870  bool lookInDatabase,
1871  bool updateNewCosts,
1872  float linearVelocity, // m/sec
1873  float angularVelocity) // rad/sec
1874 {
1875  UASSERT(memory!=0);
1876  UASSERT(fromId>=0);
1877  UASSERT(toId!=0);
1878  std::list<std::pair<int, Transform> > path;
1879  UDEBUG("fromId=%d, toId=%d, lookInDatabase=%d, updateNewCosts=%d, linearVelocity=%f, angularVelocity=%f",
1880  fromId,
1881  toId,
1882  lookInDatabase?1:0,
1883  updateNewCosts?1:0,
1884  linearVelocity,
1885  angularVelocity);
1886 
1887  std::multimap<int, Link> allLinks;
1888  if(lookInDatabase)
1889  {
1890  // Faster to load all links in one query
1891  UTimer t;
1892  allLinks = memory->getAllLinks(lookInDatabase, true, true);
1893  for(std::multimap<int, Link>::iterator iter=allLinks.begin(); iter!=allLinks.end(); ++iter)
1894  {
1895  if(iter->second.to() < 0)
1896  {
1897  allLinks.insert(std::make_pair(iter->second.to(), iter->second.inverse()));
1898  }
1899  }
1900  UINFO("getting all %d links time = %f s", (int)allLinks.size(), t.ticks());
1901  }
1902 
1903  //dijkstra
1904  int startNode = fromId;
1905  int endNode = toId;
1906  std::map<int, Node> nodes;
1907  nodes.insert(std::make_pair(startNode, Node(startNode, 0, Transform::getIdentity())));
1908  std::priority_queue<Pair, std::vector<Pair>, Order> pq;
1909  std::multimap<float, int> pqmap;
1910  if(updateNewCosts)
1911  {
1912  pqmap.insert(std::make_pair(0, startNode));
1913  }
1914  else
1915  {
1916  pq.push(Pair(startNode, 0));
1917  }
1918 
1919  while((updateNewCosts && pqmap.size()) || (!updateNewCosts && pq.size()))
1920  {
1921  Node * currentNode;
1922  if(updateNewCosts)
1923  {
1924  currentNode = &nodes.find(pqmap.begin()->second)->second;
1925  pqmap.erase(pqmap.begin());
1926  }
1927  else
1928  {
1929  currentNode = &nodes.find(pq.top().first)->second;
1930  pq.pop();
1931  }
1932 
1933  currentNode->setClosed(true);
1934 
1935  if(currentNode->id() == endNode)
1936  {
1937  while(currentNode->id()!=startNode)
1938  {
1939  path.push_front(std::make_pair(currentNode->id(), currentNode->pose()));
1940  currentNode = &nodes.find(currentNode->fromId())->second;
1941  }
1942  path.push_front(std::make_pair(startNode, currentNode->pose()));
1943  break;
1944  }
1945 
1946  // lookup neighbors
1947  std::multimap<int, Link> links;
1948  if(allLinks.size() == 0)
1949  {
1950  links = memory->getLinks(currentNode->id(), lookInDatabase, true);
1951  }
1952  else
1953  {
1954  for(std::multimap<int, Link>::const_iterator iter = allLinks.lower_bound(currentNode->id());
1955  iter!=allLinks.end() && iter->first == currentNode->id();
1956  ++iter)
1957  {
1958  links.insert(std::make_pair(iter->second.to(), iter->second));
1959  }
1960  }
1961  for(std::multimap<int, Link>::const_iterator iter = links.begin(); iter!=links.end(); ++iter)
1962  {
1963  if(iter->second.from() != iter->second.to())
1964  {
1965  Transform nextPose = currentNode->pose()*iter->second.transform();
1966  float cost = 0.0f;
1967  if(linearVelocity <= 0.0f && angularVelocity <= 0.0f)
1968  {
1969  // use distance only
1970  cost = iter->second.transform().getNorm();
1971  }
1972  else // use time
1973  {
1974  if(linearVelocity > 0.0f)
1975  {
1976  cost += iter->second.transform().getNorm()/linearVelocity;
1977  }
1978  if(angularVelocity > 0.0f)
1979  {
1980  Eigen::Vector4f v1 = Eigen::Vector4f(nextPose.x()-currentNode->pose().x(), nextPose.y()-currentNode->pose().y(), nextPose.z()-currentNode->pose().z(), 1.0f);
1981  Eigen::Vector4f v2 = nextPose.rotation().toEigen4f()*Eigen::Vector4f(1,0,0,1);
1982  float angle = pcl::getAngle3D(v1, v2);
1983  cost += angle / angularVelocity;
1984  }
1985  }
1986 
1987  std::map<int, Node>::iterator nodeIter = nodes.find(iter->first);
1988  if(nodeIter == nodes.end())
1989  {
1990  Node n(iter->second.to(), currentNode->id(), nextPose);
1991 
1992  n.setCostSoFar(currentNode->costSoFar() + cost);
1993  nodes.insert(std::make_pair(iter->second.to(), n));
1994  if(updateNewCosts)
1995  {
1996  pqmap.insert(std::make_pair(n.totalCost(), n.id()));
1997  }
1998  else
1999  {
2000  pq.push(Pair(n.id(), n.totalCost()));
2001  }
2002  }
2003  else if(updateNewCosts && nodeIter->second.isOpened())
2004  {
2005  float newCostSoFar = currentNode->costSoFar() + cost;
2006  if(nodeIter->second.costSoFar() > newCostSoFar)
2007  {
2008  // update pose with new link
2009  nodeIter->second.setPose(nextPose);
2010 
2011  // update the cost in the priority queue
2012  for(std::multimap<float, int>::iterator mapIter=pqmap.begin(); mapIter!=pqmap.end(); ++mapIter)
2013  {
2014  if(mapIter->second == nodeIter->first)
2015  {
2016  pqmap.erase(mapIter);
2017  nodeIter->second.setCostSoFar(newCostSoFar);
2018  pqmap.insert(std::make_pair(nodeIter->second.totalCost(), nodeIter->first));
2019  break;
2020  }
2021  }
2022  }
2023  }
2024  }
2025  }
2026  }
2027 
2028  // Debugging stuff
2029  if(ULogger::level() == ULogger::kDebug)
2030  {
2031  std::stringstream stream;
2032  std::vector<int> linkTypes(Link::kEnd, 0);
2033  std::list<std::pair<int, Transform> >::const_iterator previousIter = path.end();
2034  float length = 0.0f;
2035  for(std::list<std::pair<int, Transform> >::const_iterator iter=path.begin(); iter!=path.end();++iter)
2036  {
2037  if(iter!=path.begin())
2038  {
2039  stream << ",";
2040  }
2041 
2042  if(previousIter!=path.end())
2043  {
2044  //UDEBUG("current %d = %s", iter->first, iter->second.prettyPrint().c_str());
2045  if(allLinks.size())
2046  {
2047  std::multimap<int, Link>::iterator jter = graph::findLink(allLinks, previousIter->first, iter->first);
2048  if(jter != allLinks.end())
2049  {
2050  //Transform nextPose = iter->second;
2051  //Eigen::Vector4f v1 = Eigen::Vector4f(nextPose.x()-previousIter->second.x(), nextPose.y()-previousIter->second.y(), nextPose.z()-previousIter->second.z(), 1.0f);
2052  //Eigen::Vector4f v2 = nextPose.linear().toEigen4f()*Eigen::Vector4f(1,0,0,1);
2053  //float angle = pcl::getAngle3D(v1, v2);
2054  //float cost = angle ;
2055  //UDEBUG("v1=%f,%f,%f v2=%f,%f,%f a=%f", v1[0], v1[1], v1[2], v2[0], v2[1], v2[2], cost);
2056 
2057  UASSERT(jter->second.type() >= Link::kNeighbor && jter->second.type()<Link::kEnd);
2058  ++linkTypes[jter->second.type()];
2059  stream << "[" << jter->second.type() << "]";
2060  length += jter->second.transform().getNorm();
2061  }
2062  }
2063  }
2064 
2065  stream << iter->first;
2066 
2067  previousIter=iter;
2068  }
2069  UDEBUG("Path (%f m) = [%s]", length, stream.str().c_str());
2070  std::stringstream streamB;
2071  for(unsigned int i=0; i<linkTypes.size(); ++i)
2072  {
2073  if(i > 0)
2074  {
2075  streamB << " ";
2076  }
2077  streamB << i << "=" << linkTypes[i];
2078  }
2079  UDEBUG("Link types = %s", streamB.str().c_str());
2080  }
2081 
2082  return path;
2083 }
2084 
2085 int findNearestNode(
2086  const std::map<int, rtabmap::Transform> & poses,
2087  const rtabmap::Transform & targetPose,
2088  float * distance)
2089 {
2090  int id = 0;
2091  std::map<int, float> nearestNodes = findNearestNodes(targetPose, poses, 0, 0, 1);
2092  if(!nearestNodes.empty())
2093  {
2094  id = nearestNodes.begin()->first;
2095  if(distance)
2096  {
2097  *distance = nearestNodes.begin()->second;
2098  }
2099  }
2100  return id;
2101 }
2102 
2103 // return <id, sqrd distance>, excluding query
2104 std::map<int, float> findNearestNodes(
2105  int nodeId,
2106  const std::map<int, Transform> & poses,
2107  float radius,
2108  float angle,
2109  int k)
2110 {
2111  UASSERT(uContains(poses, nodeId));
2112 
2113  std::map<int, Transform> nodesMinusTarget = poses;
2114  Transform targetPose = poses.at(nodeId);
2115  nodesMinusTarget.erase(nodeId);
2116  return findNearestNodes(targetPose, nodesMinusTarget, radius, angle, k);
2117 }
2118 
2119 // return <id, sqrd distance>
2120 std::map<int, float> findNearestNodes(
2121  const Transform & targetPose,
2122  const std::map<int, Transform> & poses,
2123  float radius,
2124  float angle,
2125  int k)
2126 {
2127  UASSERT(radius>=0.0f);
2128  UASSERT(k>=0);
2129  UASSERT(radius > 0.0f || k>0);
2130  std::map<int, float> foundNodes;
2131  if(poses.empty())
2132  {
2133  return foundNodes;
2134  }
2135 
2136  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
2137  cloud->resize(poses.size());
2138  std::vector<int> ids(poses.size());
2139  int oi = 0;
2140  for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
2141  {
2142  (*cloud)[oi] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
2143  UASSERT_MSG(pcl::isFinite((*cloud)[oi]), uFormat("Invalid pose (%d) %s", iter->first, iter->second.prettyPrint().c_str()).c_str());
2144  ids[oi] = iter->first;
2145  ++oi;
2146  }
2147  cloud->resize(oi);
2148  ids.resize(oi);
2149 
2150  if(cloud->size())
2151  {
2152  pcl::search::KdTree<pcl::PointXYZ>::Ptr kdTree(new pcl::search::KdTree<pcl::PointXYZ>);
2153  kdTree->setInputCloud(cloud);
2154  std::vector<int> ind;
2155  std::vector<float> sqrdDist;
2156  pcl::PointXYZ pt(targetPose.x(), targetPose.y(), targetPose.z());
2157  if(radius>0.0f)
2158  {
2159  kdTree->radiusSearch(pt, radius, ind, sqrdDist, k);
2160  }
2161  else
2162  {
2163  kdTree->nearestKSearch(pt, k, ind, sqrdDist);
2164  }
2165  Eigen::Vector3f vA = targetPose.toEigen3f().linear()*Eigen::Vector3f(1,0,0);
2166  for(unsigned int i=0; i<ind.size(); ++i)
2167  {
2168  if(ind[i] >=0)
2169  {
2170  if(angle > 0.0f)
2171  {
2172  const Transform & checkT = poses.at(ids[ind[i]]);
2173  // same orientation?
2174  Eigen::Vector3f vB = checkT.toEigen3f().linear()*Eigen::Vector3f(1,0,0);
2175  double a = pcl::getAngle3D(Eigen::Vector4f(vA[0], vA[1], vA[2], 0), Eigen::Vector4f(vB[0], vB[1], vB[2], 0));
2176  if(a <= angle)
2177  {
2178  foundNodes.insert(std::make_pair(ids[ind[i]], sqrdDist[i]));
2179  }
2180  }
2181  else
2182  {
2183  foundNodes.insert(std::make_pair(ids[ind[i]], sqrdDist[i]));
2184  }
2185  }
2186  }
2187  }
2188  UDEBUG("found nodes=%d", (int)foundNodes.size());
2189  return foundNodes;
2190 }
2191 
2192 // return <id, Transform>, excluding query
2193 std::map<int, Transform> findNearestPoses(
2194  int nodeId,
2195  const std::map<int, Transform> & poses,
2196  float radius,
2197  float angle,
2198  int k)
2199 {
2200  UASSERT(uContains(poses, nodeId));
2201 
2202  std::map<int, Transform> nodesMinusTarget = poses;
2203  Transform targetPose = poses.at(nodeId);
2204  nodesMinusTarget.erase(nodeId);
2205  return findNearestPoses(targetPose, nodesMinusTarget, radius, angle, k);
2206 }
2207 // return <id, Transform>
2208 std::map<int, Transform> findNearestPoses(
2209  const Transform & targetPose,
2210  const std::map<int, Transform> & poses,
2211  float radius,
2212  float angle,
2213  int k)
2214 {
2215  std::map<int, float> nearestNodes = findNearestNodes(targetPose, poses, radius, angle, k);
2216  std::map<int, Transform> foundPoses;
2217  for(std::map<int, float>::iterator iter=nearestNodes.begin(); iter!=nearestNodes.end(); ++iter)
2218  {
2219  foundPoses.insert(*poses.find(iter->first));
2220  }
2221  UDEBUG("found nodes=%d", (int)foundPoses.size());
2222  return foundPoses;
2223 }
2224 
2225 // deprecated stuff
2226 std::map<int, float> findNearestNodes(const std::map<int, rtabmap::Transform> & nodes, const rtabmap::Transform & targetPose, int k)
2227 {
2228  return findNearestNodes(targetPose, nodes, 0, 0, k);
2229 }
2230 std::map<int, float> getNodesInRadius(int nodeId, const std::map<int, Transform> & nodes, float radius)
2231 {
2232  return findNearestNodes(nodeId, nodes, radius);
2233 }
2234 std::map<int, float> getNodesInRadius(const Transform & targetPose, const std::map<int, Transform> & nodes, float radius)
2235 {
2236  return findNearestNodes(targetPose, nodes, radius);
2237 }
2238 std::map<int, Transform> getPosesInRadius(int nodeId, const std::map<int, Transform> & nodes, float radius, float angle)
2239 {
2240  return findNearestPoses(nodeId, nodes, radius, angle);
2241 }
2242 std::map<int, Transform> getPosesInRadius(const Transform & targetPose, const std::map<int, Transform> & nodes, float radius, float angle)
2243 {
2244  return findNearestPoses(targetPose, nodes, radius, angle);
2245 }
2246 
2247 
2248 float computePathLength(
2249  const std::vector<std::pair<int, Transform> > & path,
2250  unsigned int fromIndex,
2251  unsigned int toIndex)
2252 {
2253  float length = 0.0f;
2254  if(path.size() > 1)
2255  {
2256  UASSERT(fromIndex < path.size() && toIndex < path.size() && fromIndex <= toIndex);
2257  if(fromIndex >= toIndex)
2258  {
2259  toIndex = (unsigned int)path.size()-1;
2260  }
2261  float x=0, y=0, z=0;
2262  for(unsigned int i=fromIndex; i<toIndex-1; ++i)
2263  {
2264  x += fabs(path[i].second.x() - path[i+1].second.x());
2265  y += fabs(path[i].second.y() - path[i+1].second.y());
2266  z += fabs(path[i].second.z() - path[i+1].second.z());
2267  }
2268  length = sqrt(x*x + y*y + z*z);
2269  }
2270  return length;
2271 }
2272 
2273 float computePathLength(
2274  const std::map<int, Transform> & path)
2275 {
2276  float length = 0.0f;
2277  if(path.size() > 1)
2278  {
2279  float x=0, y=0, z=0;
2280  std::map<int, Transform>::const_iterator iter=path.begin();
2281  Transform previousPose = iter->second;
2282  ++iter;
2283  for(; iter!=path.end(); ++iter)
2284  {
2285  const Transform & currentPose = iter->second;
2286  x += fabs(previousPose.x() - currentPose.x());
2287  y += fabs(previousPose.y() - currentPose.y());
2288  z += fabs(previousPose.z() - currentPose.z());
2289  previousPose = currentPose;
2290  }
2291  length = sqrt(x*x + y*y + z*z);
2292  }
2293  return length;
2294 }
2295 
2296 // return all paths linked only by neighbor links
2297 std::list<std::map<int, Transform> > getPaths(
2298  std::map<int, Transform> poses,
2299  const std::multimap<int, Link> & links)
2300 {
2301  std::list<std::map<int, Transform> > paths;
2302  if(poses.size() && links.size())
2303  {
2304  // Segment poses connected only by neighbor links
2305  while(poses.size())
2306  {
2307  std::map<int, Transform> path;
2308  for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end();)
2309  {
2310  std::multimap<int, Link>::const_iterator jter = findLink(links, path.rbegin()->first, iter->first);
2311  if(path.size() == 0 || (jter != links.end() && (jter->second.type() == Link::kNeighbor || jter->second.type() == Link::kNeighborMerged)))
2312  {
2313  path.insert(*iter);
2314  poses.erase(iter++);
2315  }
2316  else
2317  {
2318  break;
2319  }
2320  }
2321  UASSERT(path.size());
2322  paths.push_back(path);
2323  }
2324 
2325  }
2326  return paths;
2327 }
2328 
2329 void computeMinMax(const std::map<int, Transform> & poses,
2330  cv::Vec3f & min,
2331  cv::Vec3f & max)
2332 {
2333  if(!poses.empty())
2334  {
2335  min[0] = max[0] = poses.begin()->second.x();
2336  min[1] = max[1] = poses.begin()->second.y();
2337  min[2] = max[2] = poses.begin()->second.z();
2338  for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
2339  {
2340  if(min[0] > iter->second.x())
2341  min[0] = iter->second.x();
2342  if(max[0] < iter->second.x())
2343  max[0] = iter->second.x();
2344  if(min[1] > iter->second.y())
2345  min[1] = iter->second.y();
2346  if(max[1] < iter->second.y())
2347  max[1] = iter->second.y();
2348  if(min[2] > iter->second.z())
2349  min[2] = iter->second.z();
2350  if(max[2] < iter->second.z())
2351  max[2] = iter->second.z();
2352  }
2353  }
2354 }
2355 
2356 } /* namespace graph */
2357 
2358 } /* namespace rtabmap */
uStr2Int
int UTILITE_EXP uStr2Int(const std::string &str)
Definition: UConversion.cpp:125
d
d
rtabmap::graph::Node::distFrom
float distFrom(const rtabmap::Transform &pose) const
Definition: Graph.cpp:1628
uUniqueKeys
std::list< K > uUniqueKeys(const std::multimap< K, V > &mm)
Definition: UStl.h:46
glm::roll
GLM_FUNC_DECL T roll(detail::tquat< T, P > const &x)
uVariance
T uVariance(const T *v, unsigned int size, T meanV)
Definition: UMath.h:491
rtabmap::util3d::isFinite
bool RTABMAP_EXP isFinite(const cv::Point3f &pt)
Definition: util3d.cpp:3194
rtabmap::graph::Node::setPose
void setPose(const Transform &pose)
Definition: Graph.cpp:1637
rtabmap::graph::findLinks
std::list< Link > RTABMAP_EXP findLinks(const std::multimap< int, Link > &links, int from)
Definition: Graph.cpp:1121
rtabmap::graph::Node::distToEnd
float distToEnd() const
Definition: Graph.cpp:1625
rtabmap::util3d::transformFromXYZCorrespondencesSVD
Transform RTABMAP_EXP transformFromXYZCorrespondencesSVD(const pcl::PointCloud< pcl::PointXYZ > &cloud1, const pcl::PointCloud< pcl::PointXYZ > &cloud2)
Definition: util3d_registration.cpp:50
UTimer
Definition: UTimer.h:46
rtabmap::Transform::getEulerAngles
void getEulerAngles(float &roll, float &pitch, float &yaw) const
Definition: Transform.cpp:253
rtabmap::Transform::getAngle
float getAngle(float x=1.0f, float y=0.0f, float z=0.0f) const
Definition: Transform.cpp:266
rtabmap::OptimizerG2O::saveGraph
bool saveGraph(const std::string &fileName, const std::map< int, Transform > &poses, const std::multimap< int, Link > &edgeConstraints)
Definition: OptimizerG2O.cpp:2018
rtabmap::graph::Node::isOpened
bool isOpened() const
Definition: Graph.cpp:1623
rtabmap::graph::rotationError
float rotationError(const Transform &pose_error)
Definition: Graph.cpp:634
rtabmap::graph::reduceGraph
void reduceGraph(const std::map< int, Transform > &poses, const std::multimap< int, Link > &links, std::multimap< int, int > &hyperNodes, std::multimap< int, Link > &hyperLinks)
Definition: Graph.cpp:1421
rtabmap::graph::radiusPosesFiltering
std::map< int, Transform > RTABMAP_EXP radiusPosesFiltering(const std::map< int, Transform > &poses, float radius, float angle, bool keepLatest=true)
Definition: Graph.cpp:1253
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::graph::computeMaxGraphErrors
void RTABMAP_EXP computeMaxGraphErrors(const std::map< int, Transform > &poses, const std::multimap< int, Link > &links, float &maxLinearErrorRatio, float &maxAngularErrorRatio, float &maxLinearError, float &maxAngularError, const Link **maxLinearErrorLink=0, const Link **maxAngularErrorLink=0, bool for3DoF=false)
Definition: Graph.cpp:896
glm::min
GLM_FUNC_DECL genType min(genType const &x, genType const &y)
uKeys
std::vector< K > uKeys(const std::multimap< K, V > &mm)
Definition: UStl.h:67
uStr2Double
double UTILITE_EXP uStr2Double(const std::string &str)
Definition: UConversion.cpp:147
f
f
x
x
rtabmap::graph::Node::pose
rtabmap::Transform pose() const
Definition: Graph.cpp:1627
rtabmap::graph::Node::fromId
int fromId() const
Definition: Graph.cpp:1621
rtabmap::graph::exportGPS
bool RTABMAP_EXP exportGPS(const std::string &filePath, const std::map< int, GPS > &gpsValues, unsigned int rgba=0xFFFFFFFF)
Definition: Graph.cpp:497
rtabmap::graph::Node::costSoFar
float costSoFar() const
Definition: Graph.cpp:1624
glm::sqrt
GLM_FUNC_DECL vecType< T, P > sqrt(vecType< T, P > const &x)
rtabmap::Transform::getIdentity
static Transform getIdentity()
Definition: Transform.cpp:401
rtabmap::graph::translationError
float translationError(const Transform &pose_error)
Definition: Graph.cpp:642
rtabmap::graph::findNearestNode
int RTABMAP_EXP findNearestNode(const std::map< int, rtabmap::Transform > &poses, const rtabmap::Transform &targetPose, float *distance=0)
Definition: Graph.cpp:2085
uStr2Float
float UTILITE_EXP uStr2Float(const std::string &str)
Definition: UConversion.cpp:138
rtabmap::graph::Node::id
int id() const
Definition: Graph.cpp:1620
rtabmap::graph::Node::pose_
rtabmap::Transform pose_
Definition: Graph.cpp:1645
rtabmap::ParametersMap
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:43
glm::distance
GLM_FUNC_DECL genType::value_type distance(genType const &p0, genType const &p1)
rtabmap::graph::filterLinks
std::multimap< int, Link > RTABMAP_EXP filterLinks(const std::multimap< int, Link > &links, Link::Type filteredType, bool inverted=false)
Definition: Graph.cpp:1154
UMath.h
Basic mathematics functions.
glm::angle
GLM_FUNC_DECL T angle(detail::tquat< T, P > const &x)
rtabmap::Transform::data
const float * data() const
Definition: Transform.h:88
rtabmap::Transform::getNorm
float getNorm() const
Definition: Transform.cpp:273
UConversion.h
Some conversion functions.
UFile::getExtension
std::string getExtension()
Definition: UFile.h:140
glm::pitch
GLM_FUNC_DECL T pitch(detail::tquat< T, P > const &x)
rtabmap::graph::trajectoryDistances
std::vector< float > trajectoryDistances(const std::vector< Transform > &poses)
Definition: Graph.cpp:613
rtabmap::graph::filterDuplicateLinks
std::multimap< int, Link > RTABMAP_EXP filterDuplicateLinks(const std::multimap< int, Link > &links)
Definition: Graph.cpp:1140
rtabmap::graph::Node::setDistToEnd
void setDistToEnd(float distToEnd)
Definition: Graph.cpp:1636
rtabmap::util3d::frustumFiltering
pcl::IndicesPtr RTABMAP_EXP frustumFiltering(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const pcl::IndicesPtr &indices, const Transform &cameraPose, float horizontalFOV, float verticalFOV, float nearClipPlaneDistance, float farClipPlaneDistance, bool negative=false)
Definition: util3d_filtering.cpp:940
util3d_registration.h
UASSERT
#define UASSERT(condition)
RTABMAP_EXP
#define RTABMAP_EXP
Definition: RtabmapExp.h:38
UStl.h
Wrappers of STL for convenient functions.
uSplit
std::list< std::string > uSplit(const std::string &str, char separator=' ')
Definition: UStl.h:566
uJoin
std::string uJoin(const std::list< std::string > &strings, const std::string &separator="")
Definition: UStl.h:603
uMax3
T uMax3(const T &a, const T &b, const T &c)
Definition: UMath.h:80
rtabmap::GeodeticCoords::toENU_WGS84
cv::Point3d toENU_WGS84(const GeodeticCoords &origin) const
Definition: GeodeticCoords.cpp:108
M_PI_2
#define M_PI_2
rtabmap::graph::Node::Node
Node(int id, int fromId, const rtabmap::Transform &pose)
Definition: Graph.cpp:1611
UASSERT_MSG
#define UASSERT_MSG(condition, msg_str)
Definition: ULogger.h:67
util3d_filtering.h
rtabmap::graph::getNodesInRadius
std::map< int, float > getNodesInRadius(int nodeId, const std::map< int, Transform > &nodes, float radius)
Definition: Graph.cpp:2230
rtabmap::graph::calcRMSE
Transform RTABMAP_EXP calcRMSE(const std::map< int, Transform > &groundTruth, const std::map< int, Transform > &poses, float &translational_rmse, float &translational_mean, float &translational_median, float &translational_std, float &translational_min, float &translational_max, float &rotational_rmse, float &rotational_mean, float &rotational_median, float &rotational_std, float &rotational_min, float &rotational_max)
Definition: Graph.cpp:759
rtabmap::graph::getMaxOdomInf
std::vector< double > RTABMAP_EXP getMaxOdomInf(const std::multimap< int, Link > &links)
Definition: Graph.cpp:964
rtabmap::graph::getPosesInRadius
std::map< int, Transform > getPosesInRadius(int nodeId, const std::map< int, Transform > &nodes, float radius, float angle)
Definition: Graph.cpp:2238
rtabmap::graph::Node::setClosed
void setClosed(bool closed)
Definition: Graph.cpp:1633
tree
tree
uReplaceChar
std::string UTILITE_EXP uReplaceChar(const std::string &str, char before, char after)
Definition: UConversion.cpp:33
rtabmap::graph::computePath
std::list< std::pair< int, Transform > > RTABMAP_EXP computePath(const std::map< int, rtabmap::Transform > &poses, const std::multimap< int, int > &links, int from, int to, bool updateNewCosts=false)
Definition: Graph.cpp:1658
rtabmap::Transform::toEigen3f
Eigen::Affine3f toEigen3f() const
Definition: Transform.cpp:381
ULogger::level
static ULogger::Level level()
Definition: ULogger.h:340
uIsNumber
bool uIsNumber(const std::string &str)
Definition: UStl.h:647
glm::atan2
GLM_FUNC_QUALIFIER T atan2(T x, T y)
Arc tangent. Returns an angle whose tangent is y/x. The signs of x and y are used to determine what q...
Definition: compatibility.hpp:79
uContains
bool uContains(const std::list< V > &list, const V &value)
Definition: UStl.h:409
rtabmap::graph::radiusPosesClustering
std::multimap< int, int > RTABMAP_EXP radiusPosesClustering(const std::map< int, Transform > &poses, float radius, float angle)
Definition: Graph.cpp:1365
rtabmap::graph::exportPoses
bool RTABMAP_EXP exportPoses(const std::string &filePath, int format, const std::map< int, Transform > &poses, const std::multimap< int, Link > &constraints=std::multimap< int, Link >(), const std::map< int, double > &stamps=std::map< int, double >(), const ParametersMap &parameters=ParametersMap())
Definition: Graph.cpp:54
rtabmap::Transform::rotation
Transform rotation() const
Definition: Transform.cpp:195
rtabmap::Transform::getDistance
float getDistance(const Transform &t) const
Definition: Transform.cpp:283
rtabmap::Transform::isNull
bool isNull() const
Definition: Transform.cpp:107
false
#define false
Definition: ConvertUTF.c:56
uListToVector
std::vector< V > uListToVector(const std::list< V > &list)
Definition: UStl.h:475
rtabmap::graph::Node::isClosed
bool isClosed() const
Definition: Graph.cpp:1622
rtabmap::Transform::getQuaternionf
Eigen::Quaternionf getQuaternionf() const
Definition: Transform.cpp:391
rtabmap::graph::lastFrameFromSegmentLength
int32_t lastFrameFromSegmentLength(std::vector< float > &dist, int32_t first_frame, float len)
Definition: Graph.cpp:627
uValues
std::vector< V > uValues(const std::multimap< K, V > &mm)
Definition: UStl.h:100
rtabmap::graph::Pair
std::pair< int, float > Pair
Definition: Graph.cpp:1648
glm::int32_t
detail::int32 int32_t
Definition: fwd.hpp:307
rtabmap::graph::computeMinMax
void RTABMAP_EXP computeMinMax(const std::map< int, Transform > &poses, cv::Vec3f &min, cv::Vec3f &max)
Definition: Graph.cpp:2329
UDEBUG
#define UDEBUG(...)
glm::max
GLM_FUNC_DECL genType max(genType const &x, genType const &y)
rtabmap::graph::calcRelativeErrors
void RTABMAP_EXP calcRelativeErrors(const std::vector< Transform > &poses_gt, const std::vector< Transform > &poses_result, float &t_err, float &r_err)
Definition: Graph.cpp:717
rtabmap::graph::findNearestPoses
std::map< int, Transform > RTABMAP_EXP findNearestPoses(int nodeId, const std::map< int, Transform > &poses, float radius, float angle=0.0f, int k=0)
Definition: Graph.cpp:2193
dist
dist
UERROR
#define UERROR(...)
rtabmap::graph::importPoses
bool RTABMAP_EXP importPoses(const std::string &filePath, int format, std::map< int, Transform > &poses, std::multimap< int, Link > *constraints=0, std::map< int, double > *stamps=0)
Definition: Graph.cpp:184
ULogger.h
ULogger class and convenient macros.
rtabmap::Transform::toEigen4f
Eigen::Matrix4f toEigen4f() const
Definition: Transform.cpp:362
rtabmap::graph::getPaths
std::list< std::map< int, Transform > > RTABMAP_EXP getPaths(std::map< int, Transform > poses, const std::multimap< int, Link > &links)
Definition: Graph.cpp:2297
glm::acos
GLM_FUNC_DECL genType acos(genType const &x)
UWARN
#define UWARN(...)
UTimer::ticks
double ticks()
Definition: UTimer.cpp:117
glm::yaw
GLM_FUNC_DECL T yaw(detail::tquat< T, P > const &x)
rtabmap::graph::Node::setCostSoFar
void setCostSoFar(float costSoFar)
Definition: Graph.cpp:1635
rtabmap::graph::num_lengths
int32_t num_lengths
Definition: Graph.cpp:601
rtabmap::graph::Order::operator()
bool operator()(Pair const &a, Pair const &b) const
Definition: Graph.cpp:1651
rtabmap::Memory::getLinks
std::multimap< int, Link > getLinks(int signatureId, bool lookInDatabase=false, bool withLandmarks=false) const
Definition: Memory.cpp:1303
rtabmap::Memory::getAllLinks
std::multimap< int, Link > getAllLinks(bool lookInDatabase, bool ignoreNullLinks=true, bool withLandmarks=false) const
Definition: Memory.cpp:1362
rtabmap::Transform::size
int size() const
Definition: Transform.h:90
tango_gl::kIndices
static const GLushort kIndices[]
Definition: video_overlay.cpp:28
rtabmap::graph::Node::totalCost
float totalCost() const
Definition: Graph.cpp:1626
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
glm::length
GLM_FUNC_DECL genType::value_type length(genType const &x)
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::graph::lengths
float lengths[]
Definition: Graph.cpp:600
rtabmap::Transform::fromString
static Transform fromString(const std::string &string)
Definition: Transform.cpp:465
rtabmap::graph::computePathLength
float RTABMAP_EXP computePathLength(const std::vector< std::pair< int, Transform > > &path, unsigned int fromIndex=0, unsigned int toIndex=0)
Definition: Graph.cpp:2248
uFormat
std::string UTILITE_EXP uFormat(const char *fmt,...)
Definition: UConversion.cpp:365
uNumber2Str
std::string UTILITE_EXP uNumber2Str(unsigned int number)
Definition: UConversion.cpp:91
rtabmap::graph::findNearestNodes
std::map< int, float > RTABMAP_EXP findNearestNodes(int nodeId, const std::map< int, Transform > &poses, float radius, float angle=0.0f, int k=0)
Definition: Graph.cpp:2104
uValue
V uValue(const std::map< K, V > &m, const K &key, const V &defaultValue=V())
Definition: UStl.h:240
rtabmap::graph::Node::setFromId
void setFromId(int fromId)
Definition: Graph.cpp:1634
rtabmap::Transform::inverse
Transform inverse() const
Definition: Transform.cpp:178
rtabmap::graph::frustumPosesFiltering
std::map< int, Transform > RTABMAP_EXP frustumPosesFiltering(const std::map< int, Transform > &poses, const Transform &cameraPose, float horizontalFOV=45.0f, float verticalFOV=45.0f, float nearClipPlaneDistance=0.1f, float farClipPlaneDistance=100.0f, bool negative=false)
Definition: Graph.cpp:1204
q
q
rtabmap::graph::calcKittiSequenceErrors
void RTABMAP_EXP calcKittiSequenceErrors(const std::vector< Transform > &poses_gt, const std::vector< Transform > &poses_result, float &t_err, float &r_err)
Definition: Graph.cpp:649
rtabmap::graph::errors::errors
errors(int32_t first_frame, float r_err, float t_err, float len, float speed)
Definition: Graph.cpp:609
UINFO
#define UINFO(...)

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