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