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

rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Feb 13 2025 03:44:54