00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027 #include "rtabmap/core/Graph.h"
00028
00029 #include <rtabmap/utilite/ULogger.h>
00030 #include <rtabmap/utilite/UStl.h>
00031 #include <rtabmap/utilite/UMath.h>
00032 #include <rtabmap/utilite/UConversion.h>
00033 #include <rtabmap/utilite/UTimer.h>
00034 #include <rtabmap/utilite/UFile.h>
00035 #include <rtabmap/core/GeodeticCoords.h>
00036 #include <rtabmap/core/Memory.h>
00037 #include <rtabmap/core/util3d_filtering.h>
00038 #include <rtabmap/core/util3d_registration.h>
00039 #include <pcl/search/kdtree.h>
00040 #include <pcl/common/eigen.h>
00041 #include <pcl/common/common.h>
00042 #include <set>
00043 #include <queue>
00044 #include <fstream>
00045
00046 #include <rtabmap/core/OptimizerTORO.h>
00047 #include <rtabmap/core/OptimizerG2O.h>
00048
00049 namespace rtabmap {
00050
00051 namespace graph {
00052
00053 bool exportPoses(
00054 const std::string & filePath,
00055 int format,
00056 const std::map<int, Transform> & poses,
00057 const std::multimap<int, Link> & constraints,
00058 const std::map<int, double> & stamps,
00059 bool g2oRobust)
00060 {
00061 UDEBUG("%s", filePath.c_str());
00062 std::string tmpPath = filePath;
00063 if(format==3)
00064 {
00065 if(UFile::getExtension(tmpPath).empty())
00066 {
00067 tmpPath+=".graph";
00068 }
00069 return OptimizerTORO::saveGraph(tmpPath, poses, constraints);
00070 }
00071 else if(format == 4)
00072 {
00073 if(UFile::getExtension(tmpPath).empty())
00074 {
00075 tmpPath+=".g2o";
00076 }
00077 return OptimizerG2O::saveGraph(tmpPath, poses, constraints, g2oRobust);
00078 }
00079 else
00080 {
00081 if(UFile::getExtension(tmpPath).empty())
00082 {
00083 tmpPath+=".txt";
00084 }
00085
00086 if(format == 1)
00087 {
00088 if(stamps.size() != poses.size())
00089 {
00090 UERROR("When exporting poses to format 1 (RGBD-SLAM), stamps and poses maps should have the same size!");
00091 return false;
00092 }
00093 }
00094
00095 FILE* fout = 0;
00096 #ifdef _MSC_VER
00097 fopen_s(&fout, tmpPath.c_str(), "w");
00098 #else
00099 fout = fopen(tmpPath.c_str(), "w");
00100 #endif
00101 if(fout)
00102 {
00103 for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
00104 {
00105 if(format == 1)
00106 {
00107
00108 Transform t( 0, 0, 1, 0,
00109 0, -1, 0, 0,
00110 1, 0, 0, 0);
00111 Transform pose = t.inverse() * iter->second;
00112 t = Transform( 0, 0, 1, 0,
00113 -1, 0, 0, 0,
00114 0,-1, 0, 0);
00115 pose = t.inverse() * pose * t;
00116
00117
00118 Eigen::Quaternionf q = pose.getQuaternionf();
00119
00120 UASSERT(uContains(stamps, iter->first));
00121 fprintf(fout, "%f %f %f %f %f %f %f %f\n",
00122 stamps.at(iter->first),
00123 pose.x(),
00124 pose.y(),
00125 pose.z(),
00126 q.x(),
00127 q.y(),
00128 q.z(),
00129 q.w());
00130 }
00131 else
00132 {
00133 Transform pose = iter->second;
00134 if(format == 2)
00135 {
00136
00137
00138 Transform t( 0, 0, 1, 0,
00139 -1, 0, 0, 0,
00140 0,-1, 0, 0);
00141 pose = t.inverse() * pose * t;
00142 }
00143
00144
00145 const float * p = (const float *)pose.data();
00146
00147 fprintf(fout, "%f", p[0]);
00148 for(int i=1; i<pose.size(); i++)
00149 {
00150 fprintf(fout, " %f", p[i]);
00151 }
00152 fprintf(fout, "\n");
00153 }
00154 }
00155 fclose(fout);
00156 return true;
00157 }
00158 }
00159 return false;
00160 }
00161
00162 bool importPoses(
00163 const std::string & filePath,
00164 int format,
00165 std::map<int, Transform> & poses,
00166 std::multimap<int, Link> * constraints,
00167 std::map<int, double> * stamps)
00168 {
00169 UDEBUG("%s format=%d", filePath.c_str(), format);
00170 if(format==3)
00171 {
00172 std::multimap<int, Link> constraintsTmp;
00173 if(OptimizerTORO::loadGraph(filePath, poses, constraintsTmp))
00174 {
00175 if(constraints)
00176 {
00177 *constraints = constraintsTmp;
00178 }
00179 return true;
00180 }
00181 return false;
00182 }
00183 else if(format == 4)
00184 {
00185 std::multimap<int, Link> constraintsTmp;
00186 UERROR("Cannot import from g2o format because it is not yet supported!");
00187 return false;
00188 }
00189 else
00190 {
00191 std::ifstream file;
00192 file.open(filePath.c_str(), std::ifstream::in);
00193 if(!file.good())
00194 {
00195 return false;
00196 }
00197 int id=1;
00198 GeodeticCoords origin;
00199 Transform originPose;
00200 while(file.good())
00201 {
00202 std::string str;
00203 std::getline(file, str);
00204
00205 if(str.size() && str.at(str.size()-1) == '\r')
00206 {
00207 str = str.substr(0, str.size()-1);
00208 }
00209
00210 if(str.empty() || str.at(0) == '#' || str.at(0) == '%')
00211 {
00212 continue;
00213 }
00214
00215 if(format == 9)
00216 {
00217 std::list<std::string> strList = uSplit(str, ',');
00218 if(strList.size() == 17)
00219 {
00220 double stamp = uStr2Double(strList.front())/1000000000.0;
00221 strList.pop_front();
00222 std::vector<std::string> v = uListToVector(strList);
00223 Transform pose(uStr2Float(v[0]), uStr2Float(v[1]), uStr2Float(v[2]),
00224 uStr2Float(v[4]), uStr2Float(v[5]), uStr2Float(v[6]), uStr2Float(v[3]));
00225 if(pose.isNull())
00226 {
00227 UWARN("Null transform read!? line parsed: \"%s\"", str.c_str());
00228 }
00229 else
00230 {
00231 if(stamps)
00232 {
00233 stamps->insert(std::make_pair(id, stamp));
00234 }
00235
00236 Transform t( 0, 0, 1, 0,
00237 0, -1, 0, 0,
00238 1, 0, 0, 0);
00239 pose = pose * t;
00240 poses.insert(std::make_pair(id, pose));
00241 }
00242 }
00243 else
00244 {
00245 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());
00246 }
00247 }
00248 else if(format == 8)
00249 {
00250 std::vector<std::string> strList = uListToVector(uSplit(str));
00251 if(strList.size() == 10)
00252 {
00253
00254
00255
00256 double stamp = uStr2Double(strList[0].insert(10, 1, '.'));
00257 double x = uStr2Double(strList[4]);
00258 double y = uStr2Double(strList[5]);
00259 double z = uStr2Double(strList[6]);
00260 double roll = uStr2Double(strList[8]);
00261 double pitch = uStr2Double(strList[7]);
00262 double yaw = -(uStr2Double(strList[9])-M_PI_2);
00263 if(stamps)
00264 {
00265 stamps->insert(std::make_pair(id, stamp));
00266 }
00267 Transform pose = Transform(x,y,z,roll,pitch,yaw);
00268 if(poses.size() == 0)
00269 {
00270 originPose = pose.inverse();
00271 }
00272 pose = originPose * pose;
00273 poses.insert(std::make_pair(id, pose));
00274 }
00275 else
00276 {
00277 UERROR("Error parsing \"%s\" with Karlsruhe format (should have 10 values)", str.c_str());
00278 }
00279 }
00280 else if(format == 7)
00281 {
00282 std::vector<std::string> strList = uListToVector(uSplit(str));
00283 if(strList.size() == 12)
00284 {
00285
00286
00287
00288
00289
00290
00291
00292
00293
00294
00295
00296
00297
00298
00299
00300 std::string millisecStr = strList[1];
00301 while(millisecStr.size() < 6)
00302 {
00303 millisecStr = "0" + millisecStr;
00304 }
00305 double stamp = uStr2Double(strList[0] + "." + millisecStr);
00306
00307
00308 double longitude = uStr2Double(strList[2]);
00309 double latitude = uStr2Double(strList[3]);
00310 double altitude = uStr2Double(strList[4]);
00311 if(poses.empty())
00312 {
00313 origin = GeodeticCoords(longitude, latitude, altitude);
00314 }
00315 cv::Point3d coordENU = GeodeticCoords(longitude, latitude, altitude).toENU_WGS84(origin);
00316 double roll = uStr2Double(strList[10]);
00317 double pitch = uStr2Double(strList[9]);
00318 double yaw = -(uStr2Double(strList[11])-M_PI_2);
00319 if(stamps)
00320 {
00321 stamps->insert(std::make_pair(id, stamp));
00322 }
00323 poses.insert(std::make_pair(id, Transform(coordENU.x,coordENU.y,coordENU.z, roll,pitch,yaw)));
00324 }
00325 else
00326 {
00327 UERROR("Error parsing \"%s\" with St Lucia format (should have 12 values, e.g., 101215_153851_Ins0.log)", str.c_str());
00328 }
00329 }
00330 else if(format == 6)
00331 {
00332 std::vector<std::string> strList = uListToVector(uSplit(str));
00333 if(strList.size() == 25)
00334 {
00335
00336
00337
00338
00339
00340
00341 double stamp = uStr2Double(strList[0]);
00342 double x = uStr2Double(strList[8]);
00343 double y = uStr2Double(strList[9]);
00344 double z = uStr2Double(strList[10]);
00345 if(stamps)
00346 {
00347 stamps->insert(std::make_pair(id, stamp));
00348 }
00349 float yaw = 0.0f;
00350 if(uContains(poses, id-1))
00351 {
00352
00353 Transform & previousPose = poses.at(id-1);
00354 yaw = atan2(y-previousPose.y(),x-previousPose.x());
00355 previousPose = Transform(previousPose.x(), previousPose.y(), yaw);
00356 }
00357 poses.insert(std::make_pair(id, Transform(x,y,z,0,0,yaw)));
00358 }
00359 else
00360 {
00361 UERROR("Error parsing \"%s\" with Malaga Urban format (should have 25 values, *_GPS.txt)", str.c_str());
00362 }
00363 }
00364 else if(format == 5)
00365 {
00366 std::vector<std::string> strList = uListToVector(uSplit(str));
00367 if(strList.size() == 3)
00368 {
00369 if( uIsNumber(uReplaceChar(strList[0], ' ', "")) &&
00370 uIsNumber(uReplaceChar(strList[1], ' ', "")) &&
00371 uIsNumber(uReplaceChar(strList[2], ' ', "")) &&
00372 (strList.size()==3 || uIsNumber(uReplaceChar(strList[3], ' ', ""))))
00373 {
00374 double stamp = uStr2Double(uReplaceChar(strList[0], ' ', ""));
00375 double x = uStr2Double(uReplaceChar(strList[1], ' ', ""));
00376 double y = uStr2Double(uReplaceChar(strList[2], ' ', ""));
00377
00378 if(stamps)
00379 {
00380 stamps->insert(std::make_pair(id, stamp));
00381 }
00382 float yaw = 0.0f;
00383 if(uContains(poses, id-1))
00384 {
00385
00386 Transform & previousPose = poses.at(id-1);
00387 yaw = atan2(y-previousPose.y(),x-previousPose.x());
00388 previousPose = Transform(previousPose.x(), previousPose.y(), yaw);
00389 }
00390 Transform pose = Transform(x,y,0,0,0,yaw);
00391 if(poses.size() == 0)
00392 {
00393 originPose = pose.inverse();
00394 }
00395 pose = originPose * pose;
00396 poses.insert(std::make_pair(id, pose));
00397 }
00398 else
00399 {
00400 UDEBUG("Not valid values detected: \"%s\"", str.c_str());
00401 }
00402 }
00403 else
00404 {
00405 UERROR("Error parsing \"%s\" with NewCollege format (should have 3 values: stamp x y, found %d)", str.c_str(), (int)strList.size());
00406 }
00407 }
00408 else if(format == 1)
00409 {
00410 std::list<std::string> strList = uSplit(str);
00411 if(strList.size() == 8)
00412 {
00413 double stamp = uStr2Double(strList.front());
00414 strList.pop_front();
00415 str = uJoin(strList, " ");
00416 Transform pose = Transform::fromString(str);
00417 if(pose.isNull())
00418 {
00419 UWARN("Null transform read!? line parsed: \"%s\"", str.c_str());
00420 }
00421 else
00422 {
00423 if(stamps)
00424 {
00425 stamps->insert(std::make_pair(id, stamp));
00426 }
00427
00428
00429 Transform t( 0, 0, 1, 0,
00430 -1, 0, 0, 0,
00431 0,-1, 0, 0);
00432 pose = t * pose * t.inverse();
00433 t = Transform( 0, 0, 1, 0,
00434 0, -1, 0, 0,
00435 1, 0, 0, 0);
00436 pose = t*pose;
00437 poses.insert(std::make_pair(id, pose));
00438 }
00439 }
00440 else
00441 {
00442 UERROR("Error parsing \"%s\" with RGBD-SLAM format (should have 8 values: stamp x y z qw qx qy qz)", str.c_str());
00443 }
00444 }
00445 else
00446 {
00447 Transform pose = Transform::fromString(str);
00448 if(format == 2)
00449 {
00450
00451
00452 Transform t( 0, 0, 1, 0,
00453 -1, 0, 0, 0,
00454 0,-1, 0, 0);
00455 pose = t * pose * t.inverse();
00456 }
00457 poses.insert(std::make_pair(id, pose));
00458 }
00459 ++id;
00460 }
00461 file.close();
00462 return true;
00463 }
00464 return false;
00465 }
00466
00467 bool exportGPS(
00468 const std::string & filePath,
00469 const std::map<int, GPS> & gpsValues,
00470 unsigned int rgba)
00471 {
00472 UDEBUG("%s", filePath.c_str());
00473 std::string tmpPath = filePath;
00474
00475 std::string ext = UFile::getExtension(filePath);
00476
00477 if(ext.compare("kml")!=0 && ext.compare("txt")!=0)
00478 {
00479 UERROR("Only txt and kml formats are supported!");
00480 return false;
00481 }
00482
00483 FILE* fout = 0;
00484 #ifdef _MSC_VER
00485 fopen_s(&fout, tmpPath.c_str(), "w");
00486 #else
00487 fout = fopen(tmpPath.c_str(), "w");
00488 #endif
00489 if(fout)
00490 {
00491 if(ext.compare("kml")==0)
00492 {
00493 std::string values;
00494 for(std::map<int, GPS>::const_iterator iter=gpsValues.begin(); iter!=gpsValues.end(); ++iter)
00495 {
00496 values += uFormat("%f,%f,%f ", iter->second.longitude(), iter->second.latitude(), iter->second.altitude());
00497 }
00498
00499
00500 unsigned int abgr = 0xFF << 24 | (rgba & 0xFF) << 16 | (rgba & 0xFF00) | ((rgba >> 16) &0xFF);
00501
00502 std::string colorHexa = uFormat("%08x", abgr);
00503
00504 fprintf(fout, "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n");
00505 fprintf(fout, "<kml xmlns=\"http://www.opengis.net/kml/2.2\">\n");
00506 fprintf(fout, "<Document>\n"
00507 " <name>%s</name>\n", tmpPath.c_str());
00508 fprintf(fout, " <StyleMap id=\"msn_ylw-pushpin\">\n"
00509 " <Pair>\n"
00510 " <key>normal</key>\n"
00511 " <styleUrl>#sn_ylw-pushpin</styleUrl>\n"
00512 " </Pair>\n"
00513 " <Pair>\n"
00514 " <key>highlight</key>\n"
00515 " <styleUrl>#sh_ylw-pushpin</styleUrl>\n"
00516 " </Pair>\n"
00517 " </StyleMap>\n"
00518 " <Style id=\"sh_ylw-pushpin\">\n"
00519 " <IconStyle>\n"
00520 " <scale>1.2</scale>\n"
00521 " </IconStyle>\n"
00522 " <LineStyle>\n"
00523 " <color>%s</color>\n"
00524 " </LineStyle>\n"
00525 " </Style>\n"
00526 " <Style id=\"sn_ylw-pushpin\">\n"
00527 " <LineStyle>\n"
00528 " <color>%s</color>\n"
00529 " </LineStyle>\n"
00530 " </Style>\n", colorHexa.c_str(), colorHexa.c_str());
00531 fprintf(fout, " <Placemark>\n"
00532 " <name>%s</name>\n"
00533 " <styleUrl>#msn_ylw-pushpin</styleUrl>"
00534 " <LineString>\n"
00535 " <coordinates>\n"
00536 " %s\n"
00537 " </coordinates>\n"
00538 " </LineString>\n"
00539 " </Placemark>\n"
00540 "</Document>\n"
00541 "</kml>\n",
00542 uSplit(tmpPath, '.').front().c_str(),
00543 values.c_str());
00544 }
00545 else
00546 {
00547 fprintf(fout, "# stamp longitude latitude altitude error bearing\n");
00548 for(std::map<int, GPS>::const_iterator iter=gpsValues.begin(); iter!=gpsValues.end(); ++iter)
00549 {
00550 fprintf(fout, "%f %f %f %f %f %f\n",
00551 iter->second.stamp(),
00552 iter->second.longitude(),
00553 iter->second.latitude(),
00554 iter->second.altitude(),
00555 iter->second.error(),
00556 iter->second.bearing());
00557 }
00558 }
00559
00560 fclose(fout);
00561 return true;
00562 }
00563 return false;
00564 }
00565
00566
00567 float lengths[] = {100,200,300,400,500,600,700,800};
00568 int32_t num_lengths = 8;
00569
00570 struct errors {
00571 int32_t first_frame;
00572 float r_err;
00573 float t_err;
00574 float len;
00575 float speed;
00576 errors (int32_t first_frame,float r_err,float t_err,float len,float speed) :
00577 first_frame(first_frame),r_err(r_err),t_err(t_err),len(len),speed(speed) {}
00578 };
00579
00580 std::vector<float> trajectoryDistances (const std::vector<Transform> &poses) {
00581 std::vector<float> dist;
00582 dist.push_back(0);
00583 for (unsigned int i=1; i<poses.size(); i++) {
00584 Transform P1 = poses[i-1];
00585 Transform P2 = poses[i];
00586 float dx = P1.x()-P2.x();
00587 float dy = P1.y()-P2.y();
00588 float dz = P1.z()-P2.z();
00589 dist.push_back(dist[i-1]+sqrt(dx*dx+dy*dy+dz*dz));
00590 }
00591 return dist;
00592 }
00593
00594 int32_t lastFrameFromSegmentLength(std::vector<float> &dist,int32_t first_frame,float len) {
00595 for (unsigned int i=first_frame; i<dist.size(); i++)
00596 if (dist[i]>dist[first_frame]+len)
00597 return i;
00598 return -1;
00599 }
00600
00601 inline float rotationError(const Transform &pose_error) {
00602 float a = pose_error(0,0);
00603 float b = pose_error(1,1);
00604 float c = pose_error(2,2);
00605 float d = 0.5*(a+b+c-1.0);
00606 return std::acos(std::max(std::min(d,1.0f),-1.0f));
00607 }
00608
00609 inline float translationError(const Transform &pose_error) {
00610 float dx = pose_error.x();
00611 float dy = pose_error.y();
00612 float dz = pose_error.z();
00613 return sqrt(dx*dx+dy*dy+dz*dz);
00614 }
00615
00616 void calcKittiSequenceErrors (
00617 const std::vector<Transform> &poses_gt,
00618 const std::vector<Transform> &poses_result,
00619 float & t_err,
00620 float & r_err) {
00621
00622 UASSERT(poses_gt.size() == poses_result.size());
00623
00624
00625 std::vector<errors> err;
00626
00627
00628 int32_t step_size = 10;
00629
00630
00631 std::vector<float> dist = trajectoryDistances(poses_gt);
00632
00633
00634 for (unsigned int first_frame=0; first_frame<poses_gt.size(); first_frame+=step_size) {
00635
00636
00637 for (int32_t i=0; i<num_lengths; i++) {
00638
00639
00640 float len = lengths[i];
00641
00642
00643 int32_t last_frame = lastFrameFromSegmentLength(dist,first_frame,len);
00644
00645
00646 if (last_frame==-1)
00647 continue;
00648
00649
00650 Transform pose_delta_gt = poses_gt[first_frame].inverse()*poses_gt[last_frame];
00651 Transform pose_delta_result = poses_result[first_frame].inverse()*poses_result[last_frame];
00652 Transform pose_error = pose_delta_result.inverse()*pose_delta_gt;
00653 float r_err = rotationError(pose_error);
00654 float t_err = translationError(pose_error);
00655
00656
00657 float num_frames = (float)(last_frame-first_frame+1);
00658 float speed = len/(0.1*num_frames);
00659
00660
00661 err.push_back(errors(first_frame,r_err/len,t_err/len,len,speed));
00662 }
00663 }
00664
00665 t_err = 0;
00666 r_err = 0;
00667
00668
00669 for (std::vector<errors>::iterator it=err.begin(); it!=err.end(); it++)
00670 {
00671 t_err += it->t_err;
00672 r_err += it->r_err;
00673 }
00674
00675
00676 float num = err.size();
00677 t_err /= num;
00678 r_err /= num;
00679 t_err *= 100.0f;
00680 r_err *= 180/CV_PI;
00681 }
00682
00683
00684 Transform calcRMSE (
00685 const std::map<int, Transform> & groundTruth,
00686 const std::map<int, Transform> & poses,
00687 float & translational_rmse,
00688 float & translational_mean,
00689 float & translational_median,
00690 float & translational_std,
00691 float & translational_min,
00692 float & translational_max,
00693 float & rotational_rmse,
00694 float & rotational_mean,
00695 float & rotational_median,
00696 float & rotational_std,
00697 float & rotational_min,
00698 float & rotational_max)
00699 {
00700
00701 translational_rmse = 0.0f;
00702 translational_mean = 0.0f;
00703 translational_median = 0.0f;
00704 translational_std = 0.0f;
00705 translational_min = 0.0f;
00706 translational_max = 0.0f;
00707
00708 rotational_rmse = 0.0f;
00709 rotational_mean = 0.0f;
00710 rotational_median = 0.0f;
00711 rotational_std = 0.0f;
00712 rotational_min = 0.0f;
00713 rotational_max = 0.0f;
00714
00715
00716 pcl::PointCloud<pcl::PointXYZ> cloud1, cloud2;
00717 cloud1.resize(poses.size());
00718 cloud2.resize(poses.size());
00719 int oi = 0;
00720 int idFirst = 0;
00721 for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
00722 {
00723 std::map<int, Transform>::const_iterator jter=groundTruth.find(iter->first);
00724 if(jter != groundTruth.end())
00725 {
00726 if(oi==0)
00727 {
00728 idFirst = iter->first;
00729 }
00730 cloud1[oi] = pcl::PointXYZ(jter->second.x(), jter->second.y(), jter->second.z());
00731 cloud2[oi++] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
00732 }
00733 }
00734
00735 Transform t = Transform::getIdentity();
00736 if(oi>5)
00737 {
00738 cloud1.resize(oi);
00739 cloud2.resize(oi);
00740
00741 t = util3d::transformFromXYZCorrespondencesSVD(cloud2, cloud1);
00742 }
00743 else if(idFirst)
00744 {
00745 t = groundTruth.at(idFirst) * poses.at(idFirst).inverse();
00746 }
00747
00748 std::vector<float> translationalErrors(poses.size());
00749 std::vector<float> rotationalErrors(poses.size());
00750 float sumTranslationalErrors = 0.0f;
00751 float sumRotationalErrors = 0.0f;
00752 float sumSqrdTranslationalErrors = 0.0f;
00753 float sumSqrdRotationalErrors = 0.0f;
00754 float radToDegree = 180.0f / M_PI;
00755 oi=0;
00756 for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
00757 {
00758 std::map<int, Transform>::const_iterator jter = groundTruth.find(iter->first);
00759 if(jter!=groundTruth.end())
00760 {
00761 Transform pose = t * iter->second;
00762 Eigen::Vector3f xAxis(1,0,0);
00763 Eigen::Vector3f vA = pose.toEigen3f().linear()*xAxis;
00764 Eigen::Vector3f vB = jter->second.toEigen3f().linear()*xAxis;
00765 double a = pcl::getAngle3D(Eigen::Vector4f(vA[0], vA[1], vA[2], 0), Eigen::Vector4f(vB[0], vB[1], vB[2], 0));
00766 rotationalErrors[oi] = a*radToDegree;
00767 translationalErrors[oi] = pose.getDistance(jter->second);
00768
00769 sumTranslationalErrors+=translationalErrors[oi];
00770 sumSqrdTranslationalErrors+=translationalErrors[oi]*translationalErrors[oi];
00771 sumRotationalErrors+=rotationalErrors[oi];
00772 sumSqrdRotationalErrors+=rotationalErrors[oi]*rotationalErrors[oi];
00773
00774 if(oi == 0)
00775 {
00776 translational_min = translational_max = translationalErrors[oi];
00777 rotational_min = rotational_max = rotationalErrors[oi];
00778 }
00779 else
00780 {
00781 if(translationalErrors[oi] < translational_min)
00782 {
00783 translational_min = translationalErrors[oi];
00784 }
00785 else if(translationalErrors[oi] > translational_max)
00786 {
00787 translational_max = translationalErrors[oi];
00788 }
00789
00790 if(rotationalErrors[oi] < rotational_min)
00791 {
00792 rotational_min = rotationalErrors[oi];
00793 }
00794 else if(rotationalErrors[oi] > rotational_max)
00795 {
00796 rotational_max = rotationalErrors[oi];
00797 }
00798 }
00799
00800 ++oi;
00801 }
00802 }
00803 translationalErrors.resize(oi);
00804 rotationalErrors.resize(oi);
00805 if(oi)
00806 {
00807 float total = float(oi);
00808 translational_rmse = std::sqrt(sumSqrdTranslationalErrors/total);
00809 translational_mean = sumTranslationalErrors/total;
00810 translational_median = translationalErrors[oi/2];
00811 translational_std = std::sqrt(uVariance(translationalErrors, translational_mean));
00812
00813 rotational_rmse = std::sqrt(sumSqrdRotationalErrors/total);
00814 rotational_mean = sumRotationalErrors/total;
00815 rotational_median = rotationalErrors[oi/2];
00816 rotational_std = std::sqrt(uVariance(rotationalErrors, rotational_mean));
00817 }
00818 return t;
00819 }
00820
00821
00823
00825 std::multimap<int, Link>::iterator findLink(
00826 std::multimap<int, Link> & links,
00827 int from,
00828 int to,
00829 bool checkBothWays)
00830 {
00831 std::multimap<int, Link>::iterator iter = links.find(from);
00832 while(iter != links.end() && iter->first == from)
00833 {
00834 if(iter->second.to() == to)
00835 {
00836 return iter;
00837 }
00838 ++iter;
00839 }
00840
00841 if(checkBothWays)
00842 {
00843
00844 iter = links.find(to);
00845 while(iter != links.end() && iter->first == to)
00846 {
00847 if(iter->second.to() == from)
00848 {
00849 return iter;
00850 }
00851 ++iter;
00852 }
00853 }
00854 return links.end();
00855 }
00856
00857 std::multimap<int, int>::iterator findLink(
00858 std::multimap<int, int> & links,
00859 int from,
00860 int to,
00861 bool checkBothWays)
00862 {
00863 std::multimap<int, int>::iterator iter = links.find(from);
00864 while(iter != links.end() && iter->first == from)
00865 {
00866 if(iter->second == to)
00867 {
00868 return iter;
00869 }
00870 ++iter;
00871 }
00872
00873 if(checkBothWays)
00874 {
00875
00876 iter = links.find(to);
00877 while(iter != links.end() && iter->first == to)
00878 {
00879 if(iter->second == from)
00880 {
00881 return iter;
00882 }
00883 ++iter;
00884 }
00885 }
00886 return links.end();
00887 }
00888 std::multimap<int, Link>::const_iterator findLink(
00889 const std::multimap<int, Link> & links,
00890 int from,
00891 int to,
00892 bool checkBothWays)
00893 {
00894 std::multimap<int, Link>::const_iterator iter = links.find(from);
00895 while(iter != links.end() && iter->first == from)
00896 {
00897 if(iter->second.to() == to)
00898 {
00899 return iter;
00900 }
00901 ++iter;
00902 }
00903
00904 if(checkBothWays)
00905 {
00906
00907 iter = links.find(to);
00908 while(iter != links.end() && iter->first == to)
00909 {
00910 if(iter->second.to() == from)
00911 {
00912 return iter;
00913 }
00914 ++iter;
00915 }
00916 }
00917 return links.end();
00918 }
00919
00920 std::multimap<int, int>::const_iterator findLink(
00921 const std::multimap<int, int> & links,
00922 int from,
00923 int to,
00924 bool checkBothWays)
00925 {
00926 std::multimap<int, int>::const_iterator iter = links.find(from);
00927 while(iter != links.end() && iter->first == from)
00928 {
00929 if(iter->second == to)
00930 {
00931 return iter;
00932 }
00933 ++iter;
00934 }
00935
00936 if(checkBothWays)
00937 {
00938
00939 iter = links.find(to);
00940 while(iter != links.end() && iter->first == to)
00941 {
00942 if(iter->second == from)
00943 {
00944 return iter;
00945 }
00946 ++iter;
00947 }
00948 }
00949 return links.end();
00950 }
00951
00952 std::multimap<int, Link> filterDuplicateLinks(
00953 const std::multimap<int, Link> & links)
00954 {
00955 std::multimap<int, Link> output;
00956 for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
00957 {
00958 if(graph::findLink(output, iter->second.from(), iter->second.to(), true) == output.end())
00959 {
00960 output.insert(*iter);
00961 }
00962 }
00963 return output;
00964 }
00965
00966 std::multimap<int, Link> filterLinks(
00967 const std::multimap<int, Link> & links,
00968 Link::Type filteredType)
00969 {
00970 std::multimap<int, Link> output;
00971 for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
00972 {
00973 if(iter->second.type() != filteredType)
00974 {
00975 output.insert(*iter);
00976 }
00977 }
00978 return output;
00979 }
00980
00981 std::map<int, Link> filterLinks(
00982 const std::map<int, Link> & links,
00983 Link::Type filteredType)
00984 {
00985 std::map<int, Link> output;
00986 for(std::map<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
00987 {
00988 if(iter->second.type() != filteredType)
00989 {
00990 output.insert(*iter);
00991 }
00992 }
00993 return output;
00994 }
00995
00996 std::map<int, Transform> frustumPosesFiltering(
00997 const std::map<int, Transform> & poses,
00998 const Transform & cameraPose,
00999 float horizontalFOV,
01000 float verticalFOV,
01001 float nearClipPlaneDistance,
01002 float farClipPlaneDistance,
01003 bool negative)
01004 {
01005 std::map<int, Transform> output;
01006
01007 if(poses.size())
01008 {
01009 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
01010 std::vector<int> ids(poses.size());
01011
01012 cloud->resize(poses.size());
01013 ids.resize(poses.size());
01014 int oi=0;
01015 for(std::map<int, rtabmap::Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
01016 {
01017 if(!iter->second.isNull())
01018 {
01019 (*cloud)[oi] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
01020 ids[oi++] = iter->first;
01021 }
01022 }
01023 cloud->resize(oi);
01024 ids.resize(oi);
01025
01026 pcl::IndicesPtr indices = util3d::frustumFiltering(
01027 cloud,
01028 pcl::IndicesPtr(new std::vector<int>),
01029 cameraPose,
01030 horizontalFOV,
01031 verticalFOV,
01032 nearClipPlaneDistance,
01033 farClipPlaneDistance,
01034 negative);
01035
01036
01037 for(unsigned int i=0; i<indices->size(); ++i)
01038 {
01039 output.insert(*poses.find(ids[indices->at(i)]));
01040 }
01041 }
01042 return output;
01043 }
01044
01045 std::map<int, Transform> radiusPosesFiltering(
01046 const std::map<int, Transform> & poses,
01047 float radius,
01048 float angle,
01049 bool keepLatest)
01050 {
01051 if(poses.size() > 2 && radius > 0.0f)
01052 {
01053 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
01054 cloud->resize(poses.size());
01055 int i=0;
01056 for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter, ++i)
01057 {
01058 (*cloud)[i] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
01059 UASSERT_MSG(pcl::isFinite((*cloud)[i]), uFormat("Invalid pose (%d) %s", iter->first, iter->second.prettyPrint().c_str()).c_str());
01060 }
01061
01062
01063 std::vector<int> names = uKeys(poses);
01064 std::vector<Transform> transforms = uValues(poses);
01065
01066 pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> (false));
01067 tree->setInputCloud(cloud);
01068 std::set<int> indicesChecked;
01069 std::set<int> indicesKept;
01070
01071 for(unsigned int i=0; i<cloud->size(); ++i)
01072 {
01073 if(indicesChecked.find(i) == indicesChecked.end())
01074 {
01075 std::vector<int> kIndices;
01076 std::vector<float> kDistances;
01077 tree->radiusSearch(cloud->at(i), radius, kIndices, kDistances);
01078
01079 std::set<int> cloudIndices;
01080 const Transform & currentT = transforms.at(i);
01081 Eigen::Vector3f vA = currentT.toEigen3f().linear()*Eigen::Vector3f(1,0,0);
01082 for(unsigned int j=0; j<kIndices.size(); ++j)
01083 {
01084 if(indicesChecked.find(kIndices[j]) == indicesChecked.end())
01085 {
01086 if(angle > 0.0f)
01087 {
01088 const Transform & checkT = transforms.at(kIndices[j]);
01089
01090 Eigen::Vector3f vB = checkT.toEigen3f().linear()*Eigen::Vector3f(1,0,0);
01091 double a = pcl::getAngle3D(Eigen::Vector4f(vA[0], vA[1], vA[2], 0), Eigen::Vector4f(vB[0], vB[1], vB[2], 0));
01092 if(a <= angle)
01093 {
01094 cloudIndices.insert(kIndices[j]);
01095 }
01096 }
01097 else
01098 {
01099 cloudIndices.insert(kIndices[j]);
01100 }
01101 }
01102 }
01103
01104 if(keepLatest)
01105 {
01106 bool lastAdded = false;
01107 for(std::set<int>::reverse_iterator iter = cloudIndices.rbegin(); iter!=cloudIndices.rend(); ++iter)
01108 {
01109 if(!lastAdded)
01110 {
01111 indicesKept.insert(*iter);
01112 lastAdded = true;
01113 }
01114 indicesChecked.insert(*iter);
01115 }
01116 }
01117 else
01118 {
01119 bool firstAdded = false;
01120 for(std::set<int>::iterator iter = cloudIndices.begin(); iter!=cloudIndices.end(); ++iter)
01121 {
01122 if(!firstAdded)
01123 {
01124 indicesKept.insert(*iter);
01125 firstAdded = true;
01126 }
01127 indicesChecked.insert(*iter);
01128 }
01129 }
01130 }
01131 }
01132
01133
01134
01135 UINFO("Cloud filtered In = %d, Out = %d", cloud->size(), indicesKept.size());
01136
01137
01138
01139 std::map<int, Transform> keptPoses;
01140 for(std::set<int>::iterator iter = indicesKept.begin(); iter!=indicesKept.end(); ++iter)
01141 {
01142 keptPoses.insert(std::make_pair(names.at(*iter), transforms.at(*iter)));
01143 }
01144
01145
01146 keptPoses.insert(*poses.begin());
01147 keptPoses.insert(*poses.rbegin());
01148
01149 return keptPoses;
01150 }
01151 else
01152 {
01153 return poses;
01154 }
01155 }
01156
01157 std::multimap<int, int> radiusPosesClustering(const std::map<int, Transform> & poses, float radius, float angle)
01158 {
01159 std::multimap<int, int> clusters;
01160 if(poses.size() > 1 && radius > 0.0f)
01161 {
01162 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
01163 cloud->resize(poses.size());
01164 int i=0;
01165 for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter, ++i)
01166 {
01167 (*cloud)[i] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
01168 UASSERT_MSG(pcl::isFinite((*cloud)[i]), uFormat("Invalid pose (%d) %s", iter->first, iter->second.prettyPrint().c_str()).c_str());
01169 }
01170
01171
01172 std::vector<int> ids = uKeys(poses);
01173 std::vector<Transform> transforms = uValues(poses);
01174
01175 pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> (false));
01176 tree->setInputCloud(cloud);
01177
01178 for(unsigned int i=0; i<cloud->size(); ++i)
01179 {
01180 std::vector<int> kIndices;
01181 std::vector<float> kDistances;
01182 tree->radiusSearch(cloud->at(i), radius, kIndices, kDistances);
01183
01184 std::set<int> cloudIndices;
01185 const Transform & currentT = transforms.at(i);
01186 Eigen::Vector3f vA = currentT.toEigen3f().linear()*Eigen::Vector3f(1,0,0);
01187 for(unsigned int j=0; j<kIndices.size(); ++j)
01188 {
01189 if((int)i != kIndices[j])
01190 {
01191 if(angle > 0.0f)
01192 {
01193 const Transform & checkT = transforms.at(kIndices[j]);
01194
01195 Eigen::Vector3f vB = checkT.toEigen3f().linear()*Eigen::Vector3f(1,0,0);
01196 double a = pcl::getAngle3D(Eigen::Vector4f(vA[0], vA[1], vA[2], 0), Eigen::Vector4f(vB[0], vB[1], vB[2], 0));
01197 if(a <= angle)
01198 {
01199 clusters.insert(std::make_pair(ids[i], ids[kIndices[j]]));
01200 }
01201 }
01202 else
01203 {
01204 clusters.insert(std::make_pair(ids[i], ids[kIndices[j]]));
01205 }
01206 }
01207 }
01208 }
01209 }
01210 return clusters;
01211 }
01212
01213 void reduceGraph(
01214 const std::map<int, Transform> & poses,
01215 const std::multimap<int, Link> & links,
01216 std::multimap<int, int> & hyperNodes,
01217 std::multimap<int, Link> & hyperLinks)
01218 {
01219 UINFO("Input: poses=%d links=%d", (int)poses.size(), (int)links.size());
01220 UTimer timer;
01221 std::map<int, int> posesToHyperNodes;
01222 std::map<int, std::multimap<int, Link> > clusterloopClosureLinks;
01223
01224 {
01225 std::multimap<int, Link> bidirectionalLoopClosureLinks;
01226 for(std::multimap<int, Link>::const_iterator jter=links.begin(); jter!=links.end(); ++jter)
01227 {
01228 if(jter->second.type() != Link::kNeighbor &&
01229 jter->second.type() != Link::kNeighborMerged &&
01230 jter->second.userDataCompressed().empty())
01231 {
01232 if(uContains(poses, jter->second.from()) &&
01233 uContains(poses, jter->second.to()))
01234 {
01235 UASSERT_MSG(graph::findLink(links, jter->second.to(), jter->second.from(), false) == links.end(), "Input links should be unique!");
01236 bidirectionalLoopClosureLinks.insert(std::make_pair(jter->second.from(), jter->second));
01237
01238 }
01239 }
01240 }
01241
01242 UINFO("Clustering hyper nodes...");
01243
01244 for(std::map<int, Transform>::const_reverse_iterator iter=poses.rbegin(); iter!=poses.rend(); ++iter)
01245 {
01246 if(posesToHyperNodes.find(iter->first) == posesToHyperNodes.end())
01247 {
01248 int hyperNodeId = iter->first;
01249 std::list<int> loopClosures;
01250 std::set<int> loopClosuresAdded;
01251 loopClosures.push_back(iter->first);
01252 std::multimap<int, Link> clusterLinks;
01253 while(loopClosures.size())
01254 {
01255 int id = loopClosures.front();
01256 loopClosures.pop_front();
01257
01258 UASSERT(posesToHyperNodes.find(id) == posesToHyperNodes.end());
01259
01260 posesToHyperNodes.insert(std::make_pair(id, hyperNodeId));
01261 hyperNodes.insert(std::make_pair(hyperNodeId, id));
01262
01263 for(std::multimap<int, Link>::const_iterator jter=bidirectionalLoopClosureLinks.find(id); jter!=bidirectionalLoopClosureLinks.end() && jter->first==id; ++jter)
01264 {
01265 if(posesToHyperNodes.find(jter->second.to()) == posesToHyperNodes.end() &&
01266 loopClosuresAdded.find(jter->second.to()) == loopClosuresAdded.end())
01267 {
01268 loopClosures.push_back(jter->second.to());
01269 loopClosuresAdded.insert(jter->second.to());
01270 clusterLinks.insert(*jter);
01271 clusterLinks.insert(std::make_pair(jter->second.to(), jter->second.inverse()));
01272 if(jter->second.from() < jter->second.to())
01273 {
01274 UWARN("Child to Parent link? %d->%d (type=%d)",
01275 jter->second.from(),
01276 jter->second.to(),
01277 jter->second.type());
01278 }
01279 }
01280 }
01281 }
01282 UASSERT(clusterloopClosureLinks.find(hyperNodeId) == clusterloopClosureLinks.end());
01283 clusterloopClosureLinks.insert(std::make_pair(hyperNodeId, clusterLinks));
01284 UDEBUG("Created hyper node %d with %d children (%f%%)",
01285 hyperNodeId, (int)loopClosuresAdded.size(), float(posesToHyperNodes.size())/float(poses.size())*100.0f);
01286 }
01287 }
01288 UINFO("Clustering hyper nodes... done! (%f s)", timer.ticks());
01289 }
01290
01291 UINFO("Creating hyper links...");
01292 int i=0;
01293 for(std::multimap<int, Link>::const_reverse_iterator jter=links.rbegin(); jter!=links.rend(); ++jter)
01294 {
01295 if((jter->second.type() == Link::kNeighbor ||
01296 jter->second.type() == Link::kNeighborMerged ||
01297 !jter->second.userDataCompressed().empty()) &&
01298 uContains(poses, jter->second.from()) &&
01299 uContains(poses, jter->second.to()))
01300 {
01301 UASSERT_MSG(uContains(posesToHyperNodes, jter->second.from()), uFormat("%d->%d (type=%d)", jter->second.from(), jter->second.to(), jter->second.type()).c_str());
01302 UASSERT_MSG(uContains(posesToHyperNodes, jter->second.to()), uFormat("%d->%d (type=%d)", jter->second.from(), jter->second.to(), jter->second.type()).c_str());
01303 int hyperNodeIDFrom = posesToHyperNodes.at(jter->second.from());
01304 int hyperNodeIDTo = posesToHyperNodes.at(jter->second.to());
01305
01306
01307 if(hyperNodeIDFrom != hyperNodeIDTo)
01308 {
01309 std::multimap<int, Link>::iterator tmpIter = graph::findLink(hyperLinks, hyperNodeIDFrom, hyperNodeIDTo);
01310 if(tmpIter!=hyperLinks.end() &&
01311 hyperNodeIDFrom == jter->second.from() &&
01312 hyperNodeIDTo == jter->second.to() &&
01313 tmpIter->second.type() > Link::kNeighbor &&
01314 jter->second.type() == Link::kNeighbor)
01315 {
01316
01317 hyperLinks.erase(tmpIter);
01318 }
01319
01320
01321 if(graph::findLink(hyperLinks, hyperNodeIDFrom, hyperNodeIDTo) == hyperLinks.end())
01322 {
01323 UASSERT(clusterloopClosureLinks.find(hyperNodeIDFrom) != clusterloopClosureLinks.end());
01324 UASSERT(clusterloopClosureLinks.find(hyperNodeIDTo) != clusterloopClosureLinks.end());
01325 std::multimap<int, Link> tmpLinks = clusterloopClosureLinks.at(hyperNodeIDFrom);
01326 tmpLinks.insert(clusterloopClosureLinks.at(hyperNodeIDTo).begin(), clusterloopClosureLinks.at(hyperNodeIDTo).end());
01327 tmpLinks.insert(std::make_pair(jter->second.from(), jter->second));
01328
01329 std::list<int> path = computePath(tmpLinks, hyperNodeIDFrom, hyperNodeIDTo, false, true);
01330 UASSERT_MSG(path.size()>1,
01331 uFormat("path.size()=%d, hyperNodeIDFrom=%d, hyperNodeIDTo=%d",
01332 (int)path.size(),
01333 hyperNodeIDFrom,
01334 hyperNodeIDTo).c_str());
01335
01336 if(path.size() > 10)
01337 {
01338 UWARN("Large path! %d nodes", (int)path.size());
01339 std::stringstream stream;
01340 for(std::list<int>::const_iterator iter=path.begin(); iter!=path.end();++iter)
01341 {
01342 if(iter!=path.begin())
01343 {
01344 stream << ",";
01345 }
01346
01347 stream << *iter;
01348 }
01349 UWARN("Path = [%s]", stream.str().c_str());
01350 }
01351
01352
01353 std::list<int>::iterator iter=path.begin();
01354 int from = *iter;
01355 ++iter;
01356 int to = *iter;
01357 std::multimap<int, Link>::const_iterator foundIter = graph::findLink(tmpLinks, from, to, false);
01358 UASSERT(foundIter != tmpLinks.end());
01359 Link hyperLink = foundIter->second;
01360 ++iter;
01361 from = to;
01362 for(; iter!=path.end(); ++iter)
01363 {
01364 to = *iter;
01365 std::multimap<int, Link>::const_iterator foundIter = graph::findLink(tmpLinks, from, to, false);
01366 UASSERT(foundIter != tmpLinks.end());
01367 hyperLink = hyperLink.merge(foundIter->second, jter->second.type());
01368
01369 from = to;
01370 }
01371
01372 UASSERT(hyperLink.from() == hyperNodeIDFrom);
01373 UASSERT(hyperLink.to() == hyperNodeIDTo);
01374 hyperLinks.insert(std::make_pair(hyperNodeIDFrom, hyperLink));
01375
01376 UDEBUG("Created hyper link %d->%d (%f%%)",
01377 hyperLink.from(), hyperLink.to(), float(i)/float(links.size())*100.0f);
01378 if(hyperLink.transform().getNorm() > jter->second.transform().getNorm()+1)
01379 {
01380 UWARN("Large hyper link %d->%d (%f m)! original %d->%d (%f m)",
01381 hyperLink.from(),
01382 hyperLink.to(),
01383 hyperLink.transform().getNorm(),
01384 jter->second.from(),
01385 jter->second.to(),
01386 jter->second.transform().getNorm());
01387
01388 }
01389 }
01390 }
01391 }
01392 ++i;
01393 }
01394 UINFO("Creating hyper links... done! (%f s)", timer.ticks());
01395
01396 UINFO("Output: poses=%d links=%d", (int)uUniqueKeys(hyperNodes).size(), (int)links.size());
01397 }
01398
01399
01400 class Node
01401 {
01402 public:
01403 Node(int id, int fromId, const rtabmap::Transform & pose) :
01404 id_(id),
01405 costSoFar_(0.0f),
01406 distToEnd_(0.0f),
01407 fromId_(fromId),
01408 closed_(false),
01409 pose_(pose)
01410 {}
01411
01412 int id() const {return id_;}
01413 int fromId() const {return fromId_;}
01414 bool isClosed() const {return closed_;}
01415 bool isOpened() const {return !closed_;}
01416 float costSoFar() const {return costSoFar_;}
01417 float distToEnd() const {return distToEnd_;}
01418 float totalCost() const {return costSoFar_ + distToEnd_;}
01419 rtabmap::Transform pose() const {return pose_;}
01420 float distFrom(const rtabmap::Transform & pose) const
01421 {
01422 return pose_.getDistance(pose);
01423 }
01424
01425 void setClosed(bool closed) {closed_ = closed;}
01426 void setFromId(int fromId) {fromId_ = fromId;}
01427 void setCostSoFar(float costSoFar) {costSoFar_ = costSoFar;}
01428 void setDistToEnd(float distToEnd) {distToEnd_ = distToEnd;}
01429 void setPose(const Transform & pose) {pose_ = pose;}
01430
01431 private:
01432 int id_;
01433 float costSoFar_;
01434 float distToEnd_;
01435 int fromId_;
01436 bool closed_;
01437 rtabmap::Transform pose_;
01438 };
01439
01440 typedef std::pair<int, float> Pair;
01441 struct Order
01442 {
01443 bool operator()(Pair const& a, Pair const& b) const
01444 {
01445 return a.second > b.second;
01446 }
01447 };
01448
01449
01450 std::list<std::pair<int, Transform> > computePath(
01451 const std::map<int, rtabmap::Transform> & poses,
01452 const std::multimap<int, int> & links,
01453 int from,
01454 int to,
01455 bool updateNewCosts)
01456 {
01457 std::list<std::pair<int, Transform> > path;
01458
01459 int startNode = from;
01460 int endNode = to;
01461 rtabmap::Transform endPose = poses.at(endNode);
01462 std::map<int, Node> nodes;
01463 nodes.insert(std::make_pair(startNode, Node(startNode, 0, poses.at(startNode))));
01464 std::priority_queue<Pair, std::vector<Pair>, Order> pq;
01465 std::multimap<float, int> pqmap;
01466 if(updateNewCosts)
01467 {
01468 pqmap.insert(std::make_pair(0, startNode));
01469 }
01470 else
01471 {
01472 pq.push(Pair(startNode, 0));
01473 }
01474
01475 while((updateNewCosts && pqmap.size()) || (!updateNewCosts && pq.size()))
01476 {
01477 Node * currentNode;
01478 if(updateNewCosts)
01479 {
01480 currentNode = &nodes.find(pqmap.begin()->second)->second;
01481 pqmap.erase(pqmap.begin());
01482 }
01483 else
01484 {
01485 currentNode = &nodes.find(pq.top().first)->second;
01486 pq.pop();
01487 }
01488
01489 currentNode->setClosed(true);
01490
01491 if(currentNode->id() == endNode)
01492 {
01493 while(currentNode->id()!=startNode)
01494 {
01495 path.push_front(std::make_pair(currentNode->id(), currentNode->pose()));
01496 currentNode = &nodes.find(currentNode->fromId())->second;
01497 }
01498 path.push_front(std::make_pair(startNode, poses.at(startNode)));
01499 break;
01500 }
01501
01502
01503 for(std::multimap<int, int>::const_iterator iter = links.find(currentNode->id());
01504 iter!=links.end() && iter->first == currentNode->id();
01505 ++iter)
01506 {
01507 std::map<int, Node>::iterator nodeIter = nodes.find(iter->second);
01508 if(nodeIter == nodes.end())
01509 {
01510 std::map<int, rtabmap::Transform>::const_iterator poseIter = poses.find(iter->second);
01511 if(poseIter == poses.end())
01512 {
01513 UERROR("Next pose %d (from %d) should be found in poses! Ignoring it!", iter->second, iter->first);
01514 }
01515 else
01516 {
01517 Node n(iter->second, currentNode->id(), poseIter->second);
01518 n.setCostSoFar(currentNode->costSoFar() + currentNode->distFrom(poseIter->second));
01519 n.setDistToEnd(n.distFrom(endPose));
01520
01521 nodes.insert(std::make_pair(iter->second, n));
01522 if(updateNewCosts)
01523 {
01524 pqmap.insert(std::make_pair(n.totalCost(), n.id()));
01525 }
01526 else
01527 {
01528 pq.push(Pair(n.id(), n.totalCost()));
01529 }
01530 }
01531 }
01532 else if(updateNewCosts && nodeIter->second.isOpened())
01533 {
01534 float newCostSoFar = currentNode->costSoFar() + currentNode->distFrom(nodeIter->second.pose());
01535 if(nodeIter->second.costSoFar() > newCostSoFar)
01536 {
01537
01538 for(std::multimap<float, int>::iterator mapIter=pqmap.begin(); mapIter!=pqmap.end(); ++mapIter)
01539 {
01540 if(mapIter->second == nodeIter->first)
01541 {
01542 pqmap.erase(mapIter);
01543 nodeIter->second.setCostSoFar(newCostSoFar);
01544 pqmap.insert(std::make_pair(nodeIter->second.totalCost(), nodeIter->first));
01545 break;
01546 }
01547 }
01548 }
01549 }
01550 }
01551 }
01552 return path;
01553 }
01554
01555
01556 std::list<int> RTABMAP_EXP computePath(
01557 const std::multimap<int, Link> & links,
01558 int from,
01559 int to,
01560 bool updateNewCosts,
01561 bool useSameCostForAllLinks)
01562 {
01563 std::list<int> path;
01564
01565 int startNode = from;
01566 int endNode = to;
01567 std::map<int, Node> nodes;
01568 nodes.insert(std::make_pair(startNode, Node(startNode, 0, Transform())));
01569 std::priority_queue<Pair, std::vector<Pair>, Order> pq;
01570 std::multimap<float, int> pqmap;
01571 if(updateNewCosts)
01572 {
01573 pqmap.insert(std::make_pair(0, startNode));
01574 }
01575 else
01576 {
01577 pq.push(Pair(startNode, 0));
01578 }
01579
01580 while((updateNewCosts && pqmap.size()) || (!updateNewCosts && pq.size()))
01581 {
01582 Node * currentNode;
01583 if(updateNewCosts)
01584 {
01585 currentNode = &nodes.find(pqmap.begin()->second)->second;
01586 pqmap.erase(pqmap.begin());
01587 }
01588 else
01589 {
01590 currentNode = &nodes.find(pq.top().first)->second;
01591 pq.pop();
01592 }
01593
01594 currentNode->setClosed(true);
01595
01596 if(currentNode->id() == endNode)
01597 {
01598 while(currentNode->id()!=startNode)
01599 {
01600 path.push_front(currentNode->id());
01601 currentNode = &nodes.find(currentNode->fromId())->second;
01602 }
01603 path.push_front(startNode);
01604 break;
01605 }
01606
01607
01608 for(std::multimap<int, Link>::const_iterator iter = links.find(currentNode->id());
01609 iter!=links.end() && iter->first == currentNode->id();
01610 ++iter)
01611 {
01612 std::map<int, Node>::iterator nodeIter = nodes.find(iter->second.to());
01613 float cost = 1;
01614 if(!useSameCostForAllLinks)
01615 {
01616 cost = iter->second.transform().getNorm();
01617 }
01618 if(nodeIter == nodes.end())
01619 {
01620 Node n(iter->second.to(), currentNode->id(), Transform());
01621
01622 n.setCostSoFar(currentNode->costSoFar() + cost);
01623 nodes.insert(std::make_pair(iter->second.to(), n));
01624 if(updateNewCosts)
01625 {
01626 pqmap.insert(std::make_pair(n.totalCost(), n.id()));
01627 }
01628 else
01629 {
01630 pq.push(Pair(n.id(), n.totalCost()));
01631 }
01632 }
01633 else if(!useSameCostForAllLinks && updateNewCosts && nodeIter->second.isOpened())
01634 {
01635 float newCostSoFar = currentNode->costSoFar() + cost;
01636 if(nodeIter->second.costSoFar() > newCostSoFar)
01637 {
01638
01639 for(std::multimap<float, int>::iterator mapIter=pqmap.begin(); mapIter!=pqmap.end(); ++mapIter)
01640 {
01641 if(mapIter->second == nodeIter->first)
01642 {
01643 pqmap.erase(mapIter);
01644 nodeIter->second.setCostSoFar(newCostSoFar);
01645 pqmap.insert(std::make_pair(nodeIter->second.totalCost(), nodeIter->first));
01646 break;
01647 }
01648 }
01649 }
01650 }
01651 }
01652 }
01653 return path;
01654 }
01655
01656
01657
01658 std::list<std::pair<int, Transform> > computePath(
01659 int fromId,
01660 int toId,
01661 const Memory * memory,
01662 bool lookInDatabase,
01663 bool updateNewCosts,
01664 float linearVelocity,
01665 float angularVelocity)
01666 {
01667 UASSERT(memory!=0);
01668 UASSERT(fromId>=0);
01669 UASSERT(toId>=0);
01670 std::list<std::pair<int, Transform> > path;
01671 UDEBUG("fromId=%d, toId=%d, lookInDatabase=%d, updateNewCosts=%d, linearVelocity=%f, angularVelocity=%f",
01672 fromId,
01673 toId,
01674 lookInDatabase?1:0,
01675 updateNewCosts?1:0,
01676 linearVelocity,
01677 angularVelocity);
01678
01679 std::multimap<int, Link> allLinks;
01680 if(lookInDatabase)
01681 {
01682
01683 UTimer t;
01684 allLinks = memory->getAllLinks(lookInDatabase);
01685 UINFO("getting all %d links time = %f s", (int)allLinks.size(), t.ticks());
01686 }
01687
01688
01689 int startNode = fromId;
01690 int endNode = toId;
01691 std::map<int, Node> nodes;
01692 nodes.insert(std::make_pair(startNode, Node(startNode, 0, Transform::getIdentity())));
01693 std::priority_queue<Pair, std::vector<Pair>, Order> pq;
01694 std::multimap<float, int> pqmap;
01695 if(updateNewCosts)
01696 {
01697 pqmap.insert(std::make_pair(0, startNode));
01698 }
01699 else
01700 {
01701 pq.push(Pair(startNode, 0));
01702 }
01703
01704 while((updateNewCosts && pqmap.size()) || (!updateNewCosts && pq.size()))
01705 {
01706 Node * currentNode;
01707 if(updateNewCosts)
01708 {
01709 currentNode = &nodes.find(pqmap.begin()->second)->second;
01710 pqmap.erase(pqmap.begin());
01711 }
01712 else
01713 {
01714 currentNode = &nodes.find(pq.top().first)->second;
01715 pq.pop();
01716 }
01717
01718 currentNode->setClosed(true);
01719
01720 if(currentNode->id() == endNode)
01721 {
01722 while(currentNode->id()!=startNode)
01723 {
01724 path.push_front(std::make_pair(currentNode->id(), currentNode->pose()));
01725 currentNode = &nodes.find(currentNode->fromId())->second;
01726 }
01727 path.push_front(std::make_pair(startNode, currentNode->pose()));
01728 break;
01729 }
01730
01731
01732 std::map<int, Link> links;
01733 if(allLinks.size() == 0)
01734 {
01735 links = memory->getLinks(currentNode->id(), lookInDatabase);
01736 }
01737 else
01738 {
01739 for(std::multimap<int, Link>::const_iterator iter = allLinks.lower_bound(currentNode->id());
01740 iter!=allLinks.end() && iter->first == currentNode->id();
01741 ++iter)
01742 {
01743 links.insert(std::make_pair(iter->second.to(), iter->second));
01744 }
01745 }
01746 for(std::map<int, Link>::const_iterator iter = links.begin(); iter!=links.end(); ++iter)
01747 {
01748 if(iter->second.from() != iter->second.to())
01749 {
01750 Transform nextPose = currentNode->pose()*iter->second.transform();
01751 float cost = 0.0f;
01752 if(linearVelocity <= 0.0f && angularVelocity <= 0.0f)
01753 {
01754
01755 cost = iter->second.transform().getNorm();
01756 }
01757 else
01758 {
01759 if(linearVelocity > 0.0f)
01760 {
01761 cost += iter->second.transform().getNorm()/linearVelocity;
01762 }
01763 if(angularVelocity > 0.0f)
01764 {
01765 Eigen::Vector4f v1 = Eigen::Vector4f(nextPose.x()-currentNode->pose().x(), nextPose.y()-currentNode->pose().y(), nextPose.z()-currentNode->pose().z(), 1.0f);
01766 Eigen::Vector4f v2 = nextPose.rotation().toEigen4f()*Eigen::Vector4f(1,0,0,1);
01767 float angle = pcl::getAngle3D(v1, v2);
01768 cost += angle / angularVelocity;
01769 }
01770 }
01771
01772 std::map<int, Node>::iterator nodeIter = nodes.find(iter->first);
01773 if(nodeIter == nodes.end())
01774 {
01775 Node n(iter->second.to(), currentNode->id(), nextPose);
01776
01777 n.setCostSoFar(currentNode->costSoFar() + cost);
01778 nodes.insert(std::make_pair(iter->second.to(), n));
01779 if(updateNewCosts)
01780 {
01781 pqmap.insert(std::make_pair(n.totalCost(), n.id()));
01782 }
01783 else
01784 {
01785 pq.push(Pair(n.id(), n.totalCost()));
01786 }
01787 }
01788 else if(updateNewCosts && nodeIter->second.isOpened())
01789 {
01790 float newCostSoFar = currentNode->costSoFar() + cost;
01791 if(nodeIter->second.costSoFar() > newCostSoFar)
01792 {
01793
01794 nodeIter->second.setPose(nextPose);
01795
01796
01797 for(std::multimap<float, int>::iterator mapIter=pqmap.begin(); mapIter!=pqmap.end(); ++mapIter)
01798 {
01799 if(mapIter->second == nodeIter->first)
01800 {
01801 pqmap.erase(mapIter);
01802 nodeIter->second.setCostSoFar(newCostSoFar);
01803 pqmap.insert(std::make_pair(nodeIter->second.totalCost(), nodeIter->first));
01804 break;
01805 }
01806 }
01807 }
01808 }
01809 }
01810 }
01811 }
01812
01813
01814 if(ULogger::level() == ULogger::kDebug)
01815 {
01816 std::stringstream stream;
01817 std::vector<int> linkTypes(Link::kUndef, 0);
01818 std::list<std::pair<int, Transform> >::const_iterator previousIter = path.end();
01819 float length = 0.0f;
01820 for(std::list<std::pair<int, Transform> >::const_iterator iter=path.begin(); iter!=path.end();++iter)
01821 {
01822 if(iter!=path.begin())
01823 {
01824 stream << ",";
01825 }
01826
01827 if(previousIter!=path.end())
01828 {
01829
01830 if(allLinks.size())
01831 {
01832 std::multimap<int, Link>::iterator jter = graph::findLink(allLinks, previousIter->first, iter->first);
01833 if(jter != allLinks.end())
01834 {
01835
01836
01837
01838
01839
01840
01841
01842 UASSERT(jter->second.type() >= Link::kNeighbor && jter->second.type()<Link::kUndef);
01843 ++linkTypes[jter->second.type()];
01844 stream << "[" << jter->second.type() << "]";
01845 length += jter->second.transform().getNorm();
01846 }
01847 }
01848 }
01849
01850 stream << iter->first;
01851
01852 previousIter=iter;
01853 }
01854 UDEBUG("Path (%f m) = [%s]", length, stream.str().c_str());
01855 std::stringstream streamB;
01856 for(unsigned int i=0; i<linkTypes.size(); ++i)
01857 {
01858 if(i > 0)
01859 {
01860 streamB << " ";
01861 }
01862 streamB << i << "=" << linkTypes[i];
01863 }
01864 UDEBUG("Link types = %s", streamB.str().c_str());
01865 }
01866
01867 return path;
01868 }
01869
01870 int findNearestNode(
01871 const std::map<int, rtabmap::Transform> & nodes,
01872 const rtabmap::Transform & targetPose)
01873 {
01874 int id = 0;
01875 std::vector<int> nearestNodes = findNearestNodes(nodes, targetPose, 1);
01876 if(nearestNodes.size())
01877 {
01878 id = nearestNodes[0];
01879 }
01880 return id;
01881 }
01882
01883 std::vector<int> findNearestNodes(
01884 const std::map<int, rtabmap::Transform> & nodes,
01885 const rtabmap::Transform & targetPose,
01886 int k)
01887 {
01888 std::vector<int> nearestIds;
01889 if(nodes.size() && !targetPose.isNull())
01890 {
01891 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
01892 cloud->resize(nodes.size());
01893 std::vector<int> ids(nodes.size());
01894 int oi = 0;
01895 for(std::map<int, Transform>::const_iterator iter = nodes.begin(); iter!=nodes.end(); ++iter)
01896 {
01897 (*cloud)[oi] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
01898 ids[oi++] = iter->first;
01899 }
01900
01901 pcl::search::KdTree<pcl::PointXYZ>::Ptr kdTree(new pcl::search::KdTree<pcl::PointXYZ>);
01902 kdTree->setInputCloud(cloud);
01903 std::vector<int> ind;
01904 std::vector<float> dist;
01905 pcl::PointXYZ pt(targetPose.x(), targetPose.y(), targetPose.z());
01906 kdTree->nearestKSearch(pt, k, ind, dist);
01907
01908 nearestIds.resize(ind.size());
01909 for(unsigned int i=0; i<ind.size(); ++i)
01910 {
01911 nearestIds[i] = ids[ind[i]];
01912 }
01913 }
01914 return nearestIds;
01915 }
01916
01917
01918 std::map<int, float> getNodesInRadius(
01919 int nodeId,
01920 const std::map<int, Transform> & nodes,
01921 float radius)
01922 {
01923 UASSERT(uContains(nodes, nodeId));
01924 std::map<int, float> foundNodes;
01925 if(nodes.size() <= 1)
01926 {
01927 return foundNodes;
01928 }
01929
01930 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
01931 cloud->resize(nodes.size());
01932 std::vector<int> ids(nodes.size());
01933 int oi = 0;
01934 for(std::map<int, Transform>::const_iterator iter = nodes.begin(); iter!=nodes.end(); ++iter)
01935 {
01936 if(iter->first != nodeId)
01937 {
01938 (*cloud)[oi] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
01939 UASSERT_MSG(pcl::isFinite((*cloud)[oi]), uFormat("Invalid pose (%d) %s", iter->first, iter->second.prettyPrint().c_str()).c_str());
01940 ids[oi] = iter->first;
01941 ++oi;
01942 }
01943 }
01944 cloud->resize(oi);
01945 ids.resize(oi);
01946
01947 Transform fromT = nodes.at(nodeId);
01948
01949 if(cloud->size())
01950 {
01951 pcl::search::KdTree<pcl::PointXYZ>::Ptr kdTree(new pcl::search::KdTree<pcl::PointXYZ>);
01952 kdTree->setInputCloud(cloud);
01953 std::vector<int> ind;
01954 std::vector<float> sqrdDist;
01955 pcl::PointXYZ pt(fromT.x(), fromT.y(), fromT.z());
01956 kdTree->radiusSearch(pt, radius, ind, sqrdDist, 0);
01957 for(unsigned int i=0; i<ind.size(); ++i)
01958 {
01959 if(ind[i] >=0)
01960 {
01961 foundNodes.insert(std::make_pair(ids[ind[i]], sqrdDist[i]));
01962 }
01963 }
01964 }
01965 UDEBUG("found nodes=%d", (int)foundNodes.size());
01966 return foundNodes;
01967 }
01968
01969
01970 std::map<int, Transform> getPosesInRadius(
01971 int nodeId,
01972 const std::map<int, Transform> & nodes,
01973 float radius,
01974 float angle)
01975 {
01976 UASSERT(uContains(nodes, nodeId));
01977 std::map<int, Transform> foundNodes;
01978 if(nodes.size() <= 1)
01979 {
01980 return foundNodes;
01981 }
01982
01983 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
01984 cloud->resize(nodes.size());
01985 std::vector<int> ids(nodes.size());
01986 int oi = 0;
01987 for(std::map<int, Transform>::const_iterator iter = nodes.begin(); iter!=nodes.end(); ++iter)
01988 {
01989 if(iter->first != nodeId)
01990 {
01991 (*cloud)[oi] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
01992 UASSERT_MSG(pcl::isFinite((*cloud)[oi]), uFormat("Invalid pose (%d) %s", iter->first, iter->second.prettyPrint().c_str()).c_str());
01993 ids[oi] = iter->first;
01994 ++oi;
01995 }
01996 }
01997 cloud->resize(oi);
01998 ids.resize(oi);
01999
02000 Transform fromT = nodes.at(nodeId);
02001
02002 if(cloud->size())
02003 {
02004 pcl::search::KdTree<pcl::PointXYZ>::Ptr kdTree(new pcl::search::KdTree<pcl::PointXYZ>);
02005 kdTree->setInputCloud(cloud);
02006 std::vector<int> ind;
02007 std::vector<float> sqrdDist;
02008 pcl::PointXYZ pt(fromT.x(), fromT.y(), fromT.z());
02009 kdTree->radiusSearch(pt, radius, ind, sqrdDist, 0);
02010
02011 Eigen::Vector3f vA = fromT.toEigen3f().linear()*Eigen::Vector3f(1,0,0);
02012
02013 for(unsigned int i=0; i<ind.size(); ++i)
02014 {
02015 if(ind[i] >=0)
02016 {
02017 if(angle > 0.0f)
02018 {
02019 const Transform & checkT = nodes.at(ids[ind[i]]);
02020
02021 Eigen::Vector3f vB = checkT.toEigen3f().linear()*Eigen::Vector3f(1,0,0);
02022 double a = pcl::getAngle3D(Eigen::Vector4f(vA[0], vA[1], vA[2], 0), Eigen::Vector4f(vB[0], vB[1], vB[2], 0));
02023 if(a <= angle)
02024 {
02025 foundNodes.insert(std::make_pair(ids[ind[i]], nodes.at(ids[ind[i]])));
02026 }
02027 }
02028 else
02029 {
02030 foundNodes.insert(std::make_pair(ids[ind[i]], nodes.at(ids[ind[i]])));
02031 }
02032 }
02033 }
02034 }
02035 UDEBUG("found nodes=%d", (int)foundNodes.size());
02036 return foundNodes;
02037 }
02038
02039 float computePathLength(
02040 const std::vector<std::pair<int, Transform> > & path,
02041 unsigned int fromIndex,
02042 unsigned int toIndex)
02043 {
02044 float length = 0.0f;
02045 if(path.size() > 1)
02046 {
02047 UASSERT(fromIndex < path.size() && toIndex < path.size() && fromIndex <= toIndex);
02048 if(fromIndex >= toIndex)
02049 {
02050 toIndex = (unsigned int)path.size()-1;
02051 }
02052 float x=0, y=0, z=0;
02053 for(unsigned int i=fromIndex; i<toIndex-1; ++i)
02054 {
02055 x += fabs(path[i].second.x() - path[i+1].second.x());
02056 y += fabs(path[i].second.y() - path[i+1].second.y());
02057 z += fabs(path[i].second.z() - path[i+1].second.z());
02058 }
02059 length = sqrt(x*x + y*y + z*z);
02060 }
02061 return length;
02062 }
02063
02064 float computePathLength(
02065 const std::map<int, Transform> & path)
02066 {
02067 float length = 0.0f;
02068 if(path.size() > 1)
02069 {
02070 float x=0, y=0, z=0;
02071 std::map<int, Transform>::const_iterator iter=path.begin();
02072 Transform previousPose = iter->second;
02073 ++iter;
02074 for(; iter!=path.end(); ++iter)
02075 {
02076 const Transform & currentPose = iter->second;
02077 x += fabs(previousPose.x() - currentPose.x());
02078 y += fabs(previousPose.y() - currentPose.y());
02079 z += fabs(previousPose.z() - currentPose.z());
02080 previousPose = currentPose;
02081 }
02082 length = sqrt(x*x + y*y + z*z);
02083 }
02084 return length;
02085 }
02086
02087
02088 std::list<std::map<int, Transform> > getPaths(
02089 std::map<int, Transform> poses,
02090 const std::multimap<int, Link> & links)
02091 {
02092 std::list<std::map<int, Transform> > paths;
02093 if(poses.size() && links.size())
02094 {
02095
02096 while(poses.size())
02097 {
02098 std::map<int, Transform> path;
02099 for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end();)
02100 {
02101 std::multimap<int, Link>::const_iterator jter = findLink(links, path.rbegin()->first, iter->first);
02102 if(path.size() == 0 || (jter != links.end() && (jter->second.type() == Link::kNeighbor || jter->second.type() == Link::kNeighborMerged)))
02103 {
02104 path.insert(*iter);
02105 poses.erase(iter++);
02106 }
02107 else
02108 {
02109 break;
02110 }
02111 }
02112 UASSERT(path.size());
02113 paths.push_back(path);
02114 }
02115
02116 }
02117 return paths;
02118 }
02119
02120 }
02121
02122 }