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;
962  Transform linkT;
963  if(iter->second.from() < 0)
964  {
965  // For landmarks, compare from node to landmark, in case we optimized only marker's position
966  t = t2.inverse()*t1;
967  linkT = iter->second.transform().inverse();
968  }
969  else
970  {
971  t = t1.inverse()*t2;
972  linkT = iter->second.transform();
973  }
974 
975  float linearError = uMax3(
976  fabs(linkT.x() - t.x()),
977  fabs(linkT.y() - t.y()),
978  force3DoF?0:fabs(linkT.z() - t.z()));
979  UASSERT(iter->second.transVariance(false)>0.0);
980  float stddevLinear = sqrt(iter->second.transVariance(false));
981  float linearErrorRatio = linearError/stddevLinear;
982  if(linearErrorRatio > maxLinearErrorRatio)
983  {
984  maxLinearError = linearError;
985  maxLinearErrorRatio = linearErrorRatio;
986  if(maxLinearErrorLink)
987  {
988  *maxLinearErrorLink = &iter->second;
989  }
990  }
991 
992  // For landmark links, don't compute angular error if it doesn't estimate orientation
993  if(iter->second.type() != Link::kLandmark ||
994  1.0 / static_cast<double>(iter->second.infMatrix().at<double>(5,5)) < 9999.0)
995  {
996  float opt_roll,opt_pitch,opt_yaw;
997  float link_roll,link_pitch,link_yaw;
998  t.getEulerAngles(opt_roll, opt_pitch, opt_yaw);
999  linkT.getEulerAngles(link_roll, link_pitch, link_yaw);
1000  float angularError = uMax3(
1001  force3DoF?0:fabs(opt_roll - link_roll),
1002  force3DoF?0:fabs(opt_pitch - link_pitch),
1003  fabs(opt_yaw - link_yaw));
1004  angularError = angularError>M_PI?2*M_PI-angularError:angularError;
1005  UASSERT(iter->second.rotVariance(false)>0.0);
1006  float stddevAngular = sqrt(iter->second.rotVariance(false));
1007  float angularErrorRatio = angularError/stddevAngular;
1008  if(angularErrorRatio > maxAngularErrorRatio)
1009  {
1010  maxAngularError = angularError;
1011  maxAngularErrorRatio = angularErrorRatio;
1012  if(maxAngularErrorLink)
1013  {
1014  *maxAngularErrorLink = &iter->second;
1015  }
1016  }
1017  }
1018  }
1019  }
1020 }
1021 
1022 std::vector<double> getMaxOdomInf(const std::multimap<int, Link> & links)
1023 {
1024  std::vector<double> maxOdomInf(6,0.0);
1025  maxOdomInf.resize(6,0.0);
1026  for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
1027  {
1028  if(iter->second.type() == Link::kNeighbor || iter->second.type() == Link::kNeighborMerged)
1029  {
1030  for(int i=0; i<6; ++i)
1031  {
1032  const double & v = iter->second.infMatrix().at<double>(i,i);
1033  if(maxOdomInf[i] < v)
1034  {
1035  maxOdomInf[i] = v;
1036  }
1037  }
1038  }
1039  }
1040  if(maxOdomInf[0] == 0.0)
1041  {
1042  maxOdomInf.clear();
1043  }
1044  return maxOdomInf;
1045 }
1046 
1048 // Graph utilities
1050 std::multimap<int, Link>::iterator findLink(
1051  std::multimap<int, Link> & links,
1052  int from,
1053  int to,
1054  bool checkBothWays,
1055  Link::Type type)
1056 {
1057  std::multimap<int, Link>::iterator iter = links.find(from);
1058  while(iter != links.end() && iter->first == from)
1059  {
1060  if(iter->second.to() == to && (type==Link::kUndef || type == iter->second.type()))
1061  {
1062  return iter;
1063  }
1064  ++iter;
1065  }
1066 
1067  if(checkBothWays)
1068  {
1069  // let's try to -> from
1070  iter = links.find(to);
1071  while(iter != links.end() && iter->first == to)
1072  {
1073  if(iter->second.to() == from && (type==Link::kUndef || type == iter->second.type()))
1074  {
1075  return iter;
1076  }
1077  ++iter;
1078  }
1079  }
1080  return links.end();
1081 }
1082 
1083 std::multimap<int, std::pair<int, Link::Type> >::iterator findLink(
1084  std::multimap<int, std::pair<int, Link::Type> > & links,
1085  int from,
1086  int to,
1087  bool checkBothWays,
1088  Link::Type type)
1089 {
1090  std::multimap<int, std::pair<int, Link::Type> >::iterator iter = links.find(from);
1091  while(iter != links.end() && iter->first == from)
1092  {
1093  if(iter->second.first == to && (type==Link::kUndef || type == iter->second.second))
1094  {
1095  return iter;
1096  }
1097  ++iter;
1098  }
1099 
1100  if(checkBothWays)
1101  {
1102  // let's try to -> from
1103  iter = links.find(to);
1104  while(iter != links.end() && iter->first == to)
1105  {
1106  if(iter->second.first == from && (type==Link::kUndef || type == iter->second.second))
1107  {
1108  return iter;
1109  }
1110  ++iter;
1111  }
1112  }
1113  return links.end();
1114 }
1115 
1116 std::multimap<int, int>::iterator findLink(
1117  std::multimap<int, int> & links,
1118  int from,
1119  int to,
1120  bool checkBothWays)
1121 {
1122  std::multimap<int, int>::iterator iter = links.find(from);
1123  while(iter != links.end() && iter->first == from)
1124  {
1125  if(iter->second == to)
1126  {
1127  return iter;
1128  }
1129  ++iter;
1130  }
1131 
1132  if(checkBothWays)
1133  {
1134  // let's try to -> from
1135  iter = links.find(to);
1136  while(iter != links.end() && iter->first == to)
1137  {
1138  if(iter->second == from)
1139  {
1140  return iter;
1141  }
1142  ++iter;
1143  }
1144  }
1145  return links.end();
1146 }
1147 std::multimap<int, Link>::const_iterator findLink(
1148  const std::multimap<int, Link> & links,
1149  int from,
1150  int to,
1151  bool checkBothWays,
1152  Link::Type type)
1153 {
1154  std::multimap<int, Link>::const_iterator iter = links.find(from);
1155  while(iter != links.end() && iter->first == from)
1156  {
1157  if(iter->second.to() == to && (type==Link::kUndef || type == iter->second.type()))
1158  {
1159  return iter;
1160  }
1161  ++iter;
1162  }
1163 
1164  if(checkBothWays)
1165  {
1166  // let's try to -> from
1167  iter = links.find(to);
1168  while(iter != links.end() && iter->first == to)
1169  {
1170  if(iter->second.to() == from && (type==Link::kUndef || type == iter->second.type()))
1171  {
1172  return iter;
1173  }
1174  ++iter;
1175  }
1176  }
1177  return links.end();
1178 }
1179 
1180 std::multimap<int, std::pair<int, Link::Type> >::const_iterator findLink(
1181  const std::multimap<int, std::pair<int, Link::Type> > & links,
1182  int from,
1183  int to,
1184  bool checkBothWays,
1185  Link::Type type)
1186 {
1187  std::multimap<int, std::pair<int, Link::Type> >::const_iterator iter = links.find(from);
1188  while(iter != links.end() && iter->first == from)
1189  {
1190  if(iter->second.first == to && (type==Link::kUndef || type == iter->second.second))
1191  {
1192  return iter;
1193  }
1194  ++iter;
1195  }
1196 
1197  if(checkBothWays)
1198  {
1199  // let's try to -> from
1200  iter = links.find(to);
1201  while(iter != links.end() && iter->first == to)
1202  {
1203  if(iter->second.first == from && (type==Link::kUndef || type == iter->second.second))
1204  {
1205  return iter;
1206  }
1207  ++iter;
1208  }
1209  }
1210  return links.end();
1211 }
1212 
1213 std::multimap<int, int>::const_iterator findLink(
1214  const std::multimap<int, int> & links,
1215  int from,
1216  int to,
1217  bool checkBothWays)
1218 {
1219  std::multimap<int, int>::const_iterator iter = links.find(from);
1220  while(iter != links.end() && iter->first == from)
1221  {
1222  if(iter->second == to)
1223  {
1224  return iter;
1225  }
1226  ++iter;
1227  }
1228 
1229  if(checkBothWays)
1230  {
1231  // let's try to -> from
1232  iter = links.find(to);
1233  while(iter != links.end() && iter->first == to)
1234  {
1235  if(iter->second == from)
1236  {
1237  return iter;
1238  }
1239  ++iter;
1240  }
1241  }
1242  return links.end();
1243 }
1244 
1245 std::list<Link> findLinks(
1246  const std::multimap<int, Link> & links,
1247  int from)
1248 {
1249  std::list<Link> output;
1250  for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter != links.end(); ++iter)
1251  {
1252  if(iter->second.from() == from)
1253  {
1254  output.push_back(iter->second);
1255  }
1256  else if(iter->second.to() == from)
1257  {
1258  output.push_back(iter->second.inverse());
1259  }
1260  }
1261  return output;
1262 }
1263 
1264 std::multimap<int, Link> filterDuplicateLinks(
1265  const std::multimap<int, Link> & links)
1266 {
1267  std::multimap<int, Link> output;
1268  for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
1269  {
1270  if(graph::findLink(output, iter->second.from(), iter->second.to(), true, iter->second.type()) == output.end())
1271  {
1272  output.insert(*iter);
1273  }
1274  }
1275  return output;
1276 }
1277 
1278 std::multimap<int, Link> filterLinks(
1279  const std::multimap<int, Link> & links,
1280  Link::Type filteredType,
1281  bool inverted)
1282 {
1283  std::multimap<int, Link> output;
1284  for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
1285  {
1286  if(filteredType == Link::kSelfRefLink)
1287  {
1288  if((!inverted && iter->second.from() != iter->second.to())||
1289  (inverted && iter->second.from() == iter->second.to()))
1290  {
1291  output.insert(*iter);
1292  }
1293  }
1294  else if((!inverted && iter->second.type() != filteredType)||
1295  (inverted && iter->second.type() == filteredType))
1296  {
1297  output.insert(*iter);
1298  }
1299  }
1300  return output;
1301 }
1302 
1303 std::map<int, Link> filterLinks(
1304  const std::map<int, Link> & links,
1305  Link::Type filteredType,
1306  bool inverted)
1307 {
1308  std::map<int, Link> output;
1309  for(std::map<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
1310  {
1311  if(filteredType == Link::kSelfRefLink)
1312  {
1313  if((!inverted && iter->second.from() != iter->second.to())||
1314  (inverted && iter->second.from() == iter->second.to()))
1315  {
1316  output.insert(*iter);
1317  }
1318  }
1319  else if((!inverted && iter->second.type() != filteredType)||
1320  (inverted && iter->second.type() == filteredType))
1321  {
1322  output.insert(*iter);
1323  }
1324  }
1325  return output;
1326 }
1327 
1328 std::map<int, Transform> frustumPosesFiltering(
1329  const std::map<int, Transform> & poses,
1330  const Transform & cameraPose,
1331  float horizontalFOV, // in degrees, xfov = atan((image_width/2)/fx)*2
1332  float verticalFOV, // in degrees, yfov = atan((image_height/2)/fy)*2
1333  float nearClipPlaneDistance,
1334  float farClipPlaneDistance,
1335  bool negative)
1336 {
1337  std::map<int, Transform> output;
1338 
1339  if(poses.size())
1340  {
1341  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
1342  std::vector<int> ids(poses.size());
1343 
1344  cloud->resize(poses.size());
1345  ids.resize(poses.size());
1346  int oi=0;
1347  for(std::map<int, rtabmap::Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
1348  {
1349  if(!iter->second.isNull())
1350  {
1351  (*cloud)[oi] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
1352  ids[oi++] = iter->first;
1353  }
1354  }
1355  cloud->resize(oi);
1356  ids.resize(oi);
1357 
1358  pcl::IndicesPtr indices = util3d::frustumFiltering(
1359  cloud,
1360  pcl::IndicesPtr(new std::vector<int>),
1361  cameraPose,
1362  horizontalFOV,
1363  verticalFOV,
1364  nearClipPlaneDistance,
1365  farClipPlaneDistance,
1366  negative);
1367 
1368 
1369  for(unsigned int i=0; i<indices->size(); ++i)
1370  {
1371  output.insert(*poses.find(ids[indices->at(i)]));
1372  }
1373  }
1374  return output;
1375 }
1376 
1377 std::map<int, Transform> radiusPosesFiltering(
1378  const std::map<int, Transform> & poses,
1379  float radius,
1380  float angle,
1381  bool keepLatest)
1382 {
1383  if(poses.size() > 2 && radius > 0.0f)
1384  {
1385  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
1386  cloud->resize(poses.size());
1387  int i=0;
1388  for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter, ++i)
1389  {
1390  (*cloud)[i] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
1391  UASSERT_MSG(pcl::isFinite((*cloud)[i]), uFormat("Invalid pose (%d) %s", iter->first, iter->second.prettyPrint().c_str()).c_str());
1392  }
1393 
1394  // radius filtering
1395  std::vector<int> names = uKeys(poses);
1396  std::vector<Transform> transforms = uValues(poses);
1397 
1398  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> (false));
1399  tree->setInputCloud(cloud);
1400  std::set<int> indicesChecked;
1401  std::set<int> indicesKept;
1402 
1403  for(unsigned int i=0; i<cloud->size(); ++i)
1404  {
1405  if(indicesChecked.find(i) == indicesChecked.end())
1406  {
1407  std::vector<int> kIndices;
1408  std::vector<float> kDistances;
1409  tree->radiusSearch(cloud->at(i), radius, kIndices, kDistances);
1410 
1411  std::set<int> cloudIndices;
1412  const Transform & currentT = transforms.at(i);
1413  Eigen::Vector3f vA = currentT.toEigen3f().linear()*Eigen::Vector3f(1,0,0);
1414  for(unsigned int j=0; j<kIndices.size(); ++j)
1415  {
1416  if(indicesChecked.find(kIndices[j]) == indicesChecked.end())
1417  {
1418  if(angle > 0.0f)
1419  {
1420  const Transform & checkT = transforms.at(kIndices[j]);
1421  // same orientation?
1422  Eigen::Vector3f vB = checkT.toEigen3f().linear()*Eigen::Vector3f(1,0,0);
1423  double a = pcl::getAngle3D(Eigen::Vector4f(vA[0], vA[1], vA[2], 0), Eigen::Vector4f(vB[0], vB[1], vB[2], 0));
1424  if(a <= angle)
1425  {
1426  cloudIndices.insert(kIndices[j]);
1427  }
1428  }
1429  else
1430  {
1431  cloudIndices.insert(kIndices[j]);
1432  }
1433  }
1434  }
1435 
1436  if(keepLatest)
1437  {
1438  bool lastAdded = false;
1439  for(std::set<int>::reverse_iterator iter = cloudIndices.rbegin(); iter!=cloudIndices.rend(); ++iter)
1440  {
1441  if(!lastAdded)
1442  {
1443  indicesKept.insert(*iter);
1444  lastAdded = true;
1445  }
1446  indicesChecked.insert(*iter);
1447  }
1448  }
1449  else
1450  {
1451  bool firstAdded = false;
1452  for(std::set<int>::iterator iter = cloudIndices.begin(); iter!=cloudIndices.end(); ++iter)
1453  {
1454  if(!firstAdded)
1455  {
1456  indicesKept.insert(*iter);
1457  firstAdded = true;
1458  }
1459  indicesChecked.insert(*iter);
1460  }
1461  }
1462  }
1463  }
1464 
1465  //pcl::IndicesPtr indicesOut(new std::vector<int>);
1466  //indicesOut->insert(indicesOut->end(), indicesKept.begin(), indicesKept.end());
1467  UINFO("Cloud filtered In = %d, Out = %d (radius=%f angle=%f keepLatest=%d)", cloud->size(), indicesKept.size(), radius, angle, keepLatest?1:0);
1468  //pcl::io::savePCDFile("duplicateIn.pcd", *cloud);
1469  //pcl::io::savePCDFile("duplicateOut.pcd", *cloud, *indicesOut);
1470 
1471  std::map<int, Transform> keptPoses;
1472  for(std::set<int>::iterator iter = indicesKept.begin(); iter!=indicesKept.end(); ++iter)
1473  {
1474  keptPoses.insert(std::make_pair(names.at(*iter), transforms.at(*iter)));
1475  }
1476 
1477  // make sure the first and last poses are still here
1478  keptPoses.insert(*poses.begin());
1479  keptPoses.insert(*poses.rbegin());
1480 
1481  return keptPoses;
1482  }
1483  else
1484  {
1485  return poses;
1486  }
1487 }
1488 
1489 std::multimap<int, int> radiusPosesClustering(const std::map<int, Transform> & poses, float radius, float angle)
1490 {
1491  std::multimap<int, int> clusters;
1492  if(poses.size() > 1 && radius > 0.0f)
1493  {
1494  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
1495  cloud->resize(poses.size());
1496  int i=0;
1497  for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter, ++i)
1498  {
1499  (*cloud)[i] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
1500  UASSERT_MSG(pcl::isFinite((*cloud)[i]), uFormat("Invalid pose (%d) %s", iter->first, iter->second.prettyPrint().c_str()).c_str());
1501  }
1502 
1503  // radius clustering (nearest neighbors)
1504  std::vector<int> ids = uKeys(poses);
1505  std::vector<Transform> transforms = uValues(poses);
1506 
1507  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> (false));
1508  tree->setInputCloud(cloud);
1509 
1510  for(unsigned int i=0; i<cloud->size(); ++i)
1511  {
1512  std::vector<int> kIndices;
1513  std::vector<float> kDistances;
1514  tree->radiusSearch(cloud->at(i), radius, kIndices, kDistances);
1515 
1516  std::set<int> cloudIndices;
1517  const Transform & currentT = transforms.at(i);
1518  Eigen::Vector3f vA = currentT.toEigen3f().linear()*Eigen::Vector3f(1,0,0);
1519  for(unsigned int j=0; j<kIndices.size(); ++j)
1520  {
1521  if((int)i != kIndices[j])
1522  {
1523  if(angle > 0.0f)
1524  {
1525  const Transform & checkT = transforms.at(kIndices[j]);
1526  // same orientation?
1527  Eigen::Vector3f vB = checkT.toEigen3f().linear()*Eigen::Vector3f(1,0,0);
1528  double a = pcl::getAngle3D(Eigen::Vector4f(vA[0], vA[1], vA[2], 0), Eigen::Vector4f(vB[0], vB[1], vB[2], 0));
1529  if(a <= angle)
1530  {
1531  clusters.insert(std::make_pair(ids[i], ids[kIndices[j]]));
1532  }
1533  }
1534  else
1535  {
1536  clusters.insert(std::make_pair(ids[i], ids[kIndices[j]]));
1537  }
1538  }
1539  }
1540  }
1541  }
1542  return clusters;
1543 }
1544 
1545 void reduceGraph(
1546  const std::map<int, Transform> & poses,
1547  const std::multimap<int, Link> & links,
1548  std::multimap<int, int> & hyperNodes, //<parent ID, child ID>
1549  std::multimap<int, Link> & hyperLinks)
1550 {
1551  UINFO("Input: poses=%d links=%d", (int)poses.size(), (int)links.size());
1552  UTimer timer;
1553  std::map<int, int> posesToHyperNodes;
1554  std::map<int, std::multimap<int, Link> > clusterloopClosureLinks;
1555 
1556  {
1557  std::multimap<int, Link> bidirectionalLoopClosureLinks;
1558  for(std::multimap<int, Link>::const_iterator jter=links.begin(); jter!=links.end(); ++jter)
1559  {
1560  if(jter->second.type() != Link::kNeighbor &&
1561  jter->second.type() != Link::kNeighborMerged &&
1562  jter->second.userDataCompressed().empty())
1563  {
1564  if(uContains(poses, jter->second.from()) &&
1565  uContains(poses, jter->second.to()))
1566  {
1567  UASSERT_MSG(graph::findLink(links, jter->second.to(), jter->second.from(), false) == links.end(), "Input links should be unique!");
1568  bidirectionalLoopClosureLinks.insert(std::make_pair(jter->second.from(), jter->second));
1569  //bidirectionalLoopClosureLinks.insert(std::make_pair(jter->second.to(), jter->second.inverse()));
1570  }
1571  }
1572  }
1573 
1574  UINFO("Clustering hyper nodes...");
1575  // largest ID to smallest ID
1576  for(std::map<int, Transform>::const_reverse_iterator iter=poses.rbegin(); iter!=poses.rend(); ++iter)
1577  {
1578  if(posesToHyperNodes.find(iter->first) == posesToHyperNodes.end())
1579  {
1580  int hyperNodeId = iter->first;
1581  std::list<int> loopClosures;
1582  std::set<int> loopClosuresAdded;
1583  loopClosures.push_back(iter->first);
1584  std::multimap<int, Link> clusterLinks;
1585  while(loopClosures.size())
1586  {
1587  int id = loopClosures.front();
1588  loopClosures.pop_front();
1589 
1590  UASSERT(posesToHyperNodes.find(id) == posesToHyperNodes.end());
1591 
1592  posesToHyperNodes.insert(std::make_pair(id, hyperNodeId));
1593  hyperNodes.insert(std::make_pair(hyperNodeId, id));
1594 
1595  for(std::multimap<int, Link>::const_iterator jter=bidirectionalLoopClosureLinks.find(id); jter!=bidirectionalLoopClosureLinks.end() && jter->first==id; ++jter)
1596  {
1597  if(posesToHyperNodes.find(jter->second.to()) == posesToHyperNodes.end() &&
1598  loopClosuresAdded.find(jter->second.to()) == loopClosuresAdded.end())
1599  {
1600  loopClosures.push_back(jter->second.to());
1601  loopClosuresAdded.insert(jter->second.to());
1602  clusterLinks.insert(*jter);
1603  clusterLinks.insert(std::make_pair(jter->second.to(), jter->second.inverse()));
1604  if(jter->second.from() < jter->second.to())
1605  {
1606  UWARN("Child to Parent link? %d->%d (type=%d)",
1607  jter->second.from(),
1608  jter->second.to(),
1609  jter->second.type());
1610  }
1611  }
1612  }
1613  }
1614  UASSERT(clusterloopClosureLinks.find(hyperNodeId) == clusterloopClosureLinks.end());
1615  clusterloopClosureLinks.insert(std::make_pair(hyperNodeId, clusterLinks));
1616  UDEBUG("Created hyper node %d with %d children (%f%%)",
1617  hyperNodeId, (int)loopClosuresAdded.size(), float(posesToHyperNodes.size())/float(poses.size())*100.0f);
1618  }
1619  }
1620  UINFO("Clustering hyper nodes... done! (%f s)", timer.ticks());
1621  }
1622 
1623  UINFO("Creating hyper links...");
1624  int i=0;
1625  for(std::multimap<int, Link>::const_reverse_iterator jter=links.rbegin(); jter!=links.rend(); ++jter)
1626  {
1627  if((jter->second.type() == Link::kNeighbor ||
1628  jter->second.type() == Link::kNeighborMerged ||
1629  !jter->second.userDataCompressed().empty()) &&
1630  uContains(poses, jter->second.from()) &&
1631  uContains(poses, jter->second.to()))
1632  {
1633  UASSERT_MSG(uContains(posesToHyperNodes, jter->second.from()), uFormat("%d->%d (type=%d)", jter->second.from(), jter->second.to(), jter->second.type()).c_str());
1634  UASSERT_MSG(uContains(posesToHyperNodes, jter->second.to()), uFormat("%d->%d (type=%d)", jter->second.from(), jter->second.to(), jter->second.type()).c_str());
1635  int hyperNodeIDFrom = posesToHyperNodes.at(jter->second.from());
1636  int hyperNodeIDTo = posesToHyperNodes.at(jter->second.to());
1637 
1638  // ignore links inside a hyper node
1639  if(hyperNodeIDFrom != hyperNodeIDTo)
1640  {
1641  std::multimap<int, Link>::iterator tmpIter = graph::findLink(hyperLinks, hyperNodeIDFrom, hyperNodeIDTo);
1642  if(tmpIter!=hyperLinks.end() &&
1643  hyperNodeIDFrom == jter->second.from() &&
1644  hyperNodeIDTo == jter->second.to() &&
1645  tmpIter->second.type() > Link::kNeighbor &&
1646  jter->second.type() == Link::kNeighbor)
1647  {
1648  // neighbor links have priority, so remove the previously added link
1649  hyperLinks.erase(tmpIter);
1650  }
1651 
1652  // only add unique link between two hyper nodes (keeping only the more recent)
1653  if(graph::findLink(hyperLinks, hyperNodeIDFrom, hyperNodeIDTo) == hyperLinks.end())
1654  {
1655  UASSERT(clusterloopClosureLinks.find(hyperNodeIDFrom) != clusterloopClosureLinks.end());
1656  UASSERT(clusterloopClosureLinks.find(hyperNodeIDTo) != clusterloopClosureLinks.end());
1657  std::multimap<int, Link> tmpLinks = clusterloopClosureLinks.at(hyperNodeIDFrom);
1658  tmpLinks.insert(clusterloopClosureLinks.at(hyperNodeIDTo).begin(), clusterloopClosureLinks.at(hyperNodeIDTo).end());
1659  tmpLinks.insert(std::make_pair(jter->second.from(), jter->second));
1660 
1661  std::list<int> path = computePath(tmpLinks, hyperNodeIDFrom, hyperNodeIDTo, false, true);
1662  UASSERT_MSG(path.size()>1,
1663  uFormat("path.size()=%d, hyperNodeIDFrom=%d, hyperNodeIDTo=%d",
1664  (int)path.size(),
1665  hyperNodeIDFrom,
1666  hyperNodeIDTo).c_str());
1667 
1668  if(path.size() > 10)
1669  {
1670  UWARN("Large path! %d nodes", (int)path.size());
1671  std::stringstream stream;
1672  for(std::list<int>::const_iterator iter=path.begin(); iter!=path.end();++iter)
1673  {
1674  if(iter!=path.begin())
1675  {
1676  stream << ",";
1677  }
1678 
1679  stream << *iter;
1680  }
1681  UWARN("Path = [%s]", stream.str().c_str());
1682  }
1683 
1684  // create the hyperlink
1685  std::list<int>::iterator iter=path.begin();
1686  int from = *iter;
1687  ++iter;
1688  int to = *iter;
1689  std::multimap<int, Link>::const_iterator foundIter = graph::findLink(tmpLinks, from, to, false);
1690  UASSERT(foundIter != tmpLinks.end());
1691  Link hyperLink = foundIter->second;
1692  ++iter;
1693  from = to;
1694  for(; iter!=path.end(); ++iter)
1695  {
1696  to = *iter;
1697  std::multimap<int, Link>::const_iterator foundIter = graph::findLink(tmpLinks, from, to, false);
1698  UASSERT(foundIter != tmpLinks.end());
1699  hyperLink = hyperLink.merge(foundIter->second, jter->second.type());
1700 
1701  from = to;
1702  }
1703 
1704  UASSERT(hyperLink.from() == hyperNodeIDFrom);
1705  UASSERT(hyperLink.to() == hyperNodeIDTo);
1706  hyperLinks.insert(std::make_pair(hyperNodeIDFrom, hyperLink));
1707 
1708  UDEBUG("Created hyper link %d->%d (%f%%)",
1709  hyperLink.from(), hyperLink.to(), float(i)/float(links.size())*100.0f);
1710  if(hyperLink.transform().getNorm() > jter->second.transform().getNorm()+1)
1711  {
1712  UWARN("Large hyper link %d->%d (%f m)! original %d->%d (%f m)",
1713  hyperLink.from(),
1714  hyperLink.to(),
1715  hyperLink.transform().getNorm(),
1716  jter->second.from(),
1717  jter->second.to(),
1718  jter->second.transform().getNorm());
1719 
1720  }
1721  }
1722  }
1723  }
1724  ++i;
1725  }
1726  UINFO("Creating hyper links... done! (%f s)", timer.ticks());
1727 
1728  UINFO("Output: poses=%d links=%d", (int)uUniqueKeys(hyperNodes).size(), (int)links.size());
1729 }
1730 
1731 
1732 class Node
1733 {
1734 public:
1735  Node(int id, int fromId, const rtabmap::Transform & pose) :
1736  id_(id),
1737  costSoFar_(0.0f),
1738  distToEnd_(0.0f),
1739  fromId_(fromId),
1740  closed_(false),
1741  pose_(pose)
1742  {}
1743 
1744  int id() const {return id_;}
1745  int fromId() const {return fromId_;}
1746  bool isClosed() const {return closed_;}
1747  bool isOpened() const {return !closed_;}
1748  float costSoFar() const {return costSoFar_;} // Dijkstra cost
1749  float distToEnd() const {return distToEnd_;} // Breath-first cost
1750  float totalCost() const {return costSoFar_ + distToEnd_;} // A* cost
1751  rtabmap::Transform pose() const {return pose_;}
1752  float distFrom(const rtabmap::Transform & pose) const
1753  {
1754  return pose_.getDistance(pose); // use sqrt distance
1755  }
1756 
1757  void setClosed(bool closed) {closed_ = closed;}
1758  void setFromId(int fromId) {fromId_ = fromId;}
1759  void setCostSoFar(float costSoFar) {costSoFar_ = costSoFar;}
1760  void setDistToEnd(float distToEnd) {distToEnd_ = distToEnd;}
1761  void setPose(const Transform & pose) {pose_ = pose;}
1762 
1763 private:
1764  int id_;
1765  float costSoFar_;
1766  float distToEnd_;
1767  int fromId_;
1768  bool closed_;
1769  rtabmap::Transform pose_;
1770 };
1771 
1772 typedef std::pair<int, float> Pair; // first is id, second is cost
1773 struct Order
1774 {
1775  bool operator()(Pair const& a, Pair const& b) const
1776  {
1777  return a.second > b.second;
1778  }
1779 };
1780 
1781 // A*
1782 std::list<std::pair<int, Transform> > computePath(
1783  const std::map<int, rtabmap::Transform> & poses,
1784  const std::multimap<int, int> & links,
1785  int from,
1786  int to,
1787  bool updateNewCosts)
1788 {
1789  std::list<std::pair<int, Transform> > path;
1790 
1791  int startNode = from;
1792  int endNode = to;
1793  rtabmap::Transform endPose = poses.at(endNode);
1794  std::map<int, Node> nodes;
1795  nodes.insert(std::make_pair(startNode, Node(startNode, 0, poses.at(startNode))));
1796  std::priority_queue<Pair, std::vector<Pair>, Order> pq;
1797  std::multimap<float, int> pqmap;
1798  if(updateNewCosts)
1799  {
1800  pqmap.insert(std::make_pair(0, startNode));
1801  }
1802  else
1803  {
1804  pq.push(Pair(startNode, 0));
1805  }
1806 
1807  while((updateNewCosts && pqmap.size()) || (!updateNewCosts && pq.size()))
1808  {
1809  Node * currentNode;
1810  if(updateNewCosts)
1811  {
1812  currentNode = &nodes.find(pqmap.begin()->second)->second;
1813  pqmap.erase(pqmap.begin());
1814  }
1815  else
1816  {
1817  currentNode = &nodes.find(pq.top().first)->second;
1818  pq.pop();
1819  }
1820 
1821  currentNode->setClosed(true);
1822 
1823  if(currentNode->id() == endNode)
1824  {
1825  while(currentNode->id()!=startNode)
1826  {
1827  path.push_front(std::make_pair(currentNode->id(), currentNode->pose()));
1828  currentNode = &nodes.find(currentNode->fromId())->second;
1829  }
1830  path.push_front(std::make_pair(startNode, poses.at(startNode)));
1831  break;
1832  }
1833 
1834  // lookup neighbors
1835  for(std::multimap<int, int>::const_iterator iter = links.find(currentNode->id());
1836  iter!=links.end() && iter->first == currentNode->id();
1837  ++iter)
1838  {
1839  std::map<int, Node>::iterator nodeIter = nodes.find(iter->second);
1840  if(nodeIter == nodes.end())
1841  {
1842  std::map<int, rtabmap::Transform>::const_iterator poseIter = poses.find(iter->second);
1843  if(poseIter == poses.end())
1844  {
1845  UERROR("Next pose %d (from %d) should be found in poses! Ignoring it!", iter->second, iter->first);
1846  }
1847  else
1848  {
1849  Node n(iter->second, currentNode->id(), poseIter->second);
1850  n.setCostSoFar(currentNode->costSoFar() + currentNode->distFrom(poseIter->second));
1851  n.setDistToEnd(n.distFrom(endPose));
1852 
1853  nodes.insert(std::make_pair(iter->second, n));
1854  if(updateNewCosts)
1855  {
1856  pqmap.insert(std::make_pair(n.totalCost(), n.id()));
1857  }
1858  else
1859  {
1860  pq.push(Pair(n.id(), n.totalCost()));
1861  }
1862  }
1863  }
1864  else if(updateNewCosts && nodeIter->second.isOpened())
1865  {
1866  float newCostSoFar = currentNode->costSoFar() + currentNode->distFrom(nodeIter->second.pose());
1867  if(nodeIter->second.costSoFar() > newCostSoFar)
1868  {
1869  // update the cost in the priority queue
1870  for(std::multimap<float, int>::iterator mapIter=pqmap.begin(); mapIter!=pqmap.end(); ++mapIter)
1871  {
1872  if(mapIter->second == nodeIter->first)
1873  {
1874  pqmap.erase(mapIter);
1875  nodeIter->second.setCostSoFar(newCostSoFar);
1876  pqmap.insert(std::make_pair(nodeIter->second.totalCost(), nodeIter->first));
1877  break;
1878  }
1879  }
1880  }
1881  }
1882  }
1883  }
1884  return path;
1885 }
1886 
1887 // Dijksta
1888 std::list<int> computePath(
1889  const std::multimap<int, Link> & links,
1890  int from,
1891  int to,
1892  bool updateNewCosts,
1893  bool useSameCostForAllLinks)
1894 {
1895  std::list<int> path;
1896 
1897  int startNode = from;
1898  int endNode = to;
1899  std::map<int, Node> nodes;
1900  nodes.insert(std::make_pair(startNode, Node(startNode, 0, Transform())));
1901  std::priority_queue<Pair, std::vector<Pair>, Order> pq;
1902  std::multimap<float, int> pqmap;
1903  if(updateNewCosts)
1904  {
1905  pqmap.insert(std::make_pair(0, startNode));
1906  }
1907  else
1908  {
1909  pq.push(Pair(startNode, 0));
1910  }
1911 
1912  while((updateNewCosts && pqmap.size()) || (!updateNewCosts && pq.size()))
1913  {
1914  Node * currentNode;
1915  if(updateNewCosts)
1916  {
1917  currentNode = &nodes.find(pqmap.begin()->second)->second;
1918  pqmap.erase(pqmap.begin());
1919  }
1920  else
1921  {
1922  currentNode = &nodes.find(pq.top().first)->second;
1923  pq.pop();
1924  }
1925 
1926  currentNode->setClosed(true);
1927 
1928  if(currentNode->id() == endNode)
1929  {
1930  while(currentNode->id()!=startNode)
1931  {
1932  path.push_front(currentNode->id());
1933  currentNode = &nodes.find(currentNode->fromId())->second;
1934  }
1935  path.push_front(startNode);
1936  break;
1937  }
1938 
1939  // lookup neighbors
1940  for(std::multimap<int, Link>::const_iterator iter = links.find(currentNode->id());
1941  iter!=links.end() && iter->first == currentNode->id();
1942  ++iter)
1943  {
1944  std::map<int, Node>::iterator nodeIter = nodes.find(iter->second.to());
1945  float cost = 1;
1946  if(!useSameCostForAllLinks)
1947  {
1948  cost = iter->second.transform().getNorm();
1949  }
1950  if(nodeIter == nodes.end())
1951  {
1952  Node n(iter->second.to(), currentNode->id(), Transform());
1953 
1954  n.setCostSoFar(currentNode->costSoFar() + cost);
1955  nodes.insert(std::make_pair(iter->second.to(), n));
1956  if(updateNewCosts)
1957  {
1958  pqmap.insert(std::make_pair(n.totalCost(), n.id()));
1959  }
1960  else
1961  {
1962  pq.push(Pair(n.id(), n.totalCost()));
1963  }
1964  }
1965  else if(!useSameCostForAllLinks && updateNewCosts && nodeIter->second.isOpened())
1966  {
1967  float newCostSoFar = currentNode->costSoFar() + cost;
1968  if(nodeIter->second.costSoFar() > newCostSoFar)
1969  {
1970  // update the cost in the priority queue
1971  for(std::multimap<float, int>::iterator mapIter=pqmap.begin(); mapIter!=pqmap.end(); ++mapIter)
1972  {
1973  if(mapIter->second == nodeIter->first)
1974  {
1975  pqmap.erase(mapIter);
1976  nodeIter->second.setCostSoFar(newCostSoFar);
1977  pqmap.insert(std::make_pair(nodeIter->second.totalCost(), nodeIter->first));
1978  break;
1979  }
1980  }
1981  }
1982  }
1983  }
1984  }
1985  return path;
1986 }
1987 
1988 
1989 // return path starting from "fromId" (Identity pose for the first node)
1990 std::list<std::pair<int, Transform> > computePath(
1991  int fromId,
1992  int toId,
1993  const Memory * memory,
1994  bool lookInDatabase,
1995  bool updateNewCosts,
1996  float linearVelocity, // m/sec
1997  float angularVelocity) // rad/sec
1998 {
1999  UASSERT(memory!=0);
2000  UASSERT(fromId>=0);
2001  UASSERT(toId!=0);
2002  std::list<std::pair<int, Transform> > path;
2003  UDEBUG("fromId=%d, toId=%d, lookInDatabase=%d, updateNewCosts=%d, linearVelocity=%f, angularVelocity=%f",
2004  fromId,
2005  toId,
2006  lookInDatabase?1:0,
2007  updateNewCosts?1:0,
2008  linearVelocity,
2009  angularVelocity);
2010 
2011  std::multimap<int, Link> allLinks;
2012  if(lookInDatabase)
2013  {
2014  // Faster to load all links in one query
2015  UTimer t;
2016  allLinks = memory->getAllLinks(lookInDatabase, true, true);
2017  for(std::multimap<int, Link>::iterator iter=allLinks.begin(); iter!=allLinks.end(); ++iter)
2018  {
2019  if(iter->second.to() < 0)
2020  {
2021  allLinks.insert(std::make_pair(iter->second.to(), iter->second.inverse()));
2022  }
2023  }
2024  UINFO("getting all %d links time = %f s", (int)allLinks.size(), t.ticks());
2025  }
2026 
2027  //dijkstra
2028  int startNode = fromId;
2029  int endNode = toId;
2030  std::map<int, Node> nodes;
2031  nodes.insert(std::make_pair(startNode, Node(startNode, 0, Transform::getIdentity())));
2032  std::priority_queue<Pair, std::vector<Pair>, Order> pq;
2033  std::multimap<float, int> pqmap;
2034  if(updateNewCosts)
2035  {
2036  pqmap.insert(std::make_pair(0, startNode));
2037  }
2038  else
2039  {
2040  pq.push(Pair(startNode, 0));
2041  }
2042 
2043  while((updateNewCosts && pqmap.size()) || (!updateNewCosts && pq.size()))
2044  {
2045  Node * currentNode;
2046  if(updateNewCosts)
2047  {
2048  currentNode = &nodes.find(pqmap.begin()->second)->second;
2049  pqmap.erase(pqmap.begin());
2050  }
2051  else
2052  {
2053  currentNode = &nodes.find(pq.top().first)->second;
2054  pq.pop();
2055  }
2056 
2057  currentNode->setClosed(true);
2058 
2059  if(currentNode->id() == endNode)
2060  {
2061  while(currentNode->id()!=startNode)
2062  {
2063  path.push_front(std::make_pair(currentNode->id(), currentNode->pose()));
2064  currentNode = &nodes.find(currentNode->fromId())->second;
2065  }
2066  path.push_front(std::make_pair(startNode, currentNode->pose()));
2067  break;
2068  }
2069 
2070  // lookup neighbors
2071  std::multimap<int, Link> links;
2072  if(allLinks.size() == 0)
2073  {
2074  links = memory->getLinks(currentNode->id(), lookInDatabase, true);
2075  }
2076  else
2077  {
2078  for(std::multimap<int, Link>::const_iterator iter = allLinks.lower_bound(currentNode->id());
2079  iter!=allLinks.end() && iter->first == currentNode->id();
2080  ++iter)
2081  {
2082  links.insert(std::make_pair(iter->second.to(), iter->second));
2083  }
2084  }
2085  for(std::multimap<int, Link>::const_iterator iter = links.begin(); iter!=links.end(); ++iter)
2086  {
2087  if(iter->second.from() != iter->second.to())
2088  {
2089  Transform nextPose = currentNode->pose()*iter->second.transform();
2090  float cost = 0.0f;
2091  if(linearVelocity <= 0.0f && angularVelocity <= 0.0f)
2092  {
2093  // use distance only
2094  cost = iter->second.transform().getNorm();
2095  }
2096  else // use time
2097  {
2098  if(linearVelocity > 0.0f)
2099  {
2100  cost += iter->second.transform().getNorm()/linearVelocity;
2101  }
2102  if(angularVelocity > 0.0f)
2103  {
2104  Eigen::Vector4f v1 = Eigen::Vector4f(nextPose.x()-currentNode->pose().x(), nextPose.y()-currentNode->pose().y(), nextPose.z()-currentNode->pose().z(), 1.0f);
2105  Eigen::Vector4f v2 = nextPose.rotation().toEigen4f()*Eigen::Vector4f(1,0,0,1);
2106  float angle = pcl::getAngle3D(v1, v2);
2107  cost += angle / angularVelocity;
2108  }
2109  }
2110 
2111  std::map<int, Node>::iterator nodeIter = nodes.find(iter->first);
2112  if(nodeIter == nodes.end())
2113  {
2114  Node n(iter->second.to(), currentNode->id(), nextPose);
2115 
2116  n.setCostSoFar(currentNode->costSoFar() + cost);
2117  nodes.insert(std::make_pair(iter->second.to(), n));
2118  if(updateNewCosts)
2119  {
2120  pqmap.insert(std::make_pair(n.totalCost(), n.id()));
2121  }
2122  else
2123  {
2124  pq.push(Pair(n.id(), n.totalCost()));
2125  }
2126  }
2127  else if(updateNewCosts && nodeIter->second.isOpened())
2128  {
2129  float newCostSoFar = currentNode->costSoFar() + cost;
2130  if(nodeIter->second.costSoFar() > newCostSoFar)
2131  {
2132  // update pose with new link
2133  nodeIter->second.setPose(nextPose);
2134 
2135  // update the cost in the priority queue
2136  for(std::multimap<float, int>::iterator mapIter=pqmap.begin(); mapIter!=pqmap.end(); ++mapIter)
2137  {
2138  if(mapIter->second == nodeIter->first)
2139  {
2140  pqmap.erase(mapIter);
2141  nodeIter->second.setCostSoFar(newCostSoFar);
2142  pqmap.insert(std::make_pair(nodeIter->second.totalCost(), nodeIter->first));
2143  break;
2144  }
2145  }
2146  }
2147  }
2148  }
2149  }
2150  }
2151 
2152  // Debugging stuff
2153  if(ULogger::level() == ULogger::kDebug)
2154  {
2155  std::stringstream stream;
2156  std::vector<int> linkTypes(Link::kEnd, 0);
2157  std::list<std::pair<int, Transform> >::const_iterator previousIter = path.end();
2158  float length = 0.0f;
2159  for(std::list<std::pair<int, Transform> >::const_iterator iter=path.begin(); iter!=path.end();++iter)
2160  {
2161  if(iter!=path.begin())
2162  {
2163  stream << ",";
2164  }
2165 
2166  if(previousIter!=path.end())
2167  {
2168  //UDEBUG("current %d = %s", iter->first, iter->second.prettyPrint().c_str());
2169  if(allLinks.size())
2170  {
2171  std::multimap<int, Link>::iterator jter = graph::findLink(allLinks, previousIter->first, iter->first);
2172  if(jter != allLinks.end())
2173  {
2174  //Transform nextPose = iter->second;
2175  //Eigen::Vector4f v1 = Eigen::Vector4f(nextPose.x()-previousIter->second.x(), nextPose.y()-previousIter->second.y(), nextPose.z()-previousIter->second.z(), 1.0f);
2176  //Eigen::Vector4f v2 = nextPose.linear().toEigen4f()*Eigen::Vector4f(1,0,0,1);
2177  //float angle = pcl::getAngle3D(v1, v2);
2178  //float cost = angle ;
2179  //UDEBUG("v1=%f,%f,%f v2=%f,%f,%f a=%f", v1[0], v1[1], v1[2], v2[0], v2[1], v2[2], cost);
2180 
2181  UASSERT(jter->second.type() >= Link::kNeighbor && jter->second.type()<Link::kEnd);
2182  ++linkTypes[jter->second.type()];
2183  stream << "[" << jter->second.type() << "]";
2184  length += jter->second.transform().getNorm();
2185  }
2186  }
2187  }
2188 
2189  stream << iter->first;
2190 
2191  previousIter=iter;
2192  }
2193  UDEBUG("Path (%f m) = [%s]", length, stream.str().c_str());
2194  std::stringstream streamB;
2195  for(unsigned int i=0; i<linkTypes.size(); ++i)
2196  {
2197  if(i > 0)
2198  {
2199  streamB << " ";
2200  }
2201  streamB << i << "=" << linkTypes[i];
2202  }
2203  UDEBUG("Link types = %s", streamB.str().c_str());
2204  }
2205 
2206  return path;
2207 }
2208 
2209 int findNearestNode(
2210  const std::map<int, rtabmap::Transform> & poses,
2211  const rtabmap::Transform & targetPose,
2212  float * distance)
2213 {
2214  int id = 0;
2215  std::map<int, float> nearestNodes = findNearestNodes(targetPose, poses, 0, 0, 1);
2216  if(!nearestNodes.empty())
2217  {
2218  id = nearestNodes.begin()->first;
2219  if(distance)
2220  {
2221  *distance = nearestNodes.begin()->second;
2222  }
2223  }
2224  return id;
2225 }
2226 
2227 // return <id, sqrd distance>, excluding query
2228 std::map<int, float> findNearestNodes(
2229  int nodeId,
2230  const std::map<int, Transform> & poses,
2231  float radius,
2232  float angle,
2233  int k)
2234 {
2235  UASSERT(uContains(poses, nodeId));
2236 
2237  std::map<int, Transform> nodesMinusTarget = poses;
2238  Transform targetPose = poses.at(nodeId);
2239  nodesMinusTarget.erase(nodeId);
2240  return findNearestNodes(targetPose, nodesMinusTarget, radius, angle, k);
2241 }
2242 
2243 // return <id, sqrd distance>
2244 std::map<int, float> findNearestNodes(
2245  const Transform & targetPose,
2246  const std::map<int, Transform> & poses,
2247  float radius,
2248  float angle,
2249  int k)
2250 {
2251  UASSERT(radius>=0.0f);
2252  UASSERT(k>=0);
2253  UASSERT(radius > 0.0f || k>0);
2254  std::map<int, float> foundNodes;
2255  if(poses.empty())
2256  {
2257  return foundNodes;
2258  }
2259 
2260  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
2261  cloud->resize(poses.size());
2262  std::vector<int> ids(poses.size());
2263  int oi = 0;
2264  for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
2265  {
2266  (*cloud)[oi] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
2267  UASSERT_MSG(pcl::isFinite((*cloud)[oi]), uFormat("Invalid pose (%d) %s", iter->first, iter->second.prettyPrint().c_str()).c_str());
2268  ids[oi] = iter->first;
2269  ++oi;
2270  }
2271  cloud->resize(oi);
2272  ids.resize(oi);
2273 
2274  if(cloud->size())
2275  {
2276  pcl::search::KdTree<pcl::PointXYZ>::Ptr kdTree(new pcl::search::KdTree<pcl::PointXYZ>);
2277  kdTree->setInputCloud(cloud);
2278  std::vector<int> ind;
2279  std::vector<float> sqrdDist;
2280  pcl::PointXYZ pt(targetPose.x(), targetPose.y(), targetPose.z());
2281  if(radius>0.0f)
2282  {
2283  kdTree->radiusSearch(pt, radius, ind, sqrdDist, k);
2284  }
2285  else
2286  {
2287  kdTree->nearestKSearch(pt, k, ind, sqrdDist);
2288  }
2289  Eigen::Vector3f vA = targetPose.toEigen3f().linear()*Eigen::Vector3f(1,0,0);
2290  for(unsigned int i=0; i<ind.size(); ++i)
2291  {
2292  if(ind[i] >=0)
2293  {
2294  if(angle > 0.0f)
2295  {
2296  const Transform & checkT = poses.at(ids[ind[i]]);
2297  // same orientation?
2298  Eigen::Vector3f vB = checkT.toEigen3f().linear()*Eigen::Vector3f(1,0,0);
2299  double a = pcl::getAngle3D(Eigen::Vector4f(vA[0], vA[1], vA[2], 0), Eigen::Vector4f(vB[0], vB[1], vB[2], 0));
2300  if(a <= angle)
2301  {
2302  foundNodes.insert(std::make_pair(ids[ind[i]], sqrdDist[i]));
2303  }
2304  }
2305  else
2306  {
2307  foundNodes.insert(std::make_pair(ids[ind[i]], sqrdDist[i]));
2308  }
2309  }
2310  }
2311  }
2312  UDEBUG("found nodes=%d", (int)foundNodes.size());
2313  return foundNodes;
2314 }
2315 
2316 // return <id, Transform>, excluding query
2317 std::map<int, Transform> findNearestPoses(
2318  int nodeId,
2319  const std::map<int, Transform> & poses,
2320  float radius,
2321  float angle,
2322  int k)
2323 {
2324  UASSERT(uContains(poses, nodeId));
2325 
2326  std::map<int, Transform> nodesMinusTarget = poses;
2327  Transform targetPose = poses.at(nodeId);
2328  nodesMinusTarget.erase(nodeId);
2329  return findNearestPoses(targetPose, nodesMinusTarget, radius, angle, k);
2330 }
2331 // return <id, Transform>
2332 std::map<int, Transform> findNearestPoses(
2333  const Transform & targetPose,
2334  const std::map<int, Transform> & poses,
2335  float radius,
2336  float angle,
2337  int k)
2338 {
2339  std::map<int, float> nearestNodes = findNearestNodes(targetPose, poses, radius, angle, k);
2340  std::map<int, Transform> foundPoses;
2341  for(std::map<int, float>::iterator iter=nearestNodes.begin(); iter!=nearestNodes.end(); ++iter)
2342  {
2343  foundPoses.insert(*poses.find(iter->first));
2344  }
2345  UDEBUG("found nodes=%d/%d (radius=%f, angle=%f, k=%d)", (int)foundPoses.size(), (int)poses.size(), radius, angle, k);
2346  return foundPoses;
2347 }
2348 
2349 // deprecated stuff
2350 std::map<int, float> findNearestNodes(const std::map<int, rtabmap::Transform> & nodes, const rtabmap::Transform & targetPose, int k)
2351 {
2352  return findNearestNodes(targetPose, nodes, 0, 0, k);
2353 }
2354 std::map<int, float> getNodesInRadius(int nodeId, const std::map<int, Transform> & nodes, float radius)
2355 {
2356  return findNearestNodes(nodeId, nodes, radius);
2357 }
2358 std::map<int, float> getNodesInRadius(const Transform & targetPose, const std::map<int, Transform> & nodes, float radius)
2359 {
2360  return findNearestNodes(targetPose, nodes, radius);
2361 }
2362 std::map<int, Transform> getPosesInRadius(int nodeId, const std::map<int, Transform> & nodes, float radius, float angle)
2363 {
2364  return findNearestPoses(nodeId, nodes, radius, angle);
2365 }
2366 std::map<int, Transform> getPosesInRadius(const Transform & targetPose, const std::map<int, Transform> & nodes, float radius, float angle)
2367 {
2368  return findNearestPoses(targetPose, nodes, radius, angle);
2369 }
2370 
2371 
2372 float computePathLength(
2373  const std::vector<std::pair<int, Transform> > & path,
2374  unsigned int fromIndex,
2375  unsigned int toIndex)
2376 {
2377  float length = 0.0f;
2378  if(path.size() > 1)
2379  {
2380  UASSERT(fromIndex < path.size() && toIndex < path.size() && fromIndex <= toIndex);
2381  if(fromIndex >= toIndex)
2382  {
2383  toIndex = (unsigned int)path.size()-1;
2384  }
2385  float x=0, y=0, z=0;
2386  for(unsigned int i=fromIndex; i<toIndex-1; ++i)
2387  {
2388  x += fabs(path[i].second.x() - path[i+1].second.x());
2389  y += fabs(path[i].second.y() - path[i+1].second.y());
2390  z += fabs(path[i].second.z() - path[i+1].second.z());
2391  }
2392  length = sqrt(x*x + y*y + z*z);
2393  }
2394  return length;
2395 }
2396 
2397 float computePathLength(
2398  const std::map<int, Transform> & path)
2399 {
2400  float length = 0.0f;
2401  if(path.size() > 1)
2402  {
2403  float x=0, y=0, z=0;
2404  std::map<int, Transform>::const_iterator iter=path.begin();
2405  Transform previousPose = iter->second;
2406  ++iter;
2407  for(; iter!=path.end(); ++iter)
2408  {
2409  const Transform & currentPose = iter->second;
2410  x += fabs(previousPose.x() - currentPose.x());
2411  y += fabs(previousPose.y() - currentPose.y());
2412  z += fabs(previousPose.z() - currentPose.z());
2413  previousPose = currentPose;
2414  }
2415  length = sqrt(x*x + y*y + z*z);
2416  }
2417  return length;
2418 }
2419 
2420 // return all paths linked only by neighbor links
2421 std::list<std::map<int, Transform> > getPaths(
2422  std::map<int, Transform> poses,
2423  const std::multimap<int, Link> & links)
2424 {
2425  std::list<std::map<int, Transform> > paths;
2426  if(poses.size() && links.size())
2427  {
2428  // Segment poses connected only by neighbor links
2429  while(poses.size())
2430  {
2431  std::map<int, Transform> path;
2432  for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end();)
2433  {
2434  std::multimap<int, Link>::const_iterator jter = findLink(links, path.rbegin()->first, iter->first);
2435  if(path.size() == 0 || (jter != links.end() && (jter->second.type() == Link::kNeighbor || jter->second.type() == Link::kNeighborMerged)))
2436  {
2437  path.insert(*iter);
2438  poses.erase(iter++);
2439  }
2440  else
2441  {
2442  break;
2443  }
2444  }
2445  UASSERT(path.size());
2446  paths.push_back(path);
2447  }
2448 
2449  }
2450  return paths;
2451 }
2452 
2453 void computeMinMax(const std::map<int, Transform> & poses,
2454  cv::Vec3f & min,
2455  cv::Vec3f & max)
2456 {
2457  if(!poses.empty())
2458  {
2459  min[0] = max[0] = poses.begin()->second.x();
2460  min[1] = max[1] = poses.begin()->second.y();
2461  min[2] = max[2] = poses.begin()->second.z();
2462  for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
2463  {
2464  if(min[0] > iter->second.x())
2465  min[0] = iter->second.x();
2466  if(max[0] < iter->second.x())
2467  max[0] = iter->second.x();
2468  if(min[1] > iter->second.y())
2469  min[1] = iter->second.y();
2470  if(max[1] < iter->second.y())
2471  max[1] = iter->second.y();
2472  if(min[2] > iter->second.z())
2473  min[2] = iter->second.z();
2474  if(max[2] < iter->second.z())
2475  max[2] = iter->second.z();
2476  }
2477  }
2478 }
2479 
2480 } /* namespace graph */
2481 
2482 } /* 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:1761
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:1245
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:1022
b
Array< int, 3, 1 > b
rtabmap::graph::Pair
std::pair< int, float > Pair
Definition: Graph.cpp:1772
rtabmap::graph::radiusPosesClustering
std::multimap< int, int > RTABMAP_CORE_EXPORT radiusPosesClustering(const std::map< int, Transform > &poses, float radius, float angle)
Definition: Graph.cpp:1489
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:1732
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:1767
rtabmap::graph::Node::id
int id() const
Definition: Graph.cpp:1744
GeodeticCoords.h
rtabmap::graph::Node::setCostSoFar
void setCostSoFar(float costSoFar)
Definition: Graph.cpp:1759
ULogger::kDebug
@ kDebug
Definition: ULogger.h:252
P1
Pose2 P1
type
rtabmap::graph::Node::setClosed
void setClosed(bool closed)
Definition: Graph.cpp:1757
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:1750
rtabmap::graph::Node::setFromId
void setFromId(int fromId)
Definition: Graph.cpp:1758
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:1775
indices
indices
rtabmap::graph::Order
Definition: Graph.cpp:1773
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:1328
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:2209
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::getEulerAngles
void getEulerAngles(float &roll, float &pitch, float &yaw) const
Definition: Transform.cpp:263
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:1735
rtabmap::graph::Node::distFrom
float distFrom(const rtabmap::Transform &pose) const
Definition: Graph.cpp:1752
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:2372
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:1760
rtabmap::graph::filterDuplicateLinks
std::multimap< int, Link > RTABMAP_CORE_EXPORT filterDuplicateLinks(const std::multimap< int, Link > &links)
Definition: Graph.cpp:1264
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:2421
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:1747
rtabmap::graph::computeMinMax
void RTABMAP_CORE_EXPORT computeMinMax(const std::map< int, Transform > &poses, cv::Vec3f &min, cv::Vec3f &max)
Definition: Graph.cpp:2453
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:1050
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:2228
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:1746
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:1764
ULogger::level
static ULogger::Level level()
Definition: ULogger.h:340
rtabmap::graph::Node::distToEnd_
float distToEnd_
Definition: Graph.cpp:1766
Eigen::Quaternion
ULogger.h
ULogger class and convenient macros.
a
ArrayXXi a
rtabmap::graph::Node::costSoFar_
float costSoFar_
Definition: Graph.cpp:1765
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:1745
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:2317
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:1278
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:1751
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:1353
v
Array< int, Dynamic, 1 > v
UDEBUG
#define UDEBUG(...)
UTimer
Definition: UTimer.h:46
rtabmap::Memory
Definition: Memory.h:64
P2
Pose2 P2
rtabmap::graph::Node::distToEnd
float distToEnd() const
Definition: Graph.cpp:1749
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:1768
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:1545
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:2362
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:1377
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:1769
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:1782
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:2354
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:1748
v2
v2
rtabmap::Memory::getAllLinks
std::multimap< int, Link > getAllLinks(bool lookInDatabase, bool ignoreNullLinks=true, bool withLandmarks=false) const
Definition: Memory.cpp:1412

rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Apr 28 2025 02:45:54