29 #include "rtabmap/core/Version.h"
63 #include <mrpt/poses/CPose3DPDFGaussian.h>
66 #include <pcl/search/kdtree.h>
67 #include <pcl/filters/crop_box.h>
68 #include <pcl/io/pcd_io.h>
69 #include <pcl/common/common.h>
70 #include <pcl/TextureMesh.h>
75 #define LOG_F "LogF.txt"
76 #define LOG_I "LogI.txt"
78 #define GRAPH_FILE_NAME "Graph.dot"
95 _publishStats(
Parameters::defaultRtabmapPublishStats()),
96 _publishLastSignatureData(
Parameters::defaultRtabmapPublishLastSignature()),
97 _publishPdf(
Parameters::defaultRtabmapPublishPdf()),
98 _publishLikelihood(
Parameters::defaultRtabmapPublishLikelihood()),
99 _publishRAMUsage(
Parameters::defaultRtabmapPublishRAMUsage()),
100 _computeRMSE(
Parameters::defaultRtabmapComputeRMSE()),
101 _saveWMState(
Parameters::defaultRtabmapSaveWMState()),
102 _maxTimeAllowed(
Parameters::defaultRtabmapTimeThr()),
103 _maxMemoryAllowed(
Parameters::defaultRtabmapMemoryThr()),
104 _loopThr(
Parameters::defaultRtabmapLoopThr()),
105 _loopRatio(
Parameters::defaultRtabmapLoopRatio()),
106 _aggressiveLoopThr(
Parameters::defaultRGBDAggressiveLoopThr()),
107 _virtualPlaceLikelihoodRatio(
Parameters::defaultRtabmapVirtualPlaceLikelihoodRatio()),
108 _maxLoopClosureDistance(
Parameters::defaultRGBDMaxLoopClosureDistance()),
109 _verifyLoopClosureHypothesis(
Parameters::defaultVhEpEnabled()),
110 _maxRetrieved(
Parameters::defaultRtabmapMaxRetrieved()),
111 _maxLocalRetrieved(
Parameters::defaultRGBDMaxLocalRetrieved()),
112 _maxRepublished(
Parameters::defaultRtabmapMaxRepublished()),
113 _rawDataKept(
Parameters::defaultMemImageKept()),
114 _statisticLogsBufferedInRAM(
Parameters::defaultRtabmapStatisticLogsBufferedInRAM()),
115 _statisticLogged(
Parameters::defaultRtabmapStatisticLogged()),
116 _statisticLoggedHeaders(
Parameters::defaultRtabmapStatisticLoggedHeaders()),
117 _rgbdSlamMode(
Parameters::defaultRGBDEnabled()),
118 _rgbdLinearUpdate(
Parameters::defaultRGBDLinearUpdate()),
119 _rgbdAngularUpdate(
Parameters::defaultRGBDAngularUpdate()),
120 _rgbdLinearSpeedUpdate(
Parameters::defaultRGBDLinearSpeedUpdate()),
121 _rgbdAngularSpeedUpdate(
Parameters::defaultRGBDAngularSpeedUpdate()),
122 _newMapOdomChangeDistance(
Parameters::defaultRGBDNewMapOdomChangeDistance()),
123 _neighborLinkRefining(
Parameters::defaultRGBDNeighborLinkRefining()),
124 _proximityByTime(
Parameters::defaultRGBDProximityByTime()),
125 _proximityBySpace(
Parameters::defaultRGBDProximityBySpace()),
126 _scanMatchingIdsSavedInLinks(
Parameters::defaultRGBDScanMatchingIdsSavedInLinks()),
127 _loopClosureIdentityGuess(
Parameters::defaultRGBDLoopClosureIdentityGuess()),
128 _localRadius(
Parameters::defaultRGBDLocalRadius()),
129 _localImmunizationRatio(
Parameters::defaultRGBDLocalImmunizationRatio()),
130 _proximityMaxGraphDepth(
Parameters::defaultRGBDProximityMaxGraphDepth()),
131 _proximityMaxPaths(
Parameters::defaultRGBDProximityMaxPaths()),
132 _proximityMaxNeighbors(
Parameters::defaultRGBDProximityPathMaxNeighbors()),
133 _proximityFilteringRadius(
Parameters::defaultRGBDProximityPathFilteringRadius()),
134 _proximityRawPosesUsed(
Parameters::defaultRGBDProximityPathRawPosesUsed()),
136 _proximityOdomGuess(
Parameters::defaultRGBDProximityOdomGuess()),
137 _proximityMergedScanCovFactor(
Parameters::defaultRGBDProximityMergedScanCovFactor()),
139 _optimizeFromGraphEnd(
Parameters::defaultRGBDOptimizeFromGraphEnd()),
140 _optimizationMaxError(
Parameters::defaultRGBDOptimizeMaxError()),
141 _startNewMapOnLoopClosure(
Parameters::defaultRtabmapStartNewMapOnLoopClosure()),
142 _startNewMapOnGoodSignature(
Parameters::defaultRtabmapStartNewMapOnGoodSignature()),
143 _goalReachedRadius(
Parameters::defaultRGBDGoalReachedRadius()),
144 _goalsSavedInUserData(
Parameters::defaultRGBDGoalsSavedInUserData()),
145 _pathStuckIterations(
Parameters::defaultRGBDPlanStuckIterations()),
146 _pathLinearVelocity(
Parameters::defaultRGBDPlanLinearVelocity()),
147 _pathAngularVelocity(
Parameters::defaultRGBDPlanAngularVelocity()),
148 _forceOdom3doF(
Parameters::defaultRGBDForceOdom3DoF()),
149 _restartAtOrigin(
Parameters::defaultRGBDStartAtOrigin()),
150 _loopCovLimited(
Parameters::defaultRGBDLoopCovLimited()),
151 _loopGPS(
Parameters::defaultRtabmapLoopGPS()),
152 _maxOdomCacheSize(
Parameters::defaultRGBDMaxOdomCacheSize()),
153 _localizationSmoothing(
Parameters::defaultRGBDLocalizationSmoothing()),
154 _localizationPriorInf(1.0/(
Parameters::defaultRGBDLocalizationPriorError()*
Parameters::defaultRGBDLocalizationPriorError())),
155 _createGlobalScanMap(
Parameters::defaultRGBDProximityGlobalScanMap()),
156 _markerPriorsLinearVariance(
Parameters::defaultMarkerPriorsVarianceLinear()),
157 _markerPriorsAngularVariance(
Parameters::defaultMarkerPriorsVarianceAngular()),
158 _loopClosureHypothesis(0,0.0
f),
159 _highestHypothesis(0,0.0
f),
160 _lastProcessTime(0.0),
161 _someNodesHaveBeenTransferred(
false),
162 _distanceTravelled(0.0
f),
163 _distanceTravelledSinceLastLocalization(0.0
f),
164 _optimizeFromGraphEndChanged(
false),
165 _epipolarGeometry(0),
172 _mapCorrection(
Transform::getIdentity()),
173 _lastLocalizationNodeId(0),
174 _currentSessionHasGPS(
false),
176 _pathCurrentIndex(0),
178 _pathTransformToGoal(
Transform::getIdentity()),
180 _pathStuckDistance(0.0
f)
181 #ifdef RTABMAP_PYTHON
209 std::string attributes =
"a+";
232 fprintf(
_foutFloat,
" 1-Total iteration time (s)\n");
233 fprintf(
_foutFloat,
" 2-Memory update time (s)\n");
234 fprintf(
_foutFloat,
" 3-Retrieval time (s)\n");
235 fprintf(
_foutFloat,
" 4-Likelihood time (s)\n");
236 fprintf(
_foutFloat,
" 5-Posterior time (s)\n");
237 fprintf(
_foutFloat,
" 6-Hypothesis selection time (s)\n");
238 fprintf(
_foutFloat,
" 7-Hypothesis validation time (s)\n");
239 fprintf(
_foutFloat,
" 8-Transfer time (s)\n");
240 fprintf(
_foutFloat,
" 9-Statistics creation time (s)\n");
241 fprintf(
_foutFloat,
" 10-Loop closure hypothesis value\n");
247 fprintf(
_foutFloat,
" 16-Virtual place hypothesis\n");
248 fprintf(
_foutFloat,
" 17-Join trash time (s)\n");
249 fprintf(
_foutFloat,
" 18-Weight Update (rehearsal) similarity\n");
250 fprintf(
_foutFloat,
" 19-Empty trash time (s)\n");
251 fprintf(
_foutFloat,
" 20-Retrieval database access time (s)\n");
252 fprintf(
_foutFloat,
" 21-Add loop closure link time (s)\n");
253 fprintf(
_foutFloat,
" 22-Memory cleanup time (s)\n");
254 fprintf(
_foutFloat,
" 23-Scan matching (odometry correction) time (s)\n");
255 fprintf(
_foutFloat,
" 24-Local time loop closure detection time (s)\n");
256 fprintf(
_foutFloat,
" 25-Local space loop closure detection time (s)\n");
257 fprintf(
_foutFloat,
" 26-Map optimization (s)\n");
261 fprintf(
_foutInt,
"Column headers:\n");
262 fprintf(
_foutInt,
" 1-Loop closure ID\n");
263 fprintf(
_foutInt,
" 2-Highest loop closure hypothesis\n");
264 fprintf(
_foutInt,
" 3-Locations transferred\n");
266 fprintf(
_foutInt,
" 5-Words extracted from the last image\n");
267 fprintf(
_foutInt,
" 6-Vocabulary size\n");
268 fprintf(
_foutInt,
" 7-Working memory size\n");
269 fprintf(
_foutInt,
" 8-Is loop closure hypothesis rejected?\n");
272 fprintf(
_foutInt,
" 11-Locations retrieved\n");
273 fprintf(
_foutInt,
" 12-Retrieval location ID\n");
274 fprintf(
_foutInt,
" 13-Unique words extraced from last image\n");
275 fprintf(
_foutInt,
" 14-Retrieval ID\n");
276 fprintf(
_foutInt,
" 15-Non-null likelihood values\n");
277 fprintf(
_foutInt,
" 16-Weight Update ID\n");
278 fprintf(
_foutInt,
" 17-Is last location merged through Weight Update?\n");
279 fprintf(
_foutInt,
" 18-Local graph size\n");
280 fprintf(
_foutInt,
" 19-Sensor data id\n");
281 fprintf(
_foutInt,
" 20-Indexed words\n");
282 fprintf(
_foutInt,
" 21-Index memory usage (KB)\n");
292 UWARN(
"Working directory is not set, log disabled!");
322 UDEBUG(
"path=%s", databasePath.c_str());
331 UWARN(
"Using empty database. Mapping session will not be saved unless it is closed with an output database path.");
344 allParameters.erase(Parameters::kRtabmapWorkingDirectory());
349 uInsert(allParameters, parameters);
350 ParametersMap::const_iterator
iter;
351 if((
iter=allParameters.find(Parameters::kRtabmapWorkingDirectory())) != allParameters.end())
391 UWARN(
"last localization pose is ignored (%s=true), assuming we start at the origin of the map.", Parameters::kRGBDStartAtOrigin().
c_str());
396 UINFO(
"Loaded optimizedPoses=%d firstPose %d=%s lastLocalizationPose=%s",
404 std::map<int, Transform> tmp;
411 std::map<int, float> likelihood;
417 likelihood.insert(std::make_pair(
iter->first, 0));
428 UINFO(
"Loaded optimizedPoses=0, last localization pose is ignored!");
436 std::map<int, Transform> tmp;
449 void Rtabmap::init(
const std::string & configFile,
const std::string & databasePath,
bool loadDatabaseParameters)
454 if(!configFile.empty())
456 ULOGGER_DEBUG(
"Read parameters from = %s", configFile.c_str());
460 this->
init(param, databasePath, loadDatabaseParameters);
465 UINFO(
"databaseSaved=%d", databaseSaved?1:0);
514 std::map<int, int> reducedIds;
516 for(std::map<int, int>::iterator
iter=reducedIds.begin();
iter!=reducedIds.end(); ++
iter)
555 ParametersMap::const_iterator
iter;
556 if((
iter=parameters.find(Parameters::kRtabmapWorkingDirectory())) != parameters.end())
631 double localizationPriorError = Parameters::defaultRGBDLocalizationPriorError();
632 Parameters::parse(parameters, Parameters::kRGBDLocalizationPriorError(), localizationPriorError);
633 UASSERT(localizationPriorError>0.0);
641 std::string markerPriorsStr;
645 std::list<std::string> strList =
uSplit(markerPriorsStr,
'|');
646 for(std::list<std::string>::iterator
iter=strList.begin();
iter!=strList.end(); ++
iter)
648 std::string markerStr = *
iter;
649 while(!markerStr.empty() && !
uIsDigit(markerStr[0]))
651 markerStr.erase(markerStr.begin());
653 if(!markerStr.empty())
658 if(!
prior.isNull() &&
id>0)
661 UDEBUG(
"Added landmark prior %d: %s",
id,
prior.prettyPrint().c_str());
665 UERROR(
"Failed to parse element \"%s\" in parameter %s", markerStr.c_str(), Parameters::kMarkerPriors().c_str());
668 else if(!
iter->empty())
670 UERROR(
"Failed to parse parameter %s, value=\"%s\"", Parameters::kMarkerPriors().
c_str(),
iter->c_str());
686 if((
iter=parameters.find(Parameters::kOptimizerStrategy())) != parameters.end())
692 UDEBUG(
"new detector strategy %d",
int(optimizerType));
707 optimizerType = (
Optimizer::Type)Parameters::defaultOptimizerStrategy();
722 UWARN(
"Map is now incremental, clearing global scan map...");
791 std::map<int, int> weights;
806 return std::set<int>();
846 return Parameters::defaultMemGenerateIds();
882 UWARN(
"Initial pose can only be set in localization mode (%s=false), ignoring it...", Parameters::kMemIncrementalMemory().
c_str());
908 std::map<int, int> reducedIds;
910 UINFO(
"New map triggered, new map = %d", mapId);
920 if(reducedIds.size() &&
_path.size())
922 for(
unsigned int i=0;
i<
_path.size(); ++
i)
924 std::map<int, int>::const_iterator
iter = reducedIds.find(
_path[
i].
first);
925 if(
iter!= reducedIds.end())
951 if(!nearestNodes.empty())
957 UERROR(
"No nodes found inside %s=%fm of the current pose (%s). Cannot set label \"%s\"",
958 Parameters::kRGBDLocalRadius().
c_str(),
966 UERROR(
"Last signature is null! Cannot set label \"%s\"", label.c_str());
986 UERROR(
"Last signature is null! Cannot set user data!");
1004 ids.insert(std::pair<int,int>(
id, 0));
1005 std::set<int> idsSet;
1006 for(std::map<int, int>::iterator
iter = ids.begin();
iter!=ids.end(); ++
iter)
1008 idsSet.insert(idsSet.end(),
iter->first);
1014 UERROR(
"No neighbors found for signature %d.",
id);
1028 std::map<int, Transform> poses;
1029 std::multimap<int, Link> constraints;
1042 std::map<int, double> stamps;
1045 for(std::map<int, Transform>::iterator
iter=poses.begin();
iter!=poses.end(); ++
iter)
1051 std::vector<float>
v;
1054 _memory->
getNodeInfo(
iter->first, o,
m,
w, l, stamp,
g,
v, gps, sensors,
true);
1055 stamps.insert(std::make_pair(
iter->first, stamp));
1102 UERROR(
"RTAB-Map is not initialized. No memory to reset...");
1149 const cv::Mat & image,
1151 const std::map<std::string, float> & externalStats)
1158 float odomLinearVariance,
1159 float odomAngularVariance,
1160 const std::vector<float> & odomVelocity,
1161 const std::map<std::string, float> & externalStats)
1168 cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1);
1169 covariance.at<
double>(0,0) = odomLinearVariance;
1170 covariance.at<
double>(1,1) = odomLinearVariance;
1171 covariance.at<
double>(2,2) = odomLinearVariance;
1172 covariance.at<
double>(3,3) = odomAngularVariance;
1173 covariance.at<
double>(4,4) = odomAngularVariance;
1174 covariance.at<
double>(5,5) = odomAngularVariance;
1175 return process(
data, odomPose, covariance, odomVelocity, externalStats);
1180 const cv::Mat & odomCovariance,
1181 const std::vector<float> & odomVelocity,
1182 const std::map<std::string, float> & externalStats)
1191 double timeMemoryUpdate = 0;
1192 double timeNeighborLinkRefining = 0;
1193 double timeProximityByTimeDetection = 0;
1194 double timeProximityBySpaceSearch = 0;
1195 double timeProximityBySpaceVisualDetection = 0;
1196 double timeProximityBySpaceDetection = 0;
1197 double timeCleaningNeighbors = 0;
1198 double timeReactivations = 0;
1199 double timeAddLoopClosureLink = 0;
1200 double timeMapOptimization = 0;
1201 double timeRetrievalDbAccess = 0;
1202 double timeLikelihoodCalculation = 0;
1203 double timePosteriorCalculation = 0;
1204 double timeHypothesesCreation = 0;
1205 double timeHypothesesValidation = 0;
1206 double timeRealTimeLimitReachedProcess = 0;
1207 double timeMemoryCleanup = 0;
1208 double timeEmptyingTrash = 0;
1209 double timeFinalizingStatistics = 0;
1210 double timeJoiningTrash = 0;
1211 double timeStatsCreation = 0;
1213 float hypothesisRatio = 0.0f;
1214 bool rejectedGlobalLoopClosure =
false;
1216 std::map<int, float> rawLikelihood;
1217 std::map<int, float> adjustedLikelihood;
1218 std::map<int, float> likelihood;
1219 std::map<int, int> weights;
1220 std::map<int, float> posterior;
1221 std::list<std::pair<int, float> > reactivateHypotheses;
1223 std::map<int, int> childCount;
1224 std::set<int> signaturesRetrieved;
1225 int proximityDetectionsInTimeFound = 0;
1234 std::set<int> immunizedLocations;
1237 for(std::map<std::string, float>::const_iterator
iter=externalStats.begin();
iter!=externalStats.end(); ++
iter)
1257 bool fakeOdom =
false;
1265 odomPose = odomPose.
to3DoF();
1271 UWARN(
"Input odometry is not invertible! pose = %s\n"
1276 "Trying to normalize rotation to see if it makes it invertible...",
1278 odomPose.
r11(), odomPose.
r12(), odomPose.
r13(), odomPose.
o14(),
1279 odomPose.
r21(), odomPose.
r22(), odomPose.
r23(), odomPose.
o24(),
1280 odomPose.
r31(), odomPose.
r32(), odomPose.
r33(), odomPose.
o34());
1287 odomPose.
r11(), odomPose.
r12(), odomPose.
r13(), odomPose.
o14(),
1288 odomPose.
r21(), odomPose.
r22(), odomPose.
r23(), odomPose.
o24(),
1289 odomPose.
r31(), odomPose.
r32(), odomPose.
r33(), odomPose.
o34()).c_str());
1290 UWARN(
"Normalizing rotation succeeded! fixed pose = %s\n"
1295 "If the resulting rotation is very different from original one, try to fix the odometry or TF.",
1297 odomPose.
r11(), odomPose.
r12(), odomPose.
r13(), odomPose.
o14(),
1298 odomPose.
r21(), odomPose.
r22(), odomPose.
r23(), odomPose.
o24(),
1299 odomPose.
r31(), odomPose.
r32(), odomPose.
r33(), odomPose.
o34());
1303 UDEBUG(
"incremental=%d odomPose=%s optimizedPoses=%d mapCorrection=%s lastLocalizationPose=%s lastLocalizationNodeId=%d",
1336 UWARN(
"Update map correction based on last localization saved in database! correction = %s, nearest id = %d of last pose = %s, odom = %s",
1360 iter->second = mapCorrectionInv *
iter->second;
1365 UWARN(
"Transformed map accordingly to last localization pose saved in database (%s=true)! nearest id = %d of last pose = %s",
1366 Parameters::kRGBDOptimizeFromGraphEnd().
c_str(),
1376 UERROR(
"RGB-D SLAM mode is enabled, memory is incremental but no odometry is provided. "
1377 "Image %d is ignored!",
data.id());
1409 UWARN(
"Odometry is reset (identity pose detected). Increment map id to %d!", mapId);
1420 UWARN(
"Odometry is reset (large odometry change detected > %f). A new map (%d) is created! Last pose = %s, new pose = %s",
1454 UFATAL(
"Not supposed to be here...last signature is null?!?");
1458 timeMemoryUpdate =
timer.ticks();
1459 ULOGGER_INFO(
"timeMemoryUpdate=%fs", timeMemoryUpdate);
1464 bool smallDisplacement =
false;
1465 bool tooFastMovement =
false;
1466 std::list<int> signaturesRemoved;
1467 bool neighborLinkRefined =
false;
1468 bool addedNewLandmark =
false;
1469 float distanceToClosestNodeInTheGraph = 0;
1470 float angleToClosestNodeInTheGraph = 0;
1473 double linVar = odomCovariance.empty()?1.0f:
uMax3(odomCovariance.at<
double>(0,0), odomCovariance.at<
double>(1,1)>=9999?0:odomCovariance.at<
double>(1,1), odomCovariance.at<
double>(2,2)>=9999?0:odomCovariance.at<
double>(2,2));
1474 double angVar = odomCovariance.empty()?1.0f:
uMax3(odomCovariance.at<
double>(3,3)>=9999?0:odomCovariance.at<
double>(3,3), odomCovariance.at<
double>(4,4)>=9999?0:odomCovariance.at<
double>(4,4), odomCovariance.at<
double>(5,5));
1495 const std::multimap<int, Link> & links = signature->
getLinks();
1501 if(signature->
getWeight() < 0 ||
s->getWeight() >= 0)
1503 t = links.begin()->second.transform();
1526 smallDisplacement =
true;
1531 if(odomVelocity.size() == 6)
1543 bool intermediateNodeRefining =
false;
1551 int oldId = signature->
getLinks().begin()->first;
1559 if(smallDisplacement)
1561 if(signature->
getLinks().begin()->second.transVariance() == 1)
1564 UDEBUG(
"Set small variance. The robot is not moving.");
1578 UINFO(
"Odometry refining: update neighbor link (%d->%d, variance:lin=%f, ang=%f) from %s to %s",
1581 info.covariance.at<
double>(0,0),
1582 info.covariance.at<
double>(5,5),
1584 t.prettyPrint().c_str());
1585 UASSERT(
info.covariance.at<
double>(0,0) > 0.0 &&
info.covariance.at<
double>(5,5) > 0.0);
1594 std::map<int, Transform>::iterator jter =
_optimizedPoses.find(oldId);
1600 iter->second = mapCorrectionInv * up *
iter->second;
1606 UINFO(
"Odometry refining rejected: %s",
info.rejectedMsg.c_str());
1607 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)
1611 std::cout <<
info.covariance << std::endl;
1616 neighborLinkRefined = !
t.isNull();
1625 timeNeighborLinkRefining =
timer.ticks();
1626 ULOGGER_INFO(
"timeOdometryRefining=%fs", timeNeighborLinkRefining);
1637 UERROR(
"Map correction should be identity when optimizing from the last node. T=%s NewPose=%s OldPose=%s",
1646 intermediateNodeRefining =
true;
1657 int closestNode = 0;
1658 float sqrdDistance = 0.0f;
1668 if(closestNode>0 && sqrdDistance>0.0
f)
1670 distanceToClosestNodeInTheGraph =
sqrt(sqrdDistance);
1671 UDEBUG(
"Last localization pose = %s, closest node=%d (%f m)", newPose.
prettyPrint().c_str(), closestNode, distanceToClosestNodeInTheGraph);
1686 UDEBUG(
"Added landmark %d : %s",
iter->first, (newPose*
iter->second.transform()).prettyPrint().c_str());
1687 addedNewLandmark =
true;
1701 "Only forward links should be added.");
1703 Link tmp = signature->
getLinks().begin()->second.inverse();
1705 if(!smallDisplacement)
1716 if(
s->getWeight() == -1)
1727 !smallDisplacement &&
1728 odomCovariance.cols == 6 &&
1729 odomCovariance.rows == 6 &&
1730 odomCovariance.type() == CV_64FC1 &&
1731 odomCovariance.at<
double>(0,0) < 1)
1744 mrpt::math::CMatrixDouble66 gaussian;
1745 gaussian.loadFromRawPointer((
const double*)odomCovariance.data);
1746 mrpt::poses::CPose3DPDFGaussian gaussianTransformed(mrpt::poses::CPose3D(), gaussian);
1747 gaussianTransformed.changeCoordinatesReference(pose);
1764 if(!smallDisplacement)
1780 odomCovariance.inv());
1796 for(
unsigned int i=0;
i<
_path.size(); ++
i)
1815 if(jter->second.from() ==
iter->first || jter->second.to() ==
iter->first)
1837 for(std::set<int>::const_reverse_iterator
iter = stm.rbegin();
iter!=stm.rend(); ++
iter)
1839 if(*
iter != signature->
id() &&
1844 std::string rejectedMsg;
1845 UDEBUG(
"Check local transform between %d and %d", signature->
id(), *
iter);
1859 UDEBUG(
"Add local loop closure in TIME (%d->%d) %s",
1864 UASSERT(
info.covariance.at<
double>(0,0) > 0.0 &&
info.covariance.at<
double>(5,5) > 0.0);
1867 ++proximityDetectionsInTimeFound;
1868 UINFO(
"Local loop closure found between %d and %d with t=%s",
1873 UWARN(
"Cannot add local loop closure between %d and %d ?!?",
1879 UINFO(
"Local loop closure (time) between %d and %d rejected: %s",
1880 *
iter, signature->
id(), rejectedMsg.c_str());
1893 timeProximityByTimeDetection =
timer.ticks();
1894 UINFO(
"timeProximityByTimeDetection=%fs", timeProximityByTimeDetection);
1899 bool localizationOnPreviousUpdate =
false;
1902 localizationOnPreviousUpdate =
1904 signature->
getLinks().begin()->first!=signature->
id() &&
1912 int localizationLinks = 0;
1913 int previousIdWithLocalizationLink = 0;
1917 if(previousIdWithLocalizationLink ==
iter->first)
1929 ++localizationLinks;
1930 previousIdWithLocalizationLink =
iter->first;
1934 localizationOnPreviousUpdate = localizationLinks > 1;
1938 if(!signature->
isBadSignature() && signature->
getWeight()>=0 && (!smallDisplacement || !localizationOnPreviousUpdate) && !tooFastMovement)
1952 std::list<int> signaturesToCompare;
1965 for(std::map<int, float>::reverse_iterator
iter=nearestIds.rbegin();
iter!=nearestIds.rend() &&
iter->first>0; ++
iter)
1969 if(
s->sensorData().gps().stamp() > 0.0)
1971 originGPS =
s->sensorData().gps();
1983 if(originGPS.
stamp() > 0.0)
1998 if(
s->getWeight() != -1)
2001 if(originGPS.
stamp()>0.0)
2006 GPS gps =
s->sensorData().gps();
2008 if(gps.
stamp()==0.0)
2012 if(gps.
stamp() > 0.0)
2015 std::make_pair(
s->id(),
2027 const Transform & offsetENU = cacheIter->second.second;
2028 relativePose.
x += offsetENU.
x() - originOffsetENU.
x();
2029 relativePose.y += offsetENU.
y() - originOffsetENU.
y();
2030 relativePose.z += offsetENU.
z() - originOffsetENU.
z();
2032 if(relativePose.z>
error)
2034 relativePose.z -=
error;
2036 else if(relativePose.z < -
error)
2038 relativePose.z +=
error;
2050 signaturesToCompare.push_back(
iter->first);
2057 signaturesToCompare.push_back(
iter->first);
2064 likelihood = rawLikelihood;
2067 timeLikelihoodCalculation =
timer.ticks();
2068 ULOGGER_INFO(
"timeLikelihoodCalculation=%fs",timeLikelihoodCalculation);
2078 timePosteriorCalculation =
timer.ticks();
2079 ULOGGER_INFO(
"timePosteriorCalculation=%fs",timePosteriorCalculation);
2091 if(posterior.size())
2093 for(std::map<int, float>::const_reverse_iterator
iter = posterior.rbegin();
iter != posterior.rend(); ++
iter)
2103 timeHypothesesCreation =
timer.ticks();
2109 bool hasLoopClosureConstraints =
false;
2112 hasLoopClosureConstraints =
2131 rejectedGlobalLoopClosure =
true;
2132 if(posterior.size() <= 2 && loopThr>0.0f)
2135 UDEBUG(
"rejected hypothesis: single hypothesis");
2139 UWARN(
"rejected hypothesis: by epipolar geometry");
2143 UWARN(
"rejected hypothesis: not satisfying hypothesis ratio (%f < %f * %f)",
2146 else if(
_loopRatio > 0.0
f && lastHighestHypothesis.second == 0)
2148 UWARN(
"rejected hypothesis: last closure hypothesis is null (loop ratio is on)");
2153 rejectedGlobalLoopClosure =
false;
2156 timeHypothesesValidation =
timer.ticks();
2157 ULOGGER_INFO(
"timeHypothesesValidation=%fs",timeHypothesesValidation);
2164 rejectedGlobalLoopClosure =
true;
2173 else if(!signature->
isBadSignature() && (smallDisplacement || tooFastMovement))
2176 UDEBUG(
"smallDisplacement=%d tooFastMovement=%d", smallDisplacement?1:0, tooFastMovement?1:0);
2180 UDEBUG(
"Ignoring likelihood and loop closure hypotheses as current signature doesn't have enough visual features.");
2188 timeJoiningTrash =
timer.ticks();
2189 ULOGGER_INFO(
"Time emptying memory trash = %fs, joining (actual overhead) = %fs", timeEmptyingTrash, timeJoiningTrash);
2195 std::list<int> reactivatedIds;
2196 double timeGetNeighborsTimeDb = 0.0;
2197 double timeGetNeighborsSpaceDb = 0.0;
2198 int immunizedGlobally = 0;
2199 int immunizedLocally = 0;
2200 int maxLocalLocationsImmunized = 0;
2213 ULOGGER_INFO(
"Retrieving locations... around id=%d", retrievalId);
2215 UASSERT(neighborhoodSize >= 0);
2219 unsigned int nbLoadedFromDb = 0;
2220 std::set<int> reactivatedIdsSet;
2221 std::map<int, int> neighbors;
2222 int nbDirectNeighborsInDb = 0;
2235 &timeGetNeighborsTimeDb);
2236 ULOGGER_DEBUG(
"neighbors of %d in time = %d", retrievalId, (
int)neighbors.size());
2238 bool firstPassDone =
false;
2240 while(
m < neighborhoodSize)
2242 std::set<int> idsSorted;
2243 for(std::map<int, int>::iterator
iter=neighbors.begin();
iter!=neighbors.end();)
2247 neighbors.erase(
iter++);
2249 else if(
iter->second ==
m)
2251 if(reactivatedIdsSet.find(
iter->first) == reactivatedIdsSet.end())
2253 idsSorted.insert(
iter->first);
2254 reactivatedIdsSet.insert(
iter->first);
2258 ++nbDirectNeighborsInDb;
2262 if(immunizedLocations.insert(
iter->first).second)
2264 ++immunizedGlobally;
2269 neighbors.erase(
iter++);
2276 firstPassDone =
true;
2277 reactivatedIds.insert(reactivatedIds.end(), idsSorted.rbegin(), idsSorted.rend());
2291 &timeGetNeighborsSpaceDb);
2292 ULOGGER_DEBUG(
"neighbors of %d in space = %d", retrievalId, (
int)neighbors.size());
2293 firstPassDone =
false;
2295 while(
m < neighborhoodSize)
2297 std::set<int> idsSorted;
2298 for(std::map<int, int>::iterator
iter=neighbors.begin();
iter!=neighbors.end();)
2302 neighbors.erase(
iter++);
2304 else if(
iter->second ==
m)
2306 if(reactivatedIdsSet.find(
iter->first) == reactivatedIdsSet.end())
2308 idsSorted.insert(
iter->first);
2309 reactivatedIdsSet.insert(
iter->first);
2313 ++nbDirectNeighborsInDb;
2317 neighbors.erase(
iter++);
2324 firstPassDone =
true;
2325 reactivatedIds.insert(reactivatedIds.end(), idsSorted.rbegin(), idsSorted.rend());
2329 "reactivatedIds.size=%d, "
2330 "nbLoadedFromDb=%d, "
2331 "nbDirectNeighborsInDb=%d, "
2332 "time=%fs (%fs %fs)",
2334 reactivatedIds.size(),
2335 (
int)nbLoadedFromDb,
2336 nbDirectNeighborsInDb,
2338 timeGetNeighborsTimeDb,
2339 timeGetNeighborsSpaceDb);
2347 std::list<int> retrievalLocalIds;
2355 float distanceSoFar = 0.0f;
2369 if(immunizedLocations.insert(
_path[
i].first).second)
2377 UINFO(
"retrieval of node %d on path (dist=%fm)",
_path[
i].
first, distanceSoFar);
2384 UDEBUG(
"Stop on node %d (dist=%fm > %fm)",
2394 if(immunizedLocally < maxLocalLocationsImmunized &&
2397 std::map<int ,Transform> poses;
2403 poses.insert(*
iter);
2412 std::multimap<int, int> links;
2417 links.insert(std::make_pair(
iter->second.from(),
iter->second.to()));
2418 links.insert(std::make_pair(
iter->second.to(),
iter->second.from()));
2423 if(
path.size() == 0)
2425 UWARN(
"Could not compute a path between %d and %d", nearestId, signature->
id());
2435 if(immunizedLocally >= maxLocalLocationsImmunized)
2440 UWARN(
"Could not immunize the whole local path (%d) between "
2441 "%d and %d (max location immunized=%d). You may want "
2442 "to increase RGBD/LocalImmunizationRatio (current=%f (%d of WM=%d)) "
2443 "to be able to immunize longer paths.",
2447 maxLocalLocationsImmunized,
2449 maxLocalLocationsImmunized,
2456 if(immunizedLocations.insert(
iter->first).second)
2472 std::multimap<float, int> nearNodesByDist;
2473 for(std::map<int, float>::iterator
iter=nearNodes.lower_bound(1);
iter!=nearNodes.end(); ++
iter)
2475 nearNodesByDist.insert(std::make_pair(
iter->second,
iter->first));
2477 UINFO(
"near nodes=%d, max local immunized=%d, ratio=%f WM=%d",
2478 (
int)nearNodesByDist.size(),
2479 maxLocalLocationsImmunized,
2482 for(std::multimap<float, int>::iterator
iter=nearNodesByDist.begin();
2483 iter!=nearNodesByDist.end() && (retrievalLocalIds.size() <
_maxLocalRetrieved || immunizedLocally < maxLocalLocationsImmunized);
2491 const std::multimap<int, Link> & links =
s->getLinks();
2492 for(std::multimap<int, Link>::const_reverse_iterator jter=links.rbegin();
2498 UINFO(
"retrieval of node %d on local map", jter->first);
2499 retrievalLocalIds.push_back(jter->first);
2502 if(!
_memory->
isInSTM(
s->id()) && immunizedLocally < maxLocalLocationsImmunized)
2504 if(immunizedLocations.insert(
s->id()).second)
2515 std::set<int> retrievalLocalIdsSet(retrievalLocalIds.begin(), retrievalLocalIds.end());
2516 for(std::list<int>::iterator
iter=retrievalLocalIds.begin();
2521 for(std::map<int, int>::reverse_iterator jter=ids.rbegin();
2526 retrievalLocalIdsSet.find(jter->first) == retrievalLocalIdsSet.end())
2528 UINFO(
"retrieval of node %d on local map", jter->first);
2529 retrievalLocalIds.push_back(jter->first);
2530 retrievalLocalIdsSet.insert(jter->first);
2537 for(std::multimap<float, int>::reverse_iterator
iter=nearNodesByDist.rbegin();
iter!=nearNodesByDist.rend(); ++
iter)
2543 reactivatedIds.insert(reactivatedIds.begin(), retrievalLocalIds.begin(), retrievalLocalIds.end());
2550 if(reactivatedIds.size())
2557 timeRetrievalDbAccess);
2559 ULOGGER_INFO(
"retrieval of %d (db time = %fs)", (
int)signaturesRetrieved.size(), timeRetrievalDbAccess);
2561 timeRetrievalDbAccess += timeGetNeighborsTimeDb + timeGetNeighborsSpaceDb;
2562 UINFO(
"total timeRetrievalDbAccess=%fs", timeRetrievalDbAccess);
2565 immunizedLocations.insert(signaturesRetrieved.begin(), signaturesRetrieved.end());
2569 UWARN(
"Some signatures have been retrieved from memory management, clearing global scan map...");
2574 timeReactivations =
timer.ticks();
2575 ULOGGER_INFO(
"timeReactivations=%fs", timeReactivations);
2580 std::list<std::pair<int, int> > loopClosureLinksAdded;
2581 int loopClosureVisualInliers = 0;
2582 float loopClosureVisualInliersRatio = 0.0f;
2583 int loopClosureVisualMatches = 0;
2584 float loopClosureLinearVariance = 0.0f;
2585 float loopClosureAngularVariance = 0.0f;
2586 float loopClosureVisualInliersMeanDist = 0;
2587 float loopClosureVisualInliersDistribution = 0;
2589 int proximityDetectionsAddedVisually = 0;
2590 int proximityDetectionsAddedByICPMulti = 0;
2591 int proximityDetectionsAddedByICPGlobal = 0;
2592 int lastProximitySpaceClosureId = 0;
2593 int proximitySpacePaths = 0;
2594 int localVisualPathsChecked = 0;
2595 int localScanPathsChecked = 0;
2596 int loopIdSuppressedByProximity = 0;
2608 UINFO(
"Proximity detection by space disabled as if we force to have a global loop "
2609 "closure with previous map before doing proximity detections (%s=true).",
2610 Parameters::kRtabmapStartNewMapOnLoopClosure().
c_str());
2614 UWARN(
"Cannot do local loop closure detection in space if graph optimization is disabled!");
2623 if((!smallDisplacement || !localizationOnPreviousUpdate) && !tooFastMovement)
2633 UDEBUG(
"Proximity detection (local loop closure in SPACE using matching images, local radius=%fm)",
_localRadius);
2636 std::map<int, Transform> nearestPoses;
2637 std::multimap<int, int> links;
2645 links.insert(std::make_pair(
iter->second.from(),
iter->second.to()));
2646 links.insert(std::make_pair(
iter->second.to(),
iter->second.from()));
2650 for(std::map<int, float>::iterator
iter=nearestIds.lower_bound(1);
iter!=nearestIds.end(); ++
iter)
2669 UDEBUG(
"nearestPoses=%d", (
int)nearestPoses.size());
2673 UDEBUG(
"got %d paths", (
int)nearestPathsNotSorted.size());
2675 std::map<NearestPathKey, std::map<int, Transform> > nearestPaths;
2677 for(std::map<
int, std::map<int, Transform> >::const_iterator
iter=nearestPathsNotSorted.begin();
iter!=nearestPathsNotSorted.end(); ++
iter)
2679 const std::map<int, Transform> &
path =
iter->second;
2680 float highestLikelihood = 0.0f;
2681 int highestLikelihoodId =
iter->first;
2682 float smallestDistanceSqr = -1;
2683 for(std::map<int, Transform>::const_iterator jter=
path.begin(); jter!=
path.end(); ++jter)
2685 float v =
uValue(likelihood, jter->first, 0.0f);
2686 float distance = (currentPoseInv * jter->second).getNormSquared();
2687 if(
v > highestLikelihood || (
v == highestLikelihood && (smallestDistanceSqr < 0 ||
distance < smallestDistanceSqr)))
2689 highestLikelihood =
v;
2690 highestLikelihoodId = jter->first;
2694 nearestPaths.insert(std::make_pair(
NearestPathKey(highestLikelihood, highestLikelihoodId, smallestDistanceSqr),
path));
2698 timeProximityBySpaceSearch =
timer.ticks();
2699 ULOGGER_INFO(
"timeProximityBySpaceSearch=%fs", timeProximityBySpaceSearch);
2706 for(std::map<
NearestPathKey, std::map<int, Transform> >::const_reverse_iterator
iter=nearestPaths.rbegin();
2707 iter!=nearestPaths.rend() &&
2711 std::map<int, Transform>
path =
iter->second;
2719 if(
iter->first.likelihood > 0.0f &&
2722 nearestId =
iter->first.id;
2732 if(!signature->
hasLink(nearestId) &&
2733 (proximityFilteringRadius <= 0.0f ||
2736 ++localVisualPathsChecked;
2748 if(proximityFilteringRadius <= 0 ||
transform.getNormSquared() <= proximityFilteringRadius*proximityFilteringRadius)
2750 UINFO(
"[Visual] Add local loop closure in SPACE (%d->%d) %s",
2754 UASSERT(
info.covariance.at<
double>(0,0) > 0.0 &&
info.covariance.at<
double>(5,5) > 0.0);
2757 loopClosureVisualInliersMeanDist =
info.inliersMeanDistance;
2758 loopClosureVisualInliersDistribution =
info.inliersDistribution;
2760 ++proximityDetectionsAddedVisually;
2761 lastProximitySpaceClosureId = nearestId;
2763 loopClosureVisualInliers =
info.inliers;
2764 loopClosureVisualInliersRatio =
info.inliersRatio;
2765 loopClosureVisualMatches =
info.matches;
2768 loopClosureLinearVariance = 1.0/information.at<
double>(0,0);
2769 loopClosureAngularVariance = 1.0/information.at<
double>(5,5);
2777 UDEBUG(
"Proximity detection on %d is close to loop closure %d, ignoring loop closure transform estimation...",
2783 loopIdSuppressedByProximity = nearestId;
2785 else if(loopIdSuppressedByProximity == 0)
2787 loopIdSuppressedByProximity = nearestId;
2792 loopClosureLinksAdded.push_back(std::make_pair(signature->
id(), nearestId));
2796 UWARN(
"Ignoring local loop closure with %d because resulting "
2797 "transform is too large!? (%fm > %fm)",
2798 nearestId,
transform.getNorm(), proximityFilteringRadius);
2802 else if(!signature->
hasLink(nearestId) && proximityFilteringRadius>0.0f)
2804 UDEBUG(
"Skipping path %d as most likely ID %d is too far %f > %f (%s)",
2808 proximityFilteringRadius,
2809 Parameters::kRGBDProximityPathFilteringRadius().c_str());
2814 timeProximityBySpaceVisualDetection =
timer.ticks();
2815 ULOGGER_INFO(
"timeProximityBySpaceVisualDetection=%fs", timeProximityBySpaceVisualDetection);
2820 UDEBUG(
"Proximity detection (local loop closure in SPACE with scan matching)");
2827 proximitySpacePaths = (
int)nearestPaths.size();
2828 for(std::map<
NearestPathKey, std::map<int, Transform> >::const_reverse_iterator
iter=nearestPaths.rbegin();
2829 iter!=nearestPaths.rend() &&
2833 std::map<int, Transform>
path =
iter->second;
2843 if(!signature->
hasLink(nearestId))
2847 std::map<int, Transform> filteredPath;
2849 std::map<int, Transform>::iterator nearestIdIter =
path.find(nearestId);
2855 filteredPath.insert(*
iter);
2860 filteredPath.insert(*
iter);
2862 path = filteredPath;
2866 std::map<int, Transform> optimizedLocalPath;
2875 UERROR(
"Proximity path not containing nearest ID ?! Skipping this path.");
2880 for(std::map<int, Transform>::iterator jter=
path.lower_bound(1); jter!=
path.end(); ++jter)
2882 optimizedLocalPath.insert(std::make_pair(jter->first,
t * jter->second));
2887 optimizedLocalPath =
path;
2890 std::map<int, Transform> filteredPath;
2891 if(
_globalScanMap.
empty() && optimizedLocalPath.size() > 2 && proximityFilteringRadius > 0.0f)
2896 filteredPath.insert(*optimizedLocalPath.find(nearestId));
2900 filteredPath = optimizedLocalPath;
2903 if(filteredPath.size() > 0)
2906 filteredPath.insert(std::make_pair(signature->
id(),
_optimizedPoses.at(signature->
id())));
2910 ++localScanPathsChecked;
2913 bool icpMulti =
true;
2924 assembledData.
setId(nearestId);
2931 Transform guess = filteredPath.at(nearestId).
inverse() * filteredPath.at(signature->
id());
2941 UINFO(
"[Scan matching] Add local loop closure in SPACE (%d->%d) %s",
2946 cv::Mat scanMatchingIds;
2949 std::stringstream
stream;
2951 for(std::map<int, Transform>::iterator
iter=optimizedLocalPath.begin();
iter!=optimizedLocalPath.end(); ++
iter)
2953 if(
iter != optimizedLocalPath.begin())
2959 std::string scansStr =
stream.str();
2960 scanMatchingIds = cv::Mat(1,
int(scansStr.size()+1), CV_8SC1, (
void *)scansStr.c_str());
2965 UASSERT(
info.covariance.at<
double>(0,0) > 0.0 &&
info.covariance.at<
double>(5,5) > 0.0);
2967 loopClosureLinksAdded.push_back(std::make_pair(signature->
id(), nearestId));
2971 ++proximityDetectionsAddedByICPMulti;
2975 ++proximityDetectionsAddedByICPGlobal;
2979 if(proximityDetectionsAddedVisually == 0)
2981 lastProximitySpaceClosureId = nearestId;
2986 UINFO(
"Local scan matching rejected: %s",
info.rejectedMsg.c_str());
3004 timeProximityBySpaceDetection =
timer.ticks();
3005 ULOGGER_INFO(
"timeProximityBySpaceDetection=%fs", timeProximityBySpaceDetection);
3013 if(loopIdSuppressedByProximity==0)
3018 info.covariance = cv::Mat::eye(6,6,CV_64FC1);
3027 loopClosureVisualInliersMeanDist =
info.inliersMeanDistance;
3028 loopClosureVisualInliersDistribution =
info.inliersDistribution;
3030 loopClosureVisualInliers =
info.inliers;
3031 loopClosureVisualInliersRatio =
info.inliersRatio;
3032 loopClosureVisualMatches =
info.matches;
3033 rejectedGlobalLoopClosure =
transform.isNull();
3034 if(rejectedGlobalLoopClosure)
3036 UWARN(
"Rejected loop closure %d -> %d: %s",
3041 rejectedGlobalLoopClosure =
true;
3042 UWARN(
"Rejected localization %d -> %d because distance to map (%fm) is over %s=%fm.",
3050 if(!rejectedGlobalLoopClosure)
3053 UASSERT(
info.covariance.at<
double>(0,0) > 0.0 &&
info.covariance.at<
double>(5,5) > 0.0);
3055 loopClosureLinearVariance =
uMax3(
info.covariance.at<
double>(0,0),
info.covariance.at<
double>(1,1)>=9999?0:
info.covariance.at<
double>(1,1),
info.covariance.at<
double>(2,2)>=9999?0:
info.covariance.at<
double>(2,2));
3056 loopClosureAngularVariance =
uMax3(
info.covariance.at<
double>(3,3)>=9999?0:
info.covariance.at<
double>(3,3),
info.covariance.at<
double>(4,4)>=9999?0:
info.covariance.at<
double>(4,4),
info.covariance.at<
double>(5,5));
3059 if(!rejectedGlobalLoopClosure)
3065 if(rejectedGlobalLoopClosure)
3076 timeAddLoopClosureLink =
timer.ticks();
3077 ULOGGER_INFO(
"timeAddLoopClosureLink=%fs", timeAddLoopClosureLink);
3082 std::map<int, std::set<int> > landmarksDetected;
3086 UDEBUG(
"hasGlobalLoopClosuresInOdomCache=%d", hasGlobalLoopClosuresInOdomCache?1:0);
3093 !hasGlobalLoopClosuresInOdomCache &&
3098 UWARN(
"Ignoring landmark %d for localization as it is too far (%fm > %s=%f) "
3099 "and odom cache doesn't contain global loop closure(s).",
3101 iter->second.transform().getNorm(),
3102 Parameters::kRGBDLocalRadius().c_str(),
3109 rejectedGlobalLoopClosure =
false;
3110 loopClosureLinksAdded.push_back(std::make_pair(signature->
id(),
iter->first));
3135 UERROR(
"Virtual link larger than local radius (%fm > %fm). Aborting the plan!",
3145 float maxLinearError = 0.0f;
3146 float maxLinearErrorRatio = 0.0f;
3147 float maxAngularError = 0.0f;
3148 float maxAngularErrorRatio = 0.0f;
3149 double optimizationError = 0.0;
3150 int optimizationIterations = 0;
3152 bool rejectedLandmark =
false;
3153 bool delayedLocalization =
false;
3157 UDEBUG(
"Last prox: %d", lastProximitySpaceClosureId);
3162 UDEBUG(
"Prox Time: %d", proximityDetectionsInTimeFound);
3163 UDEBUG(
"Landmarks: %d", (
int)landmarksDetected.size());
3164 UDEBUG(
"Retrieved: %d", (
int)signaturesRetrieved.size());
3170 lastProximitySpaceClosureId>0 ||
3174 proximityDetectionsInTimeFound>0 ||
3175 !landmarksDetected.empty() ||
3176 signaturesRetrieved.size())
3181 !landmarksDetected.empty()))
3190 for(std::map<
int, std::set<int> >::
iterator iter=landmarksDetected.begin();
iter!=landmarksDetected.end(); ++
iter)
3195 localizationLinks.insert(std::make_pair(
iter->first, signature->
getLandmarks().at(
iter->first)));
3200 bool allLocalizationLinksInGraph = !localizationLinks.empty();
3201 for(std::multimap<int, Link>::iterator
iter=localizationLinks.begin();
iter!=localizationLinks.end(); ++
iter)
3205 allLocalizationLinksInGraph =
false;
3215 signaturesRetrieved.empty() &&
3216 !localizationLinks.empty() &&
3217 allLocalizationLinksInGraph)
3234 constraints.insert(selfLinks.begin(), selfLinks.end());
3235 for(std::multimap<int, Link>::iterator
iter=localizationLinks.begin();
iter!=localizationLinks.end(); ++
iter)
3237 constraints.insert(std::make_pair(
iter->second.from(),
iter->second));
3240 for(std::multimap<int, Link>::iterator
iter=constraints.begin();
iter!=constraints.end(); ++
iter)
3243 if(iterPose !=
_optimizedPoses.end() && poses.find(iterPose->first) == poses.end())
3245 poses.insert(*iterPose);
3247 constraints.insert(std::make_pair(iterPose->first,
Link(iterPose->first, iterPose->first,
Link::kPosePrior, iterPose->second, priorInfMat)));
3250 UDEBUG(
"Constraint %d->%d: %s (type=%s, var = %f %f)",
iter->second.from(),
iter->second.to(),
iter->second.transform().prettyPrint().c_str(),
iter->second.typeName().c_str(),
iter->second.transVariance(),
iter->second.rotVariance());
3253 std::map<int, Transform> posesOut;
3254 std::multimap<int, Link> edgeConstraintsOut;
3256 UDEBUG(
"priorsIgnored was %s", priorsIgnored?
"true":
"false");
3262 for(std::map<int, Transform>::iterator
iter=posesOut.begin();
iter!=posesOut.end(); ++
iter)
3264 UDEBUG(
"Pose %d %s",
iter->first,
iter->second.prettyPrint().c_str());
3267 cv::Mat locOptCovariance;
3268 std::map<int, Transform> optPoses;
3269 if(!posesOut.empty() &&
3272 optPoses =
_graphOptimizer->
optimize(posesOut.begin()->first, posesOut, edgeConstraintsOut, locOptCovariance);
3276 UERROR(
"Invalid localization constraints");
3279 for(std::map<int, Transform>::iterator
iter=optPoses.begin();
iter!=optPoses.end(); ++
iter)
3281 UDEBUG(
"Opt %d %s",
iter->first,
iter->second.prettyPrint().c_str());
3284 if(optPoses.empty())
3286 UWARN(
"Optimization failed, rejecting localization!");
3287 rejectLocalization =
true;
3291 UINFO(
"Compute max graph errors...");
3292 const Link * maxLinearLink = 0;
3293 const Link * maxAngularLink = 0;
3297 maxLinearErrorRatio,
3298 maxAngularErrorRatio,
3304 if(maxLinearLink == 0 && maxAngularLink==0)
3306 UWARN(
"Could not compute graph errors! Rejecting localization!");
3307 rejectLocalization =
true;
3312 UINFO(
"Max optimization linear error = %f m (link %d->%d, var=%f, ratio error/std=%f, thr=%f)",
3314 maxLinearLink->
from(),
3315 maxLinearLink->
to(),
3321 UWARN(
"Rejecting localization (%d <-> %d) in this "
3322 "iteration because a wrong loop closure has been "
3323 "detected after graph optimization, resulting in "
3324 "a maximum graph error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). The "
3325 "maximum error ratio parameter \"%s\" is %f of std deviation.",
3326 localizationLinks.rbegin()->second.from(),
3327 localizationLinks.rbegin()->second.to(),
3328 maxLinearErrorRatio,
3329 maxLinearLink->
from(),
3330 maxLinearLink->
to(),
3331 maxLinearLink->
type(),
3334 Parameters::kRGBDOptimizeMaxError().c_str(),
3336 rejectLocalization =
true;
3340 UERROR(
"Huge optimization error detected!"
3341 "Linear error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). You may consider "
3342 "enabling \"%s\" to reject those bad optimizations by setting it to a non null value!",
3343 maxLinearErrorRatio,
3344 maxLinearLink->
from(),
3345 maxLinearLink->
to(),
3346 maxLinearLink->
type(),
3349 Parameters::kRGBDOptimizeMaxError().c_str());
3354 UINFO(
"Max optimization angular error = %f deg (link %d->%d, var=%f, ratio error/std=%f, thr=%f)",
3355 maxAngularError*180.0
f/CV_PI,
3356 maxAngularLink->
from(),
3357 maxAngularLink->
to(),
3363 UWARN(
"Rejecting localization (%d <-> %d) in this "
3364 "iteration because a wrong loop closure has been "
3365 "detected after graph optimization, resulting in "
3366 "a maximum graph error ratio of %f (edge %d->%d, type=%d, abs error=%f deg, stddev=%f). The "
3367 "maximum error ratio parameter \"%s\" is %f of std deviation.",
3368 localizationLinks.rbegin()->second.from(),
3369 localizationLinks.rbegin()->second.to(),
3370 maxAngularErrorRatio,
3371 maxAngularLink->
from(),
3372 maxAngularLink->
to(),
3373 maxAngularLink->
type(),
3374 maxAngularError*180.0f/CV_PI,
3376 Parameters::kRGBDOptimizeMaxError().c_str(),
3378 rejectLocalization =
true;
3382 UERROR(
"Huge optimization error detected!"
3383 "Angular error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). You may consider "
3384 "enabling \"%s\" to reject those bad optimizations by setting it to a non null value!",
3385 maxAngularErrorRatio,
3386 maxAngularLink->
from(),
3387 maxAngularLink->
to(),
3388 maxAngularLink->
type(),
3389 maxAngularError*180.0f/CV_PI,
3391 Parameters::kRGBDOptimizeMaxError().c_str());
3396 bool hasGlobalLoopClosuresOrLandmarks =
false;
3402 for(std::multimap<int, Link>::iterator
iter=constraints.begin();
iter!=constraints.end() && !hasGlobalLoopClosuresOrLandmarks; ++
iter)
3404 hasGlobalLoopClosuresOrLandmarks =
3408 if(hasGlobalLoopClosuresOrLandmarks && !localizationLinks.empty())
3410 rejectLocalization =
false;
3411 UWARN(
"Global and loop closures seem not tallying together, try again to optimize without local loop closures...");
3413 UDEBUG(
"priorsIgnored was %s", priorsIgnored?
"true":
"false");
3418 if(!posesOut.empty() &&
3421 optPoses =
_graphOptimizer->
optimize(posesOut.begin()->first, posesOut, edgeConstraintsOut, locOptCovariance);
3425 UERROR(
"Invalid localization constraints");
3428 for(std::map<int, Transform>::iterator
iter=optPoses.begin();
iter!=optPoses.end(); ++
iter)
3430 UDEBUG(
"Opt2 %d %s",
iter->first,
iter->second.prettyPrint().c_str());
3433 if(optPoses.empty())
3435 UWARN(
"Optimization failed, rejecting localization!");
3436 rejectLocalization =
true;
3440 UINFO(
"Compute max graph errors...");
3441 const Link * maxLinearLink = 0;
3442 const Link * maxAngularLink = 0;
3446 maxLinearErrorRatio,
3447 maxAngularErrorRatio,
3453 if(maxLinearLink == 0 && maxAngularLink==0)
3455 UWARN(
"Could not compute graph errors! Rejecting localization!");
3456 rejectLocalization =
true;
3461 UINFO(
"Max optimization linear error = %f m (link %d->%d, var=%f, ratio error/std=%f, thr=%f)",
3463 maxLinearLink->
from(),
3464 maxLinearLink->
to(),
3470 UWARN(
"Rejecting localization (%d <-> %d) in this "
3471 "iteration because a wrong loop closure has been "
3472 "detected after graph optimization, resulting in "
3473 "a maximum graph error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). The "
3474 "maximum error ratio parameter \"%s\" is %f of std deviation.",
3475 localizationLinks.rbegin()->second.from(),
3476 localizationLinks.rbegin()->second.to(),
3477 maxLinearErrorRatio,
3478 maxLinearLink->
from(),
3479 maxLinearLink->
to(),
3480 maxLinearLink->
type(),
3483 Parameters::kRGBDOptimizeMaxError().c_str(),
3485 rejectLocalization =
true;
3489 UERROR(
"Huge optimization error detected!"
3490 "Linear error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). You may consider "
3491 "enabling \"%s\" to reject those bad optimizations by setting it to a non null value!",
3492 maxLinearErrorRatio,
3493 maxLinearLink->
from(),
3494 maxLinearLink->
to(),
3495 maxLinearLink->
type(),
3498 Parameters::kRGBDOptimizeMaxError().c_str());
3503 UINFO(
"Max optimization angular error = %f deg (link %d->%d, var=%f, ratio error/std=%f, thr=%f)",
3504 maxAngularError*180.0
f/CV_PI,
3505 maxAngularLink->
from(),
3506 maxAngularLink->
to(),
3512 UWARN(
"Rejecting localization (%d <-> %d) in this "
3513 "iteration because a wrong loop closure has been "
3514 "detected after graph optimization, resulting in "
3515 "a maximum graph error ratio of %f (edge %d->%d, type=%d, abs error=%f deg, stddev=%f). The "
3516 "maximum error ratio parameter \"%s\" is %f of std deviation.",
3517 localizationLinks.rbegin()->second.from(),
3518 localizationLinks.rbegin()->second.to(),
3519 maxAngularErrorRatio,
3520 maxAngularLink->
from(),
3521 maxAngularLink->
to(),
3522 maxAngularLink->
type(),
3523 maxAngularError*180.0f/CV_PI,
3525 Parameters::kRGBDOptimizeMaxError().c_str(),
3527 rejectLocalization =
true;
3531 UERROR(
"Huge optimization error detected!"
3532 "Angular error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). You may consider "
3533 "enabling \"%s\" to reject those bad optimizations by setting it to a non null value!",
3534 maxAngularErrorRatio,
3535 maxAngularLink->
from(),
3536 maxAngularLink->
to(),
3537 maxAngularLink->
type(),
3538 maxAngularError*180.0f/CV_PI,
3540 Parameters::kRGBDOptimizeMaxError().c_str());
3547 if(!rejectLocalization)
3549 if(hasGlobalLoopClosuresOrLandmarks)
3557 UWARN(
"Successfully optimized without local loop closures! Clear them from local odometry cache. %ld/%ld have been removed.",
3562 UWARN(
"Successfully optimized without local loop closures!");
3567 bool hadAlreadyLocalizationLinks =
false;
3578 hadAlreadyLocalizationLinks =
true;
3586 for(std::multimap<int, Link>::iterator
iter=localizationLinks.begin();
iter!=localizationLinks.end(); ++
iter)
3591 UDEBUG(
"Adding new odom cache constraint %d->%d (%s)",
3592 iter->second.from(),
iter->second.to(),
iter->second.transform().prettyPrint().c_str());
3599 UDEBUG(
"Adjusted localization link %d->%d after optimization",
iter->second.from(),
iter->second.to());
3600 UDEBUG(
"from %s",
iter->second.transform().prettyPrint().c_str());
3602 iter->second.setTransform(newT);
3617 UINFO(
"Update localization");
3625 Transform u = signature->
getPose() * localizationLinks.rbegin()->second.transform();
3630 int loopId = localizationLinks.rbegin()->first;
3636 landmarkId = loopId;
3637 UASSERT(landmarksDetected.find(landmarkId) != landmarksDetected.end() &&
3638 !landmarksDetected.at(landmarkId).empty());
3639 loopId = *landmarksDetected.at(landmarkId).begin();
3646 if(iterGravityLoop!=loopS->
getLinks().end() &&
3647 iterGravitySign!=signature->
getLinks().end())
3652 iterGravityLoop->second.transform().getEulerAngles(
roll,
pitch,
yaw);
3661 iterGravityLoop->second.transform().getEulerAngles(
roll,
pitch,
yaw);
3665 Transform error =
transform.rotation().inverse() * iterGravitySign->second.transform().rotation().inverse() * targetRotation;
3669 else if(iterGravityLoop!=loopS->
getLinks().end() ||
3670 iterGravitySign!=signature->
getLinks().end())
3672 UWARN(
"Gravity link not found for %d or %d, localization won't be corrected with gravity.", loopId, signature->
id());
3683 iter->second = mapCorrectionInv * up *
iter->second;
3689 Transform newPose =
_optimizedPoses.at(localizationLinks.rbegin()->first) * localizationLinks.rbegin()->second.transform().inverse();
3694 newPose = newPose.
to3DoF();
3701 if(iterGravitySign!=signature->
getLinks().end())
3706 UDEBUG(
"Gravity link = %s", iterGravitySign->second.transform().prettyPrint().c_str());
3710 Transform error =
transform.rotation().inverse() * iterGravitySign->second.transform().rotation().inverse() * targetRotation;
3713 iterGravitySign->second.transform().getEulerAngles(
roll,
pitch, tmp1);
3718 else if(iterGravitySign!=signature->
getLinks().end())
3720 UWARN(
"Gravity link not found for %d, localization won't be corrected with gravity.", signature->
id());
3725 _localizationCovariance = locOptCovariance.empty()?localizationLinks.rbegin()->second.infMatrix().inv():locOptCovariance;
3729 UWARN(
"Localization was good, but waiting for another one to be more accurate (%s>0)", Parameters::kRGBDMaxOdomCacheSize().
c_str());
3730 delayedLocalization =
true;
3731 rejectLocalization =
true;
3736 if(rejectLocalization)
3739 lastProximitySpaceClosureId = 0;
3740 rejectedGlobalLoopClosure =
true;
3741 rejectedLandmark =
true;
3746 UINFO(
"Update map correction");
3752 UWARN(
"Optimization: clearing guess poses as %s has changed state, now %s",
3758 std::multimap<int, Link> constraints;
3760 optimizeCurrentMap(signature->
id(),
false, poses, covariance, &constraints, &optimizationError, &optimizationIterations);
3764 bool updateConstraints =
true;
3767 UWARN(
"Graph optimization failed! Rejecting last loop closures added.");
3768 for(std::list<std::pair<int, int> >::
iterator iter=loopClosureLinksAdded.begin();
iter!=loopClosureLinksAdded.end(); ++
iter)
3771 UWARN(
"Loop closure %d->%d rejected!",
iter->first,
iter->second);
3773 updateConstraints =
false;
3775 lastProximitySpaceClosureId = 0;
3776 rejectedGlobalLoopClosure =
true;
3777 rejectedLandmark =
true;
3780 loopClosureLinksAdded.size() &&
3781 optimizationIterations > 0 &&
3784 UINFO(
"Compute max graph errors...");
3785 const Link * maxLinearLink = 0;
3786 const Link * maxAngularLink = 0;
3790 maxLinearErrorRatio,
3791 maxAngularErrorRatio,
3796 if(maxLinearLink == 0 && maxAngularLink==0)
3798 UWARN(
"Could not compute graph errors! Wrong loop closures could be accepted!");
3801 bool reject =
false;
3804 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()));
3807 UWARN(
"Rejecting all added loop closures (%d, first is %d <-> %d) in this "
3808 "iteration because a wrong loop closure has been "
3809 "detected after graph optimization, resulting in "
3810 "a maximum graph error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). The "
3811 "maximum error ratio parameter \"%s\" is %f of std deviation.",
3812 (
int)loopClosureLinksAdded.size(),
3813 loopClosureLinksAdded.front().first,
3814 loopClosureLinksAdded.front().second,
3815 maxLinearErrorRatio,
3816 maxLinearLink->
from(),
3817 maxLinearLink->
to(),
3818 maxLinearLink->
type(),
3821 Parameters::kRGBDOptimizeMaxError().c_str(),
3827 UERROR(
"Huge optimization error detected!"
3828 "Linear error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). You may consider "
3829 "enabling \"%s\" to reject those bad optimizations by setting it to a non null value!",
3830 maxLinearErrorRatio,
3831 maxLinearLink->
from(),
3832 maxLinearLink->
to(),
3833 maxLinearLink->
type(),
3836 Parameters::kRGBDOptimizeMaxError().c_str());
3841 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()));
3844 UWARN(
"Rejecting all added loop closures (%d, first is %d <-> %d) in this "
3845 "iteration because a wrong loop closure has been "
3846 "detected after graph optimization, resulting in "
3847 "a maximum graph error ratio of %f (edge %d->%d, type=%d, abs error=%f deg, stddev=%f). The "
3848 "maximum error ratio parameter \"%s\" is %f of std deviation.",
3849 (
int)loopClosureLinksAdded.size(),
3850 loopClosureLinksAdded.front().first,
3851 loopClosureLinksAdded.front().second,
3852 maxAngularErrorRatio,
3853 maxAngularLink->
from(),
3854 maxAngularLink->
to(),
3855 maxAngularLink->
type(),
3856 maxAngularError*180.0f/CV_PI,
3858 Parameters::kRGBDOptimizeMaxError().c_str(),
3864 UERROR(
"Huge optimization error detected!"
3865 "Angular error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). You may consider "
3866 "enabling \"%s\" to reject those bad optimizations by setting it to a non null value!",
3867 maxAngularErrorRatio,
3868 maxAngularLink->
from(),
3869 maxAngularLink->
to(),
3870 maxAngularLink->
type(),
3871 maxAngularError*180.0f/CV_PI,
3873 Parameters::kRGBDOptimizeMaxError().c_str());
3879 for(std::list<std::pair<int, int> >::
iterator iter=loopClosureLinksAdded.begin();
iter!=loopClosureLinksAdded.end(); ++
iter)
3882 UWARN(
"Loop closure %d->%d rejected!",
iter->first,
iter->second);
3884 updateConstraints =
false;
3886 lastProximitySpaceClosureId = 0;
3887 rejectedGlobalLoopClosure =
true;
3888 rejectedLandmark =
true;
3892 if(updateConstraints)
3894 UINFO(
"Updated local map (old size=%d, new size=%d)", (
int)
_optimizedPoses.size(), (
int)poses.size());
3923 bool hasPrior = signature->
hasLink(signature->
id());
3942 if(newLocId==0 && !landmarksDetected.empty())
3947 if(
iter->second.size() && *
iter->second.begin()!=signature->
id())
3954 timeMapOptimization =
timer.ticks();
3955 ULOGGER_INFO(
"timeMapOptimization=%fs", timeMapOptimization);
3961 int dictionarySize = 0;
3962 int refWordsCount = 0;
3963 int refUniqueWordsCount = 0;
3964 int lcHypothesisReactivated = 0;
3970 lcHypothesisReactivated = sLoop->
isSaved()?1.0f:0.0f;
3973 refWordsCount = (
int)signature->
getWords().size();
4027 statistics_.
addStatistic(Statistics::kLoopLandmark_detected(), landmarksDetected.empty()?0:-landmarksDetected.begin()->first);
4028 statistics_.
addStatistic(Statistics::kLoopLandmark_detected_node_ref(), landmarksDetected.empty() || landmarksDetected.begin()->second.empty()?0:*landmarksDetected.begin()->second.begin());
4030 statistics_.
addStatistic(Statistics::kLoopVisual_inliers_distribution(), loopClosureVisualInliersDistribution);
4033 statistics_.
addStatistic(Statistics::kProximitySpace_detections_added_visually(), proximityDetectionsAddedVisually);
4034 statistics_.
addStatistic(Statistics::kProximitySpace_detections_added_icp_multi(), proximityDetectionsAddedByICPMulti);
4035 statistics_.
addStatistic(Statistics::kProximitySpace_detections_added_icp_global(), proximityDetectionsAddedByICPGlobal);
4049 if(
_loopClosureHypothesis.first || lastProximitySpaceClosureId || (!rejectedLandmark && !landmarksDetected.empty()))
4055 std::multimap<int, Link>::const_iterator loopIter = sLoop->
getLinks().find(signature->
id());
4057 UINFO(
"Set loop closure transform = %s", loopIter->second.transform().prettyPrint().c_str());
4066 Transform error = loopIter->second.transform().inverse() * transformGT;
4130 statistics_.
addStatistic(Statistics::kTimingProximity_by_space_visual(), timeProximityBySpaceVisualDetection*1000);
4158 if(distanceToClosestNodeInTheGraph>0.0)
4168 long estimatedMemoryUsage =
sizeof(
Rtabmap);
4169 estimatedMemoryUsage +=
_optimizedPoses.size() * (
sizeof(
int) +
sizeof(
Transform) + 12 *
sizeof(
float) +
sizeof(std::map<int, Transform>::iterator)) +
sizeof(std::map<int, Transform>);
4170 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>);
4173 estimatedMemoryUsage +=
_parameters.size()*(
sizeof(std::string)*2+
sizeof(ParametersMap::iterator)) +
sizeof(
ParametersMap);
4203 timeStatsCreation =
timer.ticks();
4204 ULOGGER_INFO(
"Time creating stats = %f...", timeStatsCreation);
4207 Signature lastSignatureData = *signature;
4232 if(signatureRemoved)
4234 signaturesRemoved.push_back(signatureRemoved);
4241 if(signatureRemoved != lastSignatureData.
id())
4246 (landmarksDetected.empty() || rejectedLandmark) &&
4249 UWARN(
"Ignoring location %d because a global loop closure is required before starting a new map!",
4251 signaturesRemoved.push_back(signature->
id());
4258 UWARN(
"Ignoring location %d because a good signature (with enough features or with a landmark detected) is required before starting a new map!",
4260 signaturesRemoved.push_back(signature->
id());
4263 else if((smallDisplacement || tooFastMovement) &&
4265 lastProximitySpaceClosureId == 0 &&
4266 (rejectedLandmark || landmarksDetected.empty()) &&
4270 UINFO(
"Ignoring location %d because the displacement is too small! (d=%f a=%f)",
4273 signaturesRemoved.push_back(signature->
id());
4282 (smallDisplacement || tooFastMovement) &&
4284 lastProximitySpaceClosureId == 0 &&
4285 !delayedLocalization &&
4286 (rejectedLandmark || landmarksDetected.empty()))
4291 if(
iter->second.from() == signatureRemoved ||
iter->second.to() == signatureRemoved)
4305 timeMemoryCleanup =
timer.ticks();
4306 ULOGGER_INFO(
"timeMemoryCleanup = %fs... %d signatures removed", timeMemoryCleanup, (
int)signaturesRemoved.size());
4318 double totalTime = timerTotal.
ticks();
4319 ULOGGER_INFO(
"Total time processing = %fs...", totalTime);
4325 std::list<int> transferred =
_memory->
forget(immunizedLocations);
4326 signaturesRemoved.insert(signaturesRemoved.end(), transferred.begin(), transferred.end());
4353 UDEBUG(
"Refresh local map from %d",
id);
4360 UDEBUG(
"Refresh local map from %d",
id);
4374 UDEBUG(
"Refresh local map from %d",
id);
4385 if(signaturesRemoved.size() == 1 && signaturesRemoved.front() == lastSignatureData.
id())
4387 int lastId = signaturesRemoved.front();
4388 UDEBUG(
"Detected that only last signature has been removed (lastId=%d)", lastId);
4392 if(
iter->second.to() !=
iter->second.from())
4411 UDEBUG(
"Removed %d from local map",
iter->first);
4417 UWARN(
"optimized poses have been modified, clearing global scan map...");
4443 UDEBUG(
"Optimized poses cleared!");
4458 timeRealTimeLimitReachedProcess =
timer.ticks();
4459 ULOGGER_INFO(
"Time limit reached processing = %f...", timeRealTimeLimitReachedProcess);
4464 int localGraphSize = 0;
4486 std::map<int, Transform> poses;
4487 std::multimap<int, Link> constraints;
4505 std::multimap<int, int> missingIds;
4511 missingIds.insert(std::make_pair(-1, tmpId));
4522 for(std::map<int, int>::iterator
iter=ids.begin();
iter!=ids.end(); ++
iter)
4524 if(
iter->first != loopId &&
4527 missingIds.insert(std::make_pair(
iter->second,
iter->first));
4536 if(ids.find(*
iter) == ids.end())
4550 std::stringstream
stream;
4560 UWARN(
"Republishing data of requested node(s) %s(%s=%d)",
4562 Parameters::kRtabmapMaxRepublished().c_str(),
4568 localGraphSize = (
int)poses.size();
4569 if(!lastSignatureLocalizedPose.
isNull())
4571 poses.insert(std::make_pair(lastSignatureData.
id(), lastSignatureLocalizedPose));
4585 UDEBUG(
"Computing RMSE...");
4586 float translational_rmse = 0.0f;
4587 float translational_mean = 0.0f;
4588 float translational_median = 0.0f;
4589 float translational_std = 0.0f;
4590 float translational_min = 0.0f;
4591 float translational_max = 0.0f;
4592 float rotational_rmse = 0.0f;
4593 float rotational_mean = 0.0f;
4594 float rotational_median = 0.0f;
4595 float rotational_std = 0.0f;
4596 float rotational_min = 0.0f;
4597 float rotational_max = 0.0f;
4604 translational_median,
4627 UDEBUG(
"Computing RMSE...done!");
4630 std::vector<int> ids;
4634 ids.push_back(*
iter);
4638 ids.push_back(
iter->first);
4641 UDEBUG(
"wmState=%d", (
int)ids.size());
4651 UDEBUG(
"Empty trash...");
4659 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",
4663 timeLikelihoodCalculation,
4664 timePosteriorCalculation,
4665 timeHypothesesCreation,
4666 timeHypothesesValidation,
4667 timeRealTimeLimitReachedProcess,
4679 timeRetrievalDbAccess,
4680 timeAddLoopClosureLink,
4682 timeNeighborLinkRefining,
4683 timeProximityByTimeDetection,
4684 timeProximityBySpaceDetection,
4685 timeMapOptimization);
4686 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",
4689 (
int)signaturesRemoved.size(),
4694 rejectedGlobalLoopClosure?1:0,
4697 int(signaturesRetrieved.size()),
4698 lcHypothesisReactivated,
4699 refUniqueWordsCount,
4703 rehearsalMaxId>0?1:0,
4721 fprintf(
_foutInt,
"%s", logI.c_str());
4724 UINFO(
"Time logging = %f...",
timer.ticks());
4727 timeFinalizingStatistics =
timer.ticks();
4728 UDEBUG(
"End process, timeFinalizingStatistics=%fs", timeFinalizingStatistics);
4744 ULOGGER_WARN(
"maxTimeAllowed < 0, then setting it to 0 (inf).");
4758 ULOGGER_WARN(
"maxMemoryAllowed < 0, then setting it to 0 (inf).");
4773 UWARN(
"Working directory has been changed from \"%s\" with \"%s\", new log files will be created.",
4780 else if(
path.empty())
4796 bool linksRemoved =
false;
4797 for(std::multimap<int, Link>::iterator
iter = links.begin();
iter!=links.end(); ++
iter)
4816 linksRemoved =
true;
4827 UINFO(
"Update graph");
4829 std::multimap<int, Link> constraints;
4835 UWARN(
"Graph optimization failed after removing loop closure links from last location!");
4839 UINFO(
"Updated local map (old size=%d, new size=%d)", (
int)
_optimizedPoses.size(), (
int)poses.size());
4858 UINFO(
"Update graph");
4864 if(
iter->second.from() == lastId ||
iter->second.to() == lastId)
4880 std::multimap<int, Link> constraints;
4886 UWARN(
"Graph optimization failed after deleting the last location!");
4912 UERROR(
"Working directory not set.");
4925 int maxNearestNeighbors,
4930 std::map<int, Transform> poses;
4938 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(
new pcl::PointCloud<pcl::PointXYZ>);
4944 std::map<int, float> foundIds;
4954 float radiusSqrd = radius * radius;
4957 if(
iter->first != fromId)
4959 if(stm.find(
iter->first) == stm.end() &&
4961 (radiusSqrd==0 || foundIds.at(
iter->first) <= radiusSqrd))
4963 (*cloud)[oi] = pcl::PointXYZ(
iter->second.x(),
iter->second.y(),
iter->second.z());
4964 ids[oi++] =
iter->first;
4986 pcl::CropBox<pcl::PointXYZ> cropbox;
4987 cropbox.setInputCloud(cloud);
4988 cropbox.setMin(Eigen::Vector4f(-1, -radius, -999999, 0));
4989 cropbox.setMax(Eigen::Vector4f(radius, radius, 999999, 0));
4990 cropbox.setRotation(Eigen::Vector3f(
roll,
pitch,
yaw));
4991 cropbox.setTranslation(Eigen::Vector3f(
x,
y,
z));
4993 pcl::IndicesPtr
indices(
new std::vector<int>());
5004 pcl::search::KdTree<pcl::PointXYZ>::Ptr kdTree(
new pcl::search::KdTree<pcl::PointXYZ>);
5005 kdTree->setInputCloud(cloud,
indices);
5006 std::vector<int>
ind;
5007 std::vector<float>
dist;
5008 pcl::PointXYZ pt(fromT.
x(), fromT.
y(), fromT.
z());
5009 kdTree->radiusSearch(pt, radius,
ind,
dist, maxNearestNeighbors);
5011 for(
unsigned int i=0;
i<
ind.size(); ++
i)
5018 poses.insert(std::make_pair(ids[
ind[
i]], tmp));
5039 std::map<int, std::map<int, Transform> >
Rtabmap::getPaths(
const std::map<int, Transform> & posesIn,
const Transform & target,
int maxGraphDepth)
const
5041 std::map<int, std::map<int, Transform> > paths;
5042 std::set<int> nodesSet;
5043 std::map<int, Transform> poses;
5044 for(std::map<int, Transform>::const_iterator
iter=posesIn.lower_bound(1);
iter!=posesIn.end(); ++
iter)
5046 nodesSet.insert(
iter->first);
5047 poses.insert(*
iter);
5051 double e0=0,e1=0,e2=0,e3=0,e4=0;
5057 std::map<int, Transform>
path;
5065 UWARN(
"Nearest id of %s in %d poses is 0 !? Returning empty path.",
target.prettyPrint().c_str(), (
int)poses.size());
5068 std::map<int, int> ids =
_memory->
getNeighborsId(nearestId, maxGraphDepth, 0,
true,
true,
true,
true, nodesSet);
5072 for(std::map<int, int>::iterator
iter=ids.begin();
iter!=ids.end(); ++
iter)
5074 std::map<int, Transform>::iterator jter = poses.find(
iter->first);
5075 if(jter != poses.end())
5077 bool valid =
path.empty();
5082 for(std::multimap<int, Link>::iterator kter=links.begin(); kter!=links.end() && !valid; ++kter)
5084 valid =
path.find(kter->first) !=
path.end();
5103 UWARN(
"%s=Off but path(%d) > maxGraphDepth(%d)*2+1, nearestId=%d ids=%d. Is reduce graph activated before?",
5104 Parameters::kMemReduceGraph().
c_str(), (
int)
path.size(), maxGraphDepth, nearestId, (
int)ids.size());
5106 paths.insert(std::make_pair(nearestId,
path));
5110 UWARN(
"path.size()=0!? nearestId=%d ids=%d, aborting...", nearestId, (
int)ids.size());
5116 UDEBUG(
"e0=%fs e1=%fs e2=%fs e3=%fs e4=%fs", e0, e1, e2, e3, e4);
5123 bool lookInDatabase,
5124 std::map<int, Transform> & optimizedPoses,
5125 cv::Mat & covariance,
5126 std::multimap<int, Link> * constraints,
5128 int * iterationsDone)
const
5131 UINFO(
"Optimize map: around location %d (lookInDatabase=%s)",
id, lookInDatabase?
"true":
"false");
5138 id = ids.begin()->first;
5140 UINFO(
"get %d ids time %f s", (
int)ids.size(),
timer.ticks());
5147 optimizedPoses = poses;
5152 UINFO(
"Correction (from node %d) %s",
id,
t.prettyPrint().c_str());
5157 UWARN(
"Failed to optimize the graph! returning empty optimized poses...");
5158 optimizedPoses.clear();
5161 constraints->clear();
5169 const std::set<int> & ids,
5170 const std::map<int, Transform> & guessPoses,
5171 bool lookInDatabase,
5172 cv::Mat & covariance,
5173 std::multimap<int, Link> * constraints,
5175 int * iterationsDone)
const
5178 std::map<int, Transform> optimizedPoses;
5179 std::map<int, Transform> poses;
5180 std::multimap<int, Link> edgeConstraints;
5181 UDEBUG(
"ids=%d", (
int)ids.size());
5183 UINFO(
"get constraints (ids=%d, %d poses, %d edges) time %f s", (
int)ids.size(), (
int)poses.size(), (
int)edgeConstraints.size(),
timer.ticks());
5186 for(std::map<int, Transform>::iterator
iter=poses.begin();
iter!=poses.end() &&
iter->first < 0; ++
iter)
5190 cv::Mat infMatrix = cv::Mat::eye(6, 6, CV_64FC1);
5201 for(std::map<int, Transform>::iterator
iter=poses.begin();
iter!=poses.end(); ++
iter)
5204 std::map<int, Transform>::const_iterator foundGuess = guessPoses.find(
iter->first);
5205 if(foundGuess!=guessPoses.end() &&
iter->first != fromId)
5207 iter->second = foundGuess->second;
5217 optimizedPoses = poses;
5220 *constraints = edgeConstraints;
5225 bool hasLandmarks = !edgeConstraints.empty() && edgeConstraints.begin()->first < 0;
5226 if(poses.size() != guessPoses.size() || hasLandmarks)
5228 UDEBUG(
"recompute poses using only links (robust to multi-session)");
5229 std::map<int, Transform> posesOut;
5230 std::multimap<int, Link> edgeConstraintsOut;
5235 *constraints = edgeConstraintsOut;
5240 UDEBUG(
"use input guess poses");
5244 *constraints = edgeConstraints;
5248 if(!poses.empty() && optimizedPoses.empty())
5250 UWARN(
"Optimization has failed (poses=%d, guess=%d, links=%d)...",
5251 (
int)poses.size(), (
int)guessPoses.size(), (
int)edgeConstraints.size());
5255 UINFO(
"Optimization time %f s",
timer.ticks());
5257 return optimizedPoses;
5265 if(likelihood.size()==0)
5272 bool likelihoodNullValuesIgnored =
true;
5273 for(std::map<int, float>::iterator
iter = ++likelihood.begin();
iter!=likelihood.end(); ++
iter)
5275 if((
iter->second >= 0 && !likelihoodNullValuesIgnored) ||
5276 (
iter->second > 0 && likelihoodNullValuesIgnored))
5291 for(std::map<int, float>::iterator
iter=++likelihood.begin();
iter!= likelihood.end(); ++
iter)
5294 iter->second = 1.0f;
5310 maxId =
iter->first;
5316 likelihood.begin()->second =
mean/stdDev + 1.0f;
5320 likelihood.begin()->second = stdDev/(
max-
mean) + 1.0
f;
5324 likelihood.begin()->second = 2.0f;
5328 UDEBUG(
"mean=%f, stdDev=%f, max=%f, maxId=%d, time=%fs",
mean, stdDev,
max, maxId,
time);
5337 UERROR(
"Working directory not set.");
5340 std::list<int> signaturesToCompare;
5349 if(
s->getWeight() != -1)
5351 signaturesToCompare.push_back(
iter->first);
5357 signaturesToCompare.push_back(
iter->first);
5363 std::string fileName = this->
getWorkingDir() +
"/DumpPrediction.txt";
5365 fopen_s(&fout, fileName.c_str(),
"w");
5367 fout = fopen(fileName.c_str(),
"w");
5372 for(
int i=0;
i<prediction.rows; ++
i)
5374 for(
int j=0;
j<prediction.cols; ++
j)
5376 fprintf(fout,
"%f ",((
float*)prediction.data)[
j +
i*prediction.cols]);
5378 fprintf(fout,
"\n");
5385 UWARN(
"Memory and/or the Bayes filter are not created");
5403 _memory->
getNodeInfo(
id, odomPoseLocal, mapId, weight, label, stamp, groundTruth,
velocity, gps, sensors,
true);
5406 if(images || scan || userData || occupancyGrid)
5410 if(!images && withWords)
5412 std::vector<CameraModel> models;
5413 std::vector<StereoCameraModel> stereoModels;
5415 data.setCameraModels(models);
5416 data.setStereoCameraModels(stereoModels);
5429 for(std::multimap<int, Link>::iterator
iter=links.begin();
iter!=links.end(); ++
iter)
5433 s.addLandmark(
iter->second);
5437 s.addLink(
iter->second);
5441 if(withWords || withGlobalDescriptors)
5443 std::multimap<int, int> words;
5444 std::vector<cv::KeyPoint> wordsKpts;
5445 std::vector<cv::Point3f> words3;
5446 cv::Mat wordsDescriptors;
5447 std::vector<rtabmap::GlobalDescriptor> globalDescriptors;
5451 s.setWords(words, wordsKpts, words3, wordsDescriptors);
5453 if(withGlobalDescriptors)
5455 s.sensorData().setGlobalDescriptors(globalDescriptors);
5462 s.sensorData().setGPS(gps);
5463 s.sensorData().setEnvSensors(sensors);
5469 std::map<int, Signature> & signatures,
5470 std::map<int, Transform> & poses,
5471 std::multimap<int, Link> & constraints,
5476 return getGraph(poses, constraints, optimized, global, &signatures,
true,
true,
true,
true);
5480 std::map<int, Transform> & poses,
5481 std::multimap<int, Link> & constraints,
5484 std::map<int, Signature> * signatures,
5490 bool withGlobalDescriptors)
const
5535 for(std::set<int>::iterator
iter = ids.begin();
iter!=ids.end(); ++
iter)
5537 signatures->insert(std::make_pair(*
iter,
getSignatureCopy(*
iter, withImages, withScan, withUserData, withGrid, withWords, withGlobalDescriptors)));
5543 UERROR(
"Last working signature is null!?");
5547 UWARN(
"Memory not initialized...");
5553 std::map<int, float> nearestNodesTmp;
5554 std::map<int, float> * nearestNodesPtr = distsSqr == 0? &nearestNodesTmp : distsSqr;
5556 std::map<int, Transform> nearestPoses;
5557 for(std::map<int, float>::iterator
iter=nearestNodesPtr->begin();
iter!=nearestNodesPtr->end(); ++
iter)
5561 return nearestPoses;
5566 UDEBUG(
"nodeId=%d, radius=%f", nodeId, radius);
5567 std::map<int, float> nearestNodesTmp;
5568 std::map<int, float> * nearestNodesPtr = distsSqr == 0? &nearestNodesTmp : distsSqr;
5586 std::map<int, Transform> nearestPoses;
5587 for(std::map<int, float>::iterator
iter=nearestNodesPtr->begin();
iter!=nearestNodesPtr->end(); ++
iter)
5592 return nearestPoses;
5596 float clusterRadiusMax,
5602 float clusterRadiusMin)
5608 UERROR(
"Cannot detect more loop closures if graph optimization iterations = 0");
5613 UERROR(
"Detecting more loop closures can be done only in RGBD-SLAM mode.");
5616 if(!intraSession && !interSession)
5618 UERROR(
"Intra and/or inter session argument should be true.");
5622 std::list<Link> loopClosuresAdded;
5623 std::multimap<int, int> checkedLoopClosures;
5625 std::map<int, Transform> posesToCheckLoopClosures;
5626 std::map<int, Transform> poses;
5627 std::multimap<int, Link> links;
5628 std::map<int, Signature> signatures;
5629 this->
getGraph(poses, links,
true,
true, &signatures);
5631 std::map<int, int> mapIds;
5632 UDEBUG(
"remove all invalid or intermediate nodes, fill mapIds");
5633 for(std::map<int, Transform>::iterator
iter=poses.upper_bound(0);
iter!=poses.end();++
iter)
5635 if(signatures.at(
iter->first).getWeight() >= 0)
5637 posesToCheckLoopClosures.insert(*
iter);
5638 mapIds.insert(std::make_pair(
iter->first, signatures.at(
iter->first).mapId()));
5642 for(
int n=0;
n<iterations; ++
n)
5644 UINFO(
"Looking for more loop closures, clustering poses... (iteration=%d/%d, radius=%f m angle=%f rad)",
5645 n+1, iterations, clusterRadiusMax, clusterAngle);
5648 posesToCheckLoopClosures,
5652 UINFO(
"Looking for more loop closures, clustering poses... found %d clusters.", (
int)clusters.size());
5655 std::set<int> addedLinks;
5656 for(std::multimap<int, int>::iterator
iter=clusters.begin();
iter!= clusters.end(); ++
iter, ++
i)
5658 if(processState && processState->
isCanceled())
5664 int from =
iter->first;
5665 int to =
iter->second;
5668 from =
iter->second;
5672 int mapIdFrom =
uValue(mapIds, from, 0);
5673 int mapIdTo =
uValue(mapIds, to, 0);
5675 if((interSession && mapIdFrom != mapIdTo) ||
5676 (intraSession && mapIdFrom == mapIdTo))
5679 bool alreadyChecked =
false;
5680 for(std::multimap<int, int>::iterator jter = checkedLoopClosures.lower_bound(from);
5681 !alreadyChecked && jter!=checkedLoopClosures.end() && jter->first == from;
5684 if(to == jter->second)
5686 alreadyChecked =
true;
5693 if(addedLinks.find(from) == addedLinks.end() &&
5694 addedLinks.find(to) == addedLinks.end() &&
5699 if(
delta.getNorm() < clusterRadiusMax &&
5700 delta.getNorm() >= clusterRadiusMin)
5702 checkedLoopClosures.insert(std::make_pair(from, to));
5704 UASSERT(signatures.find(from) != signatures.end());
5705 UASSERT(signatures.find(to) != signatures.end());
5710 guess = poses.at(from).
inverse() * poses.at(to);
5719 bool updateConstraints =
true;
5724 int mapId = signatures.at(from).mapId();
5726 for(std::map<int, Signature>::iterator ster=signatures.begin(); ster!=signatures.end(); ++ster)
5728 if(ster->second.mapId() == mapId)
5730 fromId = ster->first;
5734 std::multimap<int, Link> linksIn = links;
5736 const Link * maxLinearLink = 0;
5737 const Link * maxAngularLink = 0;
5738 float maxLinearError = 0.0f;
5739 float maxAngularError = 0.0f;
5740 float maxLinearErrorRatio = 0.0f;
5741 float maxAngularErrorRatio = 0.0f;
5742 std::map<int, Transform> optimizedPoses;
5743 std::multimap<int, Link> links;
5744 UASSERT(poses.find(fromId) != poses.end());
5745 UASSERT_MSG(poses.find(from) != poses.end(),
uFormat(
"id=%d poses=%d links=%d", from, (
int)poses.size(), (
int)links.size()).c_str());
5746 UASSERT_MSG(poses.find(to) != poses.end(),
uFormat(
"id=%d poses=%d links=%d", to, (
int)poses.size(), (
int)links.size()).c_str());
5748 UASSERT(optimizedPoses.find(fromId) != optimizedPoses.end());
5749 UASSERT_MSG(optimizedPoses.find(from) != optimizedPoses.end(),
uFormat(
"id=%d poses=%d links=%d", from, (
int)optimizedPoses.size(), (
int)links.size()).c_str());
5750 UASSERT_MSG(optimizedPoses.find(to) != optimizedPoses.end(),
uFormat(
"id=%d poses=%d links=%d", to, (
int)optimizedPoses.size(), (
int)links.size()).c_str());
5754 if(optimizedPoses.size())
5759 maxLinearErrorRatio,
5760 maxAngularErrorRatio,
5767 UINFO(
"Max optimization linear error = %f m (link %d->%d)", maxLinearError, maxLinearLink->
from(), maxLinearLink->
to());
5770 msg =
uFormat(
"Rejecting edge %d->%d because "
5771 "graph error is too large after optimization (%f m for edge %d->%d with ratio %f > std=%f m). "
5776 maxLinearLink->
from(),
5777 maxLinearLink->
to(),
5778 maxLinearErrorRatio,
5780 Parameters::kRGBDOptimizeMaxError().c_str(),
5785 UERROR(
"Huge optimization error detected!"
5786 "Linear error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). You may consider "
5787 "enabling \"%s\" to reject those bad optimizations by setting it to a non null value!",
5788 maxLinearErrorRatio,
5789 maxLinearLink->
from(),
5790 maxLinearLink->
to(),
5791 maxLinearLink->
type(),
5794 Parameters::kRGBDOptimizeMaxError().c_str());
5797 else if(maxAngularLink)
5799 UINFO(
"Max optimization angular error = %f deg (link %d->%d)", maxAngularError*180.0
f/
M_PI, maxAngularLink->
from(), maxAngularLink->
to());
5802 msg =
uFormat(
"Rejecting edge %d->%d because "
5803 "graph error is too large after optimization (%f deg for edge %d->%d with ratio %f > std=%f deg). "
5807 maxAngularError*180.0
f/
M_PI,
5808 maxAngularLink->
from(),
5809 maxAngularLink->
to(),
5810 maxAngularErrorRatio,
5812 Parameters::kRGBDOptimizeMaxError().c_str(),
5817 UERROR(
"Huge optimization error detected!"
5818 "Angular error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). You may consider "
5819 "enabling \"%s\" to reject those bad optimizations by setting it to a non null value!",
5820 maxAngularErrorRatio,
5821 maxAngularLink->
from(),
5822 maxAngularLink->
to(),
5823 maxAngularLink->
type(),
5824 maxAngularError*180.0f/CV_PI,
5826 Parameters::kRGBDOptimizeMaxError().c_str());
5832 msg =
uFormat(
"Rejecting edge %d->%d because graph optimization has failed!",
5839 updateConstraints =
false;
5843 poses = optimizedPoses;
5846 if(updateConstraints)
5848 addedLinks.insert(from);
5849 addedLinks.insert(to);
5853 std::string
msg =
uFormat(
"Iteration %d/%d: Added loop closure %d->%d! (%d/%d)",
n+1, iterations, from, to,
i+1, (
int)clusters.size());
5874 std::string
msg =
uFormat(
"Iteration %d/%d: Detected %d total loop closures!",
n+1, iterations, (
int)addedLinks.size()/2);
5883 UINFO(
"Iteration %d/%d: Detected %d total loop closures!",
n+1, iterations, (
int)addedLinks.size()/2);
5886 if(addedLinks.size() == 0)
5891 UINFO(
"Optimizing graph with new links (%d nodes, %d constraints)...",
5892 (
int)poses.size(), (
int)links.size());
5895 if(poses.size() == 0)
5897 UERROR(
"Optimization failed! Rejecting all loop closures...");
5898 loopClosuresAdded.clear();
5901 UINFO(
"Optimizing graph with new links... done!");
5903 UINFO(
"Total added %d loop closures.", (
int)loopClosuresAdded.size());
5905 if(loopClosuresAdded.size())
5907 for(std::list<Link>::iterator
iter=loopClosuresAdded.begin();
iter!=loopClosuresAdded.end(); ++
iter)
5914 std::map<int, Transform>::iterator jter = poses.find(
iter->first);
5915 if(jter != poses.end())
5917 iter->second = jter->second;
5920 std::map<int, Transform> tmp;
5926 return (
int)loopClosuresAdded.size();
5931 bool rematchFeatures,
5933 float pixelVariance)
5937 int iterations = Parameters::defaultOptimizerIterations();
5938 float pixelVariance = Parameters::defaultg2oPixelVariance();
5946 if(pixelVariance > 0.0
f)
5951 std::map<int, Signature> signatures;
5961 std::map<int, Transform> poses = optimizer->
optimizeBA(
5971 UERROR(
"Optimization failed!");
5989 const std::map<int, Transform> & poses,
5990 const cv::Mat & map,
6015 UERROR(
"Refining links can be done only in RGBD-SLAM mode.");
6019 std::list<Link> linksRefined;
6021 std::map<int, Transform> poses;
6022 std::multimap<int, Link> links;
6023 std::map<int, Signature> signatures;
6024 this->
getGraph(poses, links,
false,
true, &signatures);
6027 for(std::multimap<int, Link>::iterator
iter=links.lower_bound(1);
iter!= links.end(); ++
iter)
6029 int from =
iter->second.from();
6030 int to =
iter->second.to();
6032 UASSERT(signatures.find(from) != signatures.end());
6033 UASSERT(signatures.find(to) != signatures.end());
6041 linksRefined.push_back(
Link(from, to,
iter->second.type(),
t,
info.covariance.inv()));
6042 UINFO(
"Refined link %d->%d! (%d/%d)", from, to, ++
i, (
int)links.size());
6045 UINFO(
"Total refined %d links.", (
int)linksRefined.size());
6047 if(linksRefined.size())
6049 for(std::list<Link>::iterator
iter=linksRefined.begin();
iter!=linksRefined.end(); ++
iter)
6054 return (
int)linksRefined.size();
6062 UERROR(
"Adding new link can be done only in RGBD-SLAM mode.");
6067 UERROR(
"Memory is not initialized.");
6072 UERROR(
"Link's transform is null! (%d->%d type=%s)", link.
from(), link.
to(), link.
typeName().c_str());
6079 UERROR(
"Link's \"from id\" %d is not in working memory", link.
from());
6084 UERROR(
"Link's \"to id\" %d is not in working memory", link.
to());
6091 UERROR(
"Neither nodes %d or %d are in the local graph (size=%d). One of the 2 nodes should be in the local graph.", (
int)
_optimizedPoses.size(), link.
from(), link.
to());
6098 UERROR(
"Cannot add new link %d->%d to memory", link.
from(), link.
to());
6104 std::multimap<int, Link> links;
6108 if(poses.find(link.
from()) == poses.end())
6110 UERROR(
"Link's \"from id\" %d is not in the graph (size=%d)", link.
from(), (
int)poses.size());
6114 if(poses.find(link.
to()) == poses.end())
6116 UERROR(
"Link's \"to id\" %d is not in the graph (size=%d)", link.
to(), (
int)poses.size());
6124 msg =
uFormat(
"Rejecting edge %d->%d because graph optimization has failed!", link.
from(), link.
to());
6128 float maxLinearError = 0.0f;
6129 float maxLinearErrorRatio = 0.0f;
6130 float maxAngularError = 0.0f;
6131 float maxAngularErrorRatio = 0.0f;
6132 const Link * maxLinearLink = 0;
6133 const Link * maxAngularLink = 0;
6138 maxLinearErrorRatio,
6139 maxAngularErrorRatio,
6146 UINFO(
"Max optimization linear error = %f m (link %d->%d)", maxLinearError, maxLinearLink->
from(), maxLinearLink->
to());
6149 msg =
uFormat(
"Rejecting edge %d->%d because "
6150 "graph error is too large after optimization (%f m for edge %d->%d with ratio %f > std=%f m). "
6155 maxLinearLink->
from(),
6156 maxLinearLink->
to(),
6157 maxLinearErrorRatio,
6159 Parameters::kRGBDOptimizeMaxError().c_str(),
6164 UERROR(
"Huge optimization error detected!"
6165 "Linear error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). You may consider "
6166 "enabling \"%s\" to reject those bad optimizations by setting it to a non null value!",
6167 maxLinearErrorRatio,
6168 maxLinearLink->
from(),
6169 maxLinearLink->
to(),
6170 maxLinearLink->
type(),
6173 Parameters::kRGBDOptimizeMaxError().c_str());
6176 else if(maxAngularLink)
6178 UINFO(
"Max optimization angular error = %f deg (link %d->%d)", maxAngularError*180.0
f/
M_PI, maxAngularLink->
from(), maxAngularLink->
to());
6181 msg =
uFormat(
"Rejecting edge %d->%d because "
6182 "graph error is too large after optimization (%f deg for edge %d->%d with ratio %f > std=%f deg). "
6186 maxAngularError*180.0f/
M_PI,
6187 maxAngularLink->
from(),
6188 maxAngularLink->
to(),
6189 maxAngularErrorRatio,
6191 Parameters::kRGBDOptimizeMaxError().c_str(),
6196 UERROR(
"Huge optimization error detected!"
6197 "Angular error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). You may consider "
6198 "enabling \"%s\" to reject those bad optimizations by setting it to a non null value!",
6199 maxAngularErrorRatio,
6200 maxAngularLink->
from(),
6201 maxAngularLink->
to(),
6202 maxAngularLink->
type(),
6203 maxAngularError*180.0f/CV_PI,
6205 Parameters::kRGBDOptimizeMaxError().c_str());
6219 std::map<int, Transform>::iterator jter = poses.find(
iter->first);
6220 if(jter != poses.end())
6222 iter->second = jter->second;
6230 std::map<int, Transform> tmp;
6240 int oldestId = link.
from()>link.
to()?link.
to():link.
from();
6241 int newestId = link.
from()<link.
to()?link.
to():link.
from();
6245 UERROR(
"Link's id %d is not in working memory", oldestId);
6250 UERROR(
"Link's id %d is not in the optimized graph (_optimizedPoses=%d)", oldestId, (
int)
_optimizedPoses.size());
6255 UERROR(
"Adding link with %s=true in localization mode is not supported.", Parameters::kRGBDOptimizeFromGraphEnd().
c_str());
6262 UERROR(
"Link's id %d is not in the odometry cache (oldest=%d, newest=%d, %s=%d)",
6266 Parameters::kRGBDMaxOdomCacheSize().c_str(),
6271 UERROR(
"Link's id %d is not in the odometry cache (%s=%d).",
6273 Parameters::kRGBDMaxOdomCacheSize().
c_str(),
6285 constraints.insert(std::make_pair(link.
from(), link));
6286 for(std::multimap<int, Link>::iterator
iter=constraints.begin();
iter!=constraints.end(); ++
iter)
6289 if(iterPose !=
_optimizedPoses.end() && poses.find(iterPose->first) == poses.end())
6291 poses.insert(*iterPose);
6293 constraints.insert(std::make_pair(iterPose->first,
Link(iterPose->first, iterPose->first,
Link::kPosePrior, iterPose->second, cv::Mat::eye(6,6, CV_64FC1)*999999)));
6297 std::map<int, Transform> posesOut;
6298 std::multimap<int, Link> edgeConstraintsOut;
6302 std::map<int, Transform> optPoses =
_graphOptimizer->
optimize(poses.begin()->first, posesOut, edgeConstraintsOut);
6305 bool rejectLocalization =
false;
6306 if(optPoses.empty())
6308 UWARN(
"Optimization failed, rejecting localization!");
6309 rejectLocalization =
true;
6313 UINFO(
"Compute max graph errors...");
6314 float maxLinearError = 0.0f;
6315 float maxLinearErrorRatio = 0.0f;
6316 float maxAngularError = 0.0f;
6317 float maxAngularErrorRatio = 0.0f;
6318 const Link * maxLinearLink = 0;
6319 const Link * maxAngularLink = 0;
6323 maxLinearErrorRatio,
6324 maxAngularErrorRatio,
6330 if(maxLinearLink == 0 && maxAngularLink==0)
6332 UWARN(
"Could not compute graph errors! Wrong loop closures could be accepted!");
6337 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()));
6340 UWARN(
"Rejecting localization (%d <-> %d) in this "
6341 "iteration because a wrong loop closure has been "
6342 "detected after graph optimization, resulting in "
6343 "a maximum graph error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). The "
6344 "maximum error ratio parameter \"%s\" is %f of std deviation.",
6347 maxLinearErrorRatio,
6348 maxLinearLink->
from(),
6349 maxLinearLink->
to(),
6350 maxLinearLink->
type(),
6353 Parameters::kRGBDOptimizeMaxError().c_str(),
6355 rejectLocalization =
true;
6359 UERROR(
"Huge optimization error detected!"
6360 "Linear error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). You may consider "
6361 "enabling \"%s\" to reject those bad optimizations by setting it to a non null value!",
6362 maxLinearErrorRatio,
6363 maxLinearLink->
from(),
6364 maxLinearLink->
to(),
6365 maxLinearLink->
type(),
6368 Parameters::kRGBDOptimizeMaxError().c_str());
6373 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()));
6376 UWARN(
"Rejecting localization (%d <-> %d) in this "
6377 "iteration because a wrong loop closure has been "
6378 "detected after graph optimization, resulting in "
6379 "a maximum graph error ratio of %f (edge %d->%d, type=%d, abs error=%f deg, stddev=%f). The "
6380 "maximum error ratio parameter \"%s\" is %f of std deviation.",
6383 maxAngularErrorRatio,
6384 maxAngularLink->
from(),
6385 maxAngularLink->
to(),
6386 maxAngularLink->
type(),
6387 maxAngularError*180.0f/CV_PI,
6389 Parameters::kRGBDOptimizeMaxError().c_str(),
6391 rejectLocalization =
true;
6395 UERROR(
"Huge optimization error detected!"
6396 "Angular error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). You may consider "
6397 "enabling \"%s\" to reject those bad optimizations by setting it to a non null value!",
6398 maxAngularErrorRatio,
6399 maxAngularLink->
from(),
6400 maxAngularLink->
to(),
6401 maxAngularLink->
type(),
6402 maxAngularError*180.0f/CV_PI,
6404 Parameters::kRGBDOptimizeMaxError().c_str());
6409 if(!rejectLocalization)
6412 Transform newT = newOptPoseInv * optPoses.at(link.
to());
6413 Link linkTmp = link;
6415 if(oldestId == link.
from())
6442 cv::Mat information = covariance.inv();
6446 if(odomMaxInf.size() == 6)
6448 for(
int i=0;
i<6; ++
i)
6450 if(information.at<
double>(
i,
i) > odomMaxInf[
i])
6452 information.at<
double>(
i,
i) = odomMaxInf[
i];
6472 UWARN(
"%s=0, so cannot republish the %d requested nodes.", Parameters::kRtabmapMaxRepublished().
c_str(), (
int)ids.size());
6476 UWARN(
"%s=false, so cannot republish the %d requested nodes.", Parameters::kRtabmapPublishLastSignature().
c_str(), (
int)ids.size());
6482 UINFO(
"status=%d", status);
6504 UINFO(
"Planning a path to node %d (global=%d)", targetNode, global?1:0);
6508 UINFO(
"Planning a path to landmark %d (global=%d)", -targetNode, global?1:0);
6513 UWARN(
"A path can only be computed in RGBD-SLAM mode");
6524 int currentNode = 0;
6529 UWARN(
"Working memory is empty... cannot compute a path");
6538 UWARN(
"Last localization pose is null or optimized graph is empty... cannot compute a path");
6551 if(currentNode && targetNode)
6576 if(!
_path.empty() && !
path.empty() &&
path.rbegin()->first < 0)
6578 transformToLandmark =
_path.back().second.inverse() *
t *
path.rbegin()->second;
6581 else if(currentNode == 0)
6583 UWARN(
"We should be localized before planning.");
6588 if(
_path.size() == 0)
6591 UWARN(
"Cannot compute a path!");
6596 UINFO(
"Path generated! Size=%d", (
int)
_path.size());
6599 std::stringstream
stream;
6600 for(
unsigned int i=0;
i<
_path.size(); ++
i)
6613 std::string goalStr =
uFormat(
"GOAL:%d", targetNode);
6626 std::map<int, std::string>::iterator
iter =
labels.find(targetNode);
6629 goalStr = std::string(
"GOAL:")+
labels.at(targetNode);
6632 setUserData(0, cv::Mat(1,
int(goalStr.size()+1), CV_8SC1, (
void *)goalStr.c_str()).clone());
6648 if(tolerance < 0.0
f)
6653 std::list<std::pair<int, Transform> > pathPoses;
6657 UWARN(
"This method can only be used in RGBD-SLAM mode");
6664 std::multimap<int, int> links;
6669 for(std::map<int, Link>::const_iterator jter=
s->getLinks().begin(); jter!=
s->getLinks().end(); ++jter)
6672 if(jter->second.from() != jter->second.to() &&
uContains(
nodes, jter->second.to()))
6674 links.insert(std::make_pair(jter->second.from(), jter->second.to()));
6679 UINFO(
"Time getting links = %fs",
timer.ticks());
6681 int currentNode = 0;
6686 UWARN(
"Working memory is empty... cannot compute a path");
6695 UWARN(
"Last localization pose is null... cannot compute a path");
6713 nearestId = currentNode;
6719 UINFO(
"Nearest node found=%d ,%fs", nearestId,
timer.ticks());
6722 if(tolerance != 0.0
f && targetPose.
getDistance(
nodes.at(nearestId)) > tolerance)
6724 UWARN(
"Cannot plan farther than %f m from the graph! (distance=%f m from node %d)",
6729 UINFO(
"Computing path from location %d to %d", currentNode, nearestId);
6734 if(
_path.size() == 0)
6736 UWARN(
"Cannot compute a path!");
6740 UINFO(
"Path generated! Size=%d", (
int)
_path.size());
6743 std::stringstream
stream;
6744 for(
unsigned int i=0;
i<
_path.size(); ++
i)
6766 UWARN(
"Nearest node not found in graph (size=%d) for pose %s", (
int)
nodes.size(), targetPose.
prettyPrint().c_str());
6774 std::vector<std::pair<int, Transform> > poses;
6785 poses[oi++] = *
iter;
6799 std::vector<int> ids;
6810 ids[oi++] =
iter->first;
6836 UWARN(
"This method can on be used in RGBD-SLAM mode!");
6858 std::multimap<int, Link> links = currentIndexS->
getLinks();
6859 bool latestVirtualLinkFound =
false;
6860 for(std::multimap<int, Link>::reverse_iterator
iter=links.rbegin();
iter!=links.rend(); ++
iter)
6864 if(latestVirtualLinkFound)
6870 latestVirtualLinkFound =
true;
6876 float distanceSoFar = 0.0f;
6917 UERROR(
"Last node is null in memory or not in optimized poses. Aborting the plan...");
6927 UERROR(
"Last localization pose is null. Aborting the plan...");
6934 int goalId =
_path.back().first;
6941 UINFO(
"Goal %d reached!", goalId);
6950 float distanceFromCurrentNode = 0.0f;
6951 bool sameGoalIndex =
false;
6982 UINFO(
"Updated current goal from %d to %d (%d/%d)",
6988 sameGoalIndex =
true;
6992 unsigned int nearestNodeIndex = 0;
6994 bool sameCurrentIndex =
false;
7005 nearestNodeIndex =
i;
7011 UERROR(
"The nearest pose on the path not found! Aborting the plan...");
7025 sameCurrentIndex =
true;
7028 bool isStuck =
false;
7031 float distanceToCurrentGoal = 0.0f;
7046 if(distanceToCurrentGoal > 0.0
f)
7067 UWARN(
"Current goal %d not reached since %d iterations (\"RGBD/PlanStuckIterations\"=%d), mark that node as unreachable.",
7080 UERROR(
"No upcoming nodes on the path are reachable! Aborting the plan...");
7097 UDEBUG(
"Creating global scan map (if scans are available)");
7100 std::vector<int> scanIndices;
7101 std::map<int, Transform> scanViewpoints;
7105 if(!
data.laserScanCompressed().empty())
7108 data.uncompressDataConst(0, 0, &scan, 0, 0, 0, 0);
7122 UWARN(
"Incompatible scan formats (%s vs %s), cannot create global scan map.",
7132 UDEBUG(
"Ignored %d (scan is empty), pose still added.",
iter->first);
7138 UDEBUG(
"Ignored %d (no scan), pose still added.",
iter->first);
7144 float voxelSize = 0.0f;
7146 float normalRadius = 0.0f;
7151 if(voxelSize > 0.0
f)
7166 UINFO(
"Global scan map has been assembled (size=%d points, %d poses) "
7167 "for proximity detection (only in localization mode %s=false and with %s=true)",
7170 Parameters::kMemIncrementalMemory().c_str(),
7171 Parameters::kRGBDProximityGlobalScanMap().c_str());
7178 UWARN(
"Saving %s/rtabmap_global_scan_map.pcd (only saved when logger level is debug)",
_wDir.c_str());
7180 pcl::io::savePCDFile(
_wDir+
"/rtabmap_global_scan_map.pcd", *cloud2);
7184 UWARN(
"%s is enabled and logger is debug, but %s is not set, cannot save global scan map for debugging.",
7185 Parameters::kRGBDProximityGlobalScanMap().
c_str(), Parameters::kRtabmapWorkingDirectory().
c_str());