39 #include <pcl/search/kdtree.h>
40 #include <pcl/common/eigen.h>
41 #include <pcl/common/common.h>
42 #include <pcl/common/point_tests.h>
55 const std::string & filePath,
57 const std::map<int, Transform> & poses,
58 const std::multimap<int, Link> & constraints,
59 const std::map<int, double> & stamps,
62 UDEBUG(
"%s", filePath.c_str());
63 std::string tmpPath = filePath;
71 return toro.
saveGraph(tmpPath, poses, constraints);
80 return g2o.saveGraph(tmpPath, poses, constraints);
91 if(stamps.size() != poses.size())
93 UERROR(
"When exporting poses to format 1 (RGBD-SLAM), stamps and poses maps should have the same size! stamps=%d poses=%d",
94 (
int)stamps.size(), (
int)poses.size());
101 fopen_s(&fout, tmpPath.c_str(),
"w");
103 fout = fopen(tmpPath.c_str(),
"w");
107 for(std::map<int, Transform>::const_iterator
iter=poses.begin();
iter!=poses.end(); ++
iter)
118 pose =
t.inverse() * pose;
122 pose =
t.inverse() * pose *
t;
128 if(
iter == poses.begin())
133 fprintf(fout,
"#timestamp x y z qx qy qz qw id\n");
137 fprintf(fout,
"#timestamp x y z qx qy qz qw\n");
142 fprintf(fout,
"%f %f %f %f %f %f %f %f%s\n",
143 stamps.at(
iter->first),
163 pose =
t.inverse() * pose *
t;
167 const float *
p = (
const float *)pose.
data();
169 fprintf(fout,
"%f",
p[0]);
170 for(
int i=1;
i<pose.
size();
i++)
172 fprintf(fout,
" %f",
p[
i]);
185 const std::string & filePath,
187 std::map<int, Transform> & poses,
188 std::multimap<int, Link> * constraints,
189 std::map<int, double> * stamps)
194 std::multimap<int, Link> constraintsTmp;
199 *constraints = constraintsTmp;
207 std::multimap<int, Link> constraintsTmp;
208 UERROR(
"Cannot import from g2o format because it is not yet supported!");
214 file.open(filePath.c_str(), std::ifstream::in);
227 if(
str.size() &&
str.at(
str.size()-1) ==
'\r')
232 if(
str.empty() ||
str.at(0) ==
'#' ||
str.at(0) ==
'%')
239 std::list<std::string> strList =
uSplit(
str,
',');
240 if(strList.size() == 17)
242 double stamp =
uStr2Double(strList.front())/1000000000.0;
249 UWARN(
"Null transform read!? line parsed: \"%s\"",
str.c_str());
255 stamps->insert(std::make_pair(
id, stamp));
262 poses.insert(std::make_pair(
id, pose));
267 UERROR(
"Error parsing \"%s\" with EuRoC MAV format (should have 17 values: stamp x y z qw qx qy qz vx vy vz vr vp vy ax ay az)",
str.c_str());
273 if(strList.size() == 10)
287 stamps->insert(std::make_pair(
id, stamp));
290 if(poses.size() == 0)
294 pose = originPose * pose;
295 poses.insert(std::make_pair(
id, pose));
299 UERROR(
"Error parsing \"%s\" with Karlsruhe format (should have 10 values)",
str.c_str());
305 if(strList.size() == 12)
322 std::string millisecStr = strList[1];
323 while(millisecStr.size() < 6)
325 millisecStr =
"0" + millisecStr;
327 double stamp =
uStr2Double(strList[0] +
"." + millisecStr);
343 stamps->insert(std::make_pair(
id, stamp));
349 UERROR(
"Error parsing \"%s\" with St Lucia format (should have 12 values, e.g., 101215_153851_Ins0.log)",
str.c_str());
355 if(strList.size() == 25)
369 stamps->insert(std::make_pair(
id, stamp));
375 Transform & previousPose = poses.at(
id-1);
377 previousPose =
Transform(previousPose.
x(), previousPose.
y(),
yaw);
383 UERROR(
"Error parsing \"%s\" with Malaga Urban format (should have 25 values, *_GPS.txt)",
str.c_str());
389 if(strList.size() == 3)
402 stamps->insert(std::make_pair(
id, stamp));
408 Transform & previousPose = poses.at(
id-1);
410 previousPose =
Transform(previousPose.
x(), previousPose.
y(),
yaw);
413 if(poses.size() == 0)
417 pose = originPose * pose;
418 poses.insert(std::make_pair(
id, pose));
422 UDEBUG(
"Not valid values detected: \"%s\"",
str.c_str());
427 UERROR(
"Error parsing \"%s\" with NewCollege format (should have 3 values: stamp x y, found %d)",
str.c_str(), (
int)strList.size());
432 std::list<std::string> strList =
uSplit(
str);
433 if((strList.size() >= 8 &&
format!=11) || (strList.size() == 9 &&
format==11))
446 UWARN(
"Null transform read!? line parsed: \"%s\"",
str.c_str());
452 stamps->insert(std::make_pair(
id, stamp));
461 pose =
t * pose *
t.inverse();
467 poses.insert(std::make_pair(
id, pose));
472 UERROR(
"Error parsing \"%s\" with RGBD-SLAM format (should have 8 values (or 9 with id): stamp x y z qw qx qy qz [id])",
str.c_str());
485 pose =
t * pose *
t.inverse();
487 poses.insert(std::make_pair(
id, pose));
498 const std::string & filePath,
499 const std::map<int, GPS> & gpsValues,
502 UDEBUG(
"%s", filePath.c_str());
503 std::string tmpPath = filePath;
507 if(ext.compare(
"kml")!=0 && ext.compare(
"txt")!=0)
509 UERROR(
"Only txt and kml formats are supported!");
515 fopen_s(&fout, tmpPath.c_str(),
"w");
517 fout = fopen(tmpPath.c_str(),
"w");
521 if(ext.compare(
"kml")==0)
524 for(std::map<int, GPS>::const_iterator
iter=gpsValues.begin();
iter!=gpsValues.end(); ++
iter)
533 unsigned int abgr = 0xFF << 24 | (rgba & 0xFF) << 16 | (rgba & 0xFF00) | ((rgba >> 16) &0xFF);
535 std::string colorHexa =
uFormat(
"%08x", abgr);
537 fprintf(fout,
"<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n");
538 fprintf(fout,
"<kml xmlns=\"http://www.opengis.net/kml/2.2\">\n");
539 fprintf(fout,
"<Document>\n"
540 " <name>%s</name>\n", tmpPath.c_str());
541 fprintf(fout,
" <StyleMap id=\"msn_ylw-pushpin\">\n"
543 " <key>normal</key>\n"
544 " <styleUrl>#sn_ylw-pushpin</styleUrl>\n"
547 " <key>highlight</key>\n"
548 " <styleUrl>#sh_ylw-pushpin</styleUrl>\n"
551 " <Style id=\"sh_ylw-pushpin\">\n"
553 " <scale>1.2</scale>\n"
556 " <color>%s</color>\n"
559 " <Style id=\"sn_ylw-pushpin\">\n"
561 " <color>%s</color>\n"
563 " </Style>\n", colorHexa.c_str(), colorHexa.c_str());
564 fprintf(fout,
" <Placemark>\n"
566 " <styleUrl>#msn_ylw-pushpin</styleUrl>"
580 fprintf(fout,
"# stamp longitude latitude altitude error bearing\n");
581 for(std::map<int, GPS>::const_iterator
iter=gpsValues.begin();
iter!=gpsValues.end(); ++
iter)
583 fprintf(fout,
"%f %.*f %.*f %.*f %.*f %.*f\n",
584 iter->second.stamp(),
585 8,
iter->second.longitude(),
586 8,
iter->second.latitude(),
587 8,
iter->second.altitude(),
588 8,
iter->second.error(),
589 8,
iter->second.bearing());
600 float lengths[] = {100,200,300,400,500,600,700,800};
614 std::vector<float>
dist;
616 for (
unsigned int i=1;
i<poses.size();
i++) {
619 float dx = P1.
x()-P2.
x();
620 float dy = P1.
y()-P2.
y();
621 float dz = P1.
z()-P2.
z();
628 for (
unsigned int i=first_frame;
i<
dist.size();
i++)
635 float a = pose_error(0,0);
636 float b = pose_error(1,1);
637 float c = pose_error(2,2);
638 float d = 0.5*(
a+
b+
c-1.0);
643 float dx = pose_error.
x();
644 float dy = pose_error.
y();
645 float dz = pose_error.
z();
646 return sqrt(dx*dx+dy*dy+dz*dz);
650 const std::vector<Transform> &poses_gt,
651 const std::vector<Transform> &poses_result,
655 UASSERT(poses_gt.size() == poses_result.size());
658 std::vector<errors> err;
667 for (
unsigned int first_frame=0; first_frame<poses_gt.size(); first_frame+=step_size) {
683 Transform pose_delta_gt = poses_gt[first_frame].
inverse()*poses_gt[last_frame];
684 Transform pose_delta_result = poses_result[first_frame].
inverse()*poses_result[last_frame];
690 float num_frames = (
float)(last_frame-first_frame+1);
691 float speed =
len/(0.1*num_frames);
702 for (std::vector<errors>::iterator it=err.begin(); it!=err.end(); it++)
709 float num = err.size();
718 const std::vector<Transform> &poses_gt,
719 const std::vector<Transform> &poses_result,
723 UASSERT(poses_gt.size() == poses_result.size());
726 std::vector<errors> err;
729 for (
unsigned int i=0;
i<poses_gt.size()-1; ++
i)
735 float r_err = pose_error.
getAngle();
736 float t_err = pose_error.
getNorm();
739 err.push_back(
errors(
i,r_err,t_err,0,0));
746 for (std::vector<errors>::iterator it=err.begin(); it!=err.end(); it++)
753 float num = err.size();
760 const std::map<int, Transform> & groundTruth,
761 const std::map<int, Transform> & poses,
762 float & translational_rmse,
763 float & translational_mean,
764 float & translational_median,
765 float & translational_std,
766 float & translational_min,
767 float & translational_max,
768 float & rotational_rmse,
769 float & rotational_mean,
770 float & rotational_median,
771 float & rotational_std,
772 float & rotational_min,
773 float & rotational_max)
776 translational_rmse = 0.0f;
777 translational_mean = 0.0f;
778 translational_median = 0.0f;
779 translational_std = 0.0f;
780 translational_min = 0.0f;
781 translational_max = 0.0f;
783 rotational_rmse = 0.0f;
784 rotational_mean = 0.0f;
785 rotational_median = 0.0f;
786 rotational_std = 0.0f;
787 rotational_min = 0.0f;
788 rotational_max = 0.0f;
791 pcl::PointCloud<pcl::PointXYZ> cloud1, cloud2;
792 cloud1.resize(poses.size());
793 cloud2.resize(poses.size());
796 for(std::map<int, Transform>::const_iterator
iter=poses.begin();
iter!=poses.end(); ++
iter)
798 std::map<int, Transform>::const_iterator jter=groundTruth.find(
iter->first);
799 if(jter != groundTruth.end())
803 idFirst =
iter->first;
805 cloud1[oi] = pcl::PointXYZ(jter->second.x(), jter->second.y(), jter->second.z());
806 cloud2[oi++] = pcl::PointXYZ(
iter->second.x(),
iter->second.y(),
iter->second.z());
820 t = groundTruth.at(idFirst) * poses.at(idFirst).inverse();
823 std::vector<float> translationalErrors(poses.size());
824 std::vector<float> rotationalErrors(poses.size());
825 float sumTranslationalErrors = 0.0f;
826 float sumRotationalErrors = 0.0f;
827 float sumSqrdTranslationalErrors = 0.0f;
828 float sumSqrdRotationalErrors = 0.0f;
829 float radToDegree = 180.0f /
M_PI;
831 for(std::map<int, Transform>::const_iterator
iter=poses.begin();
iter!=poses.end(); ++
iter)
833 std::map<int, Transform>::const_iterator jter = groundTruth.find(
iter->first);
834 if(jter!=groundTruth.end())
837 Eigen::Vector3f xAxis(1,0,0);
839 Eigen::Vector3f
vB = jter->second.toEigen3f().linear()*xAxis;
840 double a = pcl::getAngle3D(Eigen::Vector4f(
vA[0],
vA[1],
vA[2], 0), Eigen::Vector4f(
vB[0],
vB[1],
vB[2], 0));
841 rotationalErrors[oi] =
a*radToDegree;
842 translationalErrors[oi] = pose.
getDistance(jter->second);
844 sumTranslationalErrors+=translationalErrors[oi];
845 sumSqrdTranslationalErrors+=translationalErrors[oi]*translationalErrors[oi];
846 sumRotationalErrors+=rotationalErrors[oi];
847 sumSqrdRotationalErrors+=rotationalErrors[oi]*rotationalErrors[oi];
851 translational_min = translational_max = translationalErrors[oi];
852 rotational_min = rotational_max = rotationalErrors[oi];
856 if(translationalErrors[oi] < translational_min)
858 translational_min = translationalErrors[oi];
860 else if(translationalErrors[oi] > translational_max)
862 translational_max = translationalErrors[oi];
865 if(rotationalErrors[oi] < rotational_min)
867 rotational_min = rotationalErrors[oi];
869 else if(rotationalErrors[oi] > rotational_max)
871 rotational_max = rotationalErrors[oi];
878 translationalErrors.resize(oi);
879 rotationalErrors.resize(oi);
882 float total =
float(oi);
883 translational_rmse =
std::sqrt(sumSqrdTranslationalErrors/total);
884 translational_mean = sumTranslationalErrors/total;
885 translational_median = translationalErrors[oi/2];
888 rotational_rmse =
std::sqrt(sumSqrdRotationalErrors/total);
889 rotational_mean = sumRotationalErrors/total;
890 rotational_median = rotationalErrors[oi/2];
897 const std::map<int, Transform> & poses,
898 const std::multimap<int, Link> & links,
899 float & maxLinearErrorRatio,
900 float & maxAngularErrorRatio,
901 float & maxLinearError,
902 float & maxAngularError,
903 const Link ** maxLinearErrorLink,
904 const Link ** maxAngularErrorLink,
907 maxLinearErrorRatio = -1;
908 maxAngularErrorRatio = -1;
910 maxAngularError = -1;
912 UDEBUG(
"poses=%d links=%d", (
int)poses.size(), (
int)links.size());
913 for(std::multimap<int, Link>::const_iterator
iter=links.begin();
iter!=links.end(); ++
iter)
916 if(
iter->second.from() !=
iter->second.to())
926 UWARN(
"Poses are null or not invertible, aborting optimized graph max error check! (Pose %d=%s Pose %d=%s)",
932 if(maxLinearErrorLink)
934 *maxLinearErrorLink = 0;
936 if(maxAngularErrorLink)
938 *maxAngularErrorLink = 0;
940 maxLinearErrorRatio = -1;
941 maxAngularErrorRatio = -1;
943 maxAngularError = -1;
949 float linearError =
uMax3(
952 force3DoF?0:
fabs(
iter->second.transform().z() -
t.z()));
954 float stddevLinear =
sqrt(
iter->second.transVariance(
false));
955 float linearErrorRatio = linearError/stddevLinear;
956 if(linearErrorRatio > maxLinearErrorRatio)
958 maxLinearError = linearError;
959 maxLinearErrorRatio = linearErrorRatio;
960 if(maxLinearErrorLink)
962 *maxLinearErrorLink = &
iter->second;
968 1.0 /
static_cast<double>(
iter->second.infMatrix().at<
double>(5,5)) < 9999.0)
970 float opt_roll,opt_pitch,opt_yaw;
971 float link_roll,link_pitch,link_yaw;
972 t.getEulerAngles(opt_roll, opt_pitch, opt_yaw);
973 iter->second.transform().getEulerAngles(link_roll, link_pitch, link_yaw);
974 float angularError =
uMax3(
975 force3DoF?0:
fabs(opt_roll - link_roll),
976 force3DoF?0:
fabs(opt_pitch - link_pitch),
977 fabs(opt_yaw - link_yaw));
978 angularError = angularError>
M_PI?2*
M_PI-angularError:angularError;
980 float stddevAngular =
sqrt(
iter->second.rotVariance(
false));
981 float angularErrorRatio = angularError/stddevAngular;
982 if(angularErrorRatio > maxAngularErrorRatio)
984 maxAngularError = angularError;
985 maxAngularErrorRatio = angularErrorRatio;
986 if(maxAngularErrorLink)
988 *maxAngularErrorLink = &
iter->second;
998 std::vector<double> maxOdomInf(6,0.0);
999 maxOdomInf.resize(6,0.0);
1000 for(std::multimap<int, Link>::const_iterator
iter=links.begin();
iter!=links.end(); ++
iter)
1004 for(
int i=0;
i<6; ++
i)
1006 const double &
v =
iter->second.infMatrix().at<
double>(
i,
i);
1007 if(maxOdomInf[
i] <
v)
1014 if(maxOdomInf[0] == 0.0)
1025 std::multimap<int, Link> & links,
1031 std::multimap<int, Link>::iterator
iter = links.find(from);
1032 while(
iter != links.end() &&
iter->first == from)
1044 iter = links.find(to);
1045 while(
iter != links.end() &&
iter->first == to)
1058 std::multimap<
int, std::pair<int, Link::Type> > & links,
1064 std::multimap<int, std::pair<int, Link::Type> >
::iterator iter = links.find(from);
1065 while(
iter != links.end() &&
iter->first == from)
1077 iter = links.find(to);
1078 while(
iter != links.end() &&
iter->first == to)
1091 std::multimap<int, int> & links,
1096 std::multimap<int, int>::iterator
iter = links.find(from);
1097 while(
iter != links.end() &&
iter->first == from)
1099 if(
iter->second == to)
1109 iter = links.find(to);
1110 while(
iter != links.end() &&
iter->first == to)
1112 if(
iter->second == from)
1122 const std::multimap<int, Link> & links,
1128 std::multimap<int, Link>::const_iterator
iter = links.find(from);
1129 while(
iter != links.end() &&
iter->first == from)
1141 iter = links.find(to);
1142 while(
iter != links.end() &&
iter->first == to)
1154 std::multimap<int, std::pair<int, Link::Type> >::const_iterator
findLink(
1155 const std::multimap<
int, std::pair<int, Link::Type> > & links,
1161 std::multimap<int, std::pair<int, Link::Type> >::const_iterator
iter = links.find(from);
1162 while(
iter != links.end() &&
iter->first == from)
1174 iter = links.find(to);
1175 while(
iter != links.end() &&
iter->first == to)
1188 const std::multimap<int, int> & links,
1193 std::multimap<int, int>::const_iterator
iter = links.find(from);
1194 while(
iter != links.end() &&
iter->first == from)
1196 if(
iter->second == to)
1206 iter = links.find(to);
1207 while(
iter != links.end() &&
iter->first == to)
1209 if(
iter->second == from)
1220 const std::multimap<int, Link> & links,
1223 std::list<Link> output;
1224 for(std::multimap<int, Link>::const_iterator
iter=links.begin();
iter != links.end(); ++
iter)
1226 if(
iter->second.from() == from)
1228 output.push_back(
iter->second);
1230 else if(
iter->second.to() == from)
1232 output.push_back(
iter->second.inverse());
1239 const std::multimap<int, Link> & links)
1241 std::multimap<int, Link> output;
1242 for(std::multimap<int, Link>::const_iterator
iter=links.begin();
iter!=links.end(); ++
iter)
1246 output.insert(*
iter);
1253 const std::multimap<int, Link> & links,
1257 std::multimap<int, Link> output;
1258 for(std::multimap<int, Link>::const_iterator
iter=links.begin();
iter!=links.end(); ++
iter)
1262 if((!inverted &&
iter->second.from() !=
iter->second.to())||
1263 (inverted &&
iter->second.from() ==
iter->second.to()))
1265 output.insert(*
iter);
1268 else if((!inverted &&
iter->second.type() != filteredType)||
1269 (inverted &&
iter->second.type() == filteredType))
1271 output.insert(*
iter);
1278 const std::map<int, Link> & links,
1282 std::map<int, Link> output;
1283 for(std::map<int, Link>::const_iterator
iter=links.begin();
iter!=links.end(); ++
iter)
1287 if((!inverted &&
iter->second.from() !=
iter->second.to())||
1288 (inverted &&
iter->second.from() ==
iter->second.to()))
1290 output.insert(*
iter);
1293 else if((!inverted &&
iter->second.type() != filteredType)||
1294 (inverted &&
iter->second.type() == filteredType))
1296 output.insert(*
iter);
1303 const std::map<int, Transform> & poses,
1305 float horizontalFOV,
1307 float nearClipPlaneDistance,
1308 float farClipPlaneDistance,
1311 std::map<int, Transform> output;
1315 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(
new pcl::PointCloud<pcl::PointXYZ>);
1316 std::vector<int> ids(poses.size());
1318 cloud->resize(poses.size());
1319 ids.resize(poses.size());
1321 for(std::map<int, rtabmap::Transform>::const_iterator
iter=poses.begin();
iter!=poses.end(); ++
iter)
1323 if(!
iter->second.isNull())
1325 (*cloud)[oi] = pcl::PointXYZ(
iter->second.x(),
iter->second.y(),
iter->second.z());
1326 ids[oi++] =
iter->first;
1334 pcl::IndicesPtr(
new std::vector<int>),
1338 nearClipPlaneDistance,
1339 farClipPlaneDistance,
1343 for(
unsigned int i=0;
i<
indices->size(); ++
i)
1345 output.insert(*poses.find(ids[
indices->at(
i)]));
1352 const std::map<int, Transform> & poses,
1357 if(poses.size() > 2 && radius > 0.0f)
1359 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(
new pcl::PointCloud<pcl::PointXYZ>);
1360 cloud->resize(poses.size());
1362 for(std::map<int, Transform>::const_iterator
iter = poses.begin();
iter!=poses.end(); ++
iter, ++
i)
1364 (*cloud)[
i] = pcl::PointXYZ(
iter->second.x(),
iter->second.y(),
iter->second.z());
1365 UASSERT_MSG(pcl::isFinite((*cloud)[
i]),
uFormat(
"Invalid pose (%d) %s",
iter->first,
iter->second.prettyPrint().c_str()).c_str());
1370 std::vector<Transform> transforms =
uValues(poses);
1372 pcl::search::KdTree<pcl::PointXYZ>::Ptr
tree (
new pcl::search::KdTree<pcl::PointXYZ> (
false));
1373 tree->setInputCloud(cloud);
1374 std::set<int> indicesChecked;
1375 std::set<int> indicesKept;
1377 for(
unsigned int i=0;
i<cloud->size(); ++
i)
1379 if(indicesChecked.find(
i) == indicesChecked.end())
1382 std::vector<float> kDistances;
1383 tree->radiusSearch(cloud->at(
i), radius,
kIndices, kDistances);
1385 std::set<int> cloudIndices;
1386 const Transform & currentT = transforms.at(
i);
1390 if(indicesChecked.find(
kIndices[
j]) == indicesChecked.end())
1397 double a = pcl::getAngle3D(Eigen::Vector4f(
vA[0],
vA[1],
vA[2], 0), Eigen::Vector4f(
vB[0],
vB[1],
vB[2], 0));
1412 bool lastAdded =
false;
1413 for(std::set<int>::reverse_iterator
iter = cloudIndices.rbegin();
iter!=cloudIndices.rend(); ++
iter)
1417 indicesKept.insert(*
iter);
1420 indicesChecked.insert(*
iter);
1425 bool firstAdded =
false;
1426 for(std::set<int>::iterator
iter = cloudIndices.begin();
iter!=cloudIndices.end(); ++
iter)
1430 indicesKept.insert(*
iter);
1433 indicesChecked.insert(*
iter);
1441 UINFO(
"Cloud filtered In = %d, Out = %d (radius=%f angle=%f keepLatest=%d)", cloud->size(), indicesKept.size(), radius,
angle, keepLatest?1:0);
1445 std::map<int, Transform> keptPoses;
1446 for(std::set<int>::iterator
iter = indicesKept.begin();
iter!=indicesKept.end(); ++
iter)
1448 keptPoses.insert(std::make_pair(
names.at(*
iter), transforms.at(*
iter)));
1452 keptPoses.insert(*poses.begin());
1453 keptPoses.insert(*poses.rbegin());
1465 std::multimap<int, int> clusters;
1466 if(poses.size() > 1 && radius > 0.0f)
1468 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(
new pcl::PointCloud<pcl::PointXYZ>);
1469 cloud->resize(poses.size());
1471 for(std::map<int, Transform>::const_iterator
iter = poses.begin();
iter!=poses.end(); ++
iter, ++
i)
1473 (*cloud)[
i] = pcl::PointXYZ(
iter->second.x(),
iter->second.y(),
iter->second.z());
1474 UASSERT_MSG(pcl::isFinite((*cloud)[
i]),
uFormat(
"Invalid pose (%d) %s",
iter->first,
iter->second.prettyPrint().c_str()).c_str());
1478 std::vector<int> ids =
uKeys(poses);
1479 std::vector<Transform> transforms =
uValues(poses);
1481 pcl::search::KdTree<pcl::PointXYZ>::Ptr
tree (
new pcl::search::KdTree<pcl::PointXYZ> (
false));
1482 tree->setInputCloud(cloud);
1484 for(
unsigned int i=0;
i<cloud->size(); ++
i)
1487 std::vector<float> kDistances;
1488 tree->radiusSearch(cloud->at(
i), radius,
kIndices, kDistances);
1490 std::set<int> cloudIndices;
1491 const Transform & currentT = transforms.at(
i);
1502 double a = pcl::getAngle3D(Eigen::Vector4f(
vA[0],
vA[1],
vA[2], 0), Eigen::Vector4f(
vB[0],
vB[1],
vB[2], 0));
1505 clusters.insert(std::make_pair(ids[
i], ids[
kIndices[
j]]));
1510 clusters.insert(std::make_pair(ids[
i], ids[
kIndices[
j]]));
1520 const std::map<int, Transform> & poses,
1521 const std::multimap<int, Link> & links,
1522 std::multimap<int, int> & hyperNodes,
1523 std::multimap<int, Link> & hyperLinks)
1525 UINFO(
"Input: poses=%d links=%d", (
int)poses.size(), (
int)links.size());
1527 std::map<int, int> posesToHyperNodes;
1528 std::map<int, std::multimap<int, Link> > clusterloopClosureLinks;
1531 std::multimap<int, Link> bidirectionalLoopClosureLinks;
1532 for(std::multimap<int, Link>::const_iterator jter=links.begin(); jter!=links.end(); ++jter)
1536 jter->second.userDataCompressed().empty())
1538 if(
uContains(poses, jter->second.from()) &&
1541 UASSERT_MSG(
graph::findLink(links, jter->second.to(), jter->second.from(),
false) == links.end(),
"Input links should be unique!");
1542 bidirectionalLoopClosureLinks.insert(std::make_pair(jter->second.from(), jter->second));
1548 UINFO(
"Clustering hyper nodes...");
1550 for(std::map<int, Transform>::const_reverse_iterator
iter=poses.rbegin();
iter!=poses.rend(); ++
iter)
1552 if(posesToHyperNodes.find(
iter->first) == posesToHyperNodes.end())
1554 int hyperNodeId =
iter->first;
1555 std::list<int> loopClosures;
1556 std::set<int> loopClosuresAdded;
1557 loopClosures.push_back(
iter->first);
1558 std::multimap<int, Link> clusterLinks;
1559 while(loopClosures.size())
1561 int id = loopClosures.front();
1562 loopClosures.pop_front();
1564 UASSERT(posesToHyperNodes.find(
id) == posesToHyperNodes.end());
1566 posesToHyperNodes.insert(std::make_pair(
id, hyperNodeId));
1567 hyperNodes.insert(std::make_pair(hyperNodeId,
id));
1569 for(std::multimap<int, Link>::const_iterator jter=bidirectionalLoopClosureLinks.find(
id); jter!=bidirectionalLoopClosureLinks.end() && jter->first==
id; ++jter)
1571 if(posesToHyperNodes.find(jter->second.to()) == posesToHyperNodes.end() &&
1572 loopClosuresAdded.find(jter->second.to()) == loopClosuresAdded.end())
1574 loopClosures.push_back(jter->second.to());
1575 loopClosuresAdded.insert(jter->second.to());
1576 clusterLinks.insert(*jter);
1577 clusterLinks.insert(std::make_pair(jter->second.to(), jter->second.inverse()));
1578 if(jter->second.from() < jter->second.to())
1580 UWARN(
"Child to Parent link? %d->%d (type=%d)",
1581 jter->second.from(),
1583 jter->second.type());
1588 UASSERT(clusterloopClosureLinks.find(hyperNodeId) == clusterloopClosureLinks.end());
1589 clusterloopClosureLinks.insert(std::make_pair(hyperNodeId, clusterLinks));
1590 UDEBUG(
"Created hyper node %d with %d children (%f%%)",
1591 hyperNodeId, (
int)loopClosuresAdded.size(),
float(posesToHyperNodes.size())/
float(poses.size())*100.0f);
1594 UINFO(
"Clustering hyper nodes... done! (%f s)",
timer.ticks());
1597 UINFO(
"Creating hyper links...");
1599 for(std::multimap<int, Link>::const_reverse_iterator jter=links.rbegin(); jter!=links.rend(); ++jter)
1603 !jter->second.userDataCompressed().empty()) &&
1604 uContains(poses, jter->second.from()) &&
1607 UASSERT_MSG(
uContains(posesToHyperNodes, jter->second.from()),
uFormat(
"%d->%d (type=%d)", jter->second.from(), jter->second.to(), jter->second.type()).c_str());
1608 UASSERT_MSG(
uContains(posesToHyperNodes, jter->second.to()),
uFormat(
"%d->%d (type=%d)", jter->second.from(), jter->second.to(), jter->second.type()).c_str());
1609 int hyperNodeIDFrom = posesToHyperNodes.at(jter->second.from());
1610 int hyperNodeIDTo = posesToHyperNodes.at(jter->second.to());
1613 if(hyperNodeIDFrom != hyperNodeIDTo)
1615 std::multimap<int, Link>::iterator tmpIter =
graph::findLink(hyperLinks, hyperNodeIDFrom, hyperNodeIDTo);
1616 if(tmpIter!=hyperLinks.end() &&
1617 hyperNodeIDFrom == jter->second.from() &&
1618 hyperNodeIDTo == jter->second.to() &&
1623 hyperLinks.erase(tmpIter);
1627 if(
graph::findLink(hyperLinks, hyperNodeIDFrom, hyperNodeIDTo) == hyperLinks.end())
1629 UASSERT(clusterloopClosureLinks.find(hyperNodeIDFrom) != clusterloopClosureLinks.end());
1630 UASSERT(clusterloopClosureLinks.find(hyperNodeIDTo) != clusterloopClosureLinks.end());
1631 std::multimap<int, Link> tmpLinks = clusterloopClosureLinks.at(hyperNodeIDFrom);
1632 tmpLinks.insert(clusterloopClosureLinks.at(hyperNodeIDTo).begin(), clusterloopClosureLinks.at(hyperNodeIDTo).end());
1633 tmpLinks.insert(std::make_pair(jter->second.from(), jter->second));
1635 std::list<int>
path =
computePath(tmpLinks, hyperNodeIDFrom, hyperNodeIDTo,
false,
true);
1637 uFormat(
"path.size()=%d, hyperNodeIDFrom=%d, hyperNodeIDTo=%d",
1640 hyperNodeIDTo).c_str());
1642 if(
path.size() > 10)
1644 UWARN(
"Large path! %d nodes", (
int)
path.size());
1645 std::stringstream
stream;
1659 std::list<int>::iterator
iter=
path.begin();
1663 std::multimap<int, Link>::const_iterator foundIter =
graph::findLink(tmpLinks, from, to,
false);
1664 UASSERT(foundIter != tmpLinks.end());
1665 Link hyperLink = foundIter->second;
1671 std::multimap<int, Link>::const_iterator foundIter =
graph::findLink(tmpLinks, from, to,
false);
1672 UASSERT(foundIter != tmpLinks.end());
1673 hyperLink = hyperLink.
merge(foundIter->second, jter->second.type());
1679 UASSERT(hyperLink.
to() == hyperNodeIDTo);
1680 hyperLinks.insert(std::make_pair(hyperNodeIDFrom, hyperLink));
1682 UDEBUG(
"Created hyper link %d->%d (%f%%)",
1683 hyperLink.
from(), hyperLink.
to(),
float(
i)/
float(links.size())*100.0f);
1686 UWARN(
"Large hyper link %d->%d (%f m)! original %d->%d (%f m)",
1690 jter->second.from(),
1692 jter->second.transform().getNorm());
1700 UINFO(
"Creating hyper links... done! (%f s)",
timer.ticks());
1746 typedef std::pair<int, float>
Pair;
1751 return a.second >
b.second;
1757 const std::map<int, rtabmap::Transform> & poses,
1758 const std::multimap<int, int> & links,
1761 bool updateNewCosts)
1763 std::list<std::pair<int, Transform> >
path;
1765 int startNode = from;
1768 std::map<int, Node>
nodes;
1769 nodes.insert(std::make_pair(startNode,
Node(startNode, 0, poses.at(startNode))));
1770 std::priority_queue<Pair, std::vector<Pair>,
Order> pq;
1771 std::multimap<float, int> pqmap;
1774 pqmap.insert(std::make_pair(0, startNode));
1778 pq.push(
Pair(startNode, 0));
1781 while((updateNewCosts && pqmap.size()) || (!updateNewCosts && pq.size()))
1786 currentNode = &
nodes.find(pqmap.begin()->second)->second;
1787 pqmap.erase(pqmap.begin());
1791 currentNode = &
nodes.find(pq.top().first)->second;
1797 if(currentNode->
id() == endNode)
1799 while(currentNode->
id()!=startNode)
1801 path.push_front(std::make_pair(currentNode->
id(), currentNode->
pose()));
1802 currentNode = &
nodes.find(currentNode->
fromId())->second;
1804 path.push_front(std::make_pair(startNode, poses.at(startNode)));
1809 for(std::multimap<int, int>::const_iterator
iter = links.find(currentNode->
id());
1810 iter!=links.end() &&
iter->first == currentNode->
id();
1813 std::map<int, Node>::iterator nodeIter =
nodes.find(
iter->second);
1814 if(nodeIter ==
nodes.end())
1816 std::map<int, rtabmap::Transform>::const_iterator poseIter = poses.find(
iter->second);
1817 if(poseIter == poses.end())
1819 UERROR(
"Next pose %d (from %d) should be found in poses! Ignoring it!",
iter->second,
iter->first);
1823 Node n(
iter->second, currentNode->
id(), poseIter->second);
1825 n.setDistToEnd(
n.distFrom(endPose));
1827 nodes.insert(std::make_pair(
iter->second,
n));
1830 pqmap.insert(std::make_pair(
n.totalCost(),
n.id()));
1834 pq.push(
Pair(
n.id(),
n.totalCost()));
1838 else if(updateNewCosts && nodeIter->second.isOpened())
1840 float newCostSoFar = currentNode->
costSoFar() + currentNode->
distFrom(nodeIter->second.pose());
1841 if(nodeIter->second.costSoFar() > newCostSoFar)
1844 for(std::multimap<float, int>::iterator mapIter=pqmap.begin(); mapIter!=pqmap.end(); ++mapIter)
1846 if(mapIter->second == nodeIter->first)
1848 pqmap.erase(mapIter);
1849 nodeIter->second.setCostSoFar(newCostSoFar);
1850 pqmap.insert(std::make_pair(nodeIter->second.totalCost(), nodeIter->first));
1863 const std::multimap<int, Link> & links,
1866 bool updateNewCosts,
1867 bool useSameCostForAllLinks)
1869 std::list<int>
path;
1871 int startNode = from;
1873 std::map<int, Node>
nodes;
1875 std::priority_queue<Pair, std::vector<Pair>,
Order> pq;
1876 std::multimap<float, int> pqmap;
1879 pqmap.insert(std::make_pair(0, startNode));
1883 pq.push(
Pair(startNode, 0));
1886 while((updateNewCosts && pqmap.size()) || (!updateNewCosts && pq.size()))
1891 currentNode = &
nodes.find(pqmap.begin()->second)->second;
1892 pqmap.erase(pqmap.begin());
1896 currentNode = &
nodes.find(pq.top().first)->second;
1902 if(currentNode->
id() == endNode)
1904 while(currentNode->
id()!=startNode)
1906 path.push_front(currentNode->
id());
1907 currentNode = &
nodes.find(currentNode->
fromId())->second;
1909 path.push_front(startNode);
1914 for(std::multimap<int, Link>::const_iterator
iter = links.find(currentNode->
id());
1915 iter!=links.end() &&
iter->first == currentNode->
id();
1918 std::map<int, Node>::iterator nodeIter =
nodes.find(
iter->second.to());
1920 if(!useSameCostForAllLinks)
1922 cost =
iter->second.transform().getNorm();
1924 if(nodeIter ==
nodes.end())
1928 n.setCostSoFar(currentNode->
costSoFar() + cost);
1929 nodes.insert(std::make_pair(
iter->second.to(),
n));
1932 pqmap.insert(std::make_pair(
n.totalCost(),
n.id()));
1936 pq.push(
Pair(
n.id(),
n.totalCost()));
1939 else if(!useSameCostForAllLinks && updateNewCosts && nodeIter->second.isOpened())
1941 float newCostSoFar = currentNode->
costSoFar() + cost;
1942 if(nodeIter->second.costSoFar() > newCostSoFar)
1945 for(std::multimap<float, int>::iterator mapIter=pqmap.begin(); mapIter!=pqmap.end(); ++mapIter)
1947 if(mapIter->second == nodeIter->first)
1949 pqmap.erase(mapIter);
1950 nodeIter->second.setCostSoFar(newCostSoFar);
1951 pqmap.insert(std::make_pair(nodeIter->second.totalCost(), nodeIter->first));
1968 bool lookInDatabase,
1969 bool updateNewCosts,
1970 float linearVelocity,
1971 float angularVelocity)
1976 std::list<std::pair<int, Transform> >
path;
1977 UDEBUG(
"fromId=%d, toId=%d, lookInDatabase=%d, updateNewCosts=%d, linearVelocity=%f, angularVelocity=%f",
1985 std::multimap<int, Link> allLinks;
1990 allLinks = memory->
getAllLinks(lookInDatabase,
true,
true);
1991 for(std::multimap<int, Link>::iterator
iter=allLinks.begin();
iter!=allLinks.end(); ++
iter)
1993 if(
iter->second.to() < 0)
1995 allLinks.insert(std::make_pair(
iter->second.to(),
iter->second.inverse()));
1998 UINFO(
"getting all %d links time = %f s", (
int)allLinks.size(),
t.ticks());
2002 int startNode = fromId;
2004 std::map<int, Node>
nodes;
2006 std::priority_queue<Pair, std::vector<Pair>,
Order> pq;
2007 std::multimap<float, int> pqmap;
2010 pqmap.insert(std::make_pair(0, startNode));
2014 pq.push(
Pair(startNode, 0));
2017 while((updateNewCosts && pqmap.size()) || (!updateNewCosts && pq.size()))
2022 currentNode = &
nodes.find(pqmap.begin()->second)->second;
2023 pqmap.erase(pqmap.begin());
2027 currentNode = &
nodes.find(pq.top().first)->second;
2033 if(currentNode->
id() == endNode)
2035 while(currentNode->
id()!=startNode)
2037 path.push_front(std::make_pair(currentNode->
id(), currentNode->
pose()));
2038 currentNode = &
nodes.find(currentNode->
fromId())->second;
2040 path.push_front(std::make_pair(startNode, currentNode->
pose()));
2045 std::multimap<int, Link> links;
2046 if(allLinks.size() == 0)
2048 links = memory->
getLinks(currentNode->
id(), lookInDatabase,
true);
2052 for(std::multimap<int, Link>::const_iterator
iter = allLinks.lower_bound(currentNode->
id());
2053 iter!=allLinks.end() &&
iter->first == currentNode->
id();
2056 links.insert(std::make_pair(
iter->second.to(),
iter->second));
2059 for(std::multimap<int, Link>::const_iterator
iter = links.begin();
iter!=links.end(); ++
iter)
2061 if(
iter->second.from() !=
iter->second.to())
2065 if(linearVelocity <= 0.0
f && angularVelocity <= 0.0
f)
2068 cost =
iter->second.transform().getNorm();
2072 if(linearVelocity > 0.0
f)
2074 cost +=
iter->second.transform().getNorm()/linearVelocity;
2076 if(angularVelocity > 0.0
f)
2078 Eigen::Vector4f
v1 = Eigen::Vector4f(nextPose.
x()-currentNode->
pose().
x(), nextPose.
y()-currentNode->
pose().
y(), nextPose.
z()-currentNode->
pose().
z(), 1.0f);
2081 cost +=
angle / angularVelocity;
2085 std::map<int, Node>::iterator nodeIter =
nodes.find(
iter->first);
2086 if(nodeIter ==
nodes.end())
2088 Node n(
iter->second.to(), currentNode->
id(), nextPose);
2090 n.setCostSoFar(currentNode->
costSoFar() + cost);
2091 nodes.insert(std::make_pair(
iter->second.to(),
n));
2094 pqmap.insert(std::make_pair(
n.totalCost(),
n.id()));
2098 pq.push(
Pair(
n.id(),
n.totalCost()));
2101 else if(updateNewCosts && nodeIter->second.isOpened())
2103 float newCostSoFar = currentNode->
costSoFar() + cost;
2104 if(nodeIter->second.costSoFar() > newCostSoFar)
2107 nodeIter->second.setPose(nextPose);
2110 for(std::multimap<float, int>::iterator mapIter=pqmap.begin(); mapIter!=pqmap.end(); ++mapIter)
2112 if(mapIter->second == nodeIter->first)
2114 pqmap.erase(mapIter);
2115 nodeIter->second.setCostSoFar(newCostSoFar);
2116 pqmap.insert(std::make_pair(nodeIter->second.totalCost(), nodeIter->first));
2129 std::stringstream
stream;
2131 std::list<std::pair<int, Transform> >::const_iterator previousIter =
path.end();
2133 for(std::list<std::pair<int, Transform> >::const_iterator
iter=
path.begin();
iter!=
path.end();++
iter)
2140 if(previousIter!=
path.end())
2145 std::multimap<int, Link>::iterator jter =
graph::findLink(allLinks, previousIter->first,
iter->first);
2146 if(jter != allLinks.end())
2156 ++linkTypes[jter->second.type()];
2157 stream <<
"[" << jter->second.type() <<
"]";
2158 length += jter->second.transform().getNorm();
2168 std::stringstream streamB;
2169 for(
unsigned int i=0;
i<linkTypes.size(); ++
i)
2175 streamB <<
i <<
"=" << linkTypes[
i];
2177 UDEBUG(
"Link types = %s", streamB.str().c_str());
2184 const std::map<int, rtabmap::Transform> & poses,
2189 std::map<int, float> nearestNodes =
findNearestNodes(targetPose, poses, 0, 0, 1);
2190 if(!nearestNodes.empty())
2192 id = nearestNodes.begin()->first;
2195 *
distance = nearestNodes.begin()->second;
2204 const std::map<int, Transform> & poses,
2211 std::map<int, Transform> nodesMinusTarget = poses;
2212 Transform targetPose = poses.at(nodeId);
2213 nodesMinusTarget.erase(nodeId);
2220 const std::map<int, Transform> & poses,
2228 std::map<int, float> foundNodes;
2234 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(
new pcl::PointCloud<pcl::PointXYZ>);
2235 cloud->resize(poses.size());
2236 std::vector<int> ids(poses.size());
2238 for(std::map<int, Transform>::const_iterator
iter = poses.begin();
iter!=poses.end(); ++
iter)
2240 (*cloud)[oi] = pcl::PointXYZ(
iter->second.x(),
iter->second.y(),
iter->second.z());
2241 UASSERT_MSG(pcl::isFinite((*cloud)[oi]),
uFormat(
"Invalid pose (%d) %s",
iter->first,
iter->second.prettyPrint().c_str()).c_str());
2242 ids[oi] =
iter->first;
2250 pcl::search::KdTree<pcl::PointXYZ>::Ptr kdTree(
new pcl::search::KdTree<pcl::PointXYZ>);
2251 kdTree->setInputCloud(cloud);
2252 std::vector<int>
ind;
2253 std::vector<float> sqrdDist;
2254 pcl::PointXYZ pt(targetPose.
x(), targetPose.
y(), targetPose.
z());
2257 kdTree->radiusSearch(pt, radius,
ind, sqrdDist, k);
2261 kdTree->nearestKSearch(pt, k,
ind, sqrdDist);
2264 for(
unsigned int i=0;
i<
ind.size(); ++
i)
2273 double a = pcl::getAngle3D(Eigen::Vector4f(
vA[0],
vA[1],
vA[2], 0), Eigen::Vector4f(
vB[0],
vB[1],
vB[2], 0));
2276 foundNodes.insert(std::make_pair(ids[
ind[
i]], sqrdDist[
i]));
2281 foundNodes.insert(std::make_pair(ids[
ind[
i]], sqrdDist[
i]));
2286 UDEBUG(
"found nodes=%d", (
int)foundNodes.size());
2293 const std::map<int, Transform> & poses,
2300 std::map<int, Transform> nodesMinusTarget = poses;
2301 Transform targetPose = poses.at(nodeId);
2302 nodesMinusTarget.erase(nodeId);
2308 const std::map<int, Transform> & poses,
2314 std::map<int, Transform> foundPoses;
2315 for(std::map<int, float>::iterator
iter=nearestNodes.begin();
iter!=nearestNodes.end(); ++
iter)
2317 foundPoses.insert(*poses.find(
iter->first));
2319 UDEBUG(
"found nodes=%d/%d (radius=%f, angle=%f, k=%d)", (
int)foundPoses.size(), (
int)poses.size(), radius,
angle, k);
2328 std::map<int, float>
getNodesInRadius(
int nodeId,
const std::map<int, Transform> & nodes,
float radius)
2336 std::map<int, Transform>
getPosesInRadius(
int nodeId,
const std::map<int, Transform> & nodes,
float radius,
float angle)
2347 const std::vector<std::pair<int, Transform> > & path,
2348 unsigned int fromIndex,
2349 unsigned int toIndex)
2354 UASSERT(fromIndex <
path.size() && toIndex <
path.size() && fromIndex <= toIndex);
2355 if(fromIndex >= toIndex)
2357 toIndex = (
unsigned int)
path.size()-1;
2359 float x=0,
y=0,
z=0;
2360 for(
unsigned int i=fromIndex;
i<toIndex-1; ++
i)
2372 const std::map<int, Transform> & path)
2377 float x=0,
y=0,
z=0;
2378 std::map<int, Transform>::const_iterator
iter=
path.begin();
2384 x +=
fabs(previousPose.
x() - currentPose.
x());
2385 y +=
fabs(previousPose.
y() - currentPose.
y());
2386 z +=
fabs(previousPose.
z() - currentPose.
z());
2387 previousPose = currentPose;
2396 std::map<int, Transform> poses,
2397 const std::multimap<int, Link> & links)
2399 std::list<std::map<int, Transform> > paths;
2400 if(poses.size() && links.size())
2405 std::map<int, Transform>
path;
2406 for(std::map<int, Transform>::iterator
iter=poses.begin();
iter!=poses.end();)
2408 std::multimap<int, Link>::const_iterator jter =
findLink(links,
path.rbegin()->first,
iter->first);
2412 poses.erase(
iter++);
2420 paths.push_back(
path);
2433 min[0] =
max[0] = poses.begin()->second.x();
2434 min[1] =
max[1] = poses.begin()->second.y();
2435 min[2] =
max[2] = poses.begin()->second.z();
2436 for(std::map<int, Transform>::const_iterator
iter=poses.begin();
iter!=poses.end(); ++
iter)