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

rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Dec 14 2020 03:34:58