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

rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Jul 1 2024 02:42:28