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

rtabmap
Author(s): Mathieu Labbe
autogenerated on Wed Jun 5 2019 22:41:31