29 #include "rtabmap/core/Version.h" 51 #include <pcl/search/kdtree.h> 52 #include <pcl/filters/crop_box.h> 53 #include <pcl/io/pcd_io.h> 54 #include <pcl/common/common.h> 55 #include <pcl/TextureMesh.h> 60 #define LOG_F "LogF.txt" 61 #define LOG_I "LogI.txt" 63 #define GRAPH_FILE_NAME "Graph.dot" 80 _publishStats(
Parameters::defaultRtabmapPublishStats()),
81 _publishLastSignatureData(
Parameters::defaultRtabmapPublishLastSignature()),
82 _publishPdf(
Parameters::defaultRtabmapPublishPdf()),
83 _publishLikelihood(
Parameters::defaultRtabmapPublishLikelihood()),
84 _publishRAMUsage(
Parameters::defaultRtabmapPublishRAMUsage()),
85 _computeRMSE(
Parameters::defaultRtabmapComputeRMSE()),
86 _saveWMState(
Parameters::defaultRtabmapSaveWMState()),
87 _maxTimeAllowed(
Parameters::defaultRtabmapTimeThr()),
88 _maxMemoryAllowed(
Parameters::defaultRtabmapMemoryThr()),
90 _loopRatio(
Parameters::defaultRtabmapLoopRatio()),
91 _maxLoopClosureDistance(
Parameters::defaultRGBDMaxLoopClosureDistance()),
92 _verifyLoopClosureHypothesis(
Parameters::defaultVhEpEnabled()),
93 _maxRetrieved(
Parameters::defaultRtabmapMaxRetrieved()),
94 _maxLocalRetrieved(
Parameters::defaultRGBDMaxLocalRetrieved()),
95 _rawDataKept(
Parameters::defaultMemImageKept()),
96 _statisticLogsBufferedInRAM(
Parameters::defaultRtabmapStatisticLogsBufferedInRAM()),
97 _statisticLogged(
Parameters::defaultRtabmapStatisticLogged()),
98 _statisticLoggedHeaders(
Parameters::defaultRtabmapStatisticLoggedHeaders()),
99 _rgbdSlamMode(
Parameters::defaultRGBDEnabled()),
100 _rgbdLinearUpdate(
Parameters::defaultRGBDLinearUpdate()),
101 _rgbdAngularUpdate(
Parameters::defaultRGBDAngularUpdate()),
102 _rgbdLinearSpeedUpdate(
Parameters::defaultRGBDLinearSpeedUpdate()),
103 _rgbdAngularSpeedUpdate(
Parameters::defaultRGBDAngularSpeedUpdate()),
104 _newMapOdomChangeDistance(
Parameters::defaultRGBDNewMapOdomChangeDistance()),
105 _neighborLinkRefining(
Parameters::defaultRGBDNeighborLinkRefining()),
106 _proximityByTime(
Parameters::defaultRGBDProximityByTime()),
107 _proximityBySpace(
Parameters::defaultRGBDProximityBySpace()),
108 _scanMatchingIdsSavedInLinks(
Parameters::defaultRGBDScanMatchingIdsSavedInLinks()),
109 _localRadius(
Parameters::defaultRGBDLocalRadius()),
110 _localImmunizationRatio(
Parameters::defaultRGBDLocalImmunizationRatio()),
111 _proximityMaxGraphDepth(
Parameters::defaultRGBDProximityMaxGraphDepth()),
112 _proximityMaxPaths(
Parameters::defaultRGBDProximityMaxPaths()),
113 _proximityMaxNeighbors(
Parameters::defaultRGBDProximityPathMaxNeighbors()),
114 _proximityFilteringRadius(
Parameters::defaultRGBDProximityPathFilteringRadius()),
115 _proximityRawPosesUsed(
Parameters::defaultRGBDProximityPathRawPosesUsed()),
117 _proximityOdomGuess(
Parameters::defaultRGBDProximityOdomGuess()),
119 _optimizeFromGraphEnd(
Parameters::defaultRGBDOptimizeFromGraphEnd()),
120 _optimizationMaxError(
Parameters::defaultRGBDOptimizeMaxError()),
121 _startNewMapOnLoopClosure(
Parameters::defaultRtabmapStartNewMapOnLoopClosure()),
122 _startNewMapOnGoodSignature(
Parameters::defaultRtabmapStartNewMapOnGoodSignature()),
123 _goalReachedRadius(
Parameters::defaultRGBDGoalReachedRadius()),
124 _goalsSavedInUserData(
Parameters::defaultRGBDGoalsSavedInUserData()),
125 _pathStuckIterations(
Parameters::defaultRGBDPlanStuckIterations()),
126 _pathLinearVelocity(
Parameters::defaultRGBDPlanLinearVelocity()),
127 _pathAngularVelocity(
Parameters::defaultRGBDPlanAngularVelocity()),
128 _savedLocalizationIgnored(
Parameters::defaultRGBDSavedLocalizationIgnored()),
129 _loopCovLimited(
Parameters::defaultRGBDLoopCovLimited()),
130 _loopGPS(
Parameters::defaultRtabmapLoopGPS()),
131 _maxOdomCacheSize(
Parameters::defaultRGBDMaxOdomCacheSize()),
132 _loopClosureHypothesis(0,0.0
f),
133 _highestHypothesis(0,0.0
f),
134 _lastProcessTime(0.0),
135 _someNodesHaveBeenTransferred(
false),
136 _distanceTravelled(0.0
f),
137 _distanceTravelledSinceLastLocalization(0.0
f),
138 _optimizeFromGraphEndChanged(
false),
139 _epipolarGeometry(0),
146 _mapCorrection(
Transform::getIdentity()),
147 _lastLocalizationNodeId(0),
148 _currentSessionHasGPS(
false),
149 _odomCorrectionAcc(6,0),
151 _pathCurrentIndex(0),
153 _pathTransformToGoal(
Transform::getIdentity()),
155 _pathStuckDistance(0.0
f)
181 std::string attributes =
"a+";
203 fprintf(_foutFloat,
"Column headers:\n");
204 fprintf(_foutFloat,
" 1-Total iteration time (s)\n");
205 fprintf(_foutFloat,
" 2-Memory update time (s)\n");
206 fprintf(_foutFloat,
" 3-Retrieval time (s)\n");
207 fprintf(_foutFloat,
" 4-Likelihood time (s)\n");
208 fprintf(_foutFloat,
" 5-Posterior time (s)\n");
209 fprintf(_foutFloat,
" 6-Hypothesis selection time (s)\n");
210 fprintf(_foutFloat,
" 7-Hypothesis validation time (s)\n");
211 fprintf(_foutFloat,
" 8-Transfer time (s)\n");
212 fprintf(_foutFloat,
" 9-Statistics creation time (s)\n");
213 fprintf(_foutFloat,
" 10-Loop closure hypothesis value\n");
214 fprintf(_foutFloat,
" 11-NAN\n");
215 fprintf(_foutFloat,
" 12-NAN\n");
216 fprintf(_foutFloat,
" 13-NAN\n");
217 fprintf(_foutFloat,
" 14-NAN\n");
218 fprintf(_foutFloat,
" 15-NAN\n");
219 fprintf(_foutFloat,
" 16-Virtual place hypothesis\n");
220 fprintf(_foutFloat,
" 17-Join trash time (s)\n");
221 fprintf(_foutFloat,
" 18-Weight Update (rehearsal) similarity\n");
222 fprintf(_foutFloat,
" 19-Empty trash time (s)\n");
223 fprintf(_foutFloat,
" 20-Retrieval database access time (s)\n");
224 fprintf(_foutFloat,
" 21-Add loop closure link time (s)\n");
225 fprintf(_foutFloat,
" 22-Memory cleanup time (s)\n");
226 fprintf(_foutFloat,
" 23-Scan matching (odometry correction) time (s)\n");
227 fprintf(_foutFloat,
" 24-Local time loop closure detection time (s)\n");
228 fprintf(_foutFloat,
" 25-Local space loop closure detection time (s)\n");
229 fprintf(_foutFloat,
" 26-Map optimization (s)\n");
233 fprintf(_foutInt,
"Column headers:\n");
234 fprintf(_foutInt,
" 1-Loop closure ID\n");
235 fprintf(_foutInt,
" 2-Highest loop closure hypothesis\n");
236 fprintf(_foutInt,
" 3-Locations transferred\n");
237 fprintf(_foutInt,
" 4-NAN\n");
238 fprintf(_foutInt,
" 5-Words extracted from the last image\n");
239 fprintf(_foutInt,
" 6-Vocabulary size\n");
240 fprintf(_foutInt,
" 7-Working memory size\n");
241 fprintf(_foutInt,
" 8-Is loop closure hypothesis rejected?\n");
242 fprintf(_foutInt,
" 9-NAN\n");
243 fprintf(_foutInt,
" 10-NAN\n");
244 fprintf(_foutInt,
" 11-Locations retrieved\n");
245 fprintf(_foutInt,
" 12-Retrieval location ID\n");
246 fprintf(_foutInt,
" 13-Unique words extraced from last image\n");
247 fprintf(_foutInt,
" 14-Retrieval ID\n");
248 fprintf(_foutInt,
" 15-Non-null likelihood values\n");
249 fprintf(_foutInt,
" 16-Weight Update ID\n");
250 fprintf(_foutInt,
" 17-Is last location merged through Weight Update?\n");
251 fprintf(_foutInt,
" 18-Local graph size\n");
252 fprintf(_foutInt,
" 19-Sensor data id\n");
253 fprintf(_foutInt,
" 20-Indexed words\n");
254 fprintf(_foutInt,
" 21-Index memory usage (KB)\n");
264 UWARN(
"Working directory is not set, log disabled!");
286 fprintf(
_foutInt,
"%s", iter->c_str());
294 UDEBUG(
"path=%s", databasePath.c_str());
296 if(!_databasePath.empty())
299 UINFO(
"Using database \"%s\".", _databasePath.c_str());
303 UWARN(
"Using empty database. Mapping session will not be saved unless it is closed with an output database path.");
309 if(!newDatabase && loadDatabaseParameters)
316 allParameters.erase(Parameters::kRtabmapWorkingDirectory());
321 uInsert(allParameters, parameters);
322 ParametersMap::const_iterator iter;
323 if((iter=allParameters.find(Parameters::kRtabmapWorkingDirectory())) != allParameters.end())
332 _memory->
init(_databasePath,
false, allParameters,
true);
344 UDEBUG(
"lastPose is ignored (%s=true), assuming we start at the origin of the map.", Parameters::kRGBDSavedLocalizationIgnored().c_str());
351 std::map<int, Transform> tmp;
357 std::map<int, float> likelihood;
361 likelihood.insert(std::make_pair(iter->first, 0));
368 UINFO(
"Loaded optimizedPoses=0, last localization pose is ignored!");
371 if(_databasePath.empty())
378 void Rtabmap::init(
const std::string & configFile,
const std::string & databasePath,
bool loadDatabaseParameters)
383 if(!configFile.empty())
385 ULOGGER_DEBUG(
"Read parameters from = %s", configFile.c_str());
389 this->
init(param, databasePath, loadDatabaseParameters);
394 UINFO(
"databaseSaved=%d", databaseSaved?1:0);
438 std::map<int, int> reducedIds;
440 for(std::map<int, int>::iterator iter=reducedIds.begin(); iter!=reducedIds.end(); ++iter)
479 ParametersMap::const_iterator iter;
480 if((iter=parameters.find(Parameters::kRtabmapWorkingDirectory())) != parameters.end())
518 if(Parameters::parse(parameters, Parameters::kRGBDProximityAngle(),
_proximityAngle))
532 UWARN(
"RGBD/OptimizeMaxError (value=%f) is smaller than 1.0, setting to default %f " 533 "instead (for backward compatibility issues when this parameter was previously " 559 if((iter=parameters.find(Parameters::kOptimizerStrategy())) != parameters.end())
565 UDEBUG(
"new detector strategy %d",
int(optimizerType));
580 optimizerType = (
Optimizer::Type)Parameters::defaultOptimizerStrategy();
641 std::map<int, int> weights;
656 return std::set<int>();
696 return Parameters::defaultMemGenerateIds();
732 UWARN(
"Initial pose can only be set in localization mode (%s=false), ignoring it...", Parameters::kMemIncrementalMemory().c_str());
758 std::map<int, int> reducedIds;
760 UINFO(
"New map triggered, new map = %d", mapId);
770 if(reducedIds.size() &&
_path.size())
772 for(
unsigned int i=0; i<
_path.size(); ++i)
774 std::map<int, int>::const_iterator iter = reducedIds.find(
_path[i].first);
775 if(iter!= reducedIds.end())
778 _path[i].first = iter->second;
800 UERROR(
"Last signature is null! Cannot set label \"%s\"", label.c_str());
820 UERROR(
"Last signature is null! Cannot set user data!");
838 ids.insert(std::pair<int,int>(
id, 0));
839 std::set<int> idsSet;
840 for(std::map<int, int>::iterator iter = ids.begin(); iter!=ids.end(); ++iter)
842 idsSet.insert(idsSet.end(), iter->first);
848 UERROR(
"No neighbors found for signature %d.",
id);
862 std::map<int, Transform> poses;
863 std::multimap<int, Link> constraints;
876 std::map<int, double> stamps;
879 for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
885 std::vector<float> v;
888 _memory->
getNodeInfo(iter->first, o, m, w, l, stamp, g, v, gps, sensors,
true);
889 stamps.insert(std::make_pair(iter->first, stamp));
933 UERROR(
"RTAB-Map is not initialized. No memory to reset...");
964 const cv::Mat & image,
966 const std::map<std::string, float> & externalStats)
973 float odomLinearVariance,
974 float odomAngularVariance,
975 const std::vector<float> & odomVelocity,
976 const std::map<std::string, float> & externalStats)
983 cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1);
984 covariance.at<
double>(0,0) = odomLinearVariance;
985 covariance.at<
double>(1,1) = odomLinearVariance;
986 covariance.at<
double>(2,2) = odomLinearVariance;
987 covariance.at<
double>(3,3) = odomAngularVariance;
988 covariance.at<
double>(4,4) = odomAngularVariance;
989 covariance.at<
double>(5,5) = odomAngularVariance;
990 return process(data, odomPose, covariance, odomVelocity, externalStats);
995 const cv::Mat & odomCovariance,
996 const std::vector<float> & odomVelocity,
997 const std::map<std::string, float> & externalStats)
1006 double timeMemoryUpdate = 0;
1007 double timeNeighborLinkRefining = 0;
1008 double timeProximityByTimeDetection = 0;
1009 double timeProximityBySpaceVisualDetection = 0;
1010 double timeProximityBySpaceDetection = 0;
1011 double timeCleaningNeighbors = 0;
1012 double timeReactivations = 0;
1013 double timeAddLoopClosureLink = 0;
1014 double timeMapOptimization = 0;
1015 double timeRetrievalDbAccess = 0;
1016 double timeLikelihoodCalculation = 0;
1017 double timePosteriorCalculation = 0;
1018 double timeHypothesesCreation = 0;
1019 double timeHypothesesValidation = 0;
1020 double timeRealTimeLimitReachedProcess = 0;
1021 double timeMemoryCleanup = 0;
1022 double timeEmptyingTrash = 0;
1023 double timeFinalizingStatistics = 0;
1024 double timeJoiningTrash = 0;
1025 double timeStatsCreation = 0;
1027 float hypothesisRatio = 0.0f;
1028 bool rejectedGlobalLoopClosure =
false;
1030 std::map<int, float> rawLikelihood;
1031 std::map<int, float> adjustedLikelihood;
1032 std::map<int, float> likelihood;
1033 std::map<int, int> weights;
1034 std::map<int, float> posterior;
1035 std::list<std::pair<int, float> > reactivateHypotheses;
1037 std::map<int, int> childCount;
1038 std::set<int> signaturesRetrieved;
1039 int proximityDetectionsInTimeFound = 0;
1048 std::set<int> immunizedLocations;
1051 for(std::map<std::string, float>::const_iterator iter=externalStats.begin(); iter!=externalStats.end(); ++iter)
1071 bool fakeOdom =
false;
1079 UWARN(
"Input odometry is not invertible! pose = %s\n" 1084 "Trying to normalize rotation to see if it makes it invertible...",
1086 odomPose.
r11(), odomPose.
r12(), odomPose.
r13(), odomPose.
o14(),
1087 odomPose.
r21(), odomPose.
r22(), odomPose.
r23(), odomPose.
o24(),
1088 odomPose.
r31(), odomPose.
r32(), odomPose.
r33(), odomPose.
o34());
1095 odomPose.
r11(), odomPose.
r12(), odomPose.
r13(), odomPose.
o14(),
1096 odomPose.
r21(), odomPose.
r22(), odomPose.
r23(), odomPose.
o24(),
1097 odomPose.
r31(), odomPose.
r32(), odomPose.
r33(), odomPose.
o34()).c_str());
1098 UWARN(
"Normalizing rotation succeeded! fixed pose = %s\n" 1103 "If the resulting rotation is very different from original one, try to fix the odometry or TF.",
1105 odomPose.
r11(), odomPose.
r12(), odomPose.
r13(), odomPose.
o14(),
1106 odomPose.
r21(), odomPose.
r22(), odomPose.
r23(), odomPose.
o24(),
1107 odomPose.
r31(), odomPose.
r32(), odomPose.
r33(), odomPose.
o34());
1126 UWARN(
"Update map correction based on last localization saved in database! correction = %s, nearest id = %d of last pose = %s, odom = %s",
1138 iter->second = mapCorrectionInv * iter->second;
1147 UERROR(
"RGB-D SLAM mode is enabled, memory is incremental but no odometry is provided. " 1148 "Image %d is ignored!", data.
id());
1180 UWARN(
"Odometry is reset (identity pose detected). Increment map id to %d!", mapId);
1191 UWARN(
"Odometry is reset (large odometry change detected > %f). A new map (%d) is created! Last pose = %s, new pose = %s",
1192 _newMapOdomChangeDistance,
1225 UFATAL(
"Not supposed to be here...last signature is null?!?");
1229 timeMemoryUpdate = timer.
ticks();
1230 ULOGGER_INFO(
"timeMemoryUpdate=%fs", timeMemoryUpdate);
1235 bool smallDisplacement =
false;
1236 bool tooFastMovement =
false;
1237 std::list<int> signaturesRemoved;
1240 statistics_.
addStatistic(Statistics::kMemoryOdometry_variance_lin(), odomCovariance.empty()?1.0f:(float)odomCovariance.at<
double>(0,0));
1241 statistics_.
addStatistic(Statistics::kMemoryOdometry_variance_ang(), odomCovariance.empty()?1.0f:(float)odomCovariance.at<
double>(5,5));
1256 const std::multimap<int, Link> & links = signature->
getLinks();
1265 links.begin()->second.transform().getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
1277 smallDisplacement =
true;
1278 UDEBUG(
"smallDisplacement: %f %f %f %f %f %f", x,y,z, roll,pitch,yaw);
1283 if(odomVelocity.size() == 6)
1301 int oldId = signature->
getLinks().begin()->first;
1309 if(smallDisplacement)
1311 if(signature->
getLinks().begin()->second.transVariance() == 1)
1314 UDEBUG(
"Set small variance. The robot is not moving.");
1328 UINFO(
"Odometry refining: update neighbor link (%d->%d, variance:lin=%f, ang=%f) from %s to %s",
1331 info.covariance.at<
double>(0,0),
1332 info.covariance.at<
double>(5,5),
1335 UASSERT(info.covariance.at<
double>(0,0) > 0.0 && info.covariance.at<
double>(5,5) > 0.0);
1344 std::map<int, Transform>::iterator jter =
_optimizedPoses.find(oldId);
1350 iter->second = mapCorrectionInv * up * iter->second;
1356 UINFO(
"Odometry refining rejected: %s", info.rejectedMsg.c_str());
1357 if(!info.covariance.empty() && info.covariance.at<
double>(0,0) > 0.0 && info.covariance.at<
double>(0,0) != 1.0 && info.covariance.at<
double>(5,5) > 0.0 && info.covariance.at<
double>(5,5) != 1.0)
1361 std::cout << info.covariance << std::endl;
1374 timeNeighborLinkRefining = timer.
ticks();
1375 ULOGGER_INFO(
"timeOdometryRefining=%fs", timeNeighborLinkRefining);
1386 UERROR(
"Map correction should be identity when optimizing from the last node. T=%s NewPose=%s OldPose=%s",
1394 UWARN(
"Neighbor link refining is activated but there are intermediate nodes (%d=%d %d=%d), aborting refining...",
1409 for(std::map<int, Link>::const_iterator iter = signature->
getLandmarks().begin(); iter!=signature->
getLandmarks().end(); ++iter)
1413 _optimizedPoses.insert(std::make_pair(iter->first, newPose*iter->second.transform()));
1415 _constraints.insert(std::make_pair(iter->first, iter->second.inverse()));
1427 "Only forward links should be added.");
1429 Link tmp = signature->
getLinks().begin()->second.inverse();
1441 tmp =
_constraints.rbegin()->second.merge(tmp, tmp.type());
1472 UWARN(
"Odometry poses cached for localization verification reached the " 1473 "maximum numbers of %d, clearing the buffer. The next localization " 1474 "won't be verified! Set %s to 0 if you want to disable the localization verification.",
1476 Parameters::kRGBDMaxOdomCacheSize().c_str());
1483 std::make_pair(signature->
id(),
1488 odomCovariance.inv())));
1501 for(
unsigned int i=0; i<
_path.size(); ++i)
1507 _path[i].first = iter->second;
1520 if(jter->second.from() == iter->first || jter->second.to() == iter->first)
1542 for(std::set<int>::const_reverse_iterator iter = stm.rbegin(); iter!=stm.rend(); ++iter)
1544 if(*iter != signature->
id() &&
1549 std::string rejectedMsg;
1550 UDEBUG(
"Check local transform between %d and %d", signature->
id(), *iter);
1563 transform = transform.
inverse();
1564 UDEBUG(
"Add local loop closure in TIME (%d->%d) %s",
1569 UASSERT(info.covariance.at<
double>(0,0) > 0.0 && info.covariance.at<
double>(5,5) > 0.0);
1572 ++proximityDetectionsInTimeFound;
1573 UINFO(
"Local loop closure found between %d and %d with t=%s",
1574 *iter, signature->
id(), transform.prettyPrint().c_str());
1578 UWARN(
"Cannot add local loop closure between %d and %d ?!?",
1579 *iter, signature->
id());
1584 UINFO(
"Local loop closure (time) between %d and %d rejected: %s",
1585 *iter, signature->
id(), rejectedMsg.c_str());
1592 timeProximityByTimeDetection = timer.
ticks();
1593 UINFO(
"timeProximityByTimeDetection=%fs", timeProximityByTimeDetection);
1598 int previousId = signature->
getLinks().size() && signature->
getLinks().begin()->first!=signature->
id()?signature->
getLinks().begin()->first:0;
1614 std::list<int> signaturesToCompare;
1627 for(std::map<int, float>::reverse_iterator iter=nearestIds.rbegin(); iter!=nearestIds.rend() && iter->first>0; ++iter)
1645 if(originGPS.
stamp() > 0.0)
1663 if(originGPS.
stamp()>0.0)
1665 std::map<int, std::pair<cv::Point3d, Transform> >::iterator cacheIter =
_gpsGeocentricCache.find(s->
id());
1670 if(gps.
stamp()==0.0)
1674 if(gps.
stamp() > 0.0)
1677 std::make_pair(s->
id(),
1685 std::map<int, std::pair<cv::Point3d, Transform> >::iterator originIter =
_gpsGeocentricCache.find(signature->
id());
1688 const double & error = originGPS.
error();
1689 const Transform & offsetENU = cacheIter->second.second;
1690 relativePose.
x += offsetENU.
x() - originOffsetENU.
x();
1691 relativePose.y += offsetENU.
y() - originOffsetENU.
y();
1692 relativePose.z += offsetENU.
z() - originOffsetENU.
z();
1694 if(relativePose.z>error)
1696 relativePose.z -= error;
1698 else if(relativePose.z < -error)
1700 relativePose.z += error;
1712 signaturesToCompare.push_back(iter->first);
1719 signaturesToCompare.push_back(iter->first);
1726 likelihood = rawLikelihood;
1729 timeLikelihoodCalculation = timer.
ticks();
1730 ULOGGER_INFO(
"timeLikelihoodCalculation=%fs",timeLikelihoodCalculation);
1740 timePosteriorCalculation = timer.
ticks();
1741 ULOGGER_INFO(
"timePosteriorCalculation=%fs",timePosteriorCalculation);
1753 if(posterior.size())
1755 for(std::map<int, float>::const_reverse_iterator iter = posterior.rbegin(); iter != posterior.rend(); ++iter)
1765 timeHypothesesCreation = timer.
ticks();
1788 rejectedGlobalLoopClosure =
true;
1789 if(posterior.size() <= 2 && loopThr>0.0f)
1792 UDEBUG(
"rejected hypothesis: single hypothesis");
1796 UWARN(
"rejected hypothesis: by epipolar geometry");
1800 UWARN(
"rejected hypothesis: not satisfying hypothesis ratio (%f < %f * %f)",
1803 else if(
_loopRatio > 0.0
f && lastHighestHypothesis.second == 0)
1805 UWARN(
"rejected hypothesis: last closure hypothesis is null (loop ratio is on)");
1810 rejectedGlobalLoopClosure =
false;
1813 timeHypothesesValidation = timer.
ticks();
1814 ULOGGER_INFO(
"timeHypothesesValidation=%fs",timeHypothesesValidation);
1821 rejectedGlobalLoopClosure =
true;
1830 else if(!signature->
isBadSignature() && (smallDisplacement || tooFastMovement))
1840 timeJoiningTrash = timer.
ticks();
1841 ULOGGER_INFO(
"Time emptying memory trash = %fs, joining (actual overhead) = %fs", timeEmptyingTrash, timeJoiningTrash);
1847 std::list<int> reactivatedIds;
1848 double timeGetNeighborsTimeDb = 0.0;
1849 double timeGetNeighborsSpaceDb = 0.0;
1850 int immunizedGlobally = 0;
1851 int immunizedLocally = 0;
1852 int maxLocalLocationsImmunized = 0;
1865 ULOGGER_INFO(
"Retrieving locations... around id=%d", retrievalId);
1867 UASSERT(neighborhoodSize >= 0);
1871 unsigned int nbLoadedFromDb = 0;
1872 std::set<int> reactivatedIdsSet;
1873 std::map<int, int> neighbors;
1874 int nbDirectNeighborsInDb = 0;
1887 &timeGetNeighborsTimeDb);
1888 ULOGGER_DEBUG(
"neighbors of %d in time = %d", retrievalId, (
int)neighbors.size());
1890 bool firstPassDone =
false;
1892 while(m < neighborhoodSize)
1894 std::set<int> idsSorted;
1895 for(std::map<int, int>::iterator iter=neighbors.begin(); iter!=neighbors.end();)
1899 neighbors.erase(iter++);
1901 else if(iter->second == m)
1903 if(reactivatedIdsSet.find(iter->first) == reactivatedIdsSet.end())
1905 idsSorted.insert(iter->first);
1906 reactivatedIdsSet.insert(iter->first);
1910 ++nbDirectNeighborsInDb;
1914 if(immunizedLocations.insert(iter->first).second)
1916 ++immunizedGlobally;
1921 neighbors.erase(iter++);
1928 firstPassDone =
true;
1929 reactivatedIds.insert(reactivatedIds.end(), idsSorted.rbegin(), idsSorted.rend());
1943 &timeGetNeighborsSpaceDb);
1944 ULOGGER_DEBUG(
"neighbors of %d in space = %d", retrievalId, (
int)neighbors.size());
1945 firstPassDone =
false;
1947 while(m < neighborhoodSize)
1949 std::set<int> idsSorted;
1950 for(std::map<int, int>::iterator iter=neighbors.begin(); iter!=neighbors.end();)
1954 neighbors.erase(iter++);
1956 else if(iter->second == m)
1958 if(reactivatedIdsSet.find(iter->first) == reactivatedIdsSet.end())
1960 idsSorted.insert(iter->first);
1961 reactivatedIdsSet.insert(iter->first);
1965 ++nbDirectNeighborsInDb;
1969 neighbors.erase(iter++);
1976 firstPassDone =
true;
1977 reactivatedIds.insert(reactivatedIds.end(), idsSorted.rbegin(), idsSorted.rend());
1981 "reactivatedIds.size=%d, " 1982 "nbLoadedFromDb=%d, " 1983 "nbDirectNeighborsInDb=%d, " 1984 "time=%fs (%fs %fs)",
1986 reactivatedIds.size(),
1987 (int)nbLoadedFromDb,
1988 nbDirectNeighborsInDb,
1990 timeGetNeighborsTimeDb,
1991 timeGetNeighborsSpaceDb);
1999 std::list<int> retrievalLocalIds;
2007 float distanceSoFar = 0.0f;
2014 distanceSoFar +=
_path[i-1].second.getDistance(
_path[i].second);
2021 if(immunizedLocations.insert(
_path[i].first).second)
2025 UDEBUG(
"Path immunization: node %d (dist=%fm)",
_path[i].first, distanceSoFar);
2029 UINFO(
"retrieval of node %d on path (dist=%fm)",
_path[i].first, distanceSoFar);
2030 retrievalLocalIds.push_back(
_path[i].first);
2036 UDEBUG(
"Stop on node %d (dist=%fm > %fm)",
2046 if(immunizedLocally < maxLocalLocationsImmunized &&
2049 std::map<int ,Transform> poses;
2055 poses.insert(*iter);
2064 std::multimap<int, int> links;
2069 links.insert(std::make_pair(iter->second.from(), iter->second.to()));
2070 links.insert(std::make_pair(iter->second.to(), iter->second.from()));
2075 if(path.size() == 0)
2077 UWARN(
"Could not compute a path between %d and %d", nearestId, signature->
id());
2081 for(std::list<std::pair<int, Transform> >::iterator iter=path.begin();
2087 if(immunizedLocally >= maxLocalLocationsImmunized)
2092 UWARN(
"Could not immunize the whole local path (%d) between " 2093 "%d and %d (max location immunized=%d). You may want " 2094 "to increase RGBD/LocalImmunizationRatio (current=%f (%d of WM=%d)) " 2095 "to be able to immunize longer paths.",
2099 maxLocalLocationsImmunized,
2101 maxLocalLocationsImmunized,
2108 if(immunizedLocations.insert(iter->first).second)
2124 std::multimap<float, int> nearNodesByDist;
2125 for(std::map<int, float>::iterator iter=nearNodes.lower_bound(1); iter!=nearNodes.end(); ++iter)
2127 nearNodesByDist.insert(std::make_pair(iter->second, iter->first));
2129 UINFO(
"near nodes=%d, max local immunized=%d, ratio=%f WM=%d",
2130 (
int)nearNodesByDist.size(),
2131 maxLocalLocationsImmunized,
2134 for(std::multimap<float, int>::iterator iter=nearNodesByDist.begin();
2135 iter!=nearNodesByDist.end() && (retrievalLocalIds.size() <
_maxLocalRetrieved || immunizedLocally < maxLocalLocationsImmunized);
2143 const std::multimap<int, Link> & links = s->
getLinks();
2144 for(std::multimap<int, Link>::const_reverse_iterator jter=links.rbegin();
2150 UINFO(
"retrieval of node %d on local map", jter->first);
2151 retrievalLocalIds.push_back(jter->first);
2154 if(!
_memory->
isInSTM(s->
id()) && immunizedLocally < maxLocalLocationsImmunized)
2156 if(immunizedLocations.insert(s->
id()).second)
2167 std::set<int> retrievalLocalIdsSet(retrievalLocalIds.begin(), retrievalLocalIds.end());
2168 for(std::list<int>::iterator iter=retrievalLocalIds.begin();
2173 for(std::map<int, int>::reverse_iterator jter=ids.rbegin();
2178 retrievalLocalIdsSet.find(jter->first) == retrievalLocalIdsSet.end())
2180 UINFO(
"retrieval of node %d on local map", jter->first);
2181 retrievalLocalIds.push_back(jter->first);
2182 retrievalLocalIdsSet.insert(jter->first);
2189 for(std::multimap<float, int>::reverse_iterator iter=nearNodesByDist.rbegin(); iter!=nearNodesByDist.rend(); ++iter)
2195 reactivatedIds.insert(reactivatedIds.begin(), retrievalLocalIds.begin(), retrievalLocalIds.end());
2201 if(reactivatedIds.size())
2208 timeRetrievalDbAccess);
2210 ULOGGER_INFO(
"retrieval of %d (db time = %fs)", (
int)signaturesRetrieved.size(), timeRetrievalDbAccess);
2212 timeRetrievalDbAccess += timeGetNeighborsTimeDb + timeGetNeighborsSpaceDb;
2213 UINFO(
"total timeRetrievalDbAccess=%fs", timeRetrievalDbAccess);
2216 immunizedLocations.insert(signaturesRetrieved.begin(), signaturesRetrieved.end());
2218 timeReactivations = timer.
ticks();
2219 ULOGGER_INFO(
"timeReactivations=%fs", timeReactivations);
2225 int landmarkDetected = 0;
2226 bool rejectedLandmark =
false;
2227 std::set<int> landmarkDetectedNodesRef;
2230 for(std::map<int, Link>::const_iterator iter=signature->
getLandmarks().begin(); iter!=signature->
getLandmarks().end(); ++iter)
2235 landmarkDetected = iter->first;
2237 UINFO(
"Landmark %d observed again! Seen the first time by node %d.", -iter->first, *landmarkDetectedNodesRef.begin());
2246 std::list<std::pair<int, int> > loopClosureLinksAdded;
2247 int loopClosureVisualInliers = 0;
2248 float loopClosureVisualInliersRatio = 0.0f;
2249 int loopClosureVisualMatches = 0;
2250 float loopClosureLinearVariance = 0.0f;
2251 float loopClosureAngularVariance = 0.0f;
2252 float loopClosureVisualInliersMeanDist = 0;
2253 float loopClosureVisualInliersDistribution = 0;
2255 int proximityDetectionsAddedVisually = 0;
2256 int proximityDetectionsAddedByICPOnly = 0;
2257 int lastProximitySpaceClosureId = 0;
2258 int proximitySpacePaths = 0;
2259 int localVisualPathsChecked = 0;
2260 int localScanPathsChecked = 0;
2261 int loopIdSuppressedByProximity = 0;
2270 UWARN(
"Cannot do local loop closure detection in space if graph optimization is disabled!");
2289 UDEBUG(
"Proximity detection (local loop closure in SPACE using matching images, local radius=%fm)",
_localRadius);
2290 std::map<int, float> nearestIds;
2300 std::map<int, Transform> nearestPoses;
2301 for(std::map<int, float>::iterator iter=nearestIds.lower_bound(1); iter!=nearestIds.end(); ++iter)
2305 nearestPoses.insert(std::make_pair(iter->first,
_optimizedPoses.at(iter->first)));
2308 UDEBUG(
"nearestPoses=%d", (
int)nearestPoses.size());
2312 UDEBUG(
"got %d paths", (
int)nearestPathsNotSorted.size());
2314 std::map<NearestPathKey, std::map<int, Transform> > nearestPaths;
2315 for(std::map<
int, std::map<int, Transform> >::const_iterator iter=nearestPathsNotSorted.begin();iter!=nearestPathsNotSorted.end(); ++iter)
2317 const std::map<int, Transform> & path = iter->second;
2318 float highestLikelihood = 0.0f;
2319 int highestLikelihoodId = iter->first;
2320 for(std::map<int, Transform>::const_iterator jter=path.begin(); jter!=path.end(); ++jter)
2322 float v =
uValue(likelihood, jter->first, 0.0f);
2323 if(v > highestLikelihood)
2325 highestLikelihood = v;
2326 highestLikelihoodId = jter->first;
2329 nearestPaths.insert(std::make_pair(
NearestPathKey(highestLikelihood, highestLikelihoodId), path));
2338 for(std::map<
NearestPathKey, std::map<int, Transform> >::const_reverse_iterator iter=nearestPaths.rbegin();
2339 iter!=nearestPaths.rend() &&
2344 std::map<int, Transform> path = iter->second;
2352 if(iter->first.likelihood > 0.0f &&
2353 path.find(iter->first.id)!=path.end())
2355 nearestId = iter->first.id;
2365 if(!signature->
hasLink(nearestId) &&
2366 (proximityFilteringRadius <= 0.0f ||
2369 ++localVisualPathsChecked;
2380 transform = transform.
inverse();
2381 if(proximityFilteringRadius <= 0 || transform.
getNormSquared() <= proximityFilteringRadius*proximityFilteringRadius)
2383 UINFO(
"[Visual] Add local loop closure in SPACE (%d->%d) %s",
2390 loopClosureLinksAdded.push_back(std::make_pair(signature->
id(), nearestId));
2396 ++proximityDetectionsAddedVisually;
2397 lastProximitySpaceClosureId = nearestId;
2399 loopClosureVisualInliers = info.
inliers;
2401 loopClosureVisualMatches = info.
matches;
2403 loopClosureLinearVariance = 1.0/information.at<
double>(0,0);
2404 loopClosureAngularVariance = 1.0/information.at<
double>(5,5);
2409 UDEBUG(
"Proximity detection on %d is close to loop closure %d, ignoring loop closure transform estimation...",
2420 UWARN(
"Ignoring local loop closure with %d because resulting " 2421 "transform is too large!? (%fm > %fm)",
2422 nearestId, transform.
getNorm(), proximityFilteringRadius);
2429 timeProximityBySpaceVisualDetection = timer.
ticks();
2430 ULOGGER_INFO(
"timeProximityBySpaceVisualDetection=%fs", timeProximityBySpaceVisualDetection);
2435 UDEBUG(
"Proximity detection (local loop closure in SPACE with scan matching)");
2438 UDEBUG(
"Proximity by scan matching is disabled (%s=%d).", Parameters::kRGBDProximityPathMaxNeighbors().c_str(),
_proximityMaxNeighbors);
2447 proximitySpacePaths = (int)nearestPaths.size();
2448 for(std::map<
NearestPathKey, std::map<int, Transform> >::const_reverse_iterator iter=nearestPaths.rbegin();
2449 iter!=nearestPaths.rend() &&
2454 std::map<int, Transform> path = iter->second;
2456 UASSERT(path.begin()->first > 0);
2464 if(!signature->
hasLink(nearestId))
2468 std::map<int, Transform> filteredPath;
2470 std::map<int, Transform>::iterator nearestIdIter = path.find(nearestId);
2474 for(std::map<int, Transform>::iterator iter=nearestIdIter; iter!=path.end() && i<=
_proximityMaxNeighbors-1; ++iter, ++i)
2476 filteredPath.insert(*iter);
2479 for(std::map<int, Transform>::reverse_iterator iter(nearestIdIter); iter!=path.rend() && i<=
_proximityMaxNeighbors-1; ++iter, ++i)
2481 filteredPath.insert(*iter);
2483 path = filteredPath;
2487 std::map<int, Transform> optimizedLocalPath;
2497 for(std::map<int, Transform>::iterator jter=path.lower_bound(1); jter!=path.end(); ++jter)
2499 optimizedLocalPath.insert(std::make_pair(jter->first, t * jter->second));
2504 optimizedLocalPath = path;
2507 std::map<int, Transform> filteredPath;
2508 if(optimizedLocalPath.size() > 2 && proximityFilteringRadius > 0.0f)
2513 filteredPath.insert(*optimizedLocalPath.find(nearestId));
2517 filteredPath = optimizedLocalPath;
2520 if(filteredPath.size() > 0)
2523 filteredPath.insert(std::make_pair(signature->
id(),
_optimizedPoses.at(signature->
id())));
2527 ++localScanPathsChecked;
2532 UINFO(
"[Scan matching] Add local loop closure in SPACE (%d->%d) %s",
2537 cv::Mat scanMatchingIds;
2540 std::stringstream stream;
2542 for(std::map<int, Transform>::iterator iter=optimizedLocalPath.begin(); iter!=optimizedLocalPath.end(); ++iter)
2544 if(iter != optimizedLocalPath.begin())
2550 std::string scansStr = stream.str();
2551 scanMatchingIds = cv::Mat(1,
int(scansStr.size()+1), CV_8SC1, (
void *)scansStr.c_str());
2558 loopClosureLinksAdded.push_back(std::make_pair(signature->
id(), nearestId));
2560 ++proximityDetectionsAddedByICPOnly;
2563 if(proximityDetectionsAddedVisually == 0)
2565 lastProximitySpaceClosureId = nearestId;
2584 timeProximityBySpaceDetection = timer.
ticks();
2585 ULOGGER_INFO(
"timeProximityBySpaceDetection=%fs", timeProximityBySpaceDetection);
2596 info.
covariance = cv::Mat::eye(6,6,CV_64FC1);
2603 loopClosureVisualInliers = info.
inliers;
2605 loopClosureVisualMatches = info.
matches;
2606 rejectedGlobalLoopClosure = transform.
isNull();
2607 if(rejectedGlobalLoopClosure)
2609 UWARN(
"Rejected loop closure %d -> %d: %s",
2614 rejectedGlobalLoopClosure =
true;
2615 UWARN(
"Rejected localization %d -> %d because distance to map (%fm) is over %s=%fm.",
2620 transform = transform.
inverse();
2623 if(!rejectedGlobalLoopClosure)
2628 loopClosureLinearVariance = 1.0/information.at<
double>(0,0);
2629 loopClosureAngularVariance = 1.0/information.at<
double>(5,5);
2631 if(!rejectedGlobalLoopClosure)
2637 if(rejectedGlobalLoopClosure)
2643 timeAddLoopClosureLink = timer.
ticks();
2644 ULOGGER_INFO(
"timeAddLoopClosureLink=%fs", timeAddLoopClosureLink);
2665 UERROR(
"Virtual link larger than local radius (%fm > %fm). Aborting the plan!",
2675 float maxLinearError = 0.0f;
2676 float maxLinearErrorRatio = 0.0f;
2677 float maxAngularError = 0.0f;
2678 float maxAngularErrorRatio = 0.0f;
2679 double optimizationError = 0.0;
2680 int optimizationIterations = 0;
2681 cv::Mat localizationCovariance;
2686 lastProximitySpaceClosureId>0 ||
2690 proximityDetectionsInTimeFound>0 ||
2691 landmarkDetected!=0 ||
2692 signaturesRetrieved.size())
2697 landmarkDetected!=0))
2707 localizationLinks.insert(std::make_pair(landmarkDetected, signature->
getLandmarks().at(landmarkDetected)));
2715 signaturesRetrieved.size() == 0 &&
2716 localizationLinks.size() &&
2719 bool rejectLocalization =
false;
2734 optPoseRefB = iter->second * localizationLinks.begin()->second.transform().
inverse();
2738 UWARN(
"Both optimized pose references are null! Flushing cached odometry poses. Localization won't be verified.");
2748 optPoseRefA.
inverse() * optPoseRefB, cv::Mat::eye(6,6,CV_64FC1)*100)));
2752 if(optPoses.empty())
2754 UWARN(
"Optimization failed, rejecting localization!");
2755 rejectLocalization =
true;
2759 UINFO(
"Compute max graph errors...");
2760 const Link * maxLinearLink = 0;
2761 const Link * maxAngularLink = 0;
2765 maxLinearErrorRatio,
2766 maxAngularErrorRatio,
2771 if(maxLinearLink == 0 && maxAngularLink==0)
2773 UWARN(
"Could not compute graph errors! Wrong loop closures could be accepted!");
2778 UINFO(
"Max optimization linear error = %f m (link %d->%d, var=%f, ratio error/std=%f)", maxLinearError, maxLinearLink->
from(), maxLinearLink->
to(), maxLinearLink->
transVariance(), maxLinearError/
sqrt(maxLinearLink->
transVariance()));
2781 UWARN(
"Rejecting localization (%d <-> %d) in this " 2782 "iteration because a wrong loop closure has been " 2783 "detected after graph optimization, resulting in " 2784 "a maximum graph error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). The " 2785 "maximum error ratio parameter \"%s\" is %f of std deviation.",
2786 loopClosureLinksAdded.front().first,
2787 loopClosureLinksAdded.front().second,
2788 maxLinearErrorRatio,
2789 maxLinearLink->
from(),
2790 maxLinearLink->
to(),
2791 maxLinearLink->
type(),
2794 Parameters::kRGBDOptimizeMaxError().c_str(),
2796 rejectLocalization =
true;
2801 UINFO(
"Max optimization angular error = %f deg (link %d->%d, var=%f, ratio error/std=%f)", maxAngularError*180.0
f/CV_PI, maxAngularLink->
from(), maxAngularLink->
to(), maxAngularLink->
rotVariance(), maxAngularError/
sqrt(maxAngularLink->
rotVariance()));
2804 UWARN(
"Rejecting localization (%d <-> %d) in this " 2805 "iteration because a wrong loop closure has been " 2806 "detected after graph optimization, resulting in " 2807 "a maximum graph error ratio of %f (edge %d->%d, type=%d, abs error=%f deg, stddev=%f). The " 2808 "maximum error ratio parameter \"%s\" is %f of std deviation.",
2809 loopClosureLinksAdded.front().first,
2810 loopClosureLinksAdded.front().second,
2811 maxAngularErrorRatio,
2812 maxAngularLink->
from(),
2813 maxAngularLink->
to(),
2814 maxAngularLink->
type(),
2815 maxAngularError*180.0f/CV_PI,
2817 Parameters::kRGBDOptimizeMaxError().c_str(),
2819 rejectLocalization =
true;
2826 if(!rejectLocalization)
2832 UINFO(
"Localization without map optimization");
2840 Transform u = signature->
getPose() * localizationLinks.begin()->second.transform();
2849 Transform transform = localizationLinks.begin()->second.transform();
2850 int loopId = localizationLinks.begin()->first;
2855 int landmarkId = loopId;
2856 UASSERT(!landmarkDetectedNodesRef.empty());
2857 loopId = *landmarkDetectedNodesRef.begin();
2859 transform = transform * loopS->
getLandmarks().at(landmarkId).transform().inverse();
2868 if(iterGravityLoop!=loopS->
getLinks().end() &&
2869 iterGravitySign!=signature->
getLinks().end())
2872 iterGravityLoop->second.transform().getEulerAngles(roll, pitch, yaw);
2874 targetRotation =
Transform(0,0,0,roll,pitch,targetRotation.
theta());
2878 u = signature->
getPose() * transform;
2882 UWARN(
"Gravity link not found for %d and/or %d, localization won't be corrected with gravity.", loopId, signature->
id());
2888 iter->second = mapCorrectionInv * up * iter->second;
2894 Transform newPose =
_optimizedPoses.at(localizationLinks.begin()->first) * localizationLinks.begin()->second.transform().inverse();
2899 newPose = newPose.
to3DoF();
2905 Transform transform = localizationLinks.begin()->second.transform();
2906 int loopId = localizationLinks.begin()->first;
2911 int landmarkId = loopId;
2912 UASSERT(!landmarkDetectedNodesRef.empty());
2913 loopId = *landmarkDetectedNodesRef.begin();
2915 transform = transform * loopS->
getLandmarks().at(landmarkId).transform().inverse();
2922 if(iterGravityLoop!=loopS->
getLinks().end() &&
2923 iterGravitySign!=signature->
getLinks().end())
2926 iterGravityLoop->second.transform().getEulerAngles(roll, pitch, yaw);
2928 targetRotation =
Transform(0,0,0,roll,pitch,targetRotation.
theta());
2937 UWARN(
"Gravity link not found for %d and/or %d, localization won't be corrected with gravity.", loopId, signature->
id());
2942 localizationCovariance = localizationLinks.begin()->second.infMatrix().inv();
2955 lastProximitySpaceClosureId = 0;
2956 rejectedGlobalLoopClosure =
true;
2957 rejectedLandmark =
true;
2962 UINFO(
"Update map correction");
2968 UWARN(
"Optimization: clearing guess poses as %s has changed state, now %s",
2974 std::multimap<int, Link> constraints;
2976 optimizeCurrentMap(signature->
id(),
false, poses, covariance, &constraints, &optimizationError, &optimizationIterations);
2980 bool updateConstraints =
true;
2983 UWARN(
"Graph optimization failed! Rejecting last loop closures added.");
2984 for(std::list<std::pair<int, int> >::iterator iter=loopClosureLinksAdded.begin(); iter!=loopClosureLinksAdded.end(); ++iter)
2987 UWARN(
"Loop closure %d->%d rejected!", iter->first, iter->second);
2989 updateConstraints =
false;
2991 lastProximitySpaceClosureId = 0;
2992 rejectedGlobalLoopClosure =
true;
2993 rejectedLandmark =
true;
2997 loopClosureLinksAdded.size() &&
2998 optimizationIterations > 0 &&
3001 UINFO(
"Compute max graph errors...");
3002 const Link * maxLinearLink = 0;
3003 const Link * maxAngularLink = 0;
3007 maxLinearErrorRatio,
3008 maxAngularErrorRatio,
3013 if(maxLinearLink == 0 && maxAngularLink==0)
3015 UWARN(
"Could not compute graph errors! Wrong loop closures could be accepted!");
3018 bool reject =
false;
3021 UINFO(
"Max optimization linear error = %f m (link %d->%d, var=%f, ratio error/std=%f)", maxLinearError, maxLinearLink->
from(), maxLinearLink->
to(), maxLinearLink->
transVariance(), maxLinearError/
sqrt(maxLinearLink->
transVariance()));
3024 UWARN(
"Rejecting all added loop closures (%d, first is %d <-> %d) in this " 3025 "iteration because a wrong loop closure has been " 3026 "detected after graph optimization, resulting in " 3027 "a maximum graph error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). The " 3028 "maximum error ratio parameter \"%s\" is %f of std deviation.",
3029 (
int)loopClosureLinksAdded.size(),
3030 loopClosureLinksAdded.front().first,
3031 loopClosureLinksAdded.front().second,
3032 maxLinearErrorRatio,
3033 maxLinearLink->
from(),
3034 maxLinearLink->
to(),
3035 maxLinearLink->
type(),
3038 Parameters::kRGBDOptimizeMaxError().c_str(),
3045 UINFO(
"Max optimization angular error = %f deg (link %d->%d, var=%f, ratio error/std=%f)", maxAngularError*180.0
f/CV_PI, maxAngularLink->
from(), maxAngularLink->
to(), maxAngularLink->
rotVariance(), maxAngularError/
sqrt(maxAngularLink->
rotVariance()));
3048 UWARN(
"Rejecting all added loop closures (%d, first is %d <-> %d) in this " 3049 "iteration because a wrong loop closure has been " 3050 "detected after graph optimization, resulting in " 3051 "a maximum graph error ratio of %f (edge %d->%d, type=%d, abs error=%f deg, stddev=%f). The " 3052 "maximum error ratio parameter \"%s\" is %f of std deviation.",
3053 (
int)loopClosureLinksAdded.size(),
3054 loopClosureLinksAdded.front().first,
3055 loopClosureLinksAdded.front().second,
3056 maxAngularErrorRatio,
3057 maxAngularLink->
from(),
3058 maxAngularLink->
to(),
3059 maxAngularLink->
type(),
3060 maxAngularError*180.0f/CV_PI,
3062 Parameters::kRGBDOptimizeMaxError().c_str(),
3070 for(std::list<std::pair<int, int> >::iterator iter=loopClosureLinksAdded.begin(); iter!=loopClosureLinksAdded.end(); ++iter)
3073 UWARN(
"Loop closure %d->%d rejected!", iter->first, iter->second);
3075 updateConstraints =
false;
3077 lastProximitySpaceClosureId = 0;
3078 rejectedGlobalLoopClosure =
true;
3079 rejectedLandmark =
true;
3083 if(updateConstraints)
3085 UINFO(
"Updated local map (old size=%d, new size=%d)", (
int)
_optimizedPoses.size(), (int)poses.size());
3088 localizationCovariance = covariance;
3103 bool hasPrior = signature->
hasLink(signature->
id());
3106 for(std::multimap<int, Link>::reverse_iterator iter=
_constraints.rbegin(); !hasPrior && iter!=
_constraints.rend(); ++iter)
3122 if(newLocId==0 && landmarkDetected!=0)
3127 if(iter->second.size() && *iter->second.begin()!=signature->
id())
3134 timeMapOptimization = timer.
ticks();
3135 ULOGGER_INFO(
"timeMapOptimization=%fs", timeMapOptimization);
3141 int dictionarySize = 0;
3142 int refWordsCount = 0;
3143 int refUniqueWordsCount = 0;
3144 int lcHypothesisReactivated = 0;
3150 lcHypothesisReactivated = sLoop->
isSaved()?1.0f:0.0f;
3153 refWordsCount = (int)signature->
getWords().size();
3196 statistics_.
addStatistic(Statistics::kLoopLandmark_detected_node_ref(), landmarkDetectedNodesRef.empty()?0:*landmarkDetectedNodesRef.begin());
3198 statistics_.
addStatistic(Statistics::kLoopVisual_inliers_distribution(), loopClosureVisualInliersDistribution);
3201 statistics_.
addStatistic(Statistics::kProximitySpace_detections_added_visually(), proximityDetectionsAddedVisually);
3202 statistics_.
addStatistic(Statistics::kProximitySpace_detections_added_icp_only(), proximityDetectionsAddedByICPOnly);
3219 std::multimap<int, Link>::const_iterator loopIter = sLoop->
getLinks().find(signature->
id());
3221 UINFO(
"Set loop closure transform = %s", loopIter->second.transform().prettyPrint().c_str());
3306 statistics_.
addStatistic(Statistics::kTimingProximity_by_space_visual(), timeProximityBySpaceVisualDetection*1000);
3336 long estimatedMemoryUsage =
sizeof(
Rtabmap);
3337 estimatedMemoryUsage +=
_optimizedPoses.size() * (
sizeof(int) +
sizeof(
Transform) + 12 *
sizeof(float) +
sizeof(std::map<int, Transform>::iterator)) +
sizeof(std::map<int, Transform>);
3338 estimatedMemoryUsage +=
_constraints.size() * (
sizeof(int) +
sizeof(
Transform) + 12 *
sizeof(float) +
sizeof(cv::Mat) + 36 *
sizeof(double) +
sizeof(std::map<int, Link>::iterator)) +
sizeof(std::map<int, Link>);
3341 estimatedMemoryUsage +=
_parameters.size()*(
sizeof(std::string)*2+
sizeof(ParametersMap::iterator)) +
sizeof(
ParametersMap);
3371 timeStatsCreation = timer.
ticks();
3372 ULOGGER_INFO(
"Time creating stats = %f...", timeStatsCreation);
3385 lastSignatureData = *signature;
3394 if(signatureRemoved)
3396 signaturesRemoved.push_back(signatureRemoved);
3403 if(signatureRemoved != lastSignatureData.id())
3408 (landmarkDetected == 0 || rejectedLandmark) &&
3411 UWARN(
"Ignoring location %d because a global loop closure is required before starting a new map!",
3413 signaturesRemoved.push_back(signature->
id());
3420 UWARN(
"Ignoring location %d because a good signature (with enough features or with a landmark detected) is required before starting a new map!",
3422 signaturesRemoved.push_back(signature->
id());
3425 else if((smallDisplacement || tooFastMovement) &&
_loopClosureHypothesis.first == 0 && lastProximitySpaceClosureId == 0)
3428 UINFO(
"Ignoring location %d because the displacement is too small! (d=%f a=%f)",
3431 signaturesRemoved.push_back(signature->
id());
3440 (smallDisplacement || tooFastMovement) &&
3442 lastProximitySpaceClosureId == 0)
3451 timeMemoryCleanup = timer.
ticks();
3452 ULOGGER_INFO(
"timeMemoryCleanup = %fs... %d signatures removed", timeMemoryCleanup, (
int)signaturesRemoved.size());
3464 double totalTime = timerTotal.
ticks();
3465 ULOGGER_INFO(
"Total time processing = %fs...", totalTime);
3471 std::list<int> transferred =
_memory->
forget(immunizedLocations);
3472 signaturesRemoved.insert(signaturesRemoved.end(), transferred.begin(), transferred.end());
3481 for(std::list<int>::iterator iter=signaturesRemoved.begin(); iter!=signaturesRemoved.end() &&
_gpsGeocentricCache.size(); ++iter)
3499 UDEBUG(
"Refresh local map from %d",
id);
3506 UDEBUG(
"Refresh local map from %d",
id);
3519 UDEBUG(
"Refresh local map from %d",
id);
3530 if(signaturesRemoved.size() == 1 && signaturesRemoved.front() == lastSignatureData.id())
3532 UDEBUG(
"Detected that only last signature has been removed");
3533 int lastId = signaturesRemoved.front();
3535 for(std::multimap<int, Link>::iterator iter=
_constraints.find(lastId); iter!=
_constraints.end() && iter->first==lastId;++iter)
3537 if(iter->second.to() != iter->second.from())
3554 if(iter->first > 0 && !
uContains(ids, iter->first))
3556 UDEBUG(
"Removed %d from local map", iter->first);
3567 if(iter->first > 0 && (!
uContains(ids, iter->second.from()) || !
uContains(ids, iter->second.to())))
3594 timeRealTimeLimitReachedProcess = timer.
ticks();
3595 ULOGGER_INFO(
"Time limit reached processing = %f...", timeRealTimeLimitReachedProcess);
3600 int localGraphSize = 0;
3622 std::map<int, Transform> poses;
3623 std::multimap<int, Link> constraints;
3639 UINFO(
"Adding data %d [%d] (rgb/left=%d depth/right=%d)", lastSignatureData.id(), lastSignatureData.mapId(), lastSignatureData.sensorData().imageRaw().empty()?0:1, lastSignatureData.sensorData().depthOrRightRaw().empty()?0:1);
3646 lastSignatureData.id(),
3647 lastSignatureData.mapId(),
3648 lastSignatureData.getWeight(),
3649 lastSignatureData.getStamp(),
3650 lastSignatureData.getLabel(),
3651 lastSignatureData.getPose(),
3652 lastSignatureData.getGroundTruthPose());
3653 const std::vector<float> & v = lastSignatureData.
getVelocity();
3656 nodeInfo.setVelocity(v[0], v[1], v[2], v[3], v[4], v[5]);
3658 nodeInfo.sensorData().setGPS(lastSignatureData.sensorData().gps());
3659 nodeInfo.sensorData().setEnvSensors(lastSignatureData.sensorData().envSensors());
3663 localGraphSize = (int)poses.size();
3664 if(!lastSignatureLocalizedPose.isNull())
3666 poses.insert(std::make_pair(lastSignatureData.id(), lastSignatureLocalizedPose));
3675 UDEBUG(
"Computing RMSE...");
3676 float translational_rmse = 0.0f;
3677 float translational_mean = 0.0f;
3678 float translational_median = 0.0f;
3679 float translational_std = 0.0f;
3680 float translational_min = 0.0f;
3681 float translational_max = 0.0f;
3682 float rotational_rmse = 0.0f;
3683 float rotational_mean = 0.0f;
3684 float rotational_median = 0.0f;
3685 float rotational_std = 0.0f;
3686 float rotational_min = 0.0f;
3687 float rotational_max = 0.0f;
3694 translational_median,
3717 UDEBUG(
"Computing RMSE...done!");
3720 std::vector<int> ids;
3724 ids.push_back(*iter);
3728 ids.push_back(iter->first);
3731 UDEBUG(
"wmState=%d", (
int)ids.size());
3741 UDEBUG(
"Empty trash...");
3749 std::string logF =
uFormat(
"%f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\n",
3753 timeLikelihoodCalculation,
3754 timePosteriorCalculation,
3755 timeHypothesesCreation,
3756 timeHypothesesValidation,
3757 timeRealTimeLimitReachedProcess,
3769 timeRetrievalDbAccess,
3770 timeAddLoopClosureLink,
3772 timeNeighborLinkRefining,
3773 timeProximityByTimeDetection,
3774 timeProximityBySpaceDetection,
3775 timeMapOptimization);
3776 std::string logI =
uFormat(
"%d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d\n",
3779 (
int)signaturesRemoved.size(),
3784 rejectedGlobalLoopClosure?1:0,
3787 int(signaturesRetrieved.size()),
3788 lcHypothesisReactivated,
3789 refUniqueWordsCount,
3793 rehearsalMaxId>0?1:0,
3811 fprintf(
_foutInt,
"%s", logI.c_str());
3814 UINFO(
"Time logging = %f...", timer.
ticks());
3817 timeFinalizingStatistics = timer.
ticks();
3818 UDEBUG(
"End process, timeFinalizingStatistics=%fs", timeFinalizingStatistics);
3834 ULOGGER_WARN(
"maxTimeAllowed < 0, then setting it to 0 (inf).");
3847 ULOGGER_DEBUG(
"Comparing new working directory path \"%s\" with \"%s\"", path.c_str(),
_wDir.c_str());
3848 if(path.compare(
_wDir) != 0)
3852 UWARN(
"Working directory has been changed from \"%s\" with \"%s\", new log files will be created.",
3853 path.c_str(),
_wDir.c_str());
3859 else if(path.empty())
3866 ULOGGER_ERROR(
"Directory \"%s\" doesn't exist!", path.c_str());
3875 bool linksRemoved =
false;
3876 for(std::multimap<int, Link>::iterator iter = links.begin(); iter!=links.end(); ++iter)
3895 linksRemoved =
true;
3906 UINFO(
"Update graph");
3908 std::multimap<int, Link> constraints;
3914 UWARN(
"Graph optimization failed after removing loop closure links from last location!");
3918 UINFO(
"Updated local map (old size=%d, new size=%d)", (
int)
_optimizedPoses.size(), (int)poses.size());
3937 UINFO(
"Update graph");
3943 if(iter->second.from() == lastId || iter->second.to() == lastId)
3959 std::multimap<int, Link> constraints;
3965 UWARN(
"Graph optimization failed after deleting the last location!");
3990 UERROR(
"Working directory not set.");
4003 int maxNearestNeighbors,
4008 std::map<int, Transform> poses;
4016 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(
new pcl::PointCloud<pcl::PointXYZ>);
4022 std::map<int, float> foundIds;
4032 float radiusSqrd = radius * radius;
4035 if(iter->first != fromId)
4037 if(stm.find(iter->first) == stm.end() &&
4039 (radiusSqrd==0 || foundIds.at(iter->first) <= radiusSqrd))
4041 (*cloud)[oi] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
4042 ids[oi++] = iter->first;
4064 pcl::CropBox<pcl::PointXYZ> cropbox;
4065 cropbox.setInputCloud(cloud);
4066 cropbox.setMin(Eigen::Vector4f(-1, -radius, -999999, 0));
4067 cropbox.setMax(Eigen::Vector4f(radius, radius, 999999, 0));
4068 cropbox.setRotation(Eigen::Vector3f(roll, pitch, yaw));
4069 cropbox.setTranslation(Eigen::Vector3f(x, y, z));
4070 cropbox.setRotation(Eigen::Vector3f(roll,pitch,yaw));
4071 pcl::IndicesPtr indices(
new std::vector<int>());
4072 cropbox.filter(*indices);
4082 pcl::search::KdTree<pcl::PointXYZ>::Ptr kdTree(
new pcl::search::KdTree<pcl::PointXYZ>);
4083 kdTree->setInputCloud(cloud, indices);
4084 std::vector<int> ind;
4085 std::vector<float> dist;
4086 pcl::PointXYZ pt(fromT.
x(), fromT.
y(), fromT.
z());
4087 kdTree->radiusSearch(pt, radius, ind, dist, maxNearestNeighbors);
4089 for(
unsigned int i=0; i<ind.size(); ++i)
4096 poses.insert(std::make_pair(ids[ind[i]], tmp));
4117 std::map<int, std::map<int, Transform> >
Rtabmap::getPaths(
const std::map<int, Transform> & posesIn,
const Transform & target,
int maxGraphDepth)
const 4119 std::map<int, std::map<int, Transform> > paths;
4120 std::set<int> nodesSet;
4121 std::map<int, Transform> poses;
4122 for(std::map<int, Transform>::const_iterator iter=posesIn.lower_bound(1); iter!=posesIn.end(); ++iter)
4124 nodesSet.insert(iter->first);
4125 poses.insert(*iter);
4129 double e0=0,e1=0,e2=0,e3=0,e4=0;
4135 std::map<int, Transform> path;
4143 UWARN(
"Nearest id of %s in %d poses is 0 !? Returning empty path.", target.
prettyPrint().c_str(), (int)poses.size());
4146 std::map<int, int> ids =
_memory->
getNeighborsId(nearestId, maxGraphDepth, 0,
true,
true,
true,
true, nodesSet);
4150 for(std::map<int, int>::iterator iter=ids.begin(); iter!=ids.end(); ++iter)
4152 std::map<int, Transform>::iterator jter = poses.find(iter->first);
4153 if(jter != poses.end())
4155 bool valid = path.empty();
4160 for(std::multimap<int, Link>::iterator kter=links.begin(); kter!=links.end() && !valid; ++kter)
4162 valid = path.find(kter->first) != path.end();
4181 UWARN(
"%s=Off but path(%d) > maxGraphDepth(%d)*2+1, nearestId=%d ids=%d. Is reduce graph activated before?",
4182 Parameters::kMemReduceGraph().c_str(), (
int)path.size(), maxGraphDepth, nearestId, (int)ids.size());
4184 paths.insert(std::make_pair(nearestId, path));
4188 UWARN(
uFormat(
"path.size()=0!? nearestId=%d ids=%d, aborting...", (
int)path.size(), nearestId, (int)ids.size()).c_str());
4194 UDEBUG(
"e0=%fs e1=%fs e2=%fs e3=%fs e4=%fs", e0, e1, e2, e3, e4);
4201 bool lookInDatabase,
4202 std::map<int, Transform> & optimizedPoses,
4203 cv::Mat & covariance,
4204 std::multimap<int, Link> * constraints,
4206 int * iterationsDone)
const 4209 UINFO(
"Optimize map: around location %d (lookInDatabase=%s)",
id, lookInDatabase?
"true":
"false");
4216 id = ids.begin()->first;
4218 UINFO(
"get %d ids time %f s", (
int)ids.size(), timer.
ticks());
4220 std::map<int, Transform> poses =
Rtabmap::optimizeGraph(
id,
uKeysSet(ids), optimizedPoses, lookInDatabase, covariance, constraints, error, iterationsDone);
4225 optimizedPoses = poses;
4235 UWARN(
"Failed to optimize the graph! returning empty optimized poses...");
4236 optimizedPoses.clear();
4239 constraints->clear();
4247 const std::set<int> & ids,
4248 const std::map<int, Transform> & guessPoses,
4249 bool lookInDatabase,
4250 cv::Mat & covariance,
4251 std::multimap<int, Link> * constraints,
4253 int * iterationsDone)
const 4256 std::map<int, Transform> optimizedPoses;
4257 std::map<int, Transform> poses;
4258 std::multimap<int, Link> edgeConstraints;
4259 UDEBUG(
"ids=%d", (
int)ids.size());
4261 UINFO(
"get constraints (ids=%d, %d poses, %d edges) time %f s", (
int)ids.size(), (int)poses.size(), (int)edgeConstraints.size(), timer.
ticks());
4265 for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
4268 std::map<int, Transform>::const_iterator foundGuess = guessPoses.find(iter->first);
4269 if(foundGuess!=guessPoses.end())
4271 iter->second = foundGuess->second;
4281 optimizedPoses = poses;
4284 *constraints = edgeConstraints;
4289 bool hasLandmarks = edgeConstraints.begin()->first < 0;
4290 if(poses.size() != guessPoses.size() || hasLandmarks)
4292 UDEBUG(
"recompute poses using only links (robust to multi-session)");
4293 std::map<int, Transform> posesOut;
4294 std::multimap<int, Link> edgeConstraintsOut;
4296 optimizedPoses =
_graphOptimizer->
optimize(fromId, posesOut, edgeConstraintsOut, covariance, 0, error, iterationsDone);
4299 *constraints = edgeConstraintsOut;
4304 UDEBUG(
"use input guess poses");
4305 optimizedPoses =
_graphOptimizer->
optimize(fromId, poses, edgeConstraints, covariance, 0, error, iterationsDone);
4308 *constraints = edgeConstraints;
4312 if(!poses.empty() && optimizedPoses.empty())
4314 UWARN(
"Optimization has failed (poses=%d, guess=%d, links=%d)...",
4315 (
int)poses.size(), (int)guessPoses.size(), (int)edgeConstraints.size());
4319 UINFO(
"Optimization time %f s", timer.
ticks());
4321 return optimizedPoses;
4329 if(likelihood.size()==0)
4335 std::list<float> values;
4336 bool likelihoodNullValuesIgnored =
true;
4337 for(std::map<int, float>::iterator iter = ++likelihood.begin(); iter!=likelihood.end(); ++iter)
4339 if((iter->second >= 0 && !likelihoodNullValuesIgnored) ||
4340 (iter->second > 0 && likelihoodNullValuesIgnored))
4342 values.push_back(iter->second);
4345 UDEBUG(
"values.size=%d", values.size());
4347 float mean =
uMean(values);
4355 for(std::map<int, float>::iterator iter=++likelihood.begin(); iter!= likelihood.end(); ++iter)
4357 float value = iter->second;
4358 if(value > mean+stdDev && mean)
4360 iter->second = (value-(stdDev-epsilon))/mean;
4364 maxId = iter->first;
4367 else if(value == 1.0
f && stdDev == 0)
4369 iter->second = 1.0f;
4373 maxId = iter->first;
4378 iter->second = 1.0f;
4382 if(stdDev > epsilon && max)
4384 likelihood.begin()->second = mean/stdDev + 1.0f;
4388 likelihood.begin()->second = 2.0f;
4391 double time = timer.ticks();
4392 UDEBUG(
"mean=%f, stdDev=%f, max=%f, maxId=%d, time=%fs", mean, stdDev, max, maxId, time);
4401 UERROR(
"Working directory not set.");
4404 std::list<int> signaturesToCompare;
4415 signaturesToCompare.push_back(iter->first);
4421 signaturesToCompare.push_back(iter->first);
4427 std::string fileName = this->
getWorkingDir() +
"/DumpPrediction.txt";
4429 fopen_s(&fout, fileName.c_str(),
"w");
4431 fout = fopen(fileName.c_str(),
"w");
4436 for(
int i=0; i<prediction.rows; ++i)
4438 for(
int j=0; j<prediction.cols; ++j)
4440 fprintf(fout,
"%f ",((
float*)prediction.data)[j + i*prediction.cols]);
4442 fprintf(fout,
"\n");
4449 UWARN(
"Memory and/or the Bayes filter are not created");
4464 std::vector<float> velocity;
4467 _memory->
getNodeInfo(
id, odomPoseLocal, mapId, weight, label, stamp, groundTruth, velocity, gps, sensors,
true);
4470 if(images || scan || userData || occupancyGrid)
4474 if(!images && withWords)
4476 std::vector<CameraModel> models;
4493 for(std::multimap<int, Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
4505 if(withWords || withGlobalDescriptors)
4507 std::multimap<int, int> words;
4508 std::vector<cv::KeyPoint> wordsKpts;
4509 std::vector<cv::Point3f> words3;
4510 cv::Mat wordsDescriptors;
4511 std::vector<rtabmap::GlobalDescriptor> globalDescriptors;
4515 s.
setWords(words, wordsKpts, words3, wordsDescriptors);
4517 if(withGlobalDescriptors)
4522 if(velocity.size()==6)
4524 s.
setVelocity(velocity[0], velocity[1], velocity[2], velocity[3], velocity[4], velocity[5]);
4532 void Rtabmap::get3DMap(
4533 std::map<int, Signature> & signatures,
4534 std::map<int, Transform> & poses,
4535 std::multimap<int, Link> & constraints,
4540 return getGraph(poses, constraints, optimized, global, &signatures,
true,
true,
true,
true);
4544 std::map<int, Transform> & poses,
4545 std::multimap<int, Link> & constraints,
4548 std::map<int, Signature> * signatures,
4554 bool withGlobalDescriptors)
const 4593 for(std::set<int>::iterator iter = ids.begin(); iter!=ids.end(); ++iter)
4595 signatures->insert(std::make_pair(*iter,
getSignatureCopy(*iter, withImages, withScan, withUserData, withGrid, withWords, withGlobalDescriptors)));
4601 UERROR(
"Last working signature is null!?");
4605 UWARN(
"Memory not initialized...");
4616 UDEBUG(
"nodeId=%d, radius=%f", nodeId, radius);
4617 std::map<int, Transform> nearNodes;
4630 float clusterRadius,
4641 UERROR(
"Cannot detect more loop closures if graph optimization iterations = 0");
4646 UERROR(
"Detecting more loop closures can be done only in RGBD-SLAM mode.");
4649 if(!intraSession && !interSession)
4651 UERROR(
"Intra and/or inter session argument should be true.");
4655 std::list<Link> loopClosuresAdded;
4656 std::multimap<int, int> checkedLoopClosures;
4658 std::map<int, Transform> posesToCheckLoopClosures;
4659 std::map<int, Transform> poses;
4660 std::multimap<int, Link> links;
4661 std::map<int, Signature> signatures;
4662 this->
getGraph(poses, links,
true,
true, &signatures);
4664 std::map<int, int> mapIds;
4665 UDEBUG(
"remove all invalid or intermediate nodes, fill mapIds");
4666 for(std::map<int, Transform>::iterator iter=poses.upper_bound(0); iter!=poses.end();++iter)
4668 if(signatures.at(iter->first).getWeight() >= 0)
4670 posesToCheckLoopClosures.insert(*iter);
4671 mapIds.insert(std::make_pair(iter->first, signatures.at(iter->first).mapId()));
4675 for(
int n=0; n<iterations; ++n)
4677 UINFO(
"Looking for more loop closures, clustering poses... (iteration=%d/%d, radius=%f m angle=%f rad)",
4678 n+1, iterations, clusterRadius, clusterAngle);
4681 posesToCheckLoopClosures,
4685 UINFO(
"Looking for more loop closures, clustering poses... found %d clusters.", (
int)clusters.size());
4688 std::set<int> addedLinks;
4689 for(std::multimap<int, int>::iterator iter=clusters.begin(); iter!= clusters.end(); ++iter, ++i)
4691 if(processState && processState->
isCanceled())
4697 int from = iter->first;
4698 int to = iter->second;
4701 from = iter->second;
4705 int mapIdFrom =
uValue(mapIds, from, 0);
4706 int mapIdTo =
uValue(mapIds, to, 0);
4708 if((interSession && mapIdFrom != mapIdTo) ||
4709 (intraSession && mapIdFrom == mapIdTo))
4712 bool alreadyChecked =
false;
4713 for(std::multimap<int, int>::iterator jter = checkedLoopClosures.lower_bound(from);
4714 !alreadyChecked && jter!=checkedLoopClosures.end() && jter->first == from;
4717 if(to == jter->second)
4719 alreadyChecked =
true;
4726 if(addedLinks.find(from) == addedLinks.end() &&
4727 addedLinks.find(to) == addedLinks.end() &&
4730 checkedLoopClosures.insert(std::make_pair(from, to));
4732 UASSERT(signatures.find(from) != signatures.end());
4733 UASSERT(signatures.find(to) != signatures.end());
4738 guess = poses.at(from).
inverse() * poses.at(to);
4747 bool updateConstraints =
true;
4753 int mapId = signatures.at(from).mapId();
4755 for(std::map<int, Signature>::iterator ster=signatures.begin(); ster!=signatures.end(); ++ster)
4757 if(ster->second.mapId() == mapId)
4759 fromId = ster->first;
4763 std::multimap<int, Link> linksIn = links;
4765 const Link * maxLinearLink = 0;
4766 const Link * maxAngularLink = 0;
4767 float maxLinearError = 0.0f;
4768 float maxAngularError = 0.0f;
4769 float maxLinearErrorRatio = 0.0f;
4770 float maxAngularErrorRatio = 0.0f;
4771 std::map<int, Transform> optimizedPoses;
4772 std::multimap<int, Link> links;
4773 UASSERT(poses.find(fromId) != poses.end());
4774 UASSERT_MSG(poses.find(from) != poses.end(),
uFormat(
"id=%d poses=%d links=%d", from, (
int)poses.size(), (int)links.size()).c_str());
4775 UASSERT_MSG(poses.find(to) != poses.end(),
uFormat(
"id=%d poses=%d links=%d", to, (
int)poses.size(), (int)links.size()).c_str());
4777 UASSERT(optimizedPoses.find(fromId) != optimizedPoses.end());
4778 UASSERT_MSG(optimizedPoses.find(from) != optimizedPoses.end(),
uFormat(
"id=%d poses=%d links=%d", from, (
int)optimizedPoses.size(), (int)links.size()).c_str());
4779 UASSERT_MSG(optimizedPoses.find(to) != optimizedPoses.end(),
uFormat(
"id=%d poses=%d links=%d", to, (
int)optimizedPoses.size(), (int)links.size()).c_str());
4783 if(optimizedPoses.size())