61 #include <pcl/io/pcd_io.h> 62 #include <pcl/common/common.h> 65 #include <opencv2/imgproc/types_c.h> 75 _similarityThreshold(
Parameters::defaultMemRehearsalSimilarity()),
76 _binDataKept(
Parameters::defaultMemBinDataKept()),
77 _rawDescriptorsKept(
Parameters::defaultMemRawDescriptorsKept()),
78 _saveDepth16Format(
Parameters::defaultMemSaveDepth16Format()),
79 _notLinkedNodesKeptInDb(
Parameters::defaultMemNotLinkedNodesKept()),
80 _saveIntermediateNodeData(
Parameters::defaultMemIntermediateNodeDataKept()),
81 _rgbCompressionFormat(
Parameters::defaultMemImageCompressionFormat()),
82 _incrementalMemory(
Parameters::defaultMemIncrementalMemory()),
83 _localizationDataSaved(
Parameters::defaultMemLocalizationDataSaved()),
84 _reduceGraph(
Parameters::defaultMemReduceGraph()),
85 _maxStMemSize(
Parameters::defaultMemSTMSize()),
86 _recentWmRatio(
Parameters::defaultMemRecentWmRatio()),
87 _transferSortingByWeightId(
Parameters::defaultMemTransferSortingByWeightId()),
88 _idUpdatedToNewOneRehearsal(
Parameters::defaultMemRehearsalIdUpdatedToNewOne()),
89 _generateIds(
Parameters::defaultMemGenerateIds()),
90 _badSignaturesIgnored(
Parameters::defaultMemBadSignaturesIgnored()),
91 _mapLabelsAdded(
Parameters::defaultMemMapLabelsAdded()),
92 _depthAsMask(
Parameters::defaultMemDepthAsMask()),
93 _stereoFromMotion(
Parameters::defaultMemStereoFromMotion()),
94 _imagePreDecimation(
Parameters::defaultMemImagePreDecimation()),
95 _imagePostDecimation(
Parameters::defaultMemImagePostDecimation()),
96 _compressionParallelized(
Parameters::defaultMemCompressionParallelized()),
97 _laserScanDownsampleStepSize(
Parameters::defaultMemLaserScanDownsampleStepSize()),
98 _laserScanVoxelSize(
Parameters::defaultMemLaserScanVoxelSize()),
99 _laserScanNormalK(
Parameters::defaultMemLaserScanNormalK()),
100 _laserScanNormalRadius(
Parameters::defaultMemLaserScanNormalRadius()),
101 _reextractLoopClosureFeatures(
Parameters::defaultRGBDLoopClosureReextractFeatures()),
102 _localBundleOnLoopClosure(
Parameters::defaultRGBDLocalBundleOnLoopClosure()),
103 _rehearsalMaxDistance(
Parameters::defaultRGBDLinearUpdate()),
104 _rehearsalMaxAngle(
Parameters::defaultRGBDAngularUpdate()),
105 _rehearsalWeightIgnoredWhileMoving(
Parameters::defaultMemRehearsalWeightIgnoredWhileMoving()),
106 _useOdometryFeatures(
Parameters::defaultMemUseOdomFeatures()),
107 _useOdometryGravity(
Parameters::defaultMemUseOdomGravity()),
108 _createOccupancyGrid(
Parameters::defaultRGBDCreateOccupancyGrid()),
109 _visMaxFeatures(
Parameters::defaultVisMaxFeatures()),
110 _imagesAlreadyRectified(
Parameters::defaultRtabmapImagesAlreadyRectified()),
111 _rectifyOnlyFeatures(
Parameters::defaultRtabmapRectifyOnlyFeatures()),
112 _covOffDiagonalIgnored(
Parameters::defaultMemCovOffDiagIgnored()),
113 _detectMarkers(
Parameters::defaultRGBDMarkerDetection()),
114 _markerLinVariance(
Parameters::defaultMarkerVarianceLinear()),
115 _markerAngVariance(
Parameters::defaultMarkerVarianceAngular()),
117 _idMapCount(kIdStart),
119 _lastGlobalLoopClosureId(0),
120 _memoryChanged(
false),
121 _linksChanged(
false),
125 _badSignRatio(
Parameters::defaultKpBadSignRatio()),
126 _tfIdfLikelihoodUsed(
Parameters::defaultKpTfIdfLikelihoodUsed()),
127 _parallelized(
Parameters::defaultKpParallelized())
134 float corRatio = Parameters::defaultIcpCorrespondenceRatio();
140 UWARN(
"%s is >=0.5, which sets correspondence ratio for proximity detection using " 141 "laser scans to 100% (2 x Ratio). You may lower the ratio to accept proximity " 142 "detection with not full scans overlapping.", Parameters::kIcpCorrespondenceRatio().c_str());
220 bool loadAllNodesInWM = Parameters::defaultMemInitWMWithAllNodes();
224 std::list<Signature*> dbSignatures;
239 for(std::list<Signature*>::reverse_iterator iter=dbSignatures.rbegin(); iter!=dbSignatures.rend(); ++iter)
249 _signatures.insert(std::pair<int, Signature *>((*iter)->id(), *iter));
251 if(!(*iter)->getGroundTruthPose().isNull()) {
252 _groundTruths.insert(std::make_pair((*iter)->id(), (*iter)->getGroundTruthPose()));
255 if(!(*iter)->getLandmarks().empty())
258 for(std::map<int, Link>::const_iterator jter = (*iter)->getLandmarks().begin(); jter!=(*iter)->getLandmarks().end(); ++jter)
260 int landmarkId = jter->first;
263 std::map<int, std::set<int> >::iterator nter =
_landmarksIndex.find((*iter)->id());
266 nter->second.insert(landmarkId);
271 tmp.insert(landmarkId);
277 nter->second.insert((*iter)->id());
282 tmp.insert((*iter)->id());
298 UDEBUG(
"Check if all nodes are in Working Memory");
301 for(std::map<int, Link>::const_iterator jter = iter->second->getLinks().begin(); jter!=iter->second->getLinks().end(); ++jter)
312 UDEBUG(
"update odomMaxInf vector");
313 std::multimap<int, Link> links = this->
getAllLinks(
true,
true);
314 for(std::multimap<int, Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
317 iter->second.infMatrix().cols == 6 &&
318 iter->second.infMatrix().rows == 6)
324 for(
int i=0; i<6; ++i)
326 const double & v = iter->second.infMatrix().at<
double>(i,i);
334 UDEBUG(
"update odomMaxInf vector, done!");
356 UDEBUG(
"Loading dictionary...");
359 UDEBUG(
"load all referenced words in working memory");
361 std::set<int> wordIds;
362 const std::map<int, Signature *> & signatures = this->
getSignatures();
363 for(std::map<int, Signature *>::const_iterator i=signatures.begin(); i!=signatures.end(); ++i)
365 const std::multimap<int, int> & words = i->second->getWords();
367 for(std::list<int>::iterator iter=keys.begin(); iter!=keys.end(); ++iter)
371 wordIds.insert(*iter);
376 UDEBUG(
"load words %d", (
int)wordIds.size());
381 std::list<VisualWord*> words;
383 for(std::list<VisualWord*>::iterator iter = words.begin(); iter!=words.end(); ++iter)
410 const std::map<int, Signature *> & signatures = this->
getSignatures();
411 for(std::map<int, Signature *>::const_iterator i=signatures.begin(); i!=signatures.end(); ++i)
416 const std::multimap<int, int> & words = s->
getWords();
419 UDEBUG(
"node=%d, word references=%d", s->
id(), words.size());
420 for(std::multimap<int, int>::const_iterator iter = words.begin(); iter!=words.end(); ++iter)
443 parameters.erase(Parameters::kRtabmapWorkingDirectory());
460 void Memory::close(
bool databaseSaved,
bool postInitClosingEvents,
const std::string & ouputDatabasePath)
462 UINFO(
"databaseSaved=%d, postInitClosingEvents=%d", databaseSaved?1:0, postInitClosingEvents?1:0);
465 bool databaseNameChanged =
false;
475 UINFO(
"No changes added to database.");
490 UINFO(
"Saving memory...");
523 UWARN(
"Please call Memory::close() before");
538 ParametersMap::const_iterator iter;
613 if((iter=params.find(Parameters::kKpDetectorStrategy())) != params.end())
621 UWARN(
"new detector strategy %d while the vocabulary is already created. This may give problems if feature descriptors are not the same type than the one used in the current vocabulary (a memory reset would be required if so).",
int(detectorStrategy));
625 UINFO(
"new detector strategy %d.",
int(detectorStrategy));
645 if((iter=params.find(Parameters::kRegStrategy())) != params.end())
651 UDEBUG(
"new registration strategy %d",
int(regStrategy));
667 if(
uContains(params, Parameters::kIcpCorrespondenceRatio()))
670 float corRatio = Parameters::defaultIcpCorrespondenceRatio();
676 UWARN(
"%s is >=0.5, which sets correspondence ratio for proximity detection using " 677 "laser scans to 100% (2 x Ratio). You may lower the ratio to accept proximity " 678 "detection with not full scans overlapping.", Parameters::kIcpCorrespondenceRatio().c_str());
700 iter = params.find(Parameters::kMemIncrementalMemory());
701 if(iter != params.end())
703 bool value =
uStr2Bool(iter->second.c_str());
713 UWARN(
"Switching from Mapping to Localization mode, the database will be saved and reloaded.");
720 UWARN(
"Switching from Mapping to Localization mode, the database is reloaded!");
728 int visFeatureType = Parameters::defaultVisFeatureType();
729 int kpDetectorStrategy = Parameters::defaultKpDetectorStrategy();
732 if(visFeatureType != kpDetectorStrategy)
734 UWARN(
"%s is enabled, but %s and %s parameters are not the same! Disabling %s...",
735 Parameters::kMemUseOdomFeatures().c_str(),
736 Parameters::kVisFeatureType().c_str(),
737 Parameters::kKpDetectorStrategy().c_str(),
738 Parameters::kMemUseOdomFeatures().c_str());
763 return update(data,
Transform(), cv::Mat(), std::vector<float>(), stats);
769 const cv::Mat & covariance,
770 const std::vector<float> & velocity,
782 UDEBUG(
"pre-updating...");
784 t=timer.
ticks()*1000;
785 if(stats) stats->
addStatistic(Statistics::kTimingMemPre_update(), t);
786 UDEBUG(
"time preUpdate=%f ms", t);
794 UERROR(
"Failed to create a signature...");
797 if(velocity.size()==6)
799 signature->
setVelocity(velocity[0], velocity[1], velocity[2], velocity[3], velocity[4], velocity[5]);
802 t=timer.
ticks()*1000;
803 if(stats) stats->
addStatistic(Statistics::kTimingMemSignature_creation(), t);
804 UDEBUG(
"time creating signature=%f ms", t);
823 t=timer.
ticks()*1000;
824 if(stats) stats->
addStatistic(Statistics::kTimingMemRehearsal(), t);
825 UDEBUG(
"time rehearsal=%f ms", t);
831 UWARN(
"The working memory is empty and the memory is not " 832 "incremental (Mem/IncrementalMemory=False), no loop closure " 833 "can be detected! Please set Mem/IncrementalMemory=true to increase " 834 "the memory with new images or decrease the STM size (which is %d " 835 "including the new one added).", (
int)
_stMem.size());
842 int notIntermediateNodesCount = 0;
843 for(std::set<int>::iterator iter=
_stMem.begin(); iter!=
_stMem.end(); ++iter)
849 ++notIntermediateNodesCount;
852 std::map<int, int> reducedIds;
860 --notIntermediateNodesCount;
868 reducedIds.insert(std::make_pair(
id, reducedTo));
899 UASSERT(covariance.cols == 6 && covariance.rows == 6 && covariance.type() == CV_64FC1);
900 double maxAngVar = 0.0;
903 maxAngVar = covariance.at<
double>(5,5);
907 maxAngVar =
uMax3(covariance.at<
double>(3,3), covariance.at<
double>(4,4), covariance.at<
double>(5,5));
910 if(maxAngVar != 1.0 && maxAngVar > 0.1)
912 static bool warned =
false;
915 UWARN(
"Very large angular variance (%f) detected! Please fix odometry " 916 "covariance, otherwise poor graph optimizations are " 917 "expected and wrong loop closure detections creating a lot " 918 "of errors in the map could be accepted. This message is only " 919 "printed once.", maxAngVar);
927 infMatrix = cv::Mat::zeros(6,6,CV_64FC1);
928 infMatrix.at<
double>(0,0) = 1.0 / covariance.at<
double>(0,0);
929 infMatrix.at<
double>(1,1) = 1.0 / covariance.at<
double>(1,1);
930 infMatrix.at<
double>(2,2) = 1.0 / covariance.at<
double>(2,2);
931 infMatrix.at<
double>(3,3) = 1.0 / covariance.at<
double>(3,3);
932 infMatrix.at<
double>(4,4) = 1.0 / covariance.at<
double>(4,4);
933 infMatrix.at<
double>(5,5) = 1.0 / covariance.at<
double>(5,5);
937 infMatrix = covariance.inv();
939 if((
uIsFinite(covariance.at<
double>(0,0)) && covariance.at<
double>(0,0)>0.0) &&
940 !(
uIsFinite(infMatrix.at<
double>(0,0)) && infMatrix.at<
double>(0,0)>0.0))
942 UERROR(
"Failed to invert the covariance matrix! Covariance matrix should be invertible!");
943 std::cout <<
"Covariance: " << covariance << std::endl;
944 infMatrix = cv::Mat::eye(6,6,CV_64FC1);
947 UASSERT(infMatrix.rows == 6 && infMatrix.cols == 6);
952 for(
int i=0; i<6; ++i)
954 const double & v = infMatrix.at<
double>(i,i);
974 UDEBUG(
"Ignoring neighbor link between %d and %d because they are not in the same map! (%d vs %d)",
984 UINFO(
"Tagging node %d with label \"%s\"", signature->
id(), tag.c_str());
986 _labels.insert(std::make_pair(signature->
id(), tag));
997 UINFO(
"Tagging node %d with label \"%s\"", signature->
id(), tag.c_str());
999 _labels.insert(std::make_pair(signature->
id(), tag));
1012 UDEBUG(
"%d words ref for the signature %d (weight=%d)", signature->
getWords().size(), signature->
id(), signature->
getWeight());
1027 UDEBUG(
"Inserting node %d in WM...", signature->
id());
1029 _signatures.insert(std::pair<int, Signature*>(signature->
id(), signature));
1037 UERROR(
"Signature is null ?!?");
1043 UDEBUG(
"Inserting node %d from STM in WM...",
id);
1051 const std::multimap<int, Link> & links = s->getLinks();
1052 std::map<int, Link> neighbors;
1053 for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
1057 merge = iter->second.to() < s->id() &&
1058 iter->second.to() != iter->second.from() &&
1061 iter->second.userDataCompressed().empty() &&
1066 UDEBUG(
"Reduce %d to %d", s->id(), iter->second.to());
1069 *reducedTo = iter->second.to();
1076 neighbors.insert(*iter);
1081 if(s->getLabel().empty())
1083 for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
1086 if(sTo->
id()!=s->id())
1095 for(std::map<int, Link>::iterator jter=neighbors.begin(); jter!=neighbors.end(); ++jter)
1097 if(!sTo->
hasLink(jter->second.to()))
1099 UDEBUG(
"Merging link %d->%d (type=%d) to link %d->%d (type %d)",
1100 iter->second.from(), iter->second.to(), iter->second.type(),
1101 jter->second.from(), jter->second.to(), jter->second.type());
1117 std::multimap<int, Link> linksCopy = links;
1118 for(std::multimap<int, Link>::iterator iter=linksCopy.begin(); iter!=linksCopy.end(); ++iter)
1123 s->removeLink(iter->first);
1164 bool lookInDatabase)
const 1166 std::multimap<int, Link> links;
1170 const std::multimap<int, Link> & allLinks = s->
getLinks();
1171 for(std::multimap<int, Link>::const_iterator iter = allLinks.begin(); iter!=allLinks.end(); ++iter)
1176 links.insert(*iter);
1183 for(std::multimap<int, Link>::iterator iter=links.begin(); iter!=links.end();)
1188 links.erase(iter++);
1198 UWARN(
"Cannot find signature %d in memory", signatureId);
1205 bool lookInDatabase)
const 1208 std::multimap<int, Link> loopClosures;
1211 const std::multimap<int, Link> & allLinks = s->
getLinks();
1212 for(std::multimap<int, Link>::const_iterator iter = allLinks.begin(); iter!=allLinks.end(); ++iter)
1216 iter->second.from() != iter->second.to() &&
1219 loopClosures.insert(*iter);
1226 for(std::multimap<int, Link>::iterator iter=loopClosures.begin(); iter!=loopClosures.end();)
1232 loopClosures.erase(iter++);
1240 return loopClosures;
1245 bool lookInDatabase,
1246 bool withLandmarks)
const 1248 std::multimap<int, Link> links;
1266 UWARN(
"Cannot find signature %d in memory", signatureId);
1269 else if(signatureId < 0)
1271 int landmarkId = signatureId;
1275 for(std::set<int>::const_iterator jter=iter->second.begin(); jter!=iter->second.end(); ++jter)
1280 std::map<int, Link>::const_iterator kter = s->
getLandmarks().find(landmarkId);
1284 links.insert(std::make_pair(s->
id(), kter->second.inverse()));
1291 std::map<int, Link> nodes;
1293 for(std::map<int, Link>::iterator kter=nodes.begin(); kter!=nodes.end(); ++kter)
1295 links.insert(std::make_pair(kter->first, kter->second.inverse()));
1302 std::multimap<int, Link>
Memory::getAllLinks(
bool lookInDatabase,
bool ignoreNullLinks,
bool withLandmarks)
const 1304 std::multimap<int, Link> links;
1313 links.erase(iter->first);
1314 for(std::map<int, Link>::const_iterator jter=iter->second->getLinks().begin();
1315 jter!=iter->second->getLinks().end();
1318 if(!ignoreNullLinks || jter->second.isValid())
1320 links.insert(std::make_pair(iter->first, jter->second));
1325 for(std::map<int, Link>::const_iterator jter=iter->second->getLandmarks().begin();
1326 jter!=iter->second->getLandmarks().end();
1329 if(!ignoreNullLinks || jter->second.isValid())
1331 links.insert(std::make_pair(iter->first, jter->second));
1347 int maxCheckedInDatabase,
1348 bool incrementMarginOnLoop,
1350 bool ignoreIntermediateNodes,
1351 bool ignoreLocalSpaceLoopIds,
1352 const std::set<int> & nodesSet,
1353 double * dbAccessTime
1365 std::map<int, int> ids;
1370 int nbLoadedFromDb = 0;
1371 std::list<int> curentMarginList;
1372 std::set<int> currentMargin;
1373 std::set<int> nextMargin;
1374 nextMargin.insert(signatureId);
1376 std::set<int> ignoredIds;
1377 while((maxGraphDepth == 0 || m < maxGraphDepth) && nextMargin.size())
1380 curentMarginList = std::list<int>(nextMargin.rbegin(), nextMargin.rend());
1383 for(std::list<int>::iterator jter = curentMarginList.begin(); jter!=curentMarginList.end(); ++jter)
1385 if(ids.find(*jter) == ids.end() && (nodesSet.empty() || nodesSet.find(*jter) != nodesSet.end()))
1390 std::multimap<int, Link> tmpLinks;
1391 std::map<int, Link> tmpLandmarks;
1392 const std::multimap<int, Link> * links = &tmpLinks;
1393 const std::map<int, Link> * landmarks = &tmpLandmarks;
1396 if(!ignoreIntermediateNodes || s->
getWeight() != -1)
1398 ids.insert(std::pair<int, int>(*jter, m));
1402 ignoredIds.insert(*jter);
1411 else if(maxCheckedInDatabase == -1 || (maxCheckedInDatabase > 0 &&
_dbDriver && nbLoadedFromDb < maxCheckedInDatabase))
1414 ids.insert(std::pair<int, int>(*jter, m));
1418 if(tmpLinks.empty())
1420 UWARN(
"No links loaded for %d?!", *jter);
1424 for(std::multimap<int, Link>::iterator kter=tmpLinks.begin(); kter!=tmpLinks.end();)
1428 tmpLandmarks.insert(*kter);
1429 tmpLinks.erase(kter++);
1431 else if(kter->second.from() == kter->second.to())
1434 tmpLinks.erase(kter++);
1449 for(std::multimap<int, Link>::const_iterator iter=links->begin(); iter!=links->end(); ++iter)
1452 ignoredIds.find(iter->first) == ignoredIds.end())
1458 if(ignoreIntermediateNodes && s->
getWeight()==-1)
1461 if(currentMargin.insert(iter->first).second)
1463 curentMarginList.push_back(iter->first);
1468 nextMargin.insert(iter->first);
1473 if(incrementMarginOnLoop)
1475 nextMargin.insert(iter->first);
1479 if(currentMargin.insert(iter->first).second)
1481 curentMarginList.push_back(iter->first);
1489 for(std::map<int, Link>::const_iterator iter=landmarks->begin(); iter!=landmarks->end(); ++iter)
1494 for(std::set<int>::const_iterator nter=kter->second.begin(); nter!=kter->second.end(); ++nter)
1496 if( !
uContains(ids, *nter) && ignoredIds.find(*nter) == ignoredIds.end())
1498 if(incrementMarginOnLoop)
1500 nextMargin.insert(*nter);
1504 if(currentMargin.insert(*nter).second)
1506 curentMarginList.push_back(*nter);
1524 const std::map<int, Transform> & optimizedPoses,
1531 std::map<int, float> ids;
1532 std::map<int, float> checkedIds;
1533 std::list<int> curentMarginList;
1534 std::set<int> currentMargin;
1535 std::set<int> nextMargin;
1536 nextMargin.insert(signatureId);
1538 Transform referential = optimizedPoses.at(signatureId);
1540 float radiusSqrd = radius*radius;
1541 while((maxGraphDepth == 0 || m < maxGraphDepth) && nextMargin.size())
1543 curentMarginList = std::list<int>(nextMargin.begin(), nextMargin.end());
1546 for(std::list<int>::iterator jter = curentMarginList.begin(); jter!=curentMarginList.end(); ++jter)
1548 if(checkedIds.find(*jter) == checkedIds.end())
1555 const Transform & t = optimizedPoses.at(*jter);
1558 if(radiusSqrd == 0 || distanceSqrd<radiusSqrd)
1560 ids.insert(std::pair<int, float>(*jter,distanceSqrd));
1564 for(std::multimap<int, Link>::const_iterator iter=s->
getLinks().begin(); iter!=s->
getLinks().end(); ++iter)
1567 uContains(optimizedPoses, iter->first) &&
1570 nextMargin.insert(iter->first);
1596 int id = *
_stMem.begin();
1598 if(reducedIds && reducedId > 0)
1600 reducedIds->insert(std::make_pair(
id, reducedId));
1611 std::map<int, double>::iterator iter=
_workingMem.find(signatureId);
1631 std::string version =
"0.0.0";
1641 std::string url =
"";
1661 for(std::map<int, Signature*>::const_iterator iter =
_signatures.begin(); iter!=
_signatures.end(); ++iter)
1663 ids.insert(iter->first);
1708 uFormat(
"The number of signatures don't match! _workingMem=%d, _stMem=%d, _signatures=%d",
1711 UDEBUG(
"Adding statistics after run...");
1716 parameters.erase(Parameters::kRtabmapWorkingDirectory());
1730 for(std::map<int, Signature *>::iterator i=mem.begin(); i!=mem.end(); ++i)
1734 UDEBUG(
"deleting from the working and the short-term memory: %d", i->first);
1801 std::map<int, float> likelihood;
1808 else if(ids.empty())
1810 UWARN(
"ids list is empty");
1814 for(std::list<int>::const_iterator iter = ids.begin(); iter!=ids.end(); ++iter)
1822 UFATAL(
"Signature %d not found in WM ?!?", *iter);
1827 likelihood.insert(likelihood.end(), std::pair<int, float>(*iter, sim));
1830 UDEBUG(
"compute likelihood (similarity)... %f s", timer.
ticks());
1837 std::map<int, float> likelihood;
1838 std::map<int, float> calculatedWordsRatio;
1845 else if(ids.empty())
1847 UWARN(
"ids list is empty");
1851 for(std::list<int>::const_iterator iter = ids.begin(); iter!=ids.end(); ++iter)
1853 likelihood.insert(likelihood.end(), std::pair<int, float>(*iter, 0.0f));
1870 UDEBUG(
"processing... ");
1872 for(std::list<int>::const_iterator i=wordIds.begin(); i!=wordIds.end(); ++i)
1884 logNnw = log10(N/nw);
1887 for(std::map<int, int>::const_iterator j=refs.begin(); j!=refs.end(); ++j)
1889 std::map<int, float>::iterator iter = likelihood.find(j->first);
1890 if(iter != likelihood.end())
1893 ni = this->
getNi(j->first);
1897 iter->second += ( nwi * logNnw ) / ni;
1907 UDEBUG(
"compute likelihood (tf-idf) %f s", timer.
ticks());
1915 std::map<int, int> weights;
1923 UFATAL(
"Location %d must exist in memory", iter->first);
1925 weights.insert(weights.end(), std::make_pair(iter->first, s->
getWeight()));
1929 weights.insert(weights.end(), std::make_pair(iter->first, -1));
1938 std::list<int> signaturesRemoved;
1949 int wordsRemoved = 0;
1956 while(wordsRemoved < newWords)
1959 if(signatures.size())
1964 signaturesRemoved.push_back(s->
id());
1978 UDEBUG(
"newWords=%d, wordsRemoved=%d", newWords, wordsRemoved);
1986 for(std::list<Signature *>::iterator iter=signatures.begin(); iter!=signatures.end(); ++iter)
1988 signaturesRemoved.push_back((*iter)->id());
1993 if((
int)signatures.size() < signaturesAdded)
1995 UWARN(
"Less signatures transferred (%d) than added (%d)! The working memory cannot decrease in size.",
1996 (
int)signatures.size(), signaturesAdded);
2000 UDEBUG(
"signaturesRemoved=%d, _signaturesAdded=%d", (
int)signatures.size(), signaturesAdded);
2003 return signaturesRemoved;
2010 int signatureRemoved = 0;
2023 return signatureRemoved;
2064 return std::map<int, Transform>();
2085 const cv::Mat & cloud,
2086 const std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > & polygons,
2087 #
if PCL_VERSION_COMPARE(>=, 1, 8, 0)
2088 const std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > & texCoords,
2090 const std::vector<std::vector<Eigen::Vector2f> > & texCoords,
2092 const cv::Mat & textures)
const 2101 std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > * polygons,
2102 #
if PCL_VERSION_COMPARE(>=, 1, 8, 0)
2103 std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > * texCoords,
2105 std::vector<std::vector<Eigen::Vector2f> > * texCoords,
2107 cv::Mat * textures)
const 2147 else if(weight == k.
weight)
2153 else if(age == k.
age)
2168 std::list<Signature *> removableSignatures;
2169 std::map<WeightAgeIdKey, Signature *> weightAgeIdMap;
2172 UDEBUG(
"mem.size()=%d, ignoredIds.size()=%d", (
int)
_workingMem.size(), (int)ignoredIds.size());
2177 bool recentWmImmunized =
false;
2179 int currentRecentWmSize = 0;
2186 ++currentRecentWmSize;
2189 if(currentRecentWmSize>1 && currentRecentWmSize < recentWmMaxSize)
2191 recentWmImmunized =
true;
2193 else if(currentRecentWmSize == 0 &&
_workingMem.size() > 1)
2207 for(std::map<int, double>::const_iterator memIter =
_workingMem.begin(); memIter !=
_workingMem.end(); ++memIter)
2214 else if(memIter->first > 0 && ignoredIds.find(memIter->first) == ignoredIds.end() && (!lastInSTM || !lastInSTM->
hasLink(memIter->first)))
2220 bool foundInSTM =
false;
2221 for(std::map<int, Link>::const_iterator iter = s->
getLinks().begin(); iter!=s->
getLinks().end(); ++iter)
2225 UDEBUG(
"Ignored %d because it has a link (%d) to STM", s->
id(), iter->first);
2247 int recentWmCount = 0;
2250 UDEBUG(
"signatureMap.size()=%d _lastGlobalLoopClosureId=%d currentRecentWmSize=%d recentWmMaxSize=%d",
2252 for(std::map<WeightAgeIdKey, Signature*>::iterator iter=weightAgeIdMap.begin();
2253 iter!=weightAgeIdMap.end();
2256 if(!recentWmImmunized)
2258 UDEBUG(
"weight=%d, id=%d",
2259 iter->second->getWeight(),
2260 iter->second->id());
2261 removableSignatures.push_back(iter->second);
2266 if(currentRecentWmSize - recentWmCount < recentWmMaxSize)
2268 UDEBUG(
"switched recentWmImmunized");
2269 recentWmImmunized =
true;
2275 UDEBUG(
"weight=%d, id=%d",
2276 iter->second->getWeight(),
2277 iter->second->id());
2278 removableSignatures.push_back(iter->second);
2280 if(removableSignatures.size() >= (
unsigned int)count)
2288 ULOGGER_WARN(
"not enough signatures to get an old one...");
2290 return removableSignatures;
2306 int landmarkId = iter->first;
2310 nter->second.erase(landmarkId);
2311 if(nter->second.empty())
2319 nter->second.erase(s->
id());
2320 if(nter->second.empty())
2331 keepLinkedToGraph =
false;
2335 if(!keepLinkedToGraph)
2338 uFormat(
"Deleting location (%d) outside the " 2339 "STM is not implemented!", s->
id()).c_str());
2340 const std::multimap<int, Link> & links = s->
getLinks();
2341 for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
2343 if(iter->second.from() != iter->second.to() && iter->first > 0)
2348 uFormat(
"A neighbor (%d) of the deleted location %d is " 2349 "not found in WM/STM! Are you deleting a location " 2350 "outside the STM?", iter->first, s->
id()).c_str());
2352 if(iter->first > s->
id() && links.size()>1 && sTo->
hasLink(s->
id()))
2354 UWARN(
"Link %d of %d is newer, removing neighbor link " 2355 "may split the map!",
2356 iter->first, s->
id());
2386 for(std::list<int>::const_iterator i=keys.begin(); i!=keys.end(); ++i)
2392 std::vector<VisualWord*> wordToDelete;
2393 wordToDelete.push_back(w);
2397 deletedWords->push_back(w->
id());
2436 if(keepLinkedToGraph)
2462 UDEBUG(
"landmarkId=%d", landmarkId);
2463 std::map<int, Link> nodes;
2469 for(std::set<int>::const_iterator jter=iter->second.begin(); jter!=iter->second.end(); ++jter)
2474 std::map<int, Link>::const_iterator kter = s->
getLandmarks().find(landmarkId);
2477 nodes.insert(std::make_pair(s->
id(), kter->second));
2492 UDEBUG(
"label=%s", label.c_str());
2499 if(iter->second->getLabel().compare(label) == 0)
2501 id = iter->second->id();
2505 if(
id == 0 &&
_dbDriver && lookInDatabase)
2517 if(idFound == 0 || idFound ==
id)
2525 UWARN(
"Label \"%s\" set to node %d", label.c_str(), id);
2532 std::list<Signature *> signatures;
2534 if(signatures.size())
2536 uInsert(
_labels, std::make_pair(signatures.front()->id(), label));
2537 signatures.front()->setLabel(label);
2538 UWARN(
"Label \"%s\" set to node %d", label.c_str(), id);
2545 UERROR(
"Node %d not found, failed to set label \"%s\"!",
id, label.c_str());
2550 UWARN(
"Node %d has already label \"%s\"", idFound, label.c_str());
2565 UERROR(
"Node %d not found in RAM, failed to set user data (size=%d)!",
id, data.total());
2572 UDEBUG(
"Deleting location %d", locationId);
2582 UDEBUG(
"Saving location data %d", locationId);
2606 UINFO(
"removing link between location %d and %d", oldS->
id(), newS->
id());
2627 bool noChildrenAnymore =
true;
2628 for(std::map<int, Link>::const_iterator iter=newS->
getLinks().begin(); iter!=newS->
getLinks().end(); ++iter)
2632 iter->second.from() != iter->second.to() &&
2633 iter->first < newS->
id())
2635 noChildrenAnymore =
false;
2646 UERROR(
"Signatures %d and %d don't have bidirectional link!", oldS->
id(), newS->
id());
2653 UERROR(
"Signature %d is not in working memory... cannot remove link.", newS->
id());
2657 UERROR(
"Signature %d is not in working memory... cannot remove link.", oldS->
id());
2664 UDEBUG(
"id=%d image=%d scan=%d userData=%d",
id, image?1:0, scan?1:0, userData?1:0);
2681 bool useKnownCorrespondencesIfPossible)
2690 transform =
computeTransform(*fromS, *toS, guess, info, useKnownCorrespondencesIfPossible);
2694 std::string msg =
uFormat(
"Did not find nodes %d and/or %d", fromId, toId);
2710 bool useKnownCorrespondencesIfPossible)
const 2730 cv::Mat imgBuf, depthBuf, userBuf;
2745 std::vector<int> inliersV;
2757 tmpFrom.
sensorData().
setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
2761 else if(useKnownCorrespondencesIfPossible)
2785 else if(!isNeighborRefining &&
2797 std::multimap<int, int> words;
2798 std::vector<cv::Point3f> words3DMap;
2799 std::vector<cv::KeyPoint> wordsMap;
2800 cv::Mat wordsDescriptorsMap;
2802 const std::multimap<int, Link> & links = fromS.
getLinks();
2806 UDEBUG(
"fromS.getWords()=%d uniques=%d", (
int)fromS.
getWords().size(), (int)wordsFrom.size());
2807 for(std::map<int, int>::const_iterator jter=wordsFrom.begin(); jter!=wordsFrom.end(); ++jter)
2809 const cv::Point3f & pt = fromS.
getWords3()[jter->second];
2812 words.insert(std::make_pair(jter->first, words.size()));
2813 words3DMap.push_back(pt);
2814 wordsMap.push_back(fromS.
getWordsKpts()[jter->second]);
2819 UDEBUG(
"words3DMap=%d", (
int)words3DMap.size());
2821 for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
2823 int id = iter->first;
2830 for(std::map<int, int>::const_iterator jter=wordsTo.begin(); jter!=wordsTo.end(); ++jter)
2832 const cv::Point3f & pt = s->
getWords3()[jter->second];
2833 if( jter->first > 0 &&
2835 words.find(jter->first) == words.end())
2837 words.insert(words.end(), std::make_pair(jter->first, words.size()));
2846 UDEBUG(
"words3DMap=%d", (
int)words3DMap.size());
2848 tmpFrom2.
setWords(words, wordsMap, words3DMap, wordsDescriptorsMap);
2852 if(!transform.
isNull() && info && !tmpFrom2.getWords3().empty())
2854 std::map<int, cv::Point3f> points3DMap;
2856 for(std::map<int, int>::iterator iter=wordsMap.begin(); iter!=wordsMap.end(); ++iter)
2858 points3DMap.insert(std::make_pair(iter->first, tmpFrom2.getWords3()[iter->second]));
2860 std::map<int, Transform> bundlePoses;
2861 std::multimap<int, Link> bundleLinks;
2862 std::map<int, CameraModel> bundleModels;
2863 std::map<int, std::map<int, FeatureBA> > wordReferences;
2865 std::multimap<int, Link> links = fromS.
getLinks();
2868 links.insert(std::make_pair(fromS.
id(),
Link()));
2870 int totalWordReferences = 0;
2871 for(std::multimap<int, Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
2873 int id = iter->first;
2874 if(
id != fromS.
id() || iter->second.transform().isNull())
2878 if(
id == tmpTo.
id())
2906 UFATAL(
"no valid camera model to use local bundle adjustment on loop closure!");
2908 bundleModels.insert(std::make_pair(
id, model));
2910 UASSERT(iter->second.isValid() || iter->first == fromS.
id());
2912 if(iter->second.transform().isNull())
2919 bundleLinks.insert(std::make_pair(iter->second.from(), iter->second));
2920 bundlePoses.insert(std::make_pair(
id, iter->second.transform()));
2924 for(std::map<int, int>::const_iterator jter=words.begin(); jter!=words.end(); ++jter)
2926 if(points3DMap.find(jter->first)!=points3DMap.end() &&
2927 (
id == tmpTo.
id() || jter->first > 0))
2937 wordReferences.insert(std::make_pair(jter->first, std::map<int, FeatureBA>()));
2938 wordReferences.at(jter->first).insert(std::make_pair(
id,
FeatureBA(s->
getWordsKpts()[jter->second], d)));
2939 ++totalWordReferences;
2949 std::set<int> sbaOutliers;
2954 bundlePoses = sba.
optimizeBA(-toS.
id(), bundlePoses, bundleLinks, bundleModels, points3DMap, wordReferences, &sbaOutliers);
2957 UDEBUG(
"bundleTime=%fs (poses=%d wordRef=%d outliers=%d)", bundleTime.
ticks(), (int)bundlePoses.size(), totalWordReferences, (int)sbaOutliers.size());
2959 UDEBUG(
"Local Bundle Adjustment Before: %s", transform.prettyPrint().c_str());
2960 if(!bundlePoses.rbegin()->second.isNull())
2962 if(sbaOutliers.size())
2964 std::vector<int> newInliers(info->
inliersIDs.size());
2966 for(
unsigned int i=0; i<info->
inliersIDs.size(); ++i)
2968 if(sbaOutliers.find(info->
inliersIDs[i]) == sbaOutliers.end())
2973 newInliers.resize(oi);
2974 UDEBUG(
"BA outliers ratio %f",
float(sbaOutliers.size())/
float(info->
inliersIDs.size()));
2975 info->
inliers = (int)newInliers.size();
2981 transform.setNull();
2985 transform = bundlePoses.rbegin()->second;
2988 transform = transform.to3DoF();
2992 UDEBUG(
"Local Bundle Adjustment After : %s", transform.prettyPrint().c_str());
3007 const std::map<int, Transform> & poses,
3013 UDEBUG(
"%d -> %d, Guess=%s", fromId, toId, (poses.at(fromId).inverse() * poses.at(toId)).prettyPrint().c_str());
3017 for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
3019 if(iter->first != fromId)
3024 UDEBUG(
"%d vs %s", fromId, ids.c_str());
3028 std::list<Signature*> depthToLoad;
3029 for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
3037 depthToLoad.push_back(s);
3057 float guessNorm = guess.
getNorm();
3062 UINFO(
"Too far scans between %d and %d to compute transformation: guessNorm=%f, scan range from=%f to=%f", fromId, toId, guessNorm, fromScan.
rangeMax(), toScan.
rangeMax());
3070 int maxPoints = fromScan.
size();
3071 pcl::PointCloud<pcl::PointXYZ>::Ptr assembledToClouds(
new pcl::PointCloud<pcl::PointXYZ>);
3072 pcl::PointCloud<pcl::PointNormal>::Ptr assembledToNormalClouds(
new pcl::PointCloud<pcl::PointNormal>);
3073 pcl::PointCloud<pcl::PointXYZI>::Ptr assembledToIClouds(
new pcl::PointCloud<pcl::PointXYZI>);
3074 pcl::PointCloud<pcl::PointXYZINormal>::Ptr assembledToNormalIClouds(
new pcl::PointCloud<pcl::PointXYZINormal>);
3075 pcl::PointCloud<pcl::PointXYZRGB>::Ptr assembledToRGBClouds(
new pcl::PointCloud<pcl::PointXYZRGB>);
3076 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr assembledToNormalRGBClouds(
new pcl::PointCloud<pcl::PointXYZRGBNormal>);
3077 UDEBUG(
"maxPoints from(%d) = %d", fromId, maxPoints);
3078 for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
3080 if(iter->first != fromId)
3129 if(scan.
size() > maxPoints)
3131 UDEBUG(
"maxPoints scan(%d) = %d", iter->first, (
int)scan.
size());
3132 maxPoints = scan.
size();
3142 UWARN(
"Depth2D not found for signature %d", iter->first);
3147 cv::Mat assembledScan;
3148 if(assembledToNormalClouds->size())
3152 else if(assembledToClouds->size())
3156 else if(assembledToNormalIClouds->size())
3160 else if(assembledToIClouds->size())
3164 else if(assembledToNormalRGBClouds->size())
3168 UERROR(
"Cannot handle 2d scan with RGB format.");
3175 else if(assembledToRGBClouds->size())
3179 UERROR(
"Cannot handle 2d scan with RGB format.");
3186 UDEBUG(
"assembledScan=%d points", assembledScan.cols);
3211 if(toS->hasLink(link.
from()))
3214 UINFO(
"already linked! to=%d, from=%d", link.
to(), link.
from());
3218 UDEBUG(
"Add link between %d and %d", toS->id(), fromS->id());
3221 fromS->addLink(link);
3238 UASSERT(fromS->getWeight() >= 0 && toS->getWeight() >=0);
3242 fromS->setWeight(fromS->getWeight() + toS->getWeight());
3247 toS->setWeight(toS->getWeight() + fromS->getWeight());
3248 fromS->setWeight(0);
3254 else if(!addInDatabase)
3258 UERROR(
"from=%d, to=%d, Signature %d not found in working/st memories", link.
from(), link.
to(), link.
from());
3262 UERROR(
"from=%d, to=%d, Signature %d not found in working/st memories", link.
from(), link.
to(), link.
to());
3268 UDEBUG(
"Add link between %d and %d (db)", link.
from(), link.
to());
3269 fromS->addLink(link);
3274 UDEBUG(
"Add link between %d (db) and %d", link.
from(), link.
to());
3280 UDEBUG(
"Add link between %d (db) and %d (db)", link.
from(), link.
to());
3299 toS->removeLink(link.
from());
3311 UERROR(
"fromId=%d and toId=%d are not linked!", link.
from(), link.
to());
3314 else if(!updateInDatabase)
3318 UERROR(
"from=%d, to=%d, Signature %d not found in working/st memories", link.
from(), link.
to(), link.
from());
3322 UERROR(
"from=%d, to=%d, Signature %d not found in working/st memories", link.
from(), link.
to(), link.
to());
3327 UDEBUG(
"Update link between %d and %d (db)", link.
from(), link.
to());
3334 UDEBUG(
"Update link between %d (db) and %d", link.
from(), link.
to());
3335 toS->removeLink(link.
from());
3341 UDEBUG(
"Update link between %d (db) and %d (db)", link.
from(), link.
to());
3352 iter->second->removeVirtualLinks();
3362 const std::multimap<int, Link> & links = s->
getLinks();
3363 for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
3374 UERROR(
"Link %d of %d not in WM/STM?!?", iter->first, s->
id());
3382 UERROR(
"Signature %d not in WM/STM?!?", signatureId);
3388 UINFO(
"Dumping memory to directory \"%s\"", directory.c_str());
3389 this->
dumpDictionary((directory+
"/DumpMemoryWordRef.txt").c_str(), (directory+
"/DumpMemoryWordDesc.txt").c_str());
3390 this->
dumpSignatures((directory +
"/DumpMemorySign.txt").c_str(),
false);
3391 this->
dumpSignatures((directory +
"/DumpMemorySign3.txt").c_str(),
true);
3392 this->
dumpMemoryTree((directory +
"/DumpMemoryTree.txt").c_str());
3408 fopen_s(&foutSign, fileNameSign,
"w");
3410 foutSign = fopen(fileNameSign,
"w");
3415 fprintf(foutSign,
"SignatureID WordsID...\n");
3416 const std::map<int, Signature *> & signatures = this->
getSignatures();
3417 for(std::map<int, Signature *>::const_iterator iter=signatures.begin(); iter!=signatures.end(); ++iter)
3419 fprintf(foutSign,
"%d ", iter->first);
3427 const std::multimap<int, int> & ref = ss->
getWords();
3428 for(std::multimap<int, int>::const_iterator jter=ref.begin(); jter!=ref.end(); ++jter)
3430 const cv::Point3f & pt = ss->
getWords3()[jter->second];
3433 (pt.x != 0 || pt.y != 0 || pt.z != 0))
3435 fprintf(foutSign,
"%d ", (*jter).first);
3442 const std::multimap<int, int> & ref = ss->
getWords();
3443 for(std::multimap<int, int>::const_iterator jter=ref.begin(); jter!=ref.end(); ++jter)
3445 fprintf(foutSign,
"%d ", (*jter).first);
3449 fprintf(foutSign,
"\n");
3460 fopen_s(&foutTree, fileNameTree,
"w");
3462 foutTree = fopen(fileNameTree,
"w");
3467 fprintf(foutTree,
"SignatureID Weight NbLoopClosureIds LoopClosureIds... NbChildLoopClosureIds ChildLoopClosureIds...\n");
3471 fprintf(foutTree,
"%d %d", i->first, i->second->getWeight());
3473 std::map<int, Link> loopIds, childIds;
3475 for(std::map<int, Link>::const_iterator iter = i->second->getLinks().begin();
3476 iter!=i->second->getLinks().end();
3482 if(iter->first < i->first)
3484 childIds.insert(*iter);
3486 else if(iter->second.from() != iter->second.to())
3488 loopIds.insert(*iter);
3493 fprintf(foutTree,
" %d", (
int)loopIds.size());
3494 for(std::map<int, Link>::const_iterator j=loopIds.begin(); j!=loopIds.end(); ++j)
3496 fprintf(foutTree,
" %d", j->first);
3499 fprintf(foutTree,
" %d", (
int)childIds.size());
3500 for(std::map<int, Link>::const_iterator j=childIds.begin(); j!=childIds.end(); ++j)
3502 fprintf(foutTree,
" %d", j->first);
3505 fprintf(foutTree,
"\n");
3515 unsigned long memoryUsage =
sizeof(
Memory);
3516 memoryUsage +=
_signatures.size() * (
sizeof(int)+
sizeof(std::map<int, Signature *>::iterator)) +
sizeof(std::map<int, Signature *>);
3519 memoryUsage += iter->second->getMemoryUsed(
true);
3525 memoryUsage +=
_stMem.size() * (
sizeof(int)+
sizeof(std::set<int>::iterator)) +
sizeof(std::set<int>);
3526 memoryUsage +=
_workingMem.size() * (
sizeof(int)+
sizeof(
double)+
sizeof(std::map<int, double>::iterator)) +
sizeof(std::map<int, double>);
3527 memoryUsage +=
_groundTruths.size() * (
sizeof(int)+
sizeof(
Transform)+12*
sizeof(float) +
sizeof(std::map<int, Transform>::iterator)) +
sizeof(std::map<int, Transform>);
3528 memoryUsage +=
_labels.size() * (
sizeof(int)+
sizeof(std::string) +
sizeof(std::map<int, std::string>::iterator)) +
sizeof(std::map<int, std::string>);
3529 for(std::map<int, std::string>::const_iterator iter=
_labels.begin(); iter!=
_labels.end(); ++iter)
3531 memoryUsage+=iter->second.size();
3533 memoryUsage +=
_landmarksIndex.size() * (
sizeof(int)+
sizeof(std::set<int>) +
sizeof(std::map<int, std::set<int> >::iterator)) +
sizeof(std::map<int, std::set<int> >);
3534 memoryUsage +=
_landmarksInvertedIndex.size() * (
sizeof(int)+
sizeof(std::set<int>) +
sizeof(std::map<int, std::set<int> >::iterator)) +
sizeof(std::map<int, std::set<int> >);
3537 memoryUsage+=iter->second.size()*(
sizeof(int)+
sizeof(std::set<int>::iterator)) +
sizeof(std::set<int>);
3541 memoryUsage+=iter->second.size()*(
sizeof(int)+
sizeof(std::set<int>::iterator)) +
sizeof(std::set<int>);
3566 for(std::set<int>::reverse_iterator iter=
_stMem.rbegin(); iter!=
_stMem.rend(); ++iter)
3579 UDEBUG(
"Comparing with signature (%d)...",
id);
3599 if(stats) stats->
addStatistic(Statistics::kMemoryRehearsal_merged(), merged);
3600 if(stats) stats->
addStatistic(Statistics::kMemoryRehearsal_sim(), sim);
3602 UDEBUG(
"merged=%d, sim=%f t=%fs", merged, sim, timer.
ticks());
3606 if(stats) stats->
addStatistic(Statistics::kMemoryRehearsal_merged(), 0);
3607 if(stats) stats->
addStatistic(Statistics::kMemoryRehearsal_sim(), 0);
3619 std::map<int, Link>::const_iterator iter = oldS->
getLinks().find(newS->
id());
3620 if(iter != oldS->
getLinks().end() &&
3623 iter->second.from() != iter->second.to())
3626 UWARN(
"already merged, old=%d, new=%d", oldId, newId);
3631 UINFO(
"Rehearsal merging %d (w=%d) and %d (w=%d)",
3636 bool intermediateMerge =
false;
3637 if(!newS->
getLinks().empty() && !newS->
getLinks().begin()->second.transform().isNull())
3643 newS->
getLinks().begin()->second.transform().getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
3652 UINFO(
"Rehearsal ignored because the robot has moved more than %f m or %f rad (\"Mem/RehearsalWeightIgnoredWhileMoving\"=true)",
3656 fullMerge = !isMoving && newS->
hasLink(oldS->
id());
3657 intermediateMerge = !isMoving && !newS->
hasLink(oldS->
id());
3661 fullMerge = newS->
hasLink(oldS->
id()) && newS->
getLinks().begin()->second.transform().isNull();
3663 UDEBUG(
"fullMerge=%s intermediateMerge=%s _idUpdatedToNewOneRehearsal=%s",
3664 fullMerge?
"true":
"false",
3665 intermediateMerge?
"true":
"false",
3678 const std::multimap<int, Link> & links = oldS->
getLinks();
3679 for(std::multimap<int, Link>::const_iterator iter = links.begin(); iter!=links.end(); ++iter)
3681 if(iter->second.from() != iter->second.to())
3683 Link link = iter->second;
3692 s->addLink(mergedLink.
inverse());
3698 UERROR(
"Didn't find neighbor %d of %d in RAM...", link.
to(), oldS->
id());
3747 oldS->
setWeight(intermediateMerge?-1:0);
3758 newS->
setWeight(intermediateMerge?-1:0);
3766 UERROR(
"newId=%d, oldId=%d, Signature %d not found in working/st memories", newId, oldId, newId);
3770 UERROR(
"newId=%d, oldId=%d, Signature %d not found in working/st memories", newId, oldId, oldId);
3779 int mapId = -1, weight;
3782 std::vector<float> velocity;
3785 getNodeInfo(signatureId, pose, mapId, weight, label, stamp, groundTruth, velocity, gps, sensors, lookInDatabase);
3795 std::vector<float> velocity;
3798 getNodeInfo(signatureId, pose, mapId, weight, label, stamp, groundTruth, velocity, gps, sensors, lookInDatabase);
3808 std::vector<float> velocity;
3811 getNodeInfo(signatureId, pose, mapId, weight, label, stamp, groundTruth, velocity, gps, sensors, lookInDatabase);
3824 std::vector<float> velocity;
3826 getNodeInfo(
id, odomPose, mapId, weight, label, stamp, groundTruth, velocity, gps, sensors, lookInDatabase);
3828 if(gps.
stamp() == 0.0)
3832 std::map<int, int> nearestIds;
3833 nearestIds =
getNeighborsId(
id, maxGraphDepth, lookInDatabase?-1:0,
true,
false,
true);
3834 std::multimap<int, int> nearestIdsSorted;
3835 for(std::map<int, int>::iterator iter=nearestIds.begin(); iter!=nearestIds.end(); ++iter)
3837 nearestIdsSorted.insert(std::make_pair(iter->second, iter->first));
3840 for(std::map<int, int>::iterator iter=nearestIdsSorted.begin(); iter!=nearestIdsSorted.end(); ++iter)
3846 std::list<std::pair<int, Transform> > path =
graph::computePath(s->
id(), id,
this, lookInDatabase);
3847 if(path.size() >= 2)
3851 offsetENU = localToENU*(s->
getPose().
rotation()*path.rbegin()->second);
3856 UWARN(
"Failed to find path %d -> %d", s->
id(), id);
3867 std::string & label,
3870 std::vector<float> & velocity,
3873 bool lookInDatabase)
const 3881 label = std::string(s->
getLabel());
3891 return _dbDriver->
getNodeInfo(signatureId, odomPose, mapId, weight, label, stamp, groundTruth, velocity, gps, sensors);
3927 r.
setRGBDImage(cv::Mat(), cv::Mat(), std::vector<CameraModel>());
3952 std::multimap<int, int> & words,
3953 std::vector<cv::KeyPoint> & wordsKpts,
3954 std::vector<cv::Point3f> & words3,
3955 cv::Mat & wordsDescriptors,
3956 std::vector<GlobalDescriptor> & globalDescriptors)
const 3971 std::list<Signature*> signatures;
3973 ids.push_back(nodeId);
3974 std::set<int> loadedFromTrash;
3976 if(signatures.size())
3978 words = signatures.front()->getWords();
3979 wordsKpts = signatures.front()->getWordsKpts();
3980 words3 = signatures.front()->getWords3();
3981 wordsDescriptors = signatures.front()->getWordsDescriptors();
3982 globalDescriptors = signatures.front()->sensorData().globalDescriptors();
3983 if(loadedFromTrash.size())
3990 delete signatures.front();
3997 std::vector<CameraModel> & models,
4018 UERROR(
"A database must must loaded first...");
4031 ni = (int)((
Signature *)s)->getWords().size();
4051 id.push_back(to->
id());
4057 UDEBUG(
"Loaded image data from database");
4069 ULOGGER_ERROR(
"Can't merge the signatures because there are not same type.");
4095 data.
imageRaw().type() == CV_8UC1 ||
4096 data.
imageRaw().type() == CV_8UC3);
4104 uFormat(
"image=(%d/%d, type=%d, [accepted=%d,%d]) depth=(%d/%d, type=%d [accepted=%d(depth mm),%d(depth m),%d(stereo)]). " 4105 "For stereo, left and right images should be same size. " 4106 "For RGB-D, depth can be X times smaller than RGB (where X is an integer).",
4115 CV_16UC1, CV_32FC1, CV_8UC1).c_str());
4122 UERROR(
"Camera calibration not valid, calibrate your camera!");
4132 std::vector<cv::KeyPoint> keypoints;
4133 cv::Mat descriptors;
4134 bool isIntermediateNode = data.
id() < 0 || (data.
imageRaw().empty() && data.
keypoints().empty());
4144 UERROR(
"Received image ID is null. " 4145 "Please set parameter Mem/GenerateIds to \"true\" or " 4146 "make sure the input source provides image ids (seq).");
4155 UERROR(
"Id of acquired image (%d) is smaller than the last in memory (%d). " 4156 "Please set parameter Mem/GenerateIds to \"true\" or " 4157 "make sure the input source provides image ids (seq) over the last in " 4158 "memory, which is %d.",
4182 for(
unsigned int i=0; i<data.
cameraModels().size(); ++i)
4191 UWARN(
"Initializing rectification maps for camera %d (only done for the first image received)...", i);
4193 UWARN(
"Initializing rectification maps for camera %d (only done for the first image received)... done!", i);
4199 rectifiedImage.copyTo(cv::Mat(rectifiedImages, cv::Rect(subImageWidth*i, 0, subImageWidth, data.
imageRaw().rows)));
4200 imagesRectified =
true;
4204 UERROR(
"Calibration for camera %d cannot be used to rectify the image. Make sure to do a " 4205 "full calibration. If images are already rectified, set %s parameter back to true.",
4207 Parameters::kRtabmapImagesAlreadyRectified().c_str());
4220 UWARN(
"Initializing rectification maps (only done for the first image received)...");
4222 UWARN(
"Initializing rectification maps (only done for the first image received)...done!");
4231 imagesRectified =
true;
4235 UERROR(
"Stereo calibration cannot be used to rectify images. Make sure to do a " 4236 "full stereo calibration. If images are already rectified, set %s parameter back to true.",
4237 Parameters::kRtabmapImagesAlreadyRectified().c_str());
4241 if(stats) stats->
addStatistic(Statistics::kTimingMemRectification(), t*1000.0
f);
4242 UDEBUG(
"time rectification = %fs", t);
4254 UDEBUG(
"Start dictionary update thread");
4255 preUpdateThread.
start();
4258 int preDecimation = 1;
4259 std::vector<cv::Point3f> keypoints3D;
4261 UDEBUG(
"Received kpts=%d kpts3D=%d, descriptors=%d _useOdometryFeatures=%s",
4270 decimatedData = data;
4274 std::vector<CameraModel> cameraModels = decimatedData.
cameraModels();
4275 for(
unsigned int i=0; i<cameraModels.size(); ++i)
4279 if(!cameraModels.empty())
4311 UINFO(
"Extract features");
4313 if(decimatedData.
imageRaw().channels() == 3)
4315 cv::cvtColor(decimatedData.
imageRaw(), imageMono, CV_BGR2GRAY);
4319 imageMono = decimatedData.
imageRaw();
4325 if(imageMono.rows % decimatedData.
depthRaw().rows == 0 &&
4326 imageMono.cols % decimatedData.
depthRaw().cols == 0 &&
4327 imageMono.rows/decimatedData.
depthRaw().rows == imageMono.cols/decimatedData.
depthRaw().cols)
4333 UWARN(
"%s=%d is not compatible between RGB and depth images, the depth mask cannot be used! (decimated RGB=%dx%d, depth=%dx%d)",
4334 Parameters::kMemImagePreDecimation().c_str(),
4336 imageMono.cols, imageMono.rows,
4341 bool useProvided3dPoints =
false;
4344 UDEBUG(
"Using provided keypoints (%d)", (
int)data.
keypoints().size());
4350 for(
size_t i=0; i<keypoints.size(); ++i)
4352 keypoints[i].class_id = i;
4354 useProvided3dPoints =
true;
4374 if(tmpMaxFeatureParameter.size())
4376 tmpMaxFeatureParameter.at(Parameters::kKpMaxFeatures()) =
uNumber2Str(oldMaxFeatures);
4380 if(stats) stats->
addStatistic(Statistics::kTimingMemKeypoints_detection(), t*1000.0
f);
4381 UDEBUG(
"time keypoints (%d) = %fs", (
int)keypoints.size(), t);
4386 if(stats) stats->
addStatistic(Statistics::kTimingMemDescriptors_extraction(), t*1000.0
f);
4387 UDEBUG(
"time descriptors (%d) = %fs", descriptors.rows, t);
4390 if(descriptors.rows && descriptors.rows <
_badSignRatio *
float(meanWordsPerLocation))
4392 descriptors = cv::Mat();
4396 if(!imagesRectified && decimatedData.
cameraModels().size())
4398 std::vector<cv::KeyPoint> keypointsValid;
4399 keypointsValid.reserve(keypoints.size());
4400 cv::Mat descriptorsValid;
4401 descriptorsValid.reserve(descriptors.rows);
4406 std::vector<cv::Point2f> pointsIn, pointsOut;
4407 cv::KeyPoint::convert(keypoints,pointsIn);
4410 #if CV_MAJOR_VERSION > 2 or (CV_MAJOR_VERSION == 2 and (CV_MINOR_VERSION >4 or (CV_MINOR_VERSION == 4 and CV_SUBMINOR_VERSION >=10))) 4413 cv::Mat D(1, 4, CV_64FC1);
4414 D.at<
double>(0,0) = decimatedData.
cameraModels()[0].D_raw().at<
double>(0,0);
4415 D.at<
double>(0,1) = decimatedData.
cameraModels()[0].D_raw().at<
double>(0,1);
4416 D.at<
double>(0,2) = decimatedData.
cameraModels()[0].D_raw().at<
double>(0,4);
4417 D.at<
double>(0,3) = decimatedData.
cameraModels()[0].D_raw().at<
double>(0,5);
4418 cv::fisheye::undistortPoints(pointsIn, pointsOut,
4426 UWARN(
"Too old opencv version (%d,%d,%d) to support fisheye model (min 2.4.10 required)!",
4427 CV_MAJOR_VERSION, CV_MINOR_VERSION, CV_SUBMINOR_VERSION);
4432 cv::undistortPoints(pointsIn, pointsOut,
4438 UASSERT(pointsOut.size() == keypoints.size());
4439 for(
unsigned int i=0; i<pointsOut.size(); ++i)
4441 if(pointsOut.at(i).x>=0 && pointsOut.at(i).x<decimatedData.
cameraModels()[0].imageWidth() &&
4442 pointsOut.at(i).y>=0 && pointsOut.at(i).y<decimatedData.
cameraModels()[0].imageHeight())
4444 keypointsValid.push_back(keypoints.at(i));
4445 keypointsValid.back().pt.x = pointsOut.at(i).x;
4446 keypointsValid.back().pt.y = pointsOut.at(i).y;
4447 descriptorsValid.push_back(descriptors.row(i));
4455 for(
unsigned int i=0; i<keypoints.size(); ++i)
4457 int cameraIndex = int(keypoints.at(i).pt.x / subImageWidth);
4459 uFormat(
"cameraIndex=%d, models=%d, kpt.x=%f, subImageWidth=%f (Camera model image width=%d)",
4460 cameraIndex, (
int)decimatedData.
cameraModels().size(), keypoints[i].pt.x, subImageWidth, decimatedData.
cameraModels()[0].imageWidth()).c_str());
4462 std::vector<cv::Point2f> pointsIn, pointsOut;
4463 pointsIn.push_back(cv::Point2f(keypoints.at(i).pt.x-subImageWidth*cameraIndex, keypoints.at(i).pt.y));
4464 if(decimatedData.
cameraModels()[cameraIndex].D_raw().cols == 6)
4466 #if CV_MAJOR_VERSION > 2 or (CV_MAJOR_VERSION == 2 and (CV_MINOR_VERSION >4 or (CV_MINOR_VERSION == 4 and CV_SUBMINOR_VERSION >=10))) 4469 cv::Mat D(1, 4, CV_64FC1);
4470 D.at<
double>(0,0) = decimatedData.
cameraModels()[cameraIndex].D_raw().at<
double>(0,0);
4471 D.at<
double>(0,1) = decimatedData.
cameraModels()[cameraIndex].D_raw().at<
double>(0,1);
4472 D.at<
double>(0,2) = decimatedData.
cameraModels()[cameraIndex].D_raw().at<
double>(0,4);
4473 D.at<
double>(0,3) = decimatedData.
cameraModels()[cameraIndex].D_raw().at<
double>(0,5);
4474 cv::fisheye::undistortPoints(pointsIn, pointsOut,
4482 UWARN(
"Too old opencv version (%d,%d,%d) to support fisheye model (min 2.4.10 required)!",
4483 CV_MAJOR_VERSION, CV_MINOR_VERSION, CV_SUBMINOR_VERSION);
4488 cv::undistortPoints(pointsIn, pointsOut,
4495 if(pointsOut[0].x>=0 && pointsOut[0].x<decimatedData.
cameraModels()[cameraIndex].imageWidth() &&
4496 pointsOut[0].y>=0 && pointsOut[0].y<decimatedData.
cameraModels()[cameraIndex].imageHeight())
4498 keypointsValid.push_back(keypoints.at(i));
4499 keypointsValid.back().pt.x = pointsOut[0].x + subImageWidth*cameraIndex;
4500 keypointsValid.back().pt.y = pointsOut[0].y;
4501 descriptorsValid.push_back(descriptors.row(i));
4506 keypoints = keypointsValid;
4507 descriptors = descriptorsValid;
4510 if(stats) stats->
addStatistic(Statistics::kTimingMemRectification(), t*1000.0f);
4511 UDEBUG(
"time rectification = %fs", t);
4514 if(useProvided3dPoints && keypoints.size() != data.
keypoints3D().size())
4516 UDEBUG(
"Using provided 3d points (%d->%d)", (
int)data.
keypoints3D().size(), (int)keypoints.size());
4517 keypoints3D.resize(keypoints.size());
4518 for(
size_t i=0; i<keypoints.size(); ++i)
4521 keypoints3D[i] = data.
keypoints3D()[keypoints[i].class_id];
4524 else if(useProvided3dPoints && keypoints.size() == data.
keypoints3D().size())
4534 if(stats) stats->
addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0
f);
4535 UDEBUG(
"time keypoints 3D (%d) = %fs", (
int)keypoints3D.size(), t);
4539 else if(data.imageRaw().empty())
4541 UDEBUG(
"Empty image, cannot extract features...");
4549 UDEBUG(
"Intermediate node detected, don't extract features!");
4554 UINFO(
"Use odometry features: kpts=%d 3d=%d desc=%d (dim=%d, type=%d)",
4555 (
int)data.keypoints().size(),
4556 (int)data.keypoints3D().size(),
4557 data.descriptors().rows,
4558 data.descriptors().cols,
4559 data.descriptors().type());
4560 keypoints = data.keypoints();
4561 keypoints3D = data.keypoints3D();
4562 descriptors = data.descriptors().clone();
4564 UASSERT(descriptors.empty() || descriptors.rows == (int)keypoints.size());
4565 UASSERT(keypoints3D.empty() || keypoints3D.size() == keypoints.size());
4568 if((
int)keypoints.size() > maxFeatures)
4573 if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_detection(), t*1000.0
f);
4574 UDEBUG(
"time keypoints (%d) = %fs", (
int)keypoints.size(), t);
4576 if(descriptors.empty())
4579 if(data.imageRaw().channels() == 3)
4581 cv::cvtColor(data.imageRaw(), imageMono, CV_BGR2GRAY);
4585 imageMono = data.imageRaw();
4588 UASSERT_MSG(imagesRectified,
"Cannot extract descriptors on not rectified image from keypoints which assumed to be undistorted");
4592 if(stats) stats->addStatistic(Statistics::kTimingMemDescriptors_extraction(), t*1000.0f);
4593 UDEBUG(
"time descriptors (%d) = %fs", descriptors.rows, t);
4595 if(keypoints3D.empty() &&
4596 ((!data.depthRaw().empty() && data.cameraModels().size() && data.cameraModels()[0].isValidForProjection()) ||
4597 (!data.rightRaw().empty() && data.stereoCameraModel().isValidForProjection())))
4606 if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f);
4607 UDEBUG(
"time keypoints 3D (%d) = %fs", (
int)keypoints3D.size(), t);
4610 if(descriptors.rows && descriptors.rows <
_badSignRatio *
float(meanWordsPerLocation))
4612 descriptors = cv::Mat();
4618 UDEBUG(
"Joining dictionary update thread...");
4619 preUpdateThread.join();
4620 UDEBUG(
"Joining dictionary update thread... thread finished!");
4624 if(stats) stats->addStatistic(Statistics::kTimingMemJoining_dictionary_update(), t*1000.0
f);
4627 UDEBUG(
"time descriptor and memory update (%d of size=%d) = %fs", descriptors.rows, descriptors.cols, t);
4631 UDEBUG(
"time descriptor (%d of size=%d) = %fs", descriptors.rows, descriptors.cols, t);
4634 std::list<int> wordIds;
4635 if(descriptors.rows)
4639 std::vector<bool> inliers;
4640 cv::Mat descriptorsForQuantization = descriptors;
4641 std::vector<int> quantizedToRawIndices;
4644 UASSERT((
int)keypoints.size() == descriptors.rows);
4645 int inliersCount = 0;
4649 for(
size_t i=0; i<inliers.size(); ++i)
4663 descriptorsForQuantization = cv::Mat(inliersCount, descriptors.cols, descriptors.type());
4664 quantizedToRawIndices.resize(inliersCount);
4666 UASSERT((
int)inliers.size() == descriptors.rows);
4667 for(
int k=0; k < descriptors.rows; ++k)
4671 UASSERT(oi < quantizedToRawIndices.size());
4672 if(descriptors.type() == CV_32FC1)
4674 memcpy(descriptorsForQuantization.ptr<
float>(oi), descriptors.ptr<
float>(k), descriptors.cols*
sizeof(
float));
4678 memcpy(descriptorsForQuantization.ptr<
char>(oi), descriptors.ptr<
char>(k), descriptors.cols*
sizeof(
char));
4680 quantizedToRawIndices[oi] = k;
4685 uFormat(
"oi=%d inliersCount=%d (maxFeatures=%d, grid=%dx%d)",
4693 if(wordIds.size() < keypoints.size())
4695 std::vector<int> allWordIds;
4696 allWordIds.resize(keypoints.size(),-1);
4698 for(std::list<int>::iterator iter=wordIds.begin(); iter!=wordIds.end(); ++iter)
4700 allWordIds[quantizedToRawIndices[i]] = *iter;
4704 for(i=0; i<(int)allWordIds.size(); ++i)
4706 if(allWordIds[i] < 0)
4708 allWordIds[i] = negIndex--;
4715 if(stats) stats->addStatistic(Statistics::kTimingMemAdd_new_words(), t*1000.0f);
4720 UDEBUG(
"id %d is a bad signature",
id);
4723 std::multimap<int, int> words;
4724 std::vector<cv::KeyPoint> wordsKpts;
4725 std::vector<cv::Point3f> words3D;
4726 cv::Mat wordsDescriptors;
4727 int words3DValid = 0;
4728 if(wordIds.size() > 0)
4730 UASSERT(wordIds.size() == keypoints.size());
4731 UASSERT(keypoints3D.size() == 0 || keypoints3D.size() == wordIds.size());
4734 double log2value =
log(
double(preDecimation))/
log(2.0);
4735 for(std::list<int>::iterator iter=wordIds.begin(); iter!=wordIds.end() && i < keypoints.size(); ++iter, ++i)
4737 cv::KeyPoint kpt = keypoints[i];
4741 kpt.pt.x *= decimationRatio;
4742 kpt.pt.y *= decimationRatio;
4743 kpt.size *= decimationRatio;
4744 kpt.octave += log2value;
4746 words.insert(std::make_pair(*iter, words.size()));
4747 wordsKpts.push_back(kpt);
4749 if(keypoints3D.size())
4751 words3D.push_back(keypoints3D.at(i));
4759 wordsDescriptors.push_back(descriptors.row(i));
4765 if(
_detectMarkers && !isIntermediateNode && !data.imageRaw().empty())
4767 UDEBUG(
"Detecting markers...");
4768 if(landmarks.empty())
4770 std::map<int, Transform> markers;
4771 if(!data.cameraModels().empty() && data.cameraModels()[0].isValidForProjection())
4773 if(data.cameraModels().size() > 1)
4775 static bool warned =
false;
4778 UWARN(
"Detecting markers in multi-camera setup is not yet implemented, aborting marker detection. This message is only printed once.");
4787 else if(data.stereoCameraModel().isValidForProjection())
4791 for(std::map<int, Transform>::iterator iter=markers.begin(); iter!=markers.end(); ++iter)
4793 if(iter->first <= 0)
4795 UERROR(
"Invalid marker received! IDs should be > 0 (it is %d). Ignoring this marker.", iter->first);
4798 cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1);
4801 landmarks.insert(std::make_pair(iter->first,
Landmark(iter->first, iter->second, covariance)));
4803 UDEBUG(
"Markers detected = %d", (
int)markers.size());
4807 UWARN(
"Input data has already landmarks, cannot do marker detection.");
4810 if(stats) stats->addStatistic(Statistics::kTimingMemMarkers_detection(), t*1000.0f);
4811 UDEBUG(
"time markers detection = %fs", t);
4814 cv::Mat image = data.imageRaw();
4815 cv::Mat depthOrRightImage = data.depthOrRightRaw();
4816 std::vector<CameraModel> cameraModels = data.cameraModels();
4824 image = decimatedData.imageRaw();
4825 depthOrRightImage = decimatedData.depthOrRightRaw();
4826 cameraModels = decimatedData.cameraModels();
4827 stereoCameraModel = decimatedData.stereoCameraModel();
4831 if(!data.rightRaw().empty() ||
4832 (data.depthRaw().rows == image.rows && data.depthRaw().cols == image.cols))
4837 for(
unsigned int i=0; i<cameraModels.size(); ++i)
4847 if(!image.empty() && (depthOrRightImage.cols > image.cols || depthOrRightImage.rows > image.rows))
4849 UWARN(
"Depth image is bigger than RGB image after post decimation, %s=%d is too high! RGB=%dx%d, depth=%dx%d",
4851 image.cols, image.rows, depthOrRightImage.cols, depthOrRightImage.rows);
4855 if(stats) stats->addStatistic(Statistics::kTimingMemPost_decimation(), t*1000.0f);
4856 UDEBUG(
"time post-decimation = %fs", t);
4861 cameraModels.size() == 1 &&
4863 (words3D.size() == 0 || (words.size() == words3D.size() && words3DValid!=(int)words3D.size())) &&
4868 UDEBUG(
"Generate 3D words using odometry (%s=true and words3DValid=%d/%d)",
4869 Parameters::kMemStereoFromMotion().c_str(), words3DValid, (
int)words3D.size());
4871 if(previousS->getWords().size() > 8 && words.size() > 8 && !previousS->getPose().isNull())
4873 UDEBUG(
"Previous pose(%d) = %s", previousS->id(), previousS->getPose().prettyPrint().c_str());
4874 UDEBUG(
"Current pose(%d) = %s",
id, pose.prettyPrint().c_str());
4880 std::vector<cv::KeyPoint> uniqueWordsKpts;
4881 cv::Mat uniqueWordsDescriptors;
4882 std::multimap<int, int> uniqueWords;
4883 for(std::map<int, int>::iterator iter=uniqueWordsOld.begin(); iter!=uniqueWordsOld.end(); ++iter)
4885 uniqueWords.insert(std::make_pair(iter->first, uniqueWords.size()));
4886 uniqueWordsKpts.push_back(previousS->getWordsKpts()[iter->second]);
4887 uniqueWordsDescriptors.push_back(previousS->getWordsDescriptors().row(iter->second));
4890 cpPrevious.
setWords(uniqueWords, uniqueWordsKpts, std::vector<cv::Point3f>(), uniqueWordsDescriptors);
4893 uniqueWordsKpts.clear();
4894 uniqueWordsDescriptors = cv::Mat();
4895 uniqueWords.clear();
4896 for(std::map<int, int>::iterator iter=uniqueWordsOld.begin(); iter!=uniqueWordsOld.end(); ++iter)
4898 uniqueWords.insert(std::make_pair(iter->first, uniqueWords.size()));
4899 uniqueWordsKpts.push_back(wordsKpts[iter->second]);
4900 uniqueWordsDescriptors.push_back(wordsDescriptors.row(iter->second));
4904 cpCurrent.
setWords(uniqueWords, uniqueWordsKpts, std::vector<cv::Point3f>(), uniqueWordsDescriptors);
4924 std::map<int, cv::KeyPoint> currentWords;
4925 std::map<int, cv::KeyPoint> previousWords;
4926 for(std::map<int, int>::iterator iter=currentUniqueWords.begin(); iter!=currentUniqueWords.end(); ++iter)
4928 currentWords.insert(std::make_pair(iter->first, cpCurrent.
getWordsKpts()[iter->second]));
4930 for(std::map<int, int>::iterator iter=previousUniqueWords.begin(); iter!=previousUniqueWords.end(); ++iter)
4932 previousWords.insert(std::make_pair(iter->first, cpPrevious.
getWordsKpts()[iter->second]));
4940 UDEBUG(
"inliers=%d", (
int)inliers.size());
4943 float bad_point = std::numeric_limits<float>::quiet_NaN ();
4944 UASSERT(words3D.size() == 0 || words.size() == words3D.size());
4945 bool words3DWasEmpty = words3D.empty();
4946 int added3DPointsWithoutDepth = 0;
4947 for(std::multimap<int, int>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
4949 std::map<int, cv::Point3f>::iterator jter=inliers.find(iter->first);
4952 if(jter != inliers.end())
4954 words3D.push_back(jter->second);
4955 ++added3DPointsWithoutDepth;
4959 words3D.push_back(cv::Point3f(bad_point,bad_point,bad_point));
4964 words3D[iter->second] = jter->second;
4965 ++added3DPointsWithoutDepth;
4968 UDEBUG(
"added3DPointsWithoutDepth=%d", added3DPointsWithoutDepth);
4969 if(stats) stats->addStatistic(Statistics::kMemoryTriangulated_points(), (
float)added3DPointsWithoutDepth);
4972 UASSERT(words3D.size() == words.size());
4973 if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D_motion(), t*1000.0f);
4974 UDEBUG(
"time keypoints 3D by motion (%d) = %fs", (
int)words3D.size(), t);
4979 LaserScan laserScan = data.laserScanRaw();
4980 if(!isIntermediateNode && laserScan.size())
4982 if(laserScan.rangeMax() == 0.0f)
4984 bool id2d = laserScan.
is2d();
4985 float maxRange = 0.0f;
4986 for(
int i=0; i<laserScan.size(); ++i)
4988 const float * ptr = laserScan.data().ptr<
float>(0, i);
4992 r = ptr[0]*ptr[0] + ptr[1]*ptr[1];
4996 r = ptr[0]*ptr[0] + ptr[1]*ptr[1] + ptr[2]*ptr[2];
5005 laserScan=
LaserScan(laserScan.data(), laserScan.maxPoints(),
sqrt(maxRange), laserScan.format(), laserScan.localTransform());
5017 if(stats) stats->addStatistic(Statistics::kTimingMemScan_filtering(), t*1000.0f);
5018 UDEBUG(
"time normals scan = %fs", t);
5024 UDEBUG(
"Bin data kept: rgb=%d, depth=%d, scan=%d, userData=%d",
5026 depthOrRightImage.empty()?0:1,
5027 laserScan.isEmpty()?0:1,
5028 data.userDataRaw().empty()?0:1);
5030 std::vector<unsigned char> imageBytes;
5031 std::vector<unsigned char> depthBytes;
5033 if(
_saveDepth16Format && !depthOrRightImage.empty() && depthOrRightImage.type() == CV_32FC1)
5035 UWARN(
"Save depth data to 16 bits format: depth type detected is 32FC1, use 16UC1 depth format to avoid this conversion (or set parameter \"Mem/SaveDepth16Format\"=false to use 32bits format).");
5039 cv::Mat compressedImage;
5040 cv::Mat compressedDepth;
5041 cv::Mat compressedScan;
5042 cv::Mat compressedUserData;
5053 if(!depthOrRightImage.empty())
5057 if(!laserScan.isEmpty())
5059 ctLaserScan.start();
5061 if(!data.userDataRaw().empty())
5071 compressedDepth = ctDepth.getCompressedData();
5072 compressedScan = ctLaserScan.getCompressedData();
5073 compressedUserData = ctUserData.getCompressedData();
5078 compressedDepth =
compressImage2(depthOrRightImage, depthOrRightImage.type() == CV_32FC1 || depthOrRightImage.type() == CV_16UC1?std::string(
".png"):
_rgbCompressionFormat);
5085 isIntermediateNode?-1:0,
5092 laserScan.angleIncrement() == 0.0f?
5094 laserScan.maxPoints(),
5095 laserScan.rangeMax(),
5097 laserScan.localTransform()):
5100 laserScan.rangeMin(),
5101 laserScan.rangeMax(),
5102 laserScan.angleMin(),
5103 laserScan.angleMax(),
5104 laserScan.angleIncrement(),
5105 laserScan.localTransform()),
5111 compressedUserData):
5113 laserScan.angleIncrement() == 0.0f?
5115 laserScan.maxPoints(),
5116 laserScan.rangeMax(),
5118 laserScan.localTransform()):
5121 laserScan.rangeMin(),
5122 laserScan.rangeMax(),
5123 laserScan.angleMin(),
5124 laserScan.angleMax(),
5125 laserScan.angleIncrement(),
5126 laserScan.localTransform()),
5132 compressedUserData));
5136 UDEBUG(
"Bin data kept: scan=%d, userData=%d",
5137 laserScan.isEmpty()?0:1,
5138 data.userDataRaw().empty()?0:1);
5141 cv::Mat compressedScan;
5142 cv::Mat compressedUserData;