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 std::list<std::pair<int, Transform> > posesList;
108 for(std::map<int, Transform>::const_iterator
iter=poses.lower_bound(0);
iter!=poses.end(); ++
iter)
110 posesList.push_back(*
iter);
115 for(std::map<int, Transform>::const_iterator
iter=poses.begin();
iter!=poses.end() &&
iter->first < 0; ++
iter)
117 posesList.push_back(*
iter);
120 for(std::list<std::pair<int, Transform> >::const_iterator
iter=posesList.begin();
iter!=posesList.end(); ++
iter)
131 pose =
t.inverse() * pose;
135 pose =
t.inverse() * pose *
t;
141 if(
iter == posesList.begin())
146 fprintf(fout,
"#timestamp x y z qx qy qz qw id\n");
150 fprintf(fout,
"#timestamp x y z qx qy qz qw\n");
155 fprintf(fout,
"%f %f %f %f %f %f %f %f%s\n",
156 stamps.at(
iter->first),
176 pose =
t.inverse() * pose *
t;
180 const float *
p = (
const float *)pose.
data();
182 fprintf(fout,
"%f",
p[0]);
183 for(
int i=1;
i<pose.
size();
i++)
185 fprintf(fout,
" %f",
p[
i]);
198 const std::string & filePath,
200 std::map<int, Transform> & poses,
201 std::multimap<int, Link> * constraints,
202 std::map<int, double> * stamps)
207 std::multimap<int, Link> constraintsTmp;
212 *constraints = constraintsTmp;
220 std::multimap<int, Link> constraintsTmp;
221 UERROR(
"Cannot import from g2o format because it is not yet supported!");
227 file.open(filePath.c_str(), std::ifstream::in);
240 if(
str.size() &&
str.at(
str.size()-1) ==
'\r')
245 if(
str.empty() ||
str.at(0) ==
'#' ||
str.at(0) ==
'%')
252 std::list<std::string> strList =
uSplit(
str,
',');
253 if(strList.size() == 17)
255 double stamp =
uStr2Double(strList.front())/1000000000.0;
262 UWARN(
"Null transform read!? line parsed: \"%s\"",
str.c_str());
268 stamps->insert(std::make_pair(
id, stamp));
275 poses.insert(std::make_pair(
id, pose));
280 UERROR(
"Error parsing \"%s\" with EuRoC MAV format (should have 17 values: stamp x y z qw qx qy qz vx vy vz vr vp vy ax ay az)",
str.c_str());
286 if(strList.size() == 10)
300 stamps->insert(std::make_pair(
id, stamp));
303 if(poses.size() == 0)
307 pose = originPose * pose;
308 poses.insert(std::make_pair(
id, pose));
312 UERROR(
"Error parsing \"%s\" with Karlsruhe format (should have 10 values)",
str.c_str());
318 if(strList.size() == 12)
335 std::string millisecStr = strList[1];
336 while(millisecStr.size() < 6)
338 millisecStr =
"0" + millisecStr;
340 double stamp =
uStr2Double(strList[0] +
"." + millisecStr);
356 stamps->insert(std::make_pair(
id, stamp));
362 UERROR(
"Error parsing \"%s\" with St Lucia format (should have 12 values, e.g., 101215_153851_Ins0.log)",
str.c_str());
368 if(strList.size() == 25)
382 stamps->insert(std::make_pair(
id, stamp));
388 Transform & previousPose = poses.at(
id-1);
390 previousPose =
Transform(previousPose.
x(), previousPose.
y(),
yaw);
396 UERROR(
"Error parsing \"%s\" with Malaga Urban format (should have 25 values, *_GPS.txt)",
str.c_str());
402 if(strList.size() == 3)
415 stamps->insert(std::make_pair(
id, stamp));
421 Transform & previousPose = poses.at(
id-1);
423 previousPose =
Transform(previousPose.
x(), previousPose.
y(),
yaw);
426 if(poses.size() == 0)
430 pose = originPose * pose;
431 poses.insert(std::make_pair(
id, pose));
435 UDEBUG(
"Not valid values detected: \"%s\"",
str.c_str());
440 UERROR(
"Error parsing \"%s\" with NewCollege format (should have 3 values: stamp x y, found %d)",
str.c_str(), (
int)strList.size());
445 std::list<std::string> strList =
uSplit(
str);
446 if((strList.size() >= 8 &&
format!=11) || (strList.size() == 9 &&
format==11))
459 UWARN(
"Null transform read!? line parsed: \"%s\"",
str.c_str());
465 stamps->insert(std::make_pair(
id, stamp));
474 pose =
t * pose *
t.inverse();
480 poses.insert(std::make_pair(
id, pose));
485 UERROR(
"Error parsing \"%s\" with RGBD-SLAM format (should have 8 values (or 9 with id): stamp x y z qw qx qy qz [id])",
str.c_str());
498 pose =
t * pose *
t.inverse();
500 poses.insert(std::make_pair(
id, pose));
511 const std::string & filePath,
512 const std::map<int, GPS> & gpsValues,
515 UDEBUG(
"%s", filePath.c_str());
516 std::string tmpPath = filePath;
520 if(ext.compare(
"kml")!=0 && ext.compare(
"txt")!=0)
522 UERROR(
"Only txt and kml formats are supported!");
528 fopen_s(&fout, tmpPath.c_str(),
"w");
530 fout = fopen(tmpPath.c_str(),
"w");
534 if(ext.compare(
"kml")==0)
537 for(std::map<int, GPS>::const_iterator
iter=gpsValues.begin();
iter!=gpsValues.end(); ++
iter)
546 unsigned int abgr = 0xFF << 24 | (rgba & 0xFF) << 16 | (rgba & 0xFF00) | ((rgba >> 16) &0xFF);
548 std::string colorHexa =
uFormat(
"%08x", abgr);
550 fprintf(fout,
"<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n");
551 fprintf(fout,
"<kml xmlns=\"http://www.opengis.net/kml/2.2\">\n");
552 fprintf(fout,
"<Document>\n"
553 " <name>%s</name>\n", tmpPath.c_str());
554 fprintf(fout,
" <StyleMap id=\"msn_ylw-pushpin\">\n"
556 " <key>normal</key>\n"
557 " <styleUrl>#sn_ylw-pushpin</styleUrl>\n"
560 " <key>highlight</key>\n"
561 " <styleUrl>#sh_ylw-pushpin</styleUrl>\n"
564 " <Style id=\"sh_ylw-pushpin\">\n"
566 " <scale>1.2</scale>\n"
569 " <color>%s</color>\n"
572 " <Style id=\"sn_ylw-pushpin\">\n"
574 " <color>%s</color>\n"
576 " </Style>\n", colorHexa.c_str(), colorHexa.c_str());
577 fprintf(fout,
" <Placemark>\n"
579 " <styleUrl>#msn_ylw-pushpin</styleUrl>"
593 fprintf(fout,
"# stamp longitude latitude altitude error bearing\n");
594 for(std::map<int, GPS>::const_iterator
iter=gpsValues.begin();
iter!=gpsValues.end(); ++
iter)
596 fprintf(fout,
"%f %.*f %.*f %.*f %.*f %.*f\n",
597 iter->second.stamp(),
598 8,
iter->second.longitude(),
599 8,
iter->second.latitude(),
600 8,
iter->second.altitude(),
601 8,
iter->second.error(),
602 8,
iter->second.bearing());
613 float lengths[] = {100,200,300,400,500,600,700,800};
627 std::vector<float>
dist;
629 for (
unsigned int i=1;
i<poses.size();
i++) {
632 float dx = P1.
x()-P2.
x();
633 float dy = P1.
y()-P2.
y();
634 float dz = P1.
z()-P2.
z();
641 for (
unsigned int i=first_frame;
i<
dist.size();
i++)
648 float a = pose_error(0,0);
649 float b = pose_error(1,1);
650 float c = pose_error(2,2);
651 float d = 0.5*(
a+
b+
c-1.0);
656 float dx = pose_error.
x();
657 float dy = pose_error.
y();
658 float dz = pose_error.
z();
659 return sqrt(dx*dx+dy*dy+dz*dz);
663 const std::vector<Transform> &poses_gt,
664 const std::vector<Transform> &poses_result,
668 UASSERT(poses_gt.size() == poses_result.size());
671 std::vector<errors> err;
680 for (
unsigned int first_frame=0; first_frame<poses_gt.size(); first_frame+=step_size) {
696 Transform pose_delta_gt = poses_gt[first_frame].
inverse()*poses_gt[last_frame];
697 Transform pose_delta_result = poses_result[first_frame].
inverse()*poses_result[last_frame];
703 float num_frames = (
float)(last_frame-first_frame+1);
704 float speed =
len/(0.1*num_frames);
715 for (std::vector<errors>::iterator it=err.begin(); it!=err.end(); it++)
722 float num = err.size();
731 const std::vector<Transform> &poses_gt,
732 const std::vector<Transform> &poses_result,
736 UASSERT(poses_gt.size() == poses_result.size());
739 std::vector<errors> err;
742 for (
unsigned int i=0;
i<poses_gt.size()-1; ++
i)
748 float r_err = pose_error.
getAngle();
749 float t_err = pose_error.
getNorm();
752 err.push_back(
errors(
i,r_err,t_err,0,0));
759 for (std::vector<errors>::iterator it=err.begin(); it!=err.end(); it++)
766 float num = err.size();
773 const std::map<int, Transform> & groundTruth,
774 const std::map<int, Transform> & poses,
775 float & translational_rmse,
776 float & translational_mean,
777 float & translational_median,
778 float & translational_std,
779 float & translational_min,
780 float & translational_max,
781 float & rotational_rmse,
782 float & rotational_mean,
783 float & rotational_median,
784 float & rotational_std,
785 float & rotational_min,
786 float & rotational_max,
790 translational_rmse = 0.0f;
791 translational_mean = 0.0f;
792 translational_median = 0.0f;
793 translational_std = 0.0f;
794 translational_min = 0.0f;
795 translational_max = 0.0f;
797 rotational_rmse = 0.0f;
798 rotational_mean = 0.0f;
799 rotational_median = 0.0f;
800 rotational_std = 0.0f;
801 rotational_min = 0.0f;
802 rotational_max = 0.0f;
805 pcl::PointCloud<pcl::PointXYZ> cloud1, cloud2;
806 cloud1.resize(poses.size());
807 cloud2.resize(poses.size());
810 for(std::map<int, Transform>::const_iterator
iter=poses.begin();
iter!=poses.end(); ++
iter)
812 std::map<int, Transform>::const_iterator jter=groundTruth.find(
iter->first);
813 if(jter != groundTruth.end())
817 idFirst =
iter->first;
819 cloud1[oi] = pcl::PointXYZ(jter->second.x(), jter->second.y(), align2D?0:jter->second.z());
820 cloud2[oi++] = pcl::PointXYZ(
iter->second.x(),
iter->second.y(), align2D?0:
iter->second.z());
834 t = groundTruth.at(idFirst) * poses.at(idFirst).inverse();
837 std::vector<float> translationalErrors(poses.size());
838 std::vector<float> rotationalErrors(poses.size());
839 float sumTranslationalErrors = 0.0f;
840 float sumRotationalErrors = 0.0f;
841 float sumSqrdTranslationalErrors = 0.0f;
842 float sumSqrdRotationalErrors = 0.0f;
843 float radToDegree = 180.0f /
M_PI;
845 for(std::map<int, Transform>::const_iterator
iter=poses.begin();
iter!=poses.end(); ++
iter)
847 std::map<int, Transform>::const_iterator jter = groundTruth.find(
iter->first);
848 if(jter!=groundTruth.end())
851 Eigen::Vector3f xAxis(1,0,0);
853 Eigen::Vector3f
vB = jter->second.toEigen3f().linear()*xAxis;
854 double a = pcl::getAngle3D(Eigen::Vector4f(
vA[0],
vA[1],
vA[2], 0), Eigen::Vector4f(
vB[0],
vB[1],
vB[2], 0));
855 rotationalErrors[oi] =
a*radToDegree;
856 translationalErrors[oi] = pose.
getDistance(jter->second);
858 sumTranslationalErrors+=translationalErrors[oi];
859 sumSqrdTranslationalErrors+=translationalErrors[oi]*translationalErrors[oi];
860 sumRotationalErrors+=rotationalErrors[oi];
861 sumSqrdRotationalErrors+=rotationalErrors[oi]*rotationalErrors[oi];
865 translational_min = translational_max = translationalErrors[oi];
866 rotational_min = rotational_max = rotationalErrors[oi];
870 if(translationalErrors[oi] < translational_min)
872 translational_min = translationalErrors[oi];
874 else if(translationalErrors[oi] > translational_max)
876 translational_max = translationalErrors[oi];
879 if(rotationalErrors[oi] < rotational_min)
881 rotational_min = rotationalErrors[oi];
883 else if(rotationalErrors[oi] > rotational_max)
885 rotational_max = rotationalErrors[oi];
892 translationalErrors.resize(oi);
893 rotationalErrors.resize(oi);
896 float total =
float(oi);
897 translational_rmse =
std::sqrt(sumSqrdTranslationalErrors/total);
898 translational_mean = sumTranslationalErrors/total;
899 translational_median = translationalErrors[oi/2];
902 rotational_rmse =
std::sqrt(sumSqrdRotationalErrors/total);
903 rotational_mean = sumRotationalErrors/total;
904 rotational_median = rotationalErrors[oi/2];
911 const std::map<int, Transform> & poses,
912 const std::multimap<int, Link> & links,
913 float & maxLinearErrorRatio,
914 float & maxAngularErrorRatio,
915 float & maxLinearError,
916 float & maxAngularError,
917 const Link ** maxLinearErrorLink,
918 const Link ** maxAngularErrorLink,
921 maxLinearErrorRatio = -1;
922 maxAngularErrorRatio = -1;
924 maxAngularError = -1;
926 UDEBUG(
"poses=%d links=%d", (
int)poses.size(), (
int)links.size());
927 for(std::multimap<int, Link>::const_iterator
iter=links.begin();
iter!=links.end(); ++
iter)
930 if(
iter->second.from() !=
iter->second.to())
940 UWARN(
"Poses are null or not invertible, aborting optimized graph max error check! (Pose %d=%s Pose %d=%s)",
946 if(maxLinearErrorLink)
948 *maxLinearErrorLink = 0;
950 if(maxAngularErrorLink)
952 *maxAngularErrorLink = 0;
954 maxLinearErrorRatio = -1;
955 maxAngularErrorRatio = -1;
957 maxAngularError = -1;
963 float linearError =
uMax3(
966 force3DoF?0:
fabs(
iter->second.transform().z() -
t.z()));
968 float stddevLinear =
sqrt(
iter->second.transVariance(
false));
969 float linearErrorRatio = linearError/stddevLinear;
970 if(linearErrorRatio > maxLinearErrorRatio)
972 maxLinearError = linearError;
973 maxLinearErrorRatio = linearErrorRatio;
974 if(maxLinearErrorLink)
976 *maxLinearErrorLink = &
iter->second;
982 1.0 /
static_cast<double>(
iter->second.infMatrix().at<
double>(5,5)) < 9999.0)
984 float opt_roll,opt_pitch,opt_yaw;
985 float link_roll,link_pitch,link_yaw;
986 t.getEulerAngles(opt_roll, opt_pitch, opt_yaw);
987 iter->second.transform().getEulerAngles(link_roll, link_pitch, link_yaw);
988 float angularError =
uMax3(
989 force3DoF?0:
fabs(opt_roll - link_roll),
990 force3DoF?0:
fabs(opt_pitch - link_pitch),
991 fabs(opt_yaw - link_yaw));
992 angularError = angularError>
M_PI?2*
M_PI-angularError:angularError;
994 float stddevAngular =
sqrt(
iter->second.rotVariance(
false));
995 float angularErrorRatio = angularError/stddevAngular;
996 if(angularErrorRatio > maxAngularErrorRatio)
998 maxAngularError = angularError;
999 maxAngularErrorRatio = angularErrorRatio;
1000 if(maxAngularErrorLink)
1002 *maxAngularErrorLink = &
iter->second;
1012 std::vector<double> maxOdomInf(6,0.0);
1013 maxOdomInf.resize(6,0.0);
1014 for(std::multimap<int, Link>::const_iterator
iter=links.begin();
iter!=links.end(); ++
iter)
1018 for(
int i=0;
i<6; ++
i)
1020 const double &
v =
iter->second.infMatrix().at<
double>(
i,
i);
1021 if(maxOdomInf[
i] <
v)
1028 if(maxOdomInf[0] == 0.0)
1039 std::multimap<int, Link> & links,
1045 std::multimap<int, Link>::iterator
iter = links.find(from);
1046 while(
iter != links.end() &&
iter->first == from)
1058 iter = links.find(to);
1059 while(
iter != links.end() &&
iter->first == to)
1072 std::multimap<
int, std::pair<int, Link::Type> > & links,
1078 std::multimap<int, std::pair<int, Link::Type> >
::iterator iter = links.find(from);
1079 while(
iter != links.end() &&
iter->first == from)
1091 iter = links.find(to);
1092 while(
iter != links.end() &&
iter->first == to)
1105 std::multimap<int, int> & links,
1110 std::multimap<int, int>::iterator
iter = links.find(from);
1111 while(
iter != links.end() &&
iter->first == from)
1113 if(
iter->second == to)
1123 iter = links.find(to);
1124 while(
iter != links.end() &&
iter->first == to)
1126 if(
iter->second == from)
1136 const std::multimap<int, Link> & links,
1142 std::multimap<int, Link>::const_iterator
iter = links.find(from);
1143 while(
iter != links.end() &&
iter->first == from)
1155 iter = links.find(to);
1156 while(
iter != links.end() &&
iter->first == to)
1168 std::multimap<int, std::pair<int, Link::Type> >::const_iterator
findLink(
1169 const std::multimap<
int, std::pair<int, Link::Type> > & links,
1175 std::multimap<int, std::pair<int, Link::Type> >::const_iterator
iter = links.find(from);
1176 while(
iter != links.end() &&
iter->first == from)
1188 iter = links.find(to);
1189 while(
iter != links.end() &&
iter->first == to)
1202 const std::multimap<int, int> & links,
1207 std::multimap<int, int>::const_iterator
iter = links.find(from);
1208 while(
iter != links.end() &&
iter->first == from)
1210 if(
iter->second == to)
1220 iter = links.find(to);
1221 while(
iter != links.end() &&
iter->first == to)
1223 if(
iter->second == from)
1234 const std::multimap<int, Link> & links,
1237 std::list<Link> output;
1238 for(std::multimap<int, Link>::const_iterator
iter=links.begin();
iter != links.end(); ++
iter)
1240 if(
iter->second.from() == from)
1242 output.push_back(
iter->second);
1244 else if(
iter->second.to() == from)
1246 output.push_back(
iter->second.inverse());
1253 const std::multimap<int, Link> & links)
1255 std::multimap<int, Link> output;
1256 for(std::multimap<int, Link>::const_iterator
iter=links.begin();
iter!=links.end(); ++
iter)
1260 output.insert(*
iter);
1267 const std::multimap<int, Link> & links,
1271 std::multimap<int, Link> output;
1272 for(std::multimap<int, Link>::const_iterator
iter=links.begin();
iter!=links.end(); ++
iter)
1276 if((!inverted &&
iter->second.from() !=
iter->second.to())||
1277 (inverted &&
iter->second.from() ==
iter->second.to()))
1279 output.insert(*
iter);
1282 else if((!inverted &&
iter->second.type() != filteredType)||
1283 (inverted &&
iter->second.type() == filteredType))
1285 output.insert(*
iter);
1292 const std::map<int, Link> & links,
1296 std::map<int, Link> output;
1297 for(std::map<int, Link>::const_iterator
iter=links.begin();
iter!=links.end(); ++
iter)
1301 if((!inverted &&
iter->second.from() !=
iter->second.to())||
1302 (inverted &&
iter->second.from() ==
iter->second.to()))
1304 output.insert(*
iter);
1307 else if((!inverted &&
iter->second.type() != filteredType)||
1308 (inverted &&
iter->second.type() == filteredType))
1310 output.insert(*
iter);
1317 const std::map<int, Transform> & poses,
1319 float horizontalFOV,
1321 float nearClipPlaneDistance,
1322 float farClipPlaneDistance,
1325 std::map<int, Transform> output;
1329 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(
new pcl::PointCloud<pcl::PointXYZ>);
1330 std::vector<int> ids(poses.size());
1332 cloud->resize(poses.size());
1333 ids.resize(poses.size());
1335 for(std::map<int, rtabmap::Transform>::const_iterator
iter=poses.begin();
iter!=poses.end(); ++
iter)
1337 if(!
iter->second.isNull())
1339 (*cloud)[oi] = pcl::PointXYZ(
iter->second.x(),
iter->second.y(),
iter->second.z());
1340 ids[oi++] =
iter->first;
1348 pcl::IndicesPtr(
new std::vector<int>),
1352 nearClipPlaneDistance,
1353 farClipPlaneDistance,
1357 for(
unsigned int i=0;
i<
indices->size(); ++
i)
1359 output.insert(*poses.find(ids[
indices->at(
i)]));
1366 const std::map<int, Transform> & poses,
1371 if(poses.size() > 2 && radius > 0.0f)
1373 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(
new pcl::PointCloud<pcl::PointXYZ>);
1374 cloud->resize(poses.size());
1376 for(std::map<int, Transform>::const_iterator
iter = poses.begin();
iter!=poses.end(); ++
iter, ++
i)
1378 (*cloud)[
i] = pcl::PointXYZ(
iter->second.x(),
iter->second.y(),
iter->second.z());
1379 UASSERT_MSG(pcl::isFinite((*cloud)[
i]),
uFormat(
"Invalid pose (%d) %s",
iter->first,
iter->second.prettyPrint().c_str()).c_str());
1384 std::vector<Transform> transforms =
uValues(poses);
1386 pcl::search::KdTree<pcl::PointXYZ>::Ptr
tree (
new pcl::search::KdTree<pcl::PointXYZ> (
false));
1387 tree->setInputCloud(cloud);
1388 std::set<int> indicesChecked;
1389 std::set<int> indicesKept;
1391 for(
unsigned int i=0;
i<cloud->size(); ++
i)
1393 if(indicesChecked.find(
i) == indicesChecked.end())
1396 std::vector<float> kDistances;
1397 tree->radiusSearch(cloud->at(
i), radius,
kIndices, kDistances);
1399 std::set<int> cloudIndices;
1400 const Transform & currentT = transforms.at(
i);
1404 if(indicesChecked.find(
kIndices[
j]) == indicesChecked.end())
1411 double a = pcl::getAngle3D(Eigen::Vector4f(
vA[0],
vA[1],
vA[2], 0), Eigen::Vector4f(
vB[0],
vB[1],
vB[2], 0));
1426 bool lastAdded =
false;
1427 for(std::set<int>::reverse_iterator
iter = cloudIndices.rbegin();
iter!=cloudIndices.rend(); ++
iter)
1431 indicesKept.insert(*
iter);
1434 indicesChecked.insert(*
iter);
1439 bool firstAdded =
false;
1440 for(std::set<int>::iterator
iter = cloudIndices.begin();
iter!=cloudIndices.end(); ++
iter)
1444 indicesKept.insert(*
iter);
1447 indicesChecked.insert(*
iter);
1455 UINFO(
"Cloud filtered In = %d, Out = %d (radius=%f angle=%f keepLatest=%d)", cloud->size(), indicesKept.size(), radius,
angle, keepLatest?1:0);
1459 std::map<int, Transform> keptPoses;
1460 for(std::set<int>::iterator
iter = indicesKept.begin();
iter!=indicesKept.end(); ++
iter)
1462 keptPoses.insert(std::make_pair(
names.at(*
iter), transforms.at(*
iter)));
1466 keptPoses.insert(*poses.begin());
1467 keptPoses.insert(*poses.rbegin());
1479 std::multimap<int, int> clusters;
1480 if(poses.size() > 1 && radius > 0.0f)
1482 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(
new pcl::PointCloud<pcl::PointXYZ>);
1483 cloud->resize(poses.size());
1485 for(std::map<int, Transform>::const_iterator
iter = poses.begin();
iter!=poses.end(); ++
iter, ++
i)
1487 (*cloud)[
i] = pcl::PointXYZ(
iter->second.x(),
iter->second.y(),
iter->second.z());
1488 UASSERT_MSG(pcl::isFinite((*cloud)[
i]),
uFormat(
"Invalid pose (%d) %s",
iter->first,
iter->second.prettyPrint().c_str()).c_str());
1492 std::vector<int> ids =
uKeys(poses);
1493 std::vector<Transform> transforms =
uValues(poses);
1495 pcl::search::KdTree<pcl::PointXYZ>::Ptr
tree (
new pcl::search::KdTree<pcl::PointXYZ> (
false));
1496 tree->setInputCloud(cloud);
1498 for(
unsigned int i=0;
i<cloud->size(); ++
i)
1501 std::vector<float> kDistances;
1502 tree->radiusSearch(cloud->at(
i), radius,
kIndices, kDistances);
1504 std::set<int> cloudIndices;
1505 const Transform & currentT = transforms.at(
i);
1516 double a = pcl::getAngle3D(Eigen::Vector4f(
vA[0],
vA[1],
vA[2], 0), Eigen::Vector4f(
vB[0],
vB[1],
vB[2], 0));
1519 clusters.insert(std::make_pair(ids[
i], ids[
kIndices[
j]]));
1524 clusters.insert(std::make_pair(ids[
i], ids[
kIndices[
j]]));
1534 const std::map<int, Transform> & poses,
1535 const std::multimap<int, Link> & links,
1536 std::multimap<int, int> & hyperNodes,
1537 std::multimap<int, Link> & hyperLinks)
1539 UINFO(
"Input: poses=%d links=%d", (
int)poses.size(), (
int)links.size());
1541 std::map<int, int> posesToHyperNodes;
1542 std::map<int, std::multimap<int, Link> > clusterloopClosureLinks;
1545 std::multimap<int, Link> bidirectionalLoopClosureLinks;
1546 for(std::multimap<int, Link>::const_iterator jter=links.begin(); jter!=links.end(); ++jter)
1550 jter->second.userDataCompressed().empty())
1552 if(
uContains(poses, jter->second.from()) &&
1555 UASSERT_MSG(
graph::findLink(links, jter->second.to(), jter->second.from(),
false) == links.end(),
"Input links should be unique!");
1556 bidirectionalLoopClosureLinks.insert(std::make_pair(jter->second.from(), jter->second));
1562 UINFO(
"Clustering hyper nodes...");
1564 for(std::map<int, Transform>::const_reverse_iterator
iter=poses.rbegin();
iter!=poses.rend(); ++
iter)
1566 if(posesToHyperNodes.find(
iter->first) == posesToHyperNodes.end())
1568 int hyperNodeId =
iter->first;
1569 std::list<int> loopClosures;
1570 std::set<int> loopClosuresAdded;
1571 loopClosures.push_back(
iter->first);
1572 std::multimap<int, Link> clusterLinks;
1573 while(loopClosures.size())
1575 int id = loopClosures.front();
1576 loopClosures.pop_front();
1578 UASSERT(posesToHyperNodes.find(
id) == posesToHyperNodes.end());
1580 posesToHyperNodes.insert(std::make_pair(
id, hyperNodeId));
1581 hyperNodes.insert(std::make_pair(hyperNodeId,
id));
1583 for(std::multimap<int, Link>::const_iterator jter=bidirectionalLoopClosureLinks.find(
id); jter!=bidirectionalLoopClosureLinks.end() && jter->first==
id; ++jter)
1585 if(posesToHyperNodes.find(jter->second.to()) == posesToHyperNodes.end() &&
1586 loopClosuresAdded.find(jter->second.to()) == loopClosuresAdded.end())
1588 loopClosures.push_back(jter->second.to());
1589 loopClosuresAdded.insert(jter->second.to());
1590 clusterLinks.insert(*jter);
1591 clusterLinks.insert(std::make_pair(jter->second.to(), jter->second.inverse()));
1592 if(jter->second.from() < jter->second.to())
1594 UWARN(
"Child to Parent link? %d->%d (type=%d)",
1595 jter->second.from(),
1597 jter->second.type());
1602 UASSERT(clusterloopClosureLinks.find(hyperNodeId) == clusterloopClosureLinks.end());
1603 clusterloopClosureLinks.insert(std::make_pair(hyperNodeId, clusterLinks));
1604 UDEBUG(
"Created hyper node %d with %d children (%f%%)",
1605 hyperNodeId, (
int)loopClosuresAdded.size(),
float(posesToHyperNodes.size())/
float(poses.size())*100.0f);
1608 UINFO(
"Clustering hyper nodes... done! (%f s)",
timer.ticks());
1611 UINFO(
"Creating hyper links...");
1613 for(std::multimap<int, Link>::const_reverse_iterator jter=links.rbegin(); jter!=links.rend(); ++jter)
1617 !jter->second.userDataCompressed().empty()) &&
1618 uContains(poses, jter->second.from()) &&
1621 UASSERT_MSG(
uContains(posesToHyperNodes, jter->second.from()),
uFormat(
"%d->%d (type=%d)", jter->second.from(), jter->second.to(), jter->second.type()).c_str());
1622 UASSERT_MSG(
uContains(posesToHyperNodes, jter->second.to()),
uFormat(
"%d->%d (type=%d)", jter->second.from(), jter->second.to(), jter->second.type()).c_str());
1623 int hyperNodeIDFrom = posesToHyperNodes.at(jter->second.from());
1624 int hyperNodeIDTo = posesToHyperNodes.at(jter->second.to());
1627 if(hyperNodeIDFrom != hyperNodeIDTo)
1629 std::multimap<int, Link>::iterator tmpIter =
graph::findLink(hyperLinks, hyperNodeIDFrom, hyperNodeIDTo);
1630 if(tmpIter!=hyperLinks.end() &&
1631 hyperNodeIDFrom == jter->second.from() &&
1632 hyperNodeIDTo == jter->second.to() &&
1637 hyperLinks.erase(tmpIter);
1641 if(
graph::findLink(hyperLinks, hyperNodeIDFrom, hyperNodeIDTo) == hyperLinks.end())
1643 UASSERT(clusterloopClosureLinks.find(hyperNodeIDFrom) != clusterloopClosureLinks.end());
1644 UASSERT(clusterloopClosureLinks.find(hyperNodeIDTo) != clusterloopClosureLinks.end());
1645 std::multimap<int, Link> tmpLinks = clusterloopClosureLinks.at(hyperNodeIDFrom);
1646 tmpLinks.insert(clusterloopClosureLinks.at(hyperNodeIDTo).begin(), clusterloopClosureLinks.at(hyperNodeIDTo).end());
1647 tmpLinks.insert(std::make_pair(jter->second.from(), jter->second));
1649 std::list<int>
path =
computePath(tmpLinks, hyperNodeIDFrom, hyperNodeIDTo,
false,
true);
1651 uFormat(
"path.size()=%d, hyperNodeIDFrom=%d, hyperNodeIDTo=%d",
1654 hyperNodeIDTo).c_str());
1656 if(
path.size() > 10)
1658 UWARN(
"Large path! %d nodes", (
int)
path.size());
1659 std::stringstream
stream;
1673 std::list<int>::iterator
iter=
path.begin();
1677 std::multimap<int, Link>::const_iterator foundIter =
graph::findLink(tmpLinks, from, to,
false);
1678 UASSERT(foundIter != tmpLinks.end());
1679 Link hyperLink = foundIter->second;
1685 std::multimap<int, Link>::const_iterator foundIter =
graph::findLink(tmpLinks, from, to,
false);
1686 UASSERT(foundIter != tmpLinks.end());
1687 hyperLink = hyperLink.
merge(foundIter->second, jter->second.type());
1693 UASSERT(hyperLink.
to() == hyperNodeIDTo);
1694 hyperLinks.insert(std::make_pair(hyperNodeIDFrom, hyperLink));
1696 UDEBUG(
"Created hyper link %d->%d (%f%%)",
1697 hyperLink.
from(), hyperLink.
to(),
float(
i)/
float(links.size())*100.0f);
1700 UWARN(
"Large hyper link %d->%d (%f m)! original %d->%d (%f m)",
1704 jter->second.from(),
1706 jter->second.transform().getNorm());
1714 UINFO(
"Creating hyper links... done! (%f s)",
timer.ticks());
1760 typedef std::pair<int, float>
Pair;
1765 return a.second >
b.second;
1771 const std::map<int, rtabmap::Transform> & poses,
1772 const std::multimap<int, int> & links,
1775 bool updateNewCosts)
1777 std::list<std::pair<int, Transform> >
path;
1779 int startNode = from;
1782 std::map<int, Node>
nodes;
1783 nodes.insert(std::make_pair(startNode,
Node(startNode, 0, poses.at(startNode))));
1784 std::priority_queue<Pair, std::vector<Pair>,
Order> pq;
1785 std::multimap<float, int> pqmap;
1788 pqmap.insert(std::make_pair(0, startNode));
1792 pq.push(
Pair(startNode, 0));
1795 while((updateNewCosts && pqmap.size()) || (!updateNewCosts && pq.size()))
1800 currentNode = &
nodes.find(pqmap.begin()->second)->second;
1801 pqmap.erase(pqmap.begin());
1805 currentNode = &
nodes.find(pq.top().first)->second;
1811 if(currentNode->
id() == endNode)
1813 while(currentNode->
id()!=startNode)
1815 path.push_front(std::make_pair(currentNode->
id(), currentNode->
pose()));
1816 currentNode = &
nodes.find(currentNode->
fromId())->second;
1818 path.push_front(std::make_pair(startNode, poses.at(startNode)));
1823 for(std::multimap<int, int>::const_iterator
iter = links.find(currentNode->
id());
1824 iter!=links.end() &&
iter->first == currentNode->
id();
1827 std::map<int, Node>::iterator nodeIter =
nodes.find(
iter->second);
1828 if(nodeIter ==
nodes.end())
1830 std::map<int, rtabmap::Transform>::const_iterator poseIter = poses.find(
iter->second);
1831 if(poseIter == poses.end())
1833 UERROR(
"Next pose %d (from %d) should be found in poses! Ignoring it!",
iter->second,
iter->first);
1837 Node n(
iter->second, currentNode->
id(), poseIter->second);
1839 n.setDistToEnd(
n.distFrom(endPose));
1841 nodes.insert(std::make_pair(
iter->second,
n));
1844 pqmap.insert(std::make_pair(
n.totalCost(),
n.id()));
1848 pq.push(
Pair(
n.id(),
n.totalCost()));
1852 else if(updateNewCosts && nodeIter->second.isOpened())
1854 float newCostSoFar = currentNode->
costSoFar() + currentNode->
distFrom(nodeIter->second.pose());
1855 if(nodeIter->second.costSoFar() > newCostSoFar)
1858 for(std::multimap<float, int>::iterator mapIter=pqmap.begin(); mapIter!=pqmap.end(); ++mapIter)
1860 if(mapIter->second == nodeIter->first)
1862 pqmap.erase(mapIter);
1863 nodeIter->second.setCostSoFar(newCostSoFar);
1864 pqmap.insert(std::make_pair(nodeIter->second.totalCost(), nodeIter->first));
1877 const std::multimap<int, Link> & links,
1880 bool updateNewCosts,
1881 bool useSameCostForAllLinks)
1883 std::list<int>
path;
1885 int startNode = from;
1887 std::map<int, Node>
nodes;
1889 std::priority_queue<Pair, std::vector<Pair>,
Order> pq;
1890 std::multimap<float, int> pqmap;
1893 pqmap.insert(std::make_pair(0, startNode));
1897 pq.push(
Pair(startNode, 0));
1900 while((updateNewCosts && pqmap.size()) || (!updateNewCosts && pq.size()))
1905 currentNode = &
nodes.find(pqmap.begin()->second)->second;
1906 pqmap.erase(pqmap.begin());
1910 currentNode = &
nodes.find(pq.top().first)->second;
1916 if(currentNode->
id() == endNode)
1918 while(currentNode->
id()!=startNode)
1920 path.push_front(currentNode->
id());
1921 currentNode = &
nodes.find(currentNode->
fromId())->second;
1923 path.push_front(startNode);
1928 for(std::multimap<int, Link>::const_iterator
iter = links.find(currentNode->
id());
1929 iter!=links.end() &&
iter->first == currentNode->
id();
1932 std::map<int, Node>::iterator nodeIter =
nodes.find(
iter->second.to());
1934 if(!useSameCostForAllLinks)
1936 cost =
iter->second.transform().getNorm();
1938 if(nodeIter ==
nodes.end())
1942 n.setCostSoFar(currentNode->
costSoFar() + cost);
1943 nodes.insert(std::make_pair(
iter->second.to(),
n));
1946 pqmap.insert(std::make_pair(
n.totalCost(),
n.id()));
1950 pq.push(
Pair(
n.id(),
n.totalCost()));
1953 else if(!useSameCostForAllLinks && updateNewCosts && nodeIter->second.isOpened())
1955 float newCostSoFar = currentNode->
costSoFar() + cost;
1956 if(nodeIter->second.costSoFar() > newCostSoFar)
1959 for(std::multimap<float, int>::iterator mapIter=pqmap.begin(); mapIter!=pqmap.end(); ++mapIter)
1961 if(mapIter->second == nodeIter->first)
1963 pqmap.erase(mapIter);
1964 nodeIter->second.setCostSoFar(newCostSoFar);
1965 pqmap.insert(std::make_pair(nodeIter->second.totalCost(), nodeIter->first));
1982 bool lookInDatabase,
1983 bool updateNewCosts,
1984 float linearVelocity,
1985 float angularVelocity)
1990 std::list<std::pair<int, Transform> >
path;
1991 UDEBUG(
"fromId=%d, toId=%d, lookInDatabase=%d, updateNewCosts=%d, linearVelocity=%f, angularVelocity=%f",
1999 std::multimap<int, Link> allLinks;
2004 allLinks = memory->
getAllLinks(lookInDatabase,
true,
true);
2005 for(std::multimap<int, Link>::iterator
iter=allLinks.begin();
iter!=allLinks.end(); ++
iter)
2007 if(
iter->second.to() < 0)
2009 allLinks.insert(std::make_pair(
iter->second.to(),
iter->second.inverse()));
2012 UINFO(
"getting all %d links time = %f s", (
int)allLinks.size(),
t.ticks());
2016 int startNode = fromId;
2018 std::map<int, Node>
nodes;
2020 std::priority_queue<Pair, std::vector<Pair>,
Order> pq;
2021 std::multimap<float, int> pqmap;
2024 pqmap.insert(std::make_pair(0, startNode));
2028 pq.push(
Pair(startNode, 0));
2031 while((updateNewCosts && pqmap.size()) || (!updateNewCosts && pq.size()))
2036 currentNode = &
nodes.find(pqmap.begin()->second)->second;
2037 pqmap.erase(pqmap.begin());
2041 currentNode = &
nodes.find(pq.top().first)->second;
2047 if(currentNode->
id() == endNode)
2049 while(currentNode->
id()!=startNode)
2051 path.push_front(std::make_pair(currentNode->
id(), currentNode->
pose()));
2052 currentNode = &
nodes.find(currentNode->
fromId())->second;
2054 path.push_front(std::make_pair(startNode, currentNode->
pose()));
2059 std::multimap<int, Link> links;
2060 if(allLinks.size() == 0)
2062 links = memory->
getLinks(currentNode->
id(), lookInDatabase,
true);
2066 for(std::multimap<int, Link>::const_iterator
iter = allLinks.lower_bound(currentNode->
id());
2067 iter!=allLinks.end() &&
iter->first == currentNode->
id();
2070 links.insert(std::make_pair(
iter->second.to(),
iter->second));
2073 for(std::multimap<int, Link>::const_iterator
iter = links.begin();
iter!=links.end(); ++
iter)
2075 if(
iter->second.from() !=
iter->second.to())
2079 if(linearVelocity <= 0.0
f && angularVelocity <= 0.0
f)
2082 cost =
iter->second.transform().getNorm();
2086 if(linearVelocity > 0.0
f)
2088 cost +=
iter->second.transform().getNorm()/linearVelocity;
2090 if(angularVelocity > 0.0
f)
2092 Eigen::Vector4f
v1 = Eigen::Vector4f(nextPose.
x()-currentNode->
pose().
x(), nextPose.
y()-currentNode->
pose().
y(), nextPose.
z()-currentNode->
pose().
z(), 1.0f);
2095 cost +=
angle / angularVelocity;
2099 std::map<int, Node>::iterator nodeIter =
nodes.find(
iter->first);
2100 if(nodeIter ==
nodes.end())
2102 Node n(
iter->second.to(), currentNode->
id(), nextPose);
2104 n.setCostSoFar(currentNode->
costSoFar() + cost);
2105 nodes.insert(std::make_pair(
iter->second.to(),
n));
2108 pqmap.insert(std::make_pair(
n.totalCost(),
n.id()));
2112 pq.push(
Pair(
n.id(),
n.totalCost()));
2115 else if(updateNewCosts && nodeIter->second.isOpened())
2117 float newCostSoFar = currentNode->
costSoFar() + cost;
2118 if(nodeIter->second.costSoFar() > newCostSoFar)
2121 nodeIter->second.setPose(nextPose);
2124 for(std::multimap<float, int>::iterator mapIter=pqmap.begin(); mapIter!=pqmap.end(); ++mapIter)
2126 if(mapIter->second == nodeIter->first)
2128 pqmap.erase(mapIter);
2129 nodeIter->second.setCostSoFar(newCostSoFar);
2130 pqmap.insert(std::make_pair(nodeIter->second.totalCost(), nodeIter->first));
2143 std::stringstream
stream;
2145 std::list<std::pair<int, Transform> >::const_iterator previousIter =
path.end();
2147 for(std::list<std::pair<int, Transform> >::const_iterator
iter=
path.begin();
iter!=
path.end();++
iter)
2154 if(previousIter!=
path.end())
2159 std::multimap<int, Link>::iterator jter =
graph::findLink(allLinks, previousIter->first,
iter->first);
2160 if(jter != allLinks.end())
2170 ++linkTypes[jter->second.type()];
2171 stream <<
"[" << jter->second.type() <<
"]";
2172 length += jter->second.transform().getNorm();
2182 std::stringstream streamB;
2183 for(
unsigned int i=0;
i<linkTypes.size(); ++
i)
2189 streamB <<
i <<
"=" << linkTypes[
i];
2191 UDEBUG(
"Link types = %s", streamB.str().c_str());
2198 const std::map<int, rtabmap::Transform> & poses,
2203 std::map<int, float> nearestNodes =
findNearestNodes(targetPose, poses, 0, 0, 1);
2204 if(!nearestNodes.empty())
2206 id = nearestNodes.begin()->first;
2209 *
distance = nearestNodes.begin()->second;
2218 const std::map<int, Transform> & poses,
2225 std::map<int, Transform> nodesMinusTarget = poses;
2226 Transform targetPose = poses.at(nodeId);
2227 nodesMinusTarget.erase(nodeId);
2234 const std::map<int, Transform> & poses,
2242 std::map<int, float> foundNodes;
2248 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(
new pcl::PointCloud<pcl::PointXYZ>);
2249 cloud->resize(poses.size());
2250 std::vector<int> ids(poses.size());
2252 for(std::map<int, Transform>::const_iterator
iter = poses.begin();
iter!=poses.end(); ++
iter)
2254 (*cloud)[oi] = pcl::PointXYZ(
iter->second.x(),
iter->second.y(),
iter->second.z());
2255 UASSERT_MSG(pcl::isFinite((*cloud)[oi]),
uFormat(
"Invalid pose (%d) %s",
iter->first,
iter->second.prettyPrint().c_str()).c_str());
2256 ids[oi] =
iter->first;
2264 pcl::search::KdTree<pcl::PointXYZ>::Ptr kdTree(
new pcl::search::KdTree<pcl::PointXYZ>);
2265 kdTree->setInputCloud(cloud);
2266 std::vector<int>
ind;
2267 std::vector<float> sqrdDist;
2268 pcl::PointXYZ pt(targetPose.
x(), targetPose.
y(), targetPose.
z());
2271 kdTree->radiusSearch(pt, radius,
ind, sqrdDist, k);
2275 kdTree->nearestKSearch(pt, k,
ind, sqrdDist);
2278 for(
unsigned int i=0;
i<
ind.size(); ++
i)
2287 double a = pcl::getAngle3D(Eigen::Vector4f(
vA[0],
vA[1],
vA[2], 0), Eigen::Vector4f(
vB[0],
vB[1],
vB[2], 0));
2290 foundNodes.insert(std::make_pair(ids[
ind[
i]], sqrdDist[
i]));
2295 foundNodes.insert(std::make_pair(ids[
ind[
i]], sqrdDist[
i]));
2300 UDEBUG(
"found nodes=%d", (
int)foundNodes.size());
2307 const std::map<int, Transform> & poses,
2314 std::map<int, Transform> nodesMinusTarget = poses;
2315 Transform targetPose = poses.at(nodeId);
2316 nodesMinusTarget.erase(nodeId);
2322 const std::map<int, Transform> & poses,
2328 std::map<int, Transform> foundPoses;
2329 for(std::map<int, float>::iterator
iter=nearestNodes.begin();
iter!=nearestNodes.end(); ++
iter)
2331 foundPoses.insert(*poses.find(
iter->first));
2333 UDEBUG(
"found nodes=%d/%d (radius=%f, angle=%f, k=%d)", (
int)foundPoses.size(), (
int)poses.size(), radius,
angle, k);
2342 std::map<int, float>
getNodesInRadius(
int nodeId,
const std::map<int, Transform> & nodes,
float radius)
2350 std::map<int, Transform>
getPosesInRadius(
int nodeId,
const std::map<int, Transform> & nodes,
float radius,
float angle)
2361 const std::vector<std::pair<int, Transform> > & path,
2362 unsigned int fromIndex,
2363 unsigned int toIndex)
2368 UASSERT(fromIndex <
path.size() && toIndex <
path.size() && fromIndex <= toIndex);
2369 if(fromIndex >= toIndex)
2371 toIndex = (
unsigned int)
path.size()-1;
2373 float x=0,
y=0,
z=0;
2374 for(
unsigned int i=fromIndex;
i<toIndex-1; ++
i)
2386 const std::map<int, Transform> & path)
2391 float x=0,
y=0,
z=0;
2392 std::map<int, Transform>::const_iterator
iter=
path.begin();
2398 x +=
fabs(previousPose.
x() - currentPose.
x());
2399 y +=
fabs(previousPose.
y() - currentPose.
y());
2400 z +=
fabs(previousPose.
z() - currentPose.
z());
2401 previousPose = currentPose;
2410 std::map<int, Transform> poses,
2411 const std::multimap<int, Link> & links)
2413 std::list<std::map<int, Transform> > paths;
2414 if(poses.size() && links.size())
2419 std::map<int, Transform>
path;
2420 for(std::map<int, Transform>::iterator
iter=poses.begin();
iter!=poses.end();)
2422 std::multimap<int, Link>::const_iterator jter =
findLink(links,
path.rbegin()->first,
iter->first);
2426 poses.erase(
iter++);
2434 paths.push_back(
path);
2447 min[0] =
max[0] = poses.begin()->second.x();
2448 min[1] =
max[1] = poses.begin()->second.y();
2449 min[2] =
max[2] = poses.begin()->second.z();
2450 for(std::map<int, Transform>::const_iterator
iter=poses.begin();
iter!=poses.end(); ++
iter)