i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-11 22:10:21 +08:00
Code Issues Packages Projects Releases Wiki Activity

Increased version to 0.20.5

Browse Source 
Refactored how features are stored in Signature (significative memory optimization, causing major refactoring in Memory, RegistrationVis, OdometryF2M)
FLANN: optimized memory usage when Kp/IncrementalFlann is false
Added memory usage functions
Added statistics Loop/Visual_inliers_ratio/ and Memory/RAM_estimated/MB
EpipolarGeometry: templated findPairs functions
graph::filterLinks: added inverted option
LocalBundleOnLoopClosure: Force to use only neighbor links
MainWindow: fixed max depth filtering for map's features
Rtabmap::getSignatureCopy() fixed links not returned
Added UPlot::getAllCurveDataAsText() function.
DbViewer: fixed features not rendered in right view when failing ro refine a constraint
report: added --export and --export_prefix options (to export figures data)
This commit is contained in:
matlabbe
2020-10-05 17:34:32 -04:00
parent bedc771fa4
commit bbccbd63e4
41 changed files with 1456 additions and 898 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/include/rtabmap/core/BayesFilter.h
+2
View File
@@ -61,6 +61,8 @@ public:
cv::Mat generatePrediction(const Memory * memory, const std::vector<int> & ids);
unsigned long getMemoryUsed() const;
private:
cv::Mat updatePrediction(const cv::Mat & oldPrediction,
const Memory * memory,
corelib/include/rtabmap/core/DBDriver.h
+2 -2
View File
@@ -131,7 +131,7 @@ public:
bool openConnection(const std::string & url, bool overwritten = false);
void closeConnection(bool save = true, const std::string & outputUrl = "");
bool isConnected() const;
long getMemoryUsed() const; // In bytes
unsigned long getMemoryUsed() const; // In bytes
std::string getDatabaseVersion() const;
long getNodesMemoryUsed() const;
long getLinksMemoryUsed() const;
@@ -188,7 +188,7 @@ protected:
virtual bool connectDatabaseQuery(const std::string & url, bool overwritten = false) = 0;
virtual void disconnectDatabaseQuery(bool save = true, const std::string & outputUrl = "") = 0;
virtual bool isConnectedQuery() const = 0;
virtual long getMemoryUsedQuery() const = 0; // In bytes
virtual unsigned long getMemoryUsedQuery() const = 0; // In bytes
virtual bool getDatabaseVersionQuery(std::string & version) const = 0;
virtual long getNodesMemoryUsedQuery() const = 0;
virtual long getLinksMemoryUsedQuery() const = 0;
corelib/include/rtabmap/core/DBDriverSqlite3.h
+2 -2
View File
@@ -54,7 +54,7 @@ protected:
virtual bool connectDatabaseQuery(const std::string & url, bool overwritten = false);
virtual void disconnectDatabaseQuery(bool save = true, const std::string & outputUrl = "");
virtual bool isConnectedQuery() const;
virtual long getMemoryUsedQuery() const; // In bytes
virtual unsigned long getMemoryUsedQuery() const; // In bytes
virtual bool getDatabaseVersionQuery(std::string & version) const;
virtual long getNodesMemoryUsedQuery() const;
virtual long getLinksMemoryUsedQuery() const;
@@ -189,7 +189,7 @@ protected:
std::string _version;
private:
long _memoryUsedEstimate;
unsigned long _memoryUsedEstimate;
bool _dbInMemory;
unsigned int _cacheSize;
int _journalMode;
corelib/include/rtabmap/core/EpipolarGeometry.h
+109 -16
View File
@@ -29,6 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
#include "rtabmap/core/Parameters.h"
#include "rtabmap/utilite/UStl.h"
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <pcl/point_cloud.h>
@@ -91,41 +92,133 @@ public:
* if a=[1 2 3 4 6], b=[1 2 4 5 6], results= [(1,1) (2,2) (4,4) (6,6)]
* realPairsCount = 4
*/
template<typename T>
static int findPairs(
const std::map<int, cv::KeyPoint> & wordsA,
const std::map<int, cv::KeyPoint> & wordsB,
std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs,
bool ignoreNegativeIds = true);
const std::map<int, T> & wordsA,
const std::map<int, T> & wordsB,
std::list<std::pair<int, std::pair<T, T> > > & pairs,
bool ignoreNegativeIds = true)
{
int realPairsCount = 0;
pairs.clear();
for(typename std::map<int, T>::const_iterator i=wordsA.begin(); i!=wordsA.end(); ++i)
{
if(!ignoreNegativeIds || (ignoreNegativeIds && i->first>=0))
{
std::map<int, cv::KeyPoint>::const_iterator ptB = wordsB.find(i->first);
if(ptB != wordsB.end())
{
pairs.push_back(std::pair<int, std::pair<T, T> >(i->first, std::make_pair(i->second, ptB->second)));
++realPairsCount;
}
}
}
return realPairsCount;
}
/**
* if a=[1 2 3 4 6 6], b=[1 1 2 4 5 6 6], results= [(1,1a) (2,2) (4,4) (6a,6a) (6b,6b)]
* realPairsCount = 5
*/
template<typename T>
static int findPairs(
const std::multimap<int, cv::KeyPoint> & wordsA,
const std::multimap<int, cv::KeyPoint> & wordsB,
std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs,
bool ignoreNegativeIds = true);
const std::multimap<int, T> & wordsA,
const std::multimap<int, T> & wordsB,
std::list<std::pair<int, std::pair<T, T> > > & pairs,
bool ignoreNegativeIds = true)
{
const std::list<int> & ids = uUniqueKeys(wordsA);
typename std::multimap<int, T>::const_iterator iterA;
typename std::multimap<int, T>::const_iterator iterB;
pairs.clear();
int realPairsCount = 0;
for(std::list<int>::const_iterator i=ids.begin(); i!=ids.end(); ++i)
{
if(!ignoreNegativeIds || (ignoreNegativeIds && *i >= 0))
{
iterA = wordsA.find(*i);
iterB = wordsB.find(*i);
while(iterA != wordsA.end() && iterB != wordsB.end() && (*iterA).first == (*iterB).first && (*iterA).first == *i)
{
pairs.push_back(std::pair<int, std::pair<T, T> >(*i, std::make_pair((*iterA).second, (*iterB).second)));
++iterA;
++iterB;
++realPairsCount;
}
}
}
return realPairsCount;
}
/**
* if a=[1 2 3 4 6 6], b=[1 1 2 4 5 6 6], results= [(2,2) (4,4)]
* realPairsCount = 5
*/
template<typename T>
static int findPairsUnique(
const std::multimap<int, cv::KeyPoint> & wordsA,
const std::multimap<int, cv::KeyPoint> & wordsB,
std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs,
bool ignoreNegativeIds = true);
const std::multimap<int, T> & wordsA,
const std::multimap<int, T> & wordsB,
std::list<std::pair<int, std::pair<T, T> > > & pairs,
bool ignoreNegativeIds = true)
{
const std::list<int> & ids = uUniqueKeys(wordsA);
int realPairsCount = 0;
pairs.clear();
for(std::list<int>::const_iterator i=ids.begin(); i!=ids.end(); ++i)
{
if(!ignoreNegativeIds || (ignoreNegativeIds && *i>=0))
{
std::list<T> ptsA = uValues(wordsA, *i);
std::list<T> ptsB = uValues(wordsB, *i);
if(ptsA.size() == 1 && ptsB.size() == 1)
{
pairs.push_back(std::pair<int, std::pair<T, T> >(*i, std::pair<T, T>(ptsA.front(), ptsB.front())));
++realPairsCount;
}
else if(ptsA.size()>1 && ptsB.size()>1)
{
// just update the count
realPairsCount += ptsA.size() > ptsB.size() ? ptsB.size() : ptsA.size();
}
}
}
return realPairsCount;
}
/**
* if a=[1 2 3 4 6 6], b=[1 1 2 4 5 6 6], results= [(1,1a) (1,1b) (2,2) (4,4) (6a,6a) (6a,6b) (6b,6a) (6b,6b)]
* realPairsCount = 5
*/
template<typename T>
static int findPairsAll(
const std::multimap<int, cv::KeyPoint> & wordsA,
const std::multimap<int, cv::KeyPoint> & wordsB,
std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs,
bool ignoreNegativeIds = true);
const std::multimap<int, T> & wordsA,
const std::multimap<int, T> & wordsB,
std::list<std::pair<int, std::pair<T, T> > > & pairs,
bool ignoreNegativeIds = true)
{
const std::list<int> & ids = uUniqueKeys(wordsA);
pairs.clear();
int realPairsCount = 0;;
for(std::list<int>::const_iterator iter=ids.begin(); iter!=ids.end(); ++iter)
{
if(!ignoreNegativeIds || (ignoreNegativeIds && *iter>=0))
{
std::list<T> ptsA = uValues(wordsA, *iter);
std::list<T> ptsB = uValues(wordsB, *iter);
realPairsCount += ptsA.size() > ptsB.size() ? ptsB.size() : ptsA.size();
for(typename std::list<T>::iterator jter=ptsA.begin(); jter!=ptsA.end(); ++jter)
{
for(typename std::list<T>::iterator kter=ptsB.begin(); kter!=ptsB.end(); ++kter)
{
pairs.push_back(std::pair<int, std::pair<T, T> >(*iter, std::pair<T, T>(*jter, *kter)));
}
}
}
}
return realPairsCount;
}
static cv::Mat linearLSTriangulation(
cv::Point3d u, //homogenous image point (u,v,1)
corelib/include/rtabmap/core/FlannIndex.h
+3 -3
View File
@@ -43,8 +43,8 @@ public:
void release();
unsigned int indexedFeatures() const;
// return KB
unsigned int memoryUsed() const;
// return Bytes
unsigned long memoryUsed() const;
// Note that useDistanceL1 doesn't have any effect if LSH is used
void buildLinearIndex(
@@ -74,7 +74,7 @@ public:
int featuresType() const {return featuresType_;}
int featuresDim() const {return featuresDim_;}
unsigned int addPoints(const cv::Mat & features);
std::vector<unsigned int> addPoints(const cv::Mat & features);
void removePoint(unsigned int index);
corelib/include/rtabmap/core/Graph.h
+10 -2
View File
@@ -155,12 +155,20 @@ std::list<Link> RTABMAP_EXP findLinks(
std::multimap<int, Link> RTABMAP_EXP filterDuplicateLinks(
const std::multimap<int, Link> & links);
/**
* Return links not of type "filteredType". If inverted=true, return links of of type "filteredType".
*/
std::multimap<int, Link> RTABMAP_EXP filterLinks(
const std::multimap<int, Link> & links,
Link::Type filteredType);
Link::Type filteredType,
bool inverted = false);
/**
* Return links not of type "filteredType". If inverted=true, return links of of type "filteredType".
*/
std::map<int, Link> RTABMAP_EXP filterLinks(
const std::map<int, Link> & links,
Link::Type filteredType);
Link::Type filteredType,
bool inverted = false);
//Note: This assumes a coordinate system where X is forward, * Y is up, and Z is right.
std::map<int, Transform> RTABMAP_EXP frustumPosesFiltering(
corelib/include/rtabmap/core/Memory.h
+5 -3
View File
@@ -199,9 +199,10 @@ public:
cv::Mat getImageCompressed(int signatureId) const;
SensorData getNodeData(int locationId, bool images, bool scan, bool userData, bool occupancyGrid) const;
void getNodeWordsAndGlobalDescriptors(int nodeId,
std::multimap<int, cv::KeyPoint> & words,
std::multimap<int, cv::Point3f> & words3,
std::multimap<int, cv::Mat> & wordsDescriptors,
std::multimap<int, int> & words,
std::vector<cv::KeyPoint> & wordsKpts,
std::vector<cv::Point3f> & words3,
cv::Mat & wordsDescriptors,
std::vector<GlobalDescriptor> & globalDescriptors) const;
void getNodeCalibration(int nodeId,
std::vector<CameraModel> & models,
@@ -225,6 +226,7 @@ public:
virtual void dumpMemory(std::string directory) const;
virtual void dumpSignatures(const char * fileNameSign, bool words3D) const;
void dumpDictionary(const char * fileNameRef, const char * fileNameDesc) const;
unsigned long getMemoryUsed() const; //Bytes
void generateGraph(const std::string & fileName, const std::set<int> & ids = std::set<int>());
corelib/include/rtabmap/core/OccupancyGrid.h
+2
View File
@@ -104,6 +104,8 @@ public:
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & getMapObstacles() const {return assembledObstacles_;}
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & getMapEmptyCells() const {return assembledEmptyCells_;}
unsigned long getMemoryUsed() const;
private:
ParametersMap parameters_;
int cloudDecimation_;
corelib/include/rtabmap/core/RegistrationInfo.h
+1
View File
@@ -75,6 +75,7 @@ public:
// RegistrationVis
int inliers;
float inliersRatio;
float inliersMeanDistance;
float inliersDistribution;
std::vector<int> inliersIDs;
corelib/include/rtabmap/core/Rtabmap.h
-2
View File
@@ -136,11 +136,9 @@ public:
std::map<int, int> getWeights() const;
int getTotalMemSize() const;
double getLastProcessTime() const {return _lastProcessTime;};
std::multimap<int, cv::KeyPoint> getWords(int locationId) const;
bool isInSTM(int locationId) const;
bool isIDsGenerated() const;
const Statistics & getStatistics() const;
//bool getMetricData(int locationId, cv::Mat & rgb, cv::Mat & depth, float & depthConstant, Transform & pose, Transform & localTransform) const;
const std::map<int, Transform> & getLocalOptimizedPoses() const {return _optimizedPoses;}
const std::multimap<int, Link> & getLocalConstraints() const {return _constraints;}
Transform getPose(int locationId) const;
corelib/include/rtabmap/core/SensorData.h
+1 -1
View File
@@ -275,7 +275,7 @@ public:
void setLandmarks(const Landmarks & landmarks) {_landmarks = landmarks;}
const Landmarks & landmarks() const {return _landmarks;}
long getMemoryUsed() const; // Return memory usage in Bytes
unsigned long getMemoryUsed() const; // Return memory usage in Bytes
/**
* Clear compressed rgb/depth (left/right) images, compressed laser scan and compressed user data.
* Raw data are kept is set.
corelib/include/rtabmap/core/Signature.h
+11 -11
View File
@@ -104,19 +104,18 @@ public:
//visual words stuff
void removeAllWords();
void removeWord(int wordId);
void changeWordsRef(int oldWordId, int activeWordId);
void setWords(const std::multimap<int, cv::KeyPoint> & words);
void setWords(const std::multimap<int, int> & words, const std::vector<cv::KeyPoint> & keypoints, const std::vector<cv::Point3f> & words3, const cv::Mat & descriptors);
bool isEnabled() const {return _enabled;}
void setEnabled(bool enabled) {_enabled = enabled;}
const std::multimap<int, cv::KeyPoint> & getWords() const {return _words;}
const std::multimap<int, int> & getWords() const {return _words;}
const std::vector<cv::KeyPoint> & getWordsKpts() const {return _wordsKpts;}
int getInvalidWordsCount() const {return _invalidWordsCount;}
const std::map<int, int> & getWordsChanged() const {return _wordsChanged;}
const std::multimap<int, cv::Mat> & getWordsDescriptors() const {return _wordsDescriptors;}
void setWordsDescriptors(const std::multimap<int, cv::Mat> & descriptors) {_wordsDescriptors = descriptors;}
const cv::Mat & getWordsDescriptors() const {return _wordsDescriptors;}
void setWordsDescriptors(const cv::Mat & descriptors);
//metric stuff
void setWords3(const std::multimap<int, cv::Point3f> & words3) {_words3 = words3;}
void setPose(const Transform & pose) {_pose = pose;}
void setGroundTruthPose(const Transform & pose) {_groundTruthPose = pose;}
void setVelocity(float vx, float vy, float vz, float vroll, float vpitch, float vyaw) {
@@ -129,7 +128,7 @@ public:
_velocity[5]=vyaw;
}
const std::multimap<int, cv::Point3f> & getWords3() const {return _words3;}
const std::vector<cv::Point3f> & getWords3() const {return _words3;}
const Transform & getPose() const {return _pose;}
cv::Mat getPoseCovariance() const;
const Transform & getGroundTruthPose() const {return _groundTruthPose;}
@@ -138,7 +137,7 @@ public:
SensorData & sensorData() {return _sensorData;}
const SensorData & sensorData() const {return _sensorData;}
long getMemoryUsed(bool withSensorData=true) const; // Return memory usage in Bytes
unsigned long getMemoryUsed(bool withSensorData=true) const; // Return memory usage in Bytes
private:
int _id;
@@ -155,9 +154,10 @@ private:
// Contains all words (Some can be duplicates -> if a word appears 2
// times in the signature, it will be 2 times in this list)
// Words match with the CvSeq keypoints and descriptors
std::multimap<int, cv::KeyPoint> _words; // word <id, keypoint>
std::multimap<int, cv::Point3f> _words3; // word <id, point> // in base_link frame (localTransform applied))
std::multimap<int, cv::Mat> _wordsDescriptors;
std::multimap<int, int> _words; // word <id, keypoint index>
std::vector<cv::KeyPoint> _wordsKpts;
std::vector<cv::Point3f> _words3; // in base_link frame (localTransform applied))
cv::Mat _wordsDescriptors;
std::map<int, int> _wordsChanged; // <oldId, newId>
bool _enabled;
int _invalidWordsCount;
corelib/include/rtabmap/core/Statistics.h
+2
View File
@@ -65,6 +65,7 @@ class RTABMAP_EXP Statistics
RTABMAP_STATS(Loop, Map_id,);
RTABMAP_STATS(Loop, Visual_words,);
RTABMAP_STATS(Loop, Visual_inliers,);
RTABMAP_STATS(Loop, Visual_inliers_ratio,);
RTABMAP_STATS(Loop, Visual_matches,);
RTABMAP_STATS(Loop, Distance_since_last_loc,);
RTABMAP_STATS(Loop, Last_id,);
@@ -149,6 +150,7 @@ class RTABMAP_EXP Statistics
RTABMAP_STATS(Memory, Odometry_variance_lin,);
RTABMAP_STATS(Memory, Distance_travelled, m);
RTABMAP_STATS(Memory, RAM_usage, MB);
RTABMAP_STATS(Memory, RAM_estimated, MB);
RTABMAP_STATS(Memory, Triangulated_points, );
RTABMAP_STATS(Timing, Memory_update, ms);
corelib/include/rtabmap/core/VWDictionary.h
+2 -1
View File
@@ -100,7 +100,8 @@ public:
int getLastIndexedWordId() const;
int getTotalActiveReferences() const {return _totalActiveReferences;}
unsigned int getIndexedWordsCount() const;
unsigned int getIndexMemoryUsed() const;
unsigned int getIndexMemoryUsed() const; // KB
unsigned long getMemoryUsed(bool estimate = true) const; //Bytes
bool setNNStrategy(NNStrategy strategy); // Return true if the search tree has been re-initialized
bool isIncremental() const {return _incrementalDictionary;}
bool isIncrementalFlann() const {return _incrementalFlann;}
corelib/include/rtabmap/core/VisualWord.h
+1
View File
@@ -43,6 +43,7 @@ public:
void addRef(int signatureId);
int removeAllRef(int signatureId);
unsigned long getMemoryUsed() const;
int getTotalReferences() const {return _totalReferences;}
int id() const {return _id;}
corelib/src/BayesFilter.cpp
+14
View File
@@ -417,6 +417,20 @@ cv::Mat BayesFilter::generatePrediction(const Memory * memory, const std::vector
return prediction;
}
unsigned long BayesFilter::getMemoryUsed() const
{
long memoryUsage = sizeof(BayesFilter);
memoryUsage += _posterior.size() * (sizeof(float)+sizeof(int)+sizeof(std::_Rb_tree_node_base)) + sizeof(std::map<int, float>);
memoryUsage += _prediction.total() * _prediction.elemSize();
memoryUsage += _predictionLC.size() * sizeof(double);
memoryUsage += _neighborsIndex.size() * (sizeof(int)+sizeof(std::map<int, int>)+sizeof(std::_Rb_tree_node_base)) + sizeof(std::map<int, std::map<int, int> >);
for(std::map<int, std::map<int, int> >::const_iterator iter=_neighborsIndex.begin(); iter!=_neighborsIndex.end(); ++iter)
{
memoryUsage += iter->second.size() * (sizeof(int)*2+sizeof(std::_Rb_tree_node_base)) + sizeof(std::map<int, int>);
}
return memoryUsage;
}
void BayesFilter::normalize(cv::Mat & prediction, unsigned int index, float addedProbabilitiesSum, bool virtualPlaceUsed) const
{
UASSERT(index < (unsigned int)prediction.rows && index < (unsigned int)prediction.cols);
corelib/src/DBDriver.cpp
+2 -2
View File
@@ -107,9 +107,9 @@ bool DBDriver::isConnected() const
}
// In bytes
long DBDriver::getMemoryUsed() const
unsigned long DBDriver::getMemoryUsed() const
{
long bytes;
unsigned long bytes;
_dbSafeAccessMutex.lock();
bytes = getMemoryUsedQuery();
_dbSafeAccessMutex.unlock();
corelib/src/DBDriverSqlite3.cpp
+21 -25
View File
@@ -486,7 +486,7 @@ void DBDriverSqlite3::executeNoResultQuery(const std::string & sql) const
}
}
long DBDriverSqlite3::getMemoryUsedQuery() const
unsigned long DBDriverSqlite3::getMemoryUsedQuery() const
{
if(_dbInMemory)
{
@@ -3079,9 +3079,10 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
const void * descriptor = 0;
int dRealSize = 0;
cv::KeyPoint kpt;
std::multimap<int, cv::KeyPoint> visualWords;
std::multimap<int, cv::Point3f> visualWords3;
std::multimap<int, cv::Mat> descriptors;
std::multimap<int, int> visualWords;
std::vector<cv::KeyPoint> visualWordsKpts;
std::vector<cv::Point3f> visualWords3;
cv::Mat descriptors;
bool allWords3NaN = true;
cv::Point3f depth(0,0,0);
@@ -3131,8 +3132,9 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
depth.z = sqlite3_column_double(ppStmt, index++);
}
visualWords.insert(visualWords.end(), std::make_pair(visualWordId, kpt));
visualWords3.insert(visualWords3.end(), std::make_pair(visualWordId, depth));
visualWordsKpts.push_back(kpt);
visualWords.insert(visualWords.end(), std::make_pair(visualWordId, visualWordsKpts.size()-1));
visualWords3.push_back(depth);
if(allWords3NaN && util3d::isFinite(depth))
{
@@ -3165,7 +3167,7 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
memcpy(d.data, descriptor, dRealSize);
descriptors.insert(descriptors.end(), std::make_pair(visualWordId, d));
descriptors.push_back(d);
}
}
@@ -3179,13 +3181,12 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
}
else
{
(*iter)->setWords(visualWords);
if(!allWords3NaN)
if(allWords3NaN)
{
(*iter)->setWords3(visualWords3);
visualWords3.clear();
}
(*iter)->setWordsDescriptors(descriptors);
ULOGGER_DEBUG("Add %d keypoints, %d 3d points and %d descriptors to node %d", (int)visualWords.size(), allWords3NaN?0:(int)visualWords3.size(), (int)descriptors.size(), (*iter)->id());
(*iter)->setWords(visualWords, visualWordsKpts, visualWords3, descriptors);
ULOGGER_DEBUG("Add %d keypoints, %d 3d points and %d descriptors to node %d", (int)visualWords.size(), allWords3NaN?0:(int)visualWords3.size(), (int)descriptors.rows, (*iter)->id());
}
//reset
@@ -4275,30 +4276,25 @@ void DBDriverSqlite3::saveQuery(const std::list<Signature *> & signatures)
float nanFloat = std::numeric_limits<float>::quiet_NaN ();
for(std::list<Signature *>::const_iterator i=signatures.begin(); i!=signatures.end(); ++i)
{
UASSERT((*i)->getWords().size() == (*i)->getWordsKpts().size());
UASSERT((*i)->getWords3().empty() || (*i)->getWords().size() == (*i)->getWords3().size());
UASSERT((*i)->getWordsDescriptors().empty() || (*i)->getWords().size() == (*i)->getWordsDescriptors().size());
UASSERT((*i)->getWordsDescriptors().empty() || (int)(*i)->getWords().size() == (*i)->getWordsDescriptors().rows);
std::multimap<int, cv::Point3f>::const_iterator p=(*i)->getWords3().begin();
std::multimap<int, cv::Mat>::const_iterator d=(*i)->getWordsDescriptors().begin();
for(std::multimap<int, cv::KeyPoint>::const_iterator w=(*i)->getWords().begin(); w!=(*i)->getWords().end(); ++w)
for(std::multimap<int, int>::const_iterator w=(*i)->getWords().begin(); w!=(*i)->getWords().end(); ++w)
{
cv::Point3f pt(nanFloat,nanFloat,nanFloat);
if(p!=(*i)->getWords3().end())
if(!(*i)->getWords3().empty())
{
UASSERT(w->first == p->first); // must be same id!
pt = p->second;
++p;
pt = (*i)->getWords3()[w->second];
}
cv::Mat descriptor;
if(d!=(*i)->getWordsDescriptors().end())
if(!(*i)->getWordsDescriptors().empty())
{
UASSERT(w->first == d->first); // must be same id!
descriptor = d->second;
++d;
descriptor = (*i)->getWordsDescriptors().row(w->second);
}
stepKeypoint(ppStmt, (*i)->id(), w->first, w->second, pt, descriptor);
stepKeypoint(ppStmt, (*i)->id(), w->first, (*i)->getWordsKpts()[w->second], pt, descriptor);
}
}
// Finalize (delete) the statement
corelib/src/DBReader.cpp
+3 -17
View File
@@ -510,23 +510,9 @@ SensorData DBReader::getNextData(CameraInfo * info)
data.gps().stamp()!=0.0?1:0,
gravityTransform.isNull()?0:1);
cv::Mat descriptors;
if(!s->getWordsDescriptors().empty())
{
descriptors = cv::Mat(
s->getWordsDescriptors().size(),
s->getWordsDescriptors().begin()->second.cols,
s->getWordsDescriptors().begin()->second.type());
int i=0;
for(std::multimap<int, cv::Mat>::const_iterator iter=s->getWordsDescriptors().begin();
iter!=s->getWordsDescriptors().end();
++iter, ++i)
{
iter->second.copyTo(descriptors.row(i));
}
}
std::vector<cv::KeyPoint> keypoints = uValues(s->getWords());
std::vector<cv::Point3f> keypoints3D = uValues(s->getWords3());
cv::Mat descriptors = s->getWordsDescriptors().clone();
const std::vector<cv::KeyPoint> & keypoints = s->getWordsKpts();
const std::vector<cv::Point3f> & keypoints3D = s->getWords3();
if(!keypoints.empty() &&
(keypoints3D.empty() || keypoints.size() == keypoints3D.size()) &&
(descriptors.empty() || (int)keypoints.size() == descriptors.rows))
corelib/src/EpipolarGeometry.cpp
+9 -133
View File
@@ -70,15 +70,21 @@ bool EpipolarGeometry::check(const Signature * ssA, const Signature * ssB)
}
ULOGGER_DEBUG("id(%d,%d)", ssA->id(), ssB->id());
std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > pairs;
std::list<std::pair<int, std::pair<int, int> > > pairsId;
findPairsUnique(ssA->getWords(), ssB->getWords(), pairs);
findPairsUnique(ssA->getWords(), ssB->getWords(), pairsId);
if((int)pairs.size()<_matchCountMinAccepted)
if((int)pairsId.size()<_matchCountMinAccepted)
{
return false;
}
std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > pairs;
for(std::list<std::pair<int, std::pair<int, int> > >::iterator iter = pairsId.begin(); iter!=pairsId.end(); ++iter)
{
pairs.push_back(std::make_pair(iter->first, std::make_pair(ssA->getWordsKpts()[iter->second.first], ssB->getWordsKpts()[iter->second.second])));
}
std::vector<uchar> status;
cv::Mat f = findFFromWords(pairs, status, _ransacParam1, _ransacParam2);
@@ -406,136 +412,6 @@ cv::Mat EpipolarGeometry::findFFromCalibratedStereoCameras(double fx, double fy,
return K.inv().t()*E*K.inv();
}
/**
* if a=[1 2 3 4 6], b=[1 2 4 5 6], results= [(1,1) (2,2) (4,4) (6,6)]
* realPairsCount = 4
*/
int EpipolarGeometry::findPairs(
const std::map<int, cv::KeyPoint> & wordsA,
const std::map<int, cv::KeyPoint> & wordsB,
std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs,
bool ignoreInvalidIds)
{
int realPairsCount = 0;
pairs.clear();
for(std::map<int, cv::KeyPoint>::const_iterator i=wordsA.begin(); i!=wordsA.end(); ++i)
{
if(!ignoreInvalidIds || (ignoreInvalidIds && i->first>=0))
{
std::map<int, cv::KeyPoint>::const_iterator ptB = wordsB.find(i->first);
if(ptB != wordsB.end())
{
pairs.push_back(std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> >(i->first, std::pair<cv::KeyPoint, cv::KeyPoint>(i->second, ptB->second)));
++realPairsCount;
}
}
}
return realPairsCount;
}
/**
* if a=[1 2 3 4 6 6], b=[1 1 2 4 5 6 6], results= [(1,1a) (2,2) (4,4) (6a,6a) (6b,6b)]
* realPairsCount = 5
*/
int EpipolarGeometry::findPairs(const std::multimap<int, cv::KeyPoint> & wordsA,
const std::multimap<int, cv::KeyPoint> & wordsB,
std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs,
bool ignoreInvalidIds)
{
const std::list<int> & ids = uUniqueKeys(wordsA);
std::multimap<int, cv::KeyPoint>::const_iterator iterA;
std::multimap<int, cv::KeyPoint>::const_iterator iterB;
pairs.clear();
int realPairsCount = 0;
for(std::list<int>::const_iterator i=ids.begin(); i!=ids.end(); ++i)
{
if(!ignoreInvalidIds || (ignoreInvalidIds && *i >= 0))
{
iterA = wordsA.find(*i);
iterB = wordsB.find(*i);
while(iterA != wordsA.end() && iterB != wordsB.end() && (*iterA).first == (*iterB).first && (*iterA).first == *i)
{
pairs.push_back(std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> >(*i, std::pair<cv::KeyPoint, cv::KeyPoint>((*iterA).second, (*iterB).second)));
++iterA;
++iterB;
++realPairsCount;
}
}
}
return realPairsCount;
}
/**
* if a=[1 2 3 4 6 6], b=[1 1 2 4 5 6 6], results= [(2,2) (4,4)]
* realPairsCount = 5
*/
int EpipolarGeometry::findPairsUnique(
const std::multimap<int, cv::KeyPoint> & wordsA,
const std::multimap<int, cv::KeyPoint> & wordsB,
std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs,
bool ignoreInvalidIds)
{
const std::list<int> & ids = uUniqueKeys(wordsA);
int realPairsCount = 0;
pairs.clear();
for(std::list<int>::const_iterator i=ids.begin(); i!=ids.end(); ++i)
{
if(!ignoreInvalidIds || (ignoreInvalidIds && *i>=0))
{
std::list<cv::KeyPoint> ptsA = uValues(wordsA, *i);
std::list<cv::KeyPoint> ptsB = uValues(wordsB, *i);
if(ptsA.size() == 1 && ptsB.size() == 1)
{
pairs.push_back(std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> >(*i, std::pair<cv::KeyPoint, cv::KeyPoint>(ptsA.front(), ptsB.front())));
++realPairsCount;
}
else if(ptsA.size()>1 && ptsB.size()>1)
{
// just update the count
realPairsCount += ptsA.size() > ptsB.size() ? ptsB.size() : ptsA.size();
}
}
}
return realPairsCount;
}
/**
* if a=[1 2 3 4 6 6], b=[1 1 2 4 5 6 6], results= [(1,1a) (1,1b) (2,2) (4,4) (6a,6a) (6a,6b) (6b,6a) (6b,6b)]
* realPairsCount = 5
*/
int EpipolarGeometry::findPairsAll(const std::multimap<int, cv::KeyPoint> & wordsA,
const std::multimap<int, cv::KeyPoint> & wordsB,
std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs,
bool ignoreInvalidIds)
{
UTimer timer;
timer.start();
const std::list<int> & ids = uUniqueKeys(wordsA);
pairs.clear();
int realPairsCount = 0;;
for(std::list<int>::const_iterator iter=ids.begin(); iter!=ids.end(); ++iter)
{
if(!ignoreInvalidIds || (ignoreInvalidIds && *iter>=0))
{
std::list<cv::KeyPoint> ptsA = uValues(wordsA, *iter);
std::list<cv::KeyPoint> ptsB = uValues(wordsB, *iter);
realPairsCount += ptsA.size() > ptsB.size() ? ptsB.size() : ptsA.size();
for(std::list<cv::KeyPoint>::iterator jter=ptsA.begin(); jter!=ptsA.end(); ++jter)
{
for(std::list<cv::KeyPoint>::iterator kter=ptsB.begin(); kter!=ptsB.end(); ++kter)
{
pairs.push_back(std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> >(*iter, std::pair<cv::KeyPoint, cv::KeyPoint>(*jter, *kter)));
}
}
}
}
ULOGGER_DEBUG("time = %f", timer.ticks());
return realPairsCount;
}
/**
source = SfM toy library: https://github.com/royshil/SfM-Toy-Library
corelib/src/FlannIndex.cpp
+81 -28
View File
@@ -107,32 +107,36 @@ unsigned int FlannIndex::indexedFeatures() const
}
}
// return KB
unsigned int FlannIndex::memoryUsed() const
// return Bytes
unsigned long FlannIndex::memoryUsed() const
{
if(!index_)
{
return 0;
}
unsigned long memoryUsage = sizeof(FlannIndex);
memoryUsage += addedDescriptors_.size() * (sizeof(int) + sizeof(cv::Mat) + sizeof(std::_Rb_tree_node_base)) + sizeof(std::map<int, cv::Mat>);
memoryUsage += sizeof(std::list<int>) + removedIndexes_.size() * sizeof(int);
if(featuresType_ == CV_8UC1)
{
return ((const rtflann::Index<rtflann::Hamming<unsigned char> >*)index_)->usedMemory()/1000;
memoryUsage += ((const rtflann::Index<rtflann::Hamming<unsigned char> >*)index_)->usedMemory();
}
else
{
if(useDistanceL1_)
{
return ((const rtflann::Index<rtflann::L1<float> >*)index_)->usedMemory()/1000;
memoryUsage += ((const rtflann::Index<rtflann::L1<float> >*)index_)->usedMemory();
}
else if(featuresDim_ <= 3)
{
return ((const rtflann::Index<rtflann::L2_Simple<float> >*)index_)->usedMemory()/1000;
memoryUsage += ((const rtflann::Index<rtflann::L2_Simple<float> >*)index_)->usedMemory();
}
else
{
return ((const rtflann::Index<rtflann::L2<float> >*)index_)->usedMemory()/1000;
memoryUsage += ((const rtflann::Index<rtflann::L2<float> >*)index_)->usedMemory();
}
}
return memoryUsage;
}
void FlannIndex::buildLinearIndex(
@@ -177,10 +181,21 @@ void FlannIndex::buildLinearIndex(
}
}
// incremental FLANN
addedDescriptors_.insert(std::make_pair(nextIndex_, features));
nextIndex_ = features.rows;
// incremental FLANN: we should add all headers separately in case we remove
// some indexes (to keep underlying matrix data allocated)
if(rebalancingFactor_ > 1.0f)
{
for(int i=0; i<features.rows; ++i)
{
addedDescriptors_.insert(std::make_pair(nextIndex_++, features.row(i)));
}
}
else
{
// tree won't ever be rebalanced, so just keep only one header for the data
addedDescriptors_.insert(std::make_pair(nextIndex_, features));
nextIndex_ += features.rows;
}
UDEBUG("");
}
@@ -227,10 +242,21 @@ void FlannIndex::buildKDTreeIndex(
}
}
// incremental FLANN
addedDescriptors_.insert(std::make_pair(nextIndex_, features));
nextIndex_ = features.rows;
// incremental FLANN: we should add all headers separately in case we remove
// some indexes (to keep underlying matrix data allocated)
if(rebalancingFactor_ > 1.0f)
{
for(int i=0; i<features.rows; ++i)
{
addedDescriptors_.insert(std::make_pair(nextIndex_++, features.row(i)));
}
}
else
{
// tree won't ever be rebalanced, so just keep only one header for the data
addedDescriptors_.insert(std::make_pair(nextIndex_, features));
nextIndex_ += features.rows;
}
UDEBUG("");
}
@@ -278,10 +304,21 @@ void FlannIndex::buildKDTreeSingleIndex(
}
}
// incremental FLANN
addedDescriptors_.insert(std::make_pair(nextIndex_, features));
nextIndex_ = features.rows;
// incremental FLANN: we should add all headers separately in case we remove
// some indexes (to keep underlying matrix data allocated)
if(rebalancingFactor_ > 1.0f)
{
for(int i=0; i<features.rows; ++i)
{
addedDescriptors_.insert(std::make_pair(nextIndex_++, features.row(i)));
}
}
else
{
// tree won't ever be rebalanced, so just keep only one header for the data
addedDescriptors_.insert(std::make_pair(nextIndex_, features));
nextIndex_ += features.rows;
}
UDEBUG("");
}
@@ -305,10 +342,21 @@ void FlannIndex::buildLSHIndex(
index_ = new rtflann::Index<rtflann::Hamming<unsigned char> >(dataset, rtflann::LshIndexParams(12, 20, 2));
((rtflann::Index<rtflann::Hamming<unsigned char> >*)index_)->buildIndex();
// incremental FLANN
addedDescriptors_.insert(std::make_pair(nextIndex_, features));
nextIndex_ = features.rows;
// incremental FLANN: we should add all headers separately in case we remove
// some indexes (to keep underlying matrix data allocated)
if(rebalancingFactor_ > 1.0f)
{
for(int i=0; i<features.rows; ++i)
{
addedDescriptors_.insert(std::make_pair(nextIndex_++, features.row(i)));
}
}
else
{
// tree won't ever be rebalanced, so just keep only one header for the data
addedDescriptors_.insert(std::make_pair(nextIndex_, features));
nextIndex_ += features.rows;
}
UDEBUG("");
}
@@ -317,12 +365,12 @@ bool FlannIndex::isBuilt()
return index_!=0;
}
unsigned int FlannIndex::addPoints(const cv::Mat & features)
std::vector<unsigned int> FlannIndex::addPoints(const cv::Mat & features)
{
if(!index_)
{
UERROR("Flann index not yet created!");
return 0;
return std::vector<unsigned int>();
}
UASSERT(features.type() == featuresType_);
UASSERT(features.cols == featuresDim_);
@@ -401,11 +449,16 @@ unsigned int FlannIndex::addPoints(const cv::Mat & features)
removedIndexes_.clear();
}
addedDescriptors_.insert(std::make_pair(nextIndex_, features));
// incremental FLANN: we should add all headers separately in case we remove
// some indexes (to keep underlying matrix data allocated)
std::vector<unsigned int> indexes;
for(int i=0; i<features.rows; ++i)
{
indexes.push_back(nextIndex_);
addedDescriptors_.insert(std::make_pair(nextIndex_++, features.row(i)));
}
int r = nextIndex_;
nextIndex_ += features.rows;
return r;
return indexes;
}
void FlannIndex::removePoint(unsigned int index)
corelib/src/Graph.cpp
+12 -6
View File
@@ -1129,19 +1129,22 @@ std::multimap<int, Link> filterDuplicateLinks(
std::multimap<int, Link> filterLinks(
const std::multimap<int, Link> & links,
Link::Type filteredType)
Link::Type filteredType,
bool inverted)
{
std::multimap<int, Link> output;
for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
{
if(filteredType == Link::kSelfRefLink)
{
if(iter->second.from() != iter->second.to())
if((!inverted && iter->second.from() != iter->second.to())||
(inverted && iter->second.from() == iter->second.to()))
{
output.insert(*iter);
}
}
else if(iter->second.type() != filteredType)
else if((!inverted && iter->second.type() != filteredType)||
(inverted && iter->second.type() == filteredType))
{
output.insert(*iter);
}
@@ -1151,19 +1154,22 @@ std::multimap<int, Link> filterLinks(
std::map<int, Link> filterLinks(
const std::map<int, Link> & links,
Link::Type filteredType)
Link::Type filteredType,
bool inverted)
{
std::map<int, Link> output;
for(std::map<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
{
if(filteredType == Link::kSelfRefLink)
{
if(iter->second.from() != iter->second.to())
if((!inverted && iter->second.from() != iter->second.to())||
(inverted && iter->second.from() == iter->second.to()))
{
output.insert(*iter);
}
}
else if(iter->second.type() != filteredType)
else if((!inverted && iter->second.type() != filteredType)||
(inverted && iter->second.type() == filteredType))
{
output.insert(*iter);
}
corelib/src/Memory.cpp
+172 -90
View File
@@ -362,7 +362,7 @@ void Memory::loadDataFromDb(bool postInitClosingEvents)
const std::map<int, Signature *> & signatures = this->getSignatures();
for(std::map<int, Signature *>::const_iterator i=signatures.begin(); i!=signatures.end(); ++i)
{
const std::multimap<int, cv::KeyPoint> & words = i->second->getWords();
const std::multimap<int, int> & words = i->second->getWords();
std::list<int> keys = uUniqueKeys(words);
for(std::list<int>::iterator iter=keys.begin(); iter!=keys.end(); ++iter)
{
@@ -413,11 +413,11 @@ void Memory::loadDataFromDb(bool postInitClosingEvents)
Signature * s = this->_getSignature(i->first);
UASSERT(s != 0);
const std::multimap<int, cv::KeyPoint> & words = s->getWords();
const std::multimap<int, int> & words = s->getWords();
if(words.size())
{
UDEBUG("node=%d, word references=%d", s->id(), words.size());
for(std::multimap<int, cv::KeyPoint>::const_iterator iter = words.begin(); iter!=words.end(); ++iter)
for(std::multimap<int, int>::const_iterator iter = words.begin(); iter!=words.end(); ++iter)
{
if(iter->first > 0)
{
@@ -2753,20 +2753,16 @@ Transform Memory::computeTransform(
if(_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired())
{
UDEBUG("");
tmpFrom.setWords(std::multimap<int, cv::KeyPoint>());
tmpFrom.setWords3(std::multimap<int, cv::Point3f>());
tmpFrom.setWordsDescriptors(std::multimap<int, cv::Mat>());
tmpFrom.removeAllWords();
tmpFrom.sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
tmpTo.setWords(std::multimap<int, cv::KeyPoint>());
tmpTo.setWords3(std::multimap<int, cv::Point3f>());
tmpTo.setWordsDescriptors(std::multimap<int, cv::Mat>());
tmpTo.removeAllWords();
tmpTo.sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
}
else if(useKnownCorrespondencesIfPossible)
{
// This will make RegistrationVis bypassing the correspondences computation
tmpFrom.setWordsDescriptors(std::multimap<int, cv::Mat>());
tmpTo.setWordsDescriptors(std::multimap<int, cv::Mat>());
tmpFrom.setWordsDescriptors(cv::Mat());
tmpTo.setWordsDescriptors(cv::Mat());
}
bool isNeighborRefining = fromS.getLinks().find(toS.id()) != fromS.getLinks().end() && fromS.getLinks().find(toS.id())->second.type() == Link::kNeighbor;
@@ -2795,23 +2791,28 @@ Transform Memory::computeTransform(
!tmpTo.getWords().empty() &&
!tmpFrom.getWordsDescriptors().empty() &&
!tmpFrom.getWords().empty() &&
!tmpFrom.getWords3().empty())
!tmpFrom.getWords3().empty() &&
fromS.hasLink(0, Link::kNeighbor)) // If doesn't have neighbors, skip bundle
{
std::multimap<int, cv::Point3f> words3DMap;
std::multimap<int, cv::KeyPoint> wordsMap;
std::multimap<int, cv::Mat> wordsDescriptorsMap;
std::multimap<int, int> words;
std::vector<cv::Point3f> words3DMap;
std::vector<cv::KeyPoint> wordsMap;
cv::Mat wordsDescriptorsMap;
const std::multimap<int, Link> & links = fromS.getLinks();
if(!fromS.getWords3().empty())
{
const std::map<int, cv::Point3f> & words3 = uMultimapToMapUnique(fromS.getWords3());
UDEBUG("fromS.getWords3()=%d uniques=%d", (int)fromS.getWords3().size(), (int)words3.size());
for(std::map<int, cv::Point3f>::const_iterator jter=words3.begin(); jter!=words3.end(); ++jter)
const std::map<int, int> & wordsFrom = uMultimapToMapUnique(fromS.getWords());
UDEBUG("fromS.getWords()=%d uniques=%d", (int)fromS.getWords().size(), (int)wordsFrom.size());
for(std::map<int, int>::const_iterator jter=wordsFrom.begin(); jter!=wordsFrom.end(); ++jter)
{
if(util3d::isFinite(jter->second))
const cv::Point3f & pt = fromS.getWords3()[jter->second];
if(util3d::isFinite(pt))
{
words3DMap.insert(*jter);
wordsMap.insert(*fromS.getWords().find(jter->first));
wordsDescriptorsMap.insert(*fromS.getWordsDescriptors().find(jter->first));
words.insert(std::make_pair(jter->first, words.size()));
words3DMap.push_back(pt);
wordsMap.push_back(fromS.getWordsKpts()[jter->second]);
wordsDescriptorsMap.push_back(fromS.getWordsDescriptors().row(jter->second));
}
}
}
@@ -2820,21 +2821,23 @@ Transform Memory::computeTransform(
for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
{
int id = iter->first;
if(id != fromS.id())
if(id != fromS.id() && iter->second.type() == Link::kNeighbor) // assemble only neighbors for the local feature map
{
const Signature * s = this->getSignature(id);
if(s)
if(s && !s->getWords3().empty())
{
const std::map<int, cv::Point3f> & words3 = uMultimapToMapUnique(s->getWords3());
for(std::map<int, cv::Point3f>::const_iterator jter=words3.begin(); jter!=words3.end(); ++jter)
const std::map<int, int> & wordsTo = uMultimapToMapUnique(s->getWords());
for(std::map<int, int>::const_iterator jter=wordsTo.begin(); jter!=wordsTo.end(); ++jter)
{
const cv::Point3f & pt = s->getWords3()[jter->second];
if( jter->first > 0 &&
util3d::isFinite(jter->second) &&
words3DMap.find(jter->first) == words3DMap.end())
util3d::isFinite(pt) &&
words.find(jter->first) == words.end())
{
words3DMap.insert(std::make_pair(jter->first, util3d::transformPoint(jter->second, iter->second.transform())));
wordsMap.insert(*s->getWords().find(jter->first));
wordsDescriptorsMap.insert(*s->getWordsDescriptors().find(jter->first));
words.insert(words.end(), std::make_pair(jter->first, words.size()));
words3DMap.push_back(util3d::transformPoint(pt, iter->second.transform()));
wordsMap.push_back(s->getWordsKpts()[jter->second]);
wordsDescriptorsMap.push_back(s->getWordsDescriptors().row(jter->second));
}
}
}
@@ -2842,24 +2845,29 @@ Transform Memory::computeTransform(
}
UDEBUG("words3DMap=%d", (int)words3DMap.size());
Signature tmpFrom2(fromS.id());
tmpFrom2.setWords3(words3DMap);
tmpFrom2.setWords(wordsMap);
tmpFrom2.setWordsDescriptors(wordsDescriptorsMap);
tmpFrom2.setWords(words, wordsMap, words3DMap, wordsDescriptorsMap);
transform = _registrationPipeline->computeTransformationMod(tmpFrom2, tmpTo, guess, info);
if(!transform.isNull() && info)
if(!transform.isNull() && info && !tmpFrom2.getWords3().empty())
{
std::map<int, cv::Point3f> points3DMap = uMultimapToMapUnique(tmpFrom2.getWords3());
std::map<int, cv::Point3f> points3DMap;
std::map<int, int> wordsMap = uMultimapToMapUnique(tmpFrom2.getWords());
for(std::map<int, int>::iterator iter=wordsMap.begin(); iter!=wordsMap.end(); ++iter)
{
points3DMap.insert(std::make_pair(iter->first, tmpFrom2.getWords3()[iter->second]));
}
std::map<int, Transform> bundlePoses;
std::multimap<int, Link> bundleLinks;
std::map<int, CameraModel> bundleModels;
std::map<int, std::map<int, FeatureBA> > wordReferences;
std::multimap<int, Link> links = fromS.getLinks();
links = graph::filterLinks(links, Link::kNeighbor, true); // assemble only neighbors for the local feature map
links.insert(std::make_pair(toS.id(), Link(fromS.id(), toS.id(), Link::kGlobalClosure, transform, info->covariance.inv())));
links.insert(std::make_pair(fromS.id(), Link()));
int totalWordReferences = 0;
for(std::multimap<int, Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
{
int id = iter->first;
@@ -2912,33 +2920,41 @@ Transform Memory::computeTransform(
bundlePoses.insert(std::make_pair(id, iter->second.transform()));
}
const std::map<int,cv::KeyPoint> & words = uMultimapToMapUnique(s->getWords());
for(std::map<int, cv::KeyPoint>::const_iterator jter=words.begin(); jter!=words.end(); ++jter)
const std::map<int,int> & words = uMultimapToMapUnique(s->getWords());
for(std::map<int, int>::const_iterator jter=words.begin(); jter!=words.end(); ++jter)
{
if(points3DMap.find(jter->first)!=points3DMap.end() &&
(id == tmpTo.id() || jter->first > 0))
(id == tmpTo.id() || jter->first > 0)) // Since we added negative words of "from", only accept matches with current frame
{
std::multimap<int, cv::Point3f>::const_iterator kter = s->getWords3().find(jter->first);
cv::Point3f pt3d = util3d::transformPoint(kter->second, invLocalTransform);
//get depth
float d = 0.0f;
if( !s->getWords3().empty() &&
util3d::isFinite(s->getWords3()[jter->second]))
{
//move back point in camera frame (to get depth along z)
d = util3d::transformPoint(s->getWords3()[jter->second], invLocalTransform).z;
}
wordReferences.insert(std::make_pair(jter->first, std::map<int, FeatureBA>()));
wordReferences.at(jter->first).insert(std::make_pair(id, FeatureBA(jter->second, pt3d.z)));
wordReferences.at(jter->first).insert(std::make_pair(id, FeatureBA(s->getWordsKpts()[jter->second], d)));
++totalWordReferences;
}
}
}
}
}
UDEBUG("sba...start");
// set root negative to fix all other poses
std::set<int> sbaOutliers;
UTimer bundleTimer;
OptimizerG2O sba;
OptimizerG2O sba(parameters_);
sba.setIterations(5);
UTimer bundleTime;
bundlePoses = sba.optimizeBA(-toS.id(), bundlePoses, bundleLinks, bundleModels, points3DMap, wordReferences, &sbaOutliers);
UDEBUG("sba...end");
UDEBUG("bundleTime=%fs (poses=%d wordRef=%d outliers=%d)", bundleTime.ticks(), (int)bundlePoses.size(), (int)wordReferences.size(), (int)sbaOutliers.size());
UDEBUG("bundleTime=%fs (poses=%d wordRef=%d outliers=%d)", bundleTime.ticks(), (int)bundlePoses.size(), totalWordReferences, (int)sbaOutliers.size());
UDEBUG("Local Bundle Adjustment Before: %s", transform.prettyPrint().c_str());
if(!bundlePoses.rbegin()->second.isNull())
@@ -3406,21 +3422,25 @@ void Memory::dumpSignatures(const char * fileNameSign, bool words3D) const
{
if(words3D)
{
const std::multimap<int, cv::Point3f> & ref = ss->getWords3();
for(std::multimap<int, cv::Point3f>::const_iterator jter=ref.begin(); jter!=ref.end(); ++jter)
if(!ss->getWords3().empty())
{
//show only valid point according to current parameters
if(pcl::isFinite(jter->second) &&
(jter->second.x != 0 || jter->second.y != 0 || jter->second.z != 0))
const std::multimap<int, int> & ref = ss->getWords();
for(std::multimap<int, int>::const_iterator jter=ref.begin(); jter!=ref.end(); ++jter)
{
fprintf(foutSign, "%d ", (*jter).first);
const cv::Point3f & pt = ss->getWords3()[jter->second];
//show only valid point according to current parameters
if(pcl::isFinite(pt) &&
(pt.x != 0 || pt.y != 0 || pt.z != 0))
{
fprintf(foutSign, "%d ", (*jter).first);
}
}
}
}
else
{
const std::multimap<int, cv::KeyPoint> & ref = ss->getWords();
for(std::multimap<int, cv::KeyPoint>::const_iterator jter=ref.begin(); jter!=ref.end(); ++jter)
const std::multimap<int, int> & ref = ss->getWords();
for(std::multimap<int, int>::const_iterator jter=ref.begin(); jter!=ref.end(); ++jter)
{
fprintf(foutSign, "%d ", (*jter).first);
}
@@ -3490,6 +3510,47 @@ void Memory::dumpMemoryTree(const char * fileNameTree) const
}
unsigned long Memory::getMemoryUsed() const
{
unsigned long memoryUsage = sizeof(Memory);
memoryUsage += _signatures.size() * (sizeof(int)+sizeof(std::_Rb_tree_node_base)) + sizeof(std::map<int, Signature *>);
for(std::map<int, Signature*>::const_iterator iter=_signatures.begin(); iter!=_signatures.end(); ++iter)
{
memoryUsage += iter->second->getMemoryUsed(true);
}
if(_vwd)
{
memoryUsage += _vwd->getMemoryUsed();
}
memoryUsage += _stMem.size() * (sizeof(int)+sizeof(std::_Rb_tree_node_base)) + sizeof(std::set<int>);
memoryUsage += _workingMem.size() * (sizeof(int)+sizeof(double)+sizeof(std::_Rb_tree_node_base)) + sizeof(std::map<int, double>);
memoryUsage += _groundTruths.size() * (sizeof(int)+sizeof(Transform)+12*sizeof(float) + sizeof(std::_Rb_tree_node_base)) + sizeof(std::map<int, Transform>);
memoryUsage += _labels.size() * (sizeof(int)+sizeof(std::string) + sizeof(std::_Rb_tree_node_base)) + sizeof(std::map<int, std::string>);
for(std::map<int, std::string>::const_iterator iter=_labels.begin(); iter!=_labels.end(); ++iter)
{
memoryUsage+=iter->second.size();
}
memoryUsage += _landmarksIndex.size() * (sizeof(int)+sizeof(std::set<int>) + sizeof(std::_Rb_tree_node_base)) + sizeof(std::map<int, std::set<int> >);
memoryUsage += _landmarksInvertedIndex.size() * (sizeof(int)+sizeof(std::set<int>) + sizeof(std::_Rb_tree_node_base)) + sizeof(std::map<int, std::set<int> >);
for(std::map<int, std::set<int>>::const_iterator iter=_landmarksIndex.begin(); iter!=_landmarksIndex.end(); ++iter)
{
memoryUsage+=iter->second.size()*(sizeof(int)+sizeof(std::_Rb_tree_node_base)) + sizeof(std::set<int>);
}
for(std::map<int, std::set<int>>::const_iterator iter=_landmarksInvertedIndex.begin(); iter!=_landmarksInvertedIndex.end(); ++iter)
{
memoryUsage+=iter->second.size()*(sizeof(int)+sizeof(std::_Rb_tree_node_base)) + sizeof(std::set<int>);
}
memoryUsage += parameters_.size()*(sizeof(std::string)*2+sizeof(std::_Rb_tree_node_base)) + sizeof(ParametersMap);
memoryUsage += sizeof(Feature2D) + _feature2D->getParameters().size()*(sizeof(std::string)*2+sizeof(std::_Rb_tree_node_base)) + sizeof(ParametersMap);
memoryUsage += sizeof(Registration);
memoryUsage += sizeof(RegistrationIcp);
memoryUsage += _occupancy->getMemoryUsed();
memoryUsage += sizeof(MarkerDetector);
memoryUsage += sizeof(DBDriver);
return memoryUsage;
}
void Memory::rehearsal(Signature * signature, Statistics * stats)
{
UTimer timer;
@@ -3881,9 +3942,10 @@ SensorData Memory::getNodeData(int locationId, bool images, bool scan, bool user
}
void Memory::getNodeWordsAndGlobalDescriptors(int nodeId,
std::multimap<int, cv::KeyPoint> & words,
std::multimap<int, cv::Point3f> & words3,
std::multimap<int, cv::Mat> & wordsDescriptors,
std::multimap<int, int> & words,
std::vector<cv::KeyPoint> & wordsKpts,
std::vector<cv::Point3f> & words3,
cv::Mat & wordsDescriptors,
std::vector<GlobalDescriptor> & globalDescriptors) const
{
//UDEBUG("nodeId=%d", nodeId);
@@ -3891,6 +3953,7 @@ void Memory::getNodeWordsAndGlobalDescriptors(int nodeId,
if(s)
{
words = s->getWords();
wordsKpts = s->getWordsKpts();
words3 = s->getWords3();
wordsDescriptors = s->getWordsDescriptors();
globalDescriptors = s->sensorData().globalDescriptors();
@@ -3906,6 +3969,7 @@ void Memory::getNodeWordsAndGlobalDescriptors(int nodeId,
if(signatures.size())
{
words = signatures.front()->getWords();
wordsKpts = signatures.front()->getWordsKpts();
words3 = signatures.front()->getWords3();
wordsDescriptors = signatures.front()->getWordsDescriptors();
globalDescriptors = signatures.front()->sensorData().globalDescriptors();
@@ -3975,7 +4039,7 @@ void Memory::copyData(const Signature * from, Signature * to)
{
// words 2d
this->disableWordsRef(to->id());
to->setWords(from->getWords());
to->setWords(from->getWords(), from->getWordsKpts(), from->getWords3(), from->getWordsDescriptors());
std::list<int> id;
id.push_back(to->id());
this->enableWordsRef(id);
@@ -3992,8 +4056,6 @@ void Memory::copyData(const Signature * from, Signature * to)
to->sensorData().setId(to->id());
to->setPose(from->getPose());
to->setWords3(from->getWords3());
to->setWordsDescriptors(from->getWordsDescriptors());
}
else
{
@@ -4649,9 +4711,10 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
UDEBUG("id %d is a bad signature", id);
}
std::multimap<int, cv::KeyPoint> words;
std::multimap<int, cv::Point3f> words3D;
std::multimap<int, cv::Mat> wordsDescriptors;
std::multimap<int, int> words;
std::vector<cv::KeyPoint> wordsKpts;
std::vector<cv::Point3f> words3D;
cv::Mat wordsDescriptors;
int words3DValid = 0;
if(wordIds.size() > 0)
{
@@ -4671,11 +4734,12 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
kpt.size *= decimationRatio;
kpt.octave += log2value;
}
words.insert(std::pair<int, cv::KeyPoint>(*iter, kpt));
words.insert(std::make_pair(*iter, words.size()));
wordsKpts.push_back(kpt);
if(keypoints3D.size())
{
words3D.insert(std::pair<int, cv::Point3f>(*iter, keypoints3D.at(i)));
words3D.push_back(keypoints3D.at(i));
if(util3d::isFinite(keypoints3D.at(i)))
{
++words3DValid;
@@ -4683,7 +4747,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
}
if(_rawDescriptorsKept)
{
wordsDescriptors.insert(std::pair<int, cv::Mat>(*iter, descriptors.row(i).clone()));
wordsDescriptors.push_back(descriptors.row(i));
}
}
}
@@ -4803,18 +4867,32 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
Signature cpPrevious(2);
// IDs should be unique so that registration doesn't override them
std::map<int, cv::KeyPoint> uniqueWords = uMultimapToMapUnique(previousS->getWords());
std::map<int, cv::Mat> uniqueWordsDescriptors = uMultimapToMapUnique(previousS->getWordsDescriptors());
std::map<int, int> uniqueWordsOld = uMultimapToMapUnique(previousS->getWords());
std::vector<cv::KeyPoint> uniqueWordsKpts;
cv::Mat uniqueWordsDescriptors;
std::multimap<int, int> uniqueWords;
for(std::map<int, int>::iterator iter=uniqueWordsOld.begin(); iter!=uniqueWordsOld.end(); ++iter)
{
uniqueWords.insert(std::make_pair(iter->first, uniqueWords.size()));
uniqueWordsKpts.push_back(previousS->getWordsKpts()[iter->second]);
uniqueWordsDescriptors.push_back(previousS->getWordsDescriptors().row(iter->second));
}
cpPrevious.sensorData().setCameraModels(previousS->sensorData().cameraModels());
cpPrevious.setWords(std::multimap<int, cv::KeyPoint>(uniqueWords.begin(), uniqueWords.end()));
cpPrevious.setWordsDescriptors(std::multimap<int, cv::Mat>(uniqueWordsDescriptors.begin(), uniqueWordsDescriptors.end()));
cpPrevious.setWords(uniqueWords, uniqueWordsKpts, std::vector<cv::Point3f>(), uniqueWordsDescriptors);
Signature cpCurrent(1);
uniqueWords = uMultimapToMapUnique(words);
uniqueWordsDescriptors = uMultimapToMapUnique(wordsDescriptors);
uniqueWordsOld = uMultimapToMapUnique(words);
uniqueWordsKpts.clear();
uniqueWordsDescriptors = cv::Mat();
uniqueWords.clear();
for(std::map<int, int>::iterator iter=uniqueWordsOld.begin(); iter!=uniqueWordsOld.end(); ++iter)
{
uniqueWords.insert(std::make_pair(iter->first, uniqueWords.size()));
uniqueWordsKpts.push_back(wordsKpts[iter->second]);
uniqueWordsDescriptors.push_back(wordsDescriptors.row(iter->second));
}
cpCurrent.sensorData().setCameraModels(cameraModels);
// This will force comparing descriptors between both images directly
cpCurrent.setWords(std::multimap<int, cv::KeyPoint>(uniqueWords.begin(), uniqueWords.end()));
cpCurrent.setWordsDescriptors(std::multimap<int, cv::Mat>(uniqueWordsDescriptors.begin(), uniqueWordsDescriptors.end()));
cpCurrent.setWords(uniqueWords, uniqueWordsKpts, std::vector<cv::Point3f>(), uniqueWordsDescriptors);
// The following is used only to re-estimate the correspondences, the returned transform is ignored
Transform tmpt;
@@ -4832,9 +4910,21 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
UDEBUG("t=%s", tmpt.prettyPrint().c_str());
// compute 3D words by epipolar geometry with the previous signature using odometry motion
std::map<int, int> currentUniqueWords = uMultimapToMapUnique(cpCurrent.getWords());
std::map<int, int> previousUniqueWords = uMultimapToMapUnique(cpPrevious.getWords());
std::map<int, cv::KeyPoint> currentWords;
std::map<int, cv::KeyPoint> previousWords;
for(std::map<int, int>::iterator iter=currentUniqueWords.begin(); iter!=currentUniqueWords.end(); ++iter)
{
currentWords.insert(std::make_pair(iter->first, cpCurrent.getWordsKpts()[iter->second]));
}
for(std::map<int, int>::iterator iter=previousUniqueWords.begin(); iter!=previousUniqueWords.end(); ++iter)
{
previousWords.insert(std::make_pair(iter->first, cpPrevious.getWordsKpts()[iter->second]));
}
std::map<int, cv::Point3f> inliers = util3d::generateWords3DMono(
uMultimapToMapUnique(cpCurrent.getWords()),
uMultimapToMapUnique(cpPrevious.getWords()),
currentWords,
previousWords,
cameraModels[0],
cameraTransform);
@@ -4845,32 +4935,26 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
UASSERT(words3D.size() == 0 || words.size() == words3D.size());
bool words3DWasEmpty = words3D.empty();
int added3DPointsWithoutDepth = 0;
for(std::multimap<int, cv::KeyPoint>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
for(std::multimap<int, int>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
{
std::map<int, cv::Point3f>::iterator jter=inliers.find(iter->first);
std::multimap<int, cv::Point3f>::iterator iter3D = words3D.find(iter->first);
if(iter3D == words3D.end())
if(words3DWasEmpty)
{
if(jter != inliers.end())
{
words3D.insert(std::make_pair(iter->first, jter->second));
words3D.push_back(jter->second);
++added3DPointsWithoutDepth;
}
else
{
words3D.insert(std::make_pair(iter->first, cv::Point3f(bad_point,bad_point,bad_point)));
words3D.push_back(cv::Point3f(bad_point,bad_point,bad_point));
}
}
else if(!util3d::isFinite(iter3D->second) && jter != inliers.end())
else if(!util3d::isFinite(words3D[iter->second]) && jter != inliers.end())
{
iter3D->second = jter->second;
words3D[iter->second] = jter->second;
++added3DPointsWithoutDepth;
}
else if(words3DWasEmpty && jter == inliers.end())
{
// duplicate
words3D.insert(std::make_pair(iter->first, cv::Point3f(bad_point,bad_point,bad_point)));
}
}
UDEBUG("added3DPointsWithoutDepth=%d", added3DPointsWithoutDepth);
if(stats) stats->addStatistic(Statistics::kMemoryTriangulated_points(), (float)added3DPointsWithoutDepth);
@@ -5123,9 +5207,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
compressedUserData));
}
s->setWords(words);
s->setWords3(words3D);
s->setWordsDescriptors(wordsDescriptors);
s->setWords(words, wordsKpts, words3D, wordsDescriptors);
// set raw data
if(!cameraModels.empty())
@@ -5290,7 +5372,7 @@ void Memory::disableWordsRef(int signatureId)
Signature * ss = this->_getSignature(signatureId);
if(ss && ss->isEnabled())
{
const std::multimap<int, cv::KeyPoint> & words = ss->getWords();
const std::multimap<int, int> & words = ss->getWords();
const std::list<int> & keys = uUniqueKeys(words);
int count = _vwd->getTotalActiveReferences();
// First remove all references
corelib/src/OccupancyGrid.cpp
+32
View File
@@ -1532,4 +1532,36 @@ bool OccupancyGrid::update(const std::map<int, Transform> & posesIn)
return updated;
}
unsigned long OccupancyGrid::getMemoryUsed() const
{
unsigned long memoryUsage = sizeof(OccupancyGrid);
memoryUsage += parameters_.size()*(sizeof(std::string)*2+sizeof(std::_Rb_tree_node_base)) + sizeof(ParametersMap);
memoryUsage += cache_.size()*(sizeof(int) + sizeof(std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat>) + sizeof(std::_Rb_tree_node_base)) + sizeof(std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> >);
for(std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> >::const_iterator iter=cache_.begin(); iter!=cache_.end(); ++iter)
{
memoryUsage += iter->second.first.first.total() * iter->second.first.first.elemSize();
memoryUsage += iter->second.first.second.total() * iter->second.first.second.elemSize();
memoryUsage += iter->second.second.total() * iter->second.second.elemSize();
}
memoryUsage += map_.total() * map_.elemSize();
memoryUsage += mapInfo_.total() * mapInfo_.elemSize();
memoryUsage += cellCount_.size()*(sizeof(int)*3 + sizeof(std::pair<int, int>) + sizeof(std::_Rb_tree_node_base)) + sizeof(std::map<int, std::pair<int, int> >);
memoryUsage += addedNodes_.size()*(sizeof(int) + sizeof(Transform)+ sizeof(float)*12 + sizeof(std::_Rb_tree_node_base)) + sizeof(std::map<int, Transform>);
if(assembledGround_.get())
{
memoryUsage += assembledGround_->points.size() * sizeof(pcl::PointXYZRGB);
}
if(assembledObstacles_.get())
{
memoryUsage += assembledObstacles_->points.size() * sizeof(pcl::PointXYZRGB);
}
if(assembledEmptyCells_.get())
{
memoryUsage += assembledEmptyCells_->points.size() * sizeof(pcl::PointXYZRGB);
}
return memoryUsage;
}
}
corelib/src/Optimizer.cpp
+22 -17
View File
@@ -617,8 +617,8 @@ void Optimizer::computeBACorrespondences(
if(!rematchFeatures)
{
sFrom.setWordsDescriptors(std::multimap<int, cv::Mat>());
sTo.setWordsDescriptors(std::multimap<int, cv::Mat>());
sFrom.setWordsDescriptors(cv::Mat());
sTo.setWordsDescriptors(cv::Mat());
}
RegistrationInfo info;
@@ -633,13 +633,13 @@ void Optimizer::computeBACorrespondences(
// set descriptors for the output
if(sFrom.getWords().size() &&
sFrom.getWordsDescriptors().empty() &&
sFrom.getWords().size() == signatures.at(link.from()).getWordsDescriptors().size())
(int)sFrom.getWords().size() == signatures.at(link.from()).getWordsDescriptors().rows)
{
sFrom.setWordsDescriptors(signatures.at(link.from()).getWordsDescriptors());
}
if(sTo.getWords().size() &&
sTo.getWordsDescriptors().empty() &&
sTo.getWords().size() == signatures.at(link.to()).getWordsDescriptors().size())
(int)sTo.getWords().size() == signatures.at(link.to()).getWordsDescriptors().rows)
{
sTo.setWordsDescriptors(signatures.at(link.to()).getWordsDescriptors());
}
@@ -649,11 +649,13 @@ void Optimizer::computeBACorrespondences(
UASSERT(!pose.isNull());
for(unsigned int i=0; i<info.inliersIDs.size(); ++i)
{
cv::Point3f p = sFrom.getWords3().lower_bound(info.inliersIDs[i])->second;
int indexFrom = sFrom.getWords().lower_bound(info.inliersIDs[i])->second;
cv::Point3f p = sFrom.getWords3()[indexFrom];
if(p.x > 0.0f) // make sure the point is valid
{
cv::KeyPoint ptFrom = sFrom.getWords().lower_bound(info.inliersIDs[i])->second;
cv::KeyPoint ptTo = sTo.getWords().lower_bound(info.inliersIDs[i])->second;
cv::KeyPoint ptFrom = sFrom.getWordsKpts()[indexFrom];
int indexTo = sTo.getWords().lower_bound(info.inliersIDs[i])->second;
cv::KeyPoint ptTo = sTo.getWordsKpts()[indexTo];
int wordId = -1;
@@ -692,10 +694,10 @@ void Optimizer::computeBACorrespondences(
if(!fromAlreadyAdded)
{
cv::Mat descriptorFrom;
if(sFrom.getWordsDescriptors().size())
if(!sFrom.getWordsDescriptors().empty())
{
UASSERT(sFrom.getWordsDescriptors().find(info.inliersIDs[i]) != sFrom.getWordsDescriptors().end());
descriptorFrom = sFrom.getWordsDescriptors().lower_bound(info.inliersIDs[i])->second;
UASSERT(indexFrom < sFrom.getWordsDescriptors().rows);
descriptorFrom = sFrom.getWordsDescriptors().row(indexFrom);
}
wordReferences.at(wordId).insert(std::make_pair(sFrom.id(), FeatureBA(ptFrom, p.x, descriptorFrom)));
frameToWordMap.insert(std::make_pair(sFrom.id(), std::map<cv::KeyPoint, int, KeyPointCompare>()));
@@ -705,17 +707,20 @@ void Optimizer::computeBACorrespondences(
if(!toAlreadyAdded)
{
cv::Mat descriptorTo;
if(sTo.getWordsDescriptors().size())
if(!sTo.getWordsDescriptors().empty())
{
UASSERT(sTo.getWordsDescriptors().find(info.inliersIDs[i]) != sTo.getWordsDescriptors().end());
descriptorTo = sTo.getWordsDescriptors().lower_bound(info.inliersIDs[i])->second;
UASSERT(indexTo < sTo.getWordsDescriptors().rows);
descriptorTo = sTo.getWordsDescriptors().row(indexTo);
}
float depth = 0.0f;
std::multimap<int, cv::Point3f>::const_iterator iterTo = sTo.getWords3().lower_bound(info.inliersIDs[i]);
if( iterTo!=sTo.getWords3().end() &&
iterTo->second.x > 0)
if(!sTo.getWords3().empty())
{
depth = iterTo->second.x;
UASSERT(indexTo < (int)sTo.getWords3().size());
const cv::Point3f & pt = sTo.getWords3()[indexTo];
if( pt.x > 0)
{
depth = pt.x;
}
}
wordReferences.at(wordId).insert(std::make_pair(sTo.id(), FeatureBA(ptTo, depth, descriptorTo)));
frameToWordMap.insert(std::make_pair(sTo.id(), std::map<cv::KeyPoint, int, KeyPointCompare>()));
corelib/src/RegistrationVis.cpp
+205 -144
View File
@@ -303,7 +303,7 @@ Transform RegistrationVis::computeTransformationImpl(
fromSignature.id(),
(int)fromSignature.getWords().size(),
(int)fromSignature.getWords3().size(),
(int)fromSignature.getWordsDescriptors().size(),
(int)fromSignature.getWordsDescriptors().rows,
(int)fromSignature.sensorData().keypoints().size(),
(int)fromSignature.sensorData().keypoints3D().size(),
fromSignature.sensorData().descriptors().rows,
@@ -316,7 +316,7 @@ Transform RegistrationVis::computeTransformationImpl(
toSignature.id(),
(int)toSignature.getWords().size(),
(int)toSignature.getWords3().size(),
(int)toSignature.getWordsDescriptors().size(),
(int)toSignature.getWordsDescriptors().rows,
(int)toSignature.sensorData().keypoints().size(),
(int)toSignature.sensorData().keypoints3D().size(),
toSignature.sensorData().descriptors().rows,
@@ -349,16 +349,16 @@ Transform RegistrationVis::computeTransformationImpl(
fromSignature.getWords3().empty() ||
(fromSignature.getWords().size() == fromSignature.getWords3().size()));
UASSERT((int)fromSignature.sensorData().keypoints().size() == fromSignature.sensorData().descriptors().rows ||
fromSignature.getWords().size() == fromSignature.getWordsDescriptors().size() ||
fromSignature.sensorData().descriptors().rows == 0 ||
fromSignature.getWordsDescriptors().size() == 0);
(int)fromSignature.getWords().size() == fromSignature.getWordsDescriptors().rows ||
fromSignature.sensorData().descriptors().empty() ||
fromSignature.getWordsDescriptors().empty() == 0);
UASSERT((toSignature.getWords().empty() && toSignature.getWords3().empty())||
(toSignature.getWords().size() && toSignature.getWords3().empty())||
(toSignature.getWords().size() == toSignature.getWords3().size()));
UASSERT((int)toSignature.sensorData().keypoints().size() == toSignature.sensorData().descriptors().rows ||
toSignature.getWords().size() == toSignature.getWordsDescriptors().size() ||
toSignature.sensorData().descriptors().rows == 0 ||
toSignature.getWordsDescriptors().size() == 0);
(int)toSignature.getWords().size() == toSignature.getWordsDescriptors().rows ||
toSignature.sensorData().descriptors().empty() ||
toSignature.getWordsDescriptors().empty());
UASSERT(fromSignature.sensorData().imageRaw().empty() ||
fromSignature.sensorData().imageRaw().type() == CV_8UC1 ||
fromSignature.sensorData().imageRaw().type() == CV_8UC3);
@@ -371,6 +371,7 @@ Transform RegistrationVis::computeTransformationImpl(
cv::Mat imageTo = toSignature.sensorData().imageRaw();
std::vector<int> orignalWordsFromIds;
int kptsFromSource = 0;
if(fromSignature.getWords().empty())
{
if(fromSignature.sensorData().keypoints().empty())
@@ -403,22 +404,26 @@ Transform RegistrationVis::computeTransformationImpl(
else
{
kptsFrom = fromSignature.sensorData().keypoints();
kptsFromSource = 1;
}
}
else
{
kptsFrom.resize(fromSignature.getWords().size());
kptsFromSource = 2;
orignalWordsFromIds.resize(fromSignature.getWords().size());
int i=0;
bool allUniques = true;
for(std::multimap<int, cv::KeyPoint>::const_iterator iter=fromSignature.getWords().begin(); iter!=fromSignature.getWords().end(); ++iter)
int previousIdAdded = 0;
kptsFrom = fromSignature.getWordsKpts();
for(std::multimap<int, int>::const_iterator iter=fromSignature.getWords().begin(); iter!=fromSignature.getWords().end(); ++iter)
{
kptsFrom[i] = iter->second;
orignalWordsFromIds[i] = iter->first;
if(i>0 && iter->first==orignalWordsFromIds[i-1])
UASSERT(iter->second>=0 && iter->second<(int)orignalWordsFromIds.size());
orignalWordsFromIds[iter->second] = iter->first;
if(i>0 && iter->first==previousIdAdded)
{
allUniques = false;
}
previousIdAdded = iter->first;
++i;
}
if(!allUniques)
@@ -428,12 +433,14 @@ Transform RegistrationVis::computeTransformationImpl(
}
}
std::multimap<int, cv::KeyPoint> wordsFrom;
std::multimap<int, cv::KeyPoint> wordsTo;
std::multimap<int, cv::Point3f> words3From;
std::multimap<int, cv::Point3f> words3To;
std::multimap<int, cv::Mat> wordsDescFrom;
std::multimap<int, cv::Mat> wordsDescTo;
std::multimap<int, int> wordsFrom;
std::multimap<int, int> wordsTo;
std::vector<cv::KeyPoint> wordsKptsFrom;
std::vector<cv::KeyPoint> wordsKptsTo;
std::vector<cv::Point3f> words3From;
std::vector<cv::Point3f> words3To;
cv::Mat wordsDescFrom;
cv::Mat wordsDescTo;
if(_correspondencesApproach == 1) //Optical Flow
{
UDEBUG("");
@@ -454,7 +461,7 @@ Transform RegistrationVis::computeTransformationImpl(
std::vector<cv::Point3f> kptsFrom3D;
if(kptsFrom.size() == fromSignature.getWords3().size())
{
kptsFrom3D = uValues(fromSignature.getWords3());
kptsFrom3D = fromSignature.getWords3();
}
else if(kptsFrom.size() == fromSignature.sensorData().keypoints3D().size())
{
@@ -544,13 +551,16 @@ Transform RegistrationVis::computeTransformationImpl(
UASSERT(kptsTo3D.size() == 0 || kptsTo.size() == kptsTo3D.size());
for(unsigned int i=0; i< kptsFrom3DKept.size(); ++i)
{
int id = orignalWordsFromIds.size()?orignalWordsFromIds[i]:i;
wordsFrom.insert(std::make_pair(id, kptsFrom[i]));
words3From.insert(std::make_pair(id, kptsFrom3DKept[i]));
wordsTo.insert(std::make_pair(id, kptsTo[i]));
if(kptsTo3D.size())
int id = !orignalWordsFromIds.empty()?orignalWordsFromIds[i]:i;
wordsFrom.insert(wordsFrom.end(), std::make_pair(id, wordsFrom.size()));
wordsKptsFrom.push_back(kptsFrom[i]);
words3From.push_back(kptsFrom3DKept[i]);
wordsTo.insert(wordsTo.end(), std::make_pair(id, wordsTo.size()));
wordsKptsTo.push_back(kptsTo[i]);
if(!kptsTo3D.empty())
{
words3To.insert(std::make_pair(id, kptsTo3D[i]));
words3To.push_back(kptsTo3D[i]);
}
}
toSignature.sensorData().setFeatures(kptsTo, kptsTo3D, cv::Mat());
@@ -566,9 +576,10 @@ Transform RegistrationVis::computeTransformationImpl(
{
if(util3d::isFinite(kptsFrom3D[i]))
{
int id = orignalWordsFromIds.size()?orignalWordsFromIds[i]:i;
wordsFrom.insert(std::make_pair(id, kptsFrom[i]));
words3From.insert(std::make_pair(id, kptsFrom3D[i]));
int id = !orignalWordsFromIds.empty()?orignalWordsFromIds[i]:i;
wordsFrom.insert(wordsFrom.end(), std::make_pair(id, wordsFrom.size()));
wordsKptsFrom.push_back(kptsFrom[i]);
words3From.push_back(kptsFrom3D[i]);
}
}
toSignature.sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
@@ -580,6 +591,7 @@ Transform RegistrationVis::computeTransformationImpl(
{
UDEBUG("");
std::vector<cv::KeyPoint> kptsTo;
int kptsToSource = 0;
if(toSignature.getWords().empty())
{
if(toSignature.sensorData().keypoints().empty() &&
@@ -610,33 +622,28 @@ Transform RegistrationVis::computeTransformationImpl(
else
{
kptsTo = toSignature.sensorData().keypoints();
kptsToSource = 1;
}
}
else
{
kptsTo = uValues(toSignature.getWords());
kptsTo = toSignature.getWordsKpts();
kptsToSource = 2;
}
// extract descriptors
UDEBUG("kptsFrom=%d", (int)kptsFrom.size());
UDEBUG("kptsTo=%d", (int)kptsTo.size());
UDEBUG("kptsFrom=%d kptsFromSource=%d", (int)kptsFrom.size(), kptsFromSource);
UDEBUG("kptsTo=%d kptsToSource=%d", (int)kptsTo.size(), kptsToSource);
cv::Mat descriptorsFrom;
if(fromSignature.getWordsDescriptors().size() &&
((kptsFrom.empty() && fromSignature.getWordsDescriptors().size()) ||
fromSignature.getWordsDescriptors().size() == kptsFrom.size()))
if(kptsFromSource == 2 &&
fromSignature.getWordsDescriptors().rows &&
((kptsFrom.empty() && fromSignature.getWordsDescriptors().rows) ||
fromSignature.getWordsDescriptors().rows == (int)kptsFrom.size()))
{
descriptorsFrom = cv::Mat(fromSignature.getWordsDescriptors().size(),
fromSignature.getWordsDescriptors().begin()->second.cols,
fromSignature.getWordsDescriptors().begin()->second.type());
int i=0;
for(std::multimap<int, cv::Mat>::const_iterator iter=fromSignature.getWordsDescriptors().begin();
iter!=fromSignature.getWordsDescriptors().end();
++iter, ++i)
{
iter->second.copyTo(descriptorsFrom.row(i));
}
descriptorsFrom = fromSignature.getWordsDescriptors();
}
else if(fromSignature.sensorData().descriptors().rows == (int)kptsFrom.size())
else if(kptsFromSource == 1 &&
fromSignature.sensorData().descriptors().rows == (int)kptsFrom.size())
{
descriptorsFrom = fromSignature.sensorData().descriptors();
}
@@ -656,20 +663,13 @@ Transform RegistrationVis::computeTransformationImpl(
cv::Mat descriptorsTo;
if(kptsTo.size())
{
if(toSignature.getWordsDescriptors().size() == kptsTo.size())
if(kptsToSource == 2 &&
toSignature.getWordsDescriptors().rows == (int)kptsTo.size())
{
descriptorsTo = cv::Mat(toSignature.getWordsDescriptors().size(),
toSignature.getWordsDescriptors().begin()->second.cols,
toSignature.getWordsDescriptors().begin()->second.type());
int i=0;
for(std::multimap<int, cv::Mat>::const_iterator iter=toSignature.getWordsDescriptors().begin();
iter!=toSignature.getWordsDescriptors().end();
++iter, ++i)
{
iter->second.copyTo(descriptorsTo.row(i));
}
descriptorsTo = toSignature.getWordsDescriptors();
}
else if(toSignature.sensorData().descriptors().rows == (int)kptsTo.size())
else if(kptsToSource == 1 &&
toSignature.sensorData().descriptors().rows == (int)kptsTo.size())
{
descriptorsTo = toSignature.sensorData().descriptors();
}
@@ -689,11 +689,13 @@ Transform RegistrationVis::computeTransformationImpl(
// create 3D keypoints
std::vector<cv::Point3f> kptsFrom3D;
std::vector<cv::Point3f> kptsTo3D;
if(kptsFrom.size() == fromSignature.getWords3().size())
if(kptsFromSource == 2 &&
kptsFrom.size() == fromSignature.getWords3().size())
{
kptsFrom3D = uValues(fromSignature.getWords3());
kptsFrom3D = fromSignature.getWords3();
}
else if(kptsFrom.size() == fromSignature.sensorData().keypoints3D().size())
else if(kptsFromSource == 1 &&
kptsFrom.size() == fromSignature.sensorData().keypoints3D().size())
{
kptsFrom3D = fromSignature.sensorData().keypoints3D();
}
@@ -724,11 +726,12 @@ Transform RegistrationVis::computeTransformationImpl(
_detectorFrom->filterKeypointsByDepth(kptsFrom, descriptorsFrom, kptsFrom3D, _detectorFrom->getMinDepth(), _detectorFrom->getMaxDepth());
}
if(kptsTo.size() == toSignature.getWords3().size())
if(kptsToSource == 2 && kptsTo.size() == toSignature.getWords3().size())
{
kptsTo3D = uValues(toSignature.getWords3());
kptsTo3D = toSignature.getWords3();
}
else if(kptsTo.size() == toSignature.sensorData().keypoints3D().size())
else if(kptsToSource == 1 &&
kptsTo.size() == toSignature.sensorData().keypoints3D().size())
{
kptsTo3D = toSignature.sensorData().keypoints3D();
}
@@ -858,7 +861,7 @@ Transform RegistrationVis::computeTransformationImpl(
UDEBUG("radius search done for guess");
// Process results (Nearest Neighbor Distance Ratio)
int newToId = orignalWordsFromIds.size()?orignalWordsFromIds.back():descriptorsFrom.rows;
int newToId = !orignalWordsFromIds.empty()?fromSignature.getWords().rbegin()->first+1:descriptorsFrom.rows;
std::map<int,int> addedWordsFrom; //<id, index>
std::map<int, int> duplicates; //<fromId, toId>
int newWords = 0;
@@ -912,7 +915,7 @@ Transform RegistrationVis::computeTransformationImpl(
if(matchedIndex >= 0)
{
matchedIndex = projectedIndexToDescIndex[matchedIndex];
int id = orignalWordsFromIds.size()?orignalWordsFromIds[matchedIndex]:matchedIndex;
int id = !orignalWordsFromIds.empty()?orignalWordsFromIds[matchedIndex]:matchedIndex;
if(addedWordsFrom.find(matchedIndex) != addedWordsFrom.end())
{
@@ -923,29 +926,32 @@ Transform RegistrationVis::computeTransformationImpl(
{
addedWordsFrom.insert(std::make_pair(matchedIndex, id));
if(kptsFrom.size())
wordsFrom.insert(wordsFrom.end(), std::make_pair(id, wordsFrom.size()));
if(!kptsFrom.empty())
{
wordsFrom.insert(std::make_pair(id, kptsFrom[matchedIndex]));
wordsKptsFrom.push_back(kptsFrom[matchedIndex]);
}
words3From.insert(std::make_pair(id, kptsFrom3D[matchedIndex]));
wordsDescFrom.insert(std::make_pair(id, descriptorsFrom.row(matchedIndex)));
words3From.push_back(kptsFrom3D[matchedIndex]);
wordsDescFrom.push_back(descriptorsFrom.row(matchedIndex));
}
wordsTo.insert(std::make_pair(id, kptsTo[i]));
wordsDescTo.insert(std::make_pair(id, descriptorsTo.row(i)));
if(kptsTo3D.size())
wordsTo.insert(wordsTo.end(), std::make_pair(id, wordsTo.size()));
wordsKptsTo.push_back(kptsTo[i]);
wordsDescTo.push_back(descriptorsTo.row(i));
if(!kptsTo3D.empty())
{
words3To.insert(std::make_pair(id, kptsTo3D[i]));
words3To.push_back(kptsTo3D[i]);
}
}
else
{
// gen fake ids
wordsTo.insert(wordsTo.end(), std::make_pair(newToId, kptsTo[i]));
wordsDescTo.insert(wordsDescTo.end(), std::make_pair(newToId, descriptorsTo.row(i)));
if(kptsTo3D.size())
wordsTo.insert(wordsTo.end(), std::make_pair(newToId, wordsTo.size()));
wordsKptsTo.push_back(kptsTo[i]);
wordsDescTo.push_back(descriptorsTo.row(i));
if(!kptsTo3D.empty())
{
words3To.insert(words3To.end(), std::make_pair(newToId, kptsTo3D[i]));
words3To.push_back(kptsTo3D[i]);
}
++newToId;
@@ -962,10 +968,11 @@ Transform RegistrationVis::computeTransformationImpl(
{
if(util3d::isFinite(kptsFrom3D[i]) && addedWordsFrom.find(i) == addedWordsFrom.end())
{
int id = orignalWordsFromIds.size()?orignalWordsFromIds[i]:i;
wordsFrom.insert(wordsFrom.end(), std::make_pair(id, kptsFrom[i]));
wordsDescFrom.insert(wordsDescFrom.end(), std::make_pair(id, descriptorsFrom.row(i)));
words3From.insert(words3From.end(), std::make_pair(id, kptsFrom3D[i]));
int id = !orignalWordsFromIds.empty()?orignalWordsFromIds[i]:i;
wordsFrom.insert(wordsFrom.end(), std::make_pair(id, wordsFrom.size()));
wordsKptsFrom.push_back(kptsFrom[i]);
wordsDescFrom.push_back(descriptorsFrom.row(i));
words3From.push_back(kptsFrom3D[i]);
++addWordsFromNotMatched;
}
@@ -1007,7 +1014,7 @@ Transform RegistrationVis::computeTransformationImpl(
if(indices[i].size())
{
info.projectedIDs.push_back(orignalWordsFromIds.size()?orignalWordsFromIds[matchedIndexFrom]:matchedIndexFrom);
info.projectedIDs.push_back(!orignalWordsFromIds.empty()?orignalWordsFromIds[matchedIndexFrom]:matchedIndexFrom);
}
if(util3d::isFinite(kptsFrom3D[matchedIndexFrom]))
@@ -1058,26 +1065,28 @@ Transform RegistrationVis::computeTransformationImpl(
matchedIndexTo = indices[i].at(0);
}
int id = orignalWordsFromIds.size()?orignalWordsFromIds[matchedIndexFrom]:matchedIndexFrom;
int id = !orignalWordsFromIds.empty()?orignalWordsFromIds[matchedIndexFrom]:matchedIndexFrom;
addedWordsFrom.insert(addedWordsFrom.end(), matchedIndexFrom);
if(kptsFrom.size())
wordsFrom.insert(wordsFrom.end(), std::make_pair(id, wordsFrom.size()));
if(!kptsFrom.empty())
{
wordsFrom.insert(wordsFrom.end(), std::make_pair(id, kptsFrom[matchedIndexFrom]));
wordsKptsFrom.push_back(kptsFrom[matchedIndexFrom]);
}
words3From.insert(words3From.end(), std::make_pair(id, kptsFrom3D[matchedIndexFrom]));
wordsDescFrom.insert(wordsDescFrom.end(), std::make_pair(id, descriptorsFrom.row(matchedIndexFrom)));
words3From.push_back(kptsFrom3D[matchedIndexFrom]);
wordsDescFrom.push_back(descriptorsFrom.row(matchedIndexFrom));
if( matchedIndexTo >= 0 &&
addedWordsTo.find(matchedIndexTo) == addedWordsTo.end())
{
addedWordsTo.insert(matchedIndexTo);
wordsTo.insert(wordsTo.end(), std::make_pair(id, kptsTo[matchedIndexTo]));
wordsDescTo.insert(wordsDescTo.end(), std::make_pair(id, descriptorsTo.row(matchedIndexTo)));
if(kptsTo3D.size())
wordsTo.insert(wordsTo.end(), std::make_pair(id, wordsTo.size()));
wordsKptsTo.push_back(kptsTo[matchedIndexTo]);
wordsDescTo.push_back(descriptorsTo.row(matchedIndexTo));
if(!kptsTo3D.empty())
{
words3To.insert(words3To.end(), std::make_pair(id, kptsTo3D[matchedIndexTo]));
words3To.push_back(kptsTo3D[matchedIndexTo]);
}
}
}
@@ -1089,23 +1098,25 @@ Transform RegistrationVis::computeTransformationImpl(
{
if(util3d::isFinite(kptsFrom3D[i]) && addedWordsFrom.find(i) == addedWordsFrom.end())
{
int id = orignalWordsFromIds.size()?orignalWordsFromIds[i]:i;
wordsFrom.insert(wordsFrom.end(), std::make_pair(id, kptsFrom[i]));
wordsDescFrom.insert(wordsDescFrom.end(), std::make_pair(id, descriptorsFrom.row(i)));
words3From.insert(words3From.end(), std::make_pair(id, kptsFrom3D[i]));
int id = !orignalWordsFromIds.empty()?orignalWordsFromIds[i]:i;
wordsFrom.insert(wordsFrom.end(), std::make_pair(id, wordsFrom.size()));
wordsKptsFrom.push_back(kptsFrom[i]);
wordsDescFrom.push_back(descriptorsFrom.row(i));
words3From.push_back(kptsFrom3D[i]);
}
}
int newToId = orignalWordsFromIds.size()?orignalWordsFromIds.back():descriptorsFrom.rows;
int newToId = !orignalWordsFromIds.empty()?fromSignature.getWords().rbegin()->first+1:descriptorsFrom.rows;
for(unsigned int i = 0; i < kptsTo.size(); ++i)
{
if(addedWordsTo.find(i) == addedWordsTo.end())
{
wordsTo.insert(wordsTo.end(), std::make_pair(newToId, kptsTo[i]));
wordsDescTo.insert(wordsDescTo.end(), std::make_pair(newToId, descriptorsTo.row(i)));
if(kptsTo3D.size())
wordsTo.insert(wordsTo.end(), std::make_pair(newToId, wordsTo.size()));
wordsKptsTo.push_back(kptsTo[i]);
wordsDescTo.push_back(descriptorsTo.row(i));
if(!kptsTo3D.empty())
{
words3To.insert(words3To.end(), std::make_pair(newToId, kptsTo3D[i]));
words3To.push_back(kptsTo3D[i]);
}
++newToId;
}
@@ -1271,15 +1282,16 @@ Transform RegistrationVis::computeTransformationImpl(
{
if(fromWordIdsSet.count(*iter) == 1)
{
if (kptsFrom.size())
wordsFrom.insert(wordsFrom.end(), std::make_pair(*iter, wordsFrom.size()));
if (!kptsFrom.empty())
{
wordsFrom.insert(std::make_pair(*iter, kptsFrom[i]));
wordsKptsFrom.push_back(kptsFrom[i]);
}
if(kptsFrom3D.size())
if(!kptsFrom3D.empty())
{
words3From.insert(std::make_pair(*iter, kptsFrom3D[i]));
words3From.push_back(kptsFrom3D[i]);
}
wordsDescFrom.insert(std::make_pair(*iter, descriptorsFrom.row(i)));
wordsDescFrom.push_back(descriptorsFrom.row(i));
}
++i;
}
@@ -1291,11 +1303,12 @@ Transform RegistrationVis::computeTransformationImpl(
{
if(toWordIdsSet.count(*iter) == 1)
{
wordsTo.insert(std::make_pair(*iter, kptsTo[i]));
wordsDescTo.insert(std::make_pair(*iter, descriptorsTo.row(i)));
if(kptsTo3D.size())
wordsTo.insert(wordsTo.end(), std::make_pair(*iter, wordsTo.size()));
wordsKptsTo.push_back(kptsTo[i]);
wordsDescTo.push_back(descriptorsTo.row(i));
if(!kptsTo3D.empty())
{
words3To.insert(std::make_pair(*iter, kptsTo3D[i]));
words3To.push_back(kptsTo3D[i]);
}
}
++i;
@@ -1308,21 +1321,19 @@ Transform RegistrationVis::computeTransformationImpl(
UASSERT(kptsFrom3D.empty() || int(kptsFrom3D.size()) == descriptorsFrom.rows);
for(int i=0; i<descriptorsFrom.rows; ++i)
{
wordsFrom.insert(std::make_pair(i, kptsFrom[i]));
wordsDescFrom.insert(std::make_pair(i, descriptorsFrom.row(i)));
if(kptsFrom3D.size())
wordsFrom.insert(wordsFrom.end(), std::make_pair(i, wordsFrom.size()));
wordsKptsFrom.push_back(kptsFrom[i]);
wordsDescFrom.push_back(descriptorsFrom.row(i));
if(!kptsFrom3D.empty())
{
words3From.insert(std::make_pair(i, kptsFrom3D[i]));
words3From.push_back(kptsFrom3D[i]);
}
}
}
}
fromSignature.setWords(wordsFrom);
fromSignature.setWords3(words3From);
fromSignature.setWordsDescriptors(wordsDescFrom);
toSignature.setWords(wordsTo);
toSignature.setWords3(words3To);
toSignature.setWordsDescriptors(wordsDescTo);
fromSignature.setWords(wordsFrom, wordsKptsFrom, words3From, wordsDescFrom);
toSignature.setWords(wordsTo, wordsKptsTo, words3To, wordsDescTo);
}
/////////////////////
@@ -1376,14 +1387,31 @@ Transform RegistrationVis::computeTransformationImpl(
Transform cameraTransform;
double variance = 1.0f;
std::vector<int> matchesV;
std::map<int, int> uniqueWordsA = uMultimapToMapUnique(signatureA->getWords());
std::map<int, int> uniqueWordsB = uMultimapToMapUnique(signatureB->getWords());
std::map<int, cv::KeyPoint> wordsA;
std::map<int, cv::Point3f> words3A;
std::map<int, cv::KeyPoint> wordsB;
for(std::map<int, int>::iterator iter=uniqueWordsA.begin(); iter!=uniqueWordsA.end(); ++iter)
{
wordsA.insert(std::make_pair(iter->first, signatureA->getWordsKpts()[iter->second]));
if(!signatureA->getWords3().empty())
{
words3A.insert(std::make_pair(iter->first, signatureA->getWords3()[iter->second]));
}
}
for(std::map<int, int>::iterator iter=uniqueWordsB.begin(); iter!=uniqueWordsB.end(); ++iter)
{
wordsB.insert(std::make_pair(iter->first, signatureB->getWordsKpts()[iter->second]));
}
std::map<int, cv::Point3f> inliers3D = util3d::generateWords3DMono(
uMultimapToMapUnique(signatureA->getWords()),
uMultimapToMapUnique(signatureB->getWords()),
wordsA,
wordsB,
cameraModel,
cameraTransform,
_PnPReprojError,
0.99f,
uMultimapToMapUnique(signatureA->getWords3()), // for scale estimation
words3A, // for scale estimation
&variance,
&matchesV);
covariances[dir] *= variance;
@@ -1459,9 +1487,26 @@ Transform RegistrationVis::computeTransformationImpl(
std::vector<int> inliersV;
std::vector<int> matchesV;
std::map<int, int> uniqueWordsA = uMultimapToMapUnique(signatureA->getWords());
std::map<int, int> uniqueWordsB = uMultimapToMapUnique(signatureB->getWords());
std::map<int, cv::Point3f> words3A;
std::map<int, cv::Point3f> words3B;
std::map<int, cv::KeyPoint> wordsB;
for(std::map<int, int>::iterator iter=uniqueWordsA.begin(); iter!=uniqueWordsA.end(); ++iter)
{
words3A.insert(std::make_pair(iter->first, signatureA->getWords3()[iter->second]));
}
for(std::map<int, int>::iterator iter=uniqueWordsB.begin(); iter!=uniqueWordsB.end(); ++iter)
{
wordsB.insert(std::make_pair(iter->first, signatureB->getWordsKpts()[iter->second]));
if(!signatureB->getWords3().empty())
{
words3B.insert(std::make_pair(iter->first, signatureB->getWords3()[iter->second]));
}
}
transforms[dir] = util3d::estimateMotion3DTo2D(
uMultimapToMapUnique(signatureA->getWords3()),
uMultimapToMapUnique(signatureB->getWords()),
words3A,
wordsB,
cameraModel,
_minInliers,
_iterations,
@@ -1469,7 +1514,7 @@ Transform RegistrationVis::computeTransformationImpl(
_PnPFlags,
_PnPRefineIterations,
dir==0?(!guess.isNull()?guess:Transform::getIdentity()):!transforms[0].isNull()?transforms[0].inverse():(!guess.isNull()?guess.inverse():Transform::getIdentity()),
uMultimapToMapUnique(signatureB->getWords3()),
words3B,
&covariances[dir],
&matchesV,
&inliersV);
@@ -1505,9 +1550,21 @@ Transform RegistrationVis::computeTransformationImpl(
{
std::vector<int> inliersV;
std::vector<int> matchesV;
std::map<int, int> uniqueWordsA = uMultimapToMapUnique(signatureA->getWords());
std::map<int, int> uniqueWordsB = uMultimapToMapUnique(signatureB->getWords());
std::map<int, cv::Point3f> words3A;
std::map<int, cv::Point3f> words3B;
for(std::map<int, int>::iterator iter=uniqueWordsA.begin(); iter!=uniqueWordsA.end(); ++iter)
{
words3A.insert(std::make_pair(iter->first, signatureA->getWords3()[iter->second]));
}
for(std::map<int, int>::iterator iter=uniqueWordsB.begin(); iter!=uniqueWordsB.end(); ++iter)
{
words3B.insert(std::make_pair(iter->first, signatureB->getWords3()[iter->second]));
}
transforms[dir] = util3d::estimateMotion3DTo3D(
uMultimapToMapUnique(signatureA->getWords3()),
uMultimapToMapUnique(signatureB->getWords3()),
words3A,
words3B,
_minInliers,
_inlierDistance,
_iterations,
@@ -1679,7 +1736,8 @@ Transform RegistrationVis::computeTransformationImpl(
for(unsigned int i=0; i<allInliers.size(); ++i)
{
int wordId = allInliers[i];
const cv::Point3f & pt3D = fromSignature.getWords3().find(wordId)->second;
int indexFrom = fromSignature.getWords().find(wordId)->second;
const cv::Point3f & pt3D = fromSignature.getWords3()[indexFrom];
if(!util3d::isFinite(pt3D))
{
UASSERT_MSG(!_forwardEstimateOnly, uFormat("3D point %d is not finite!?", wordId).c_str());
@@ -1690,20 +1748,21 @@ Transform RegistrationVis::computeTransformationImpl(
points3DMap.insert(std::make_pair(wordId, pt3D));
std::map<int, FeatureBA> ptMap;
if(fromSignature.getWords().size() && cameraModelFrom.isValidForProjection())
if(!fromSignature.getWordsKpts().empty() && cameraModelFrom.isValidForProjection())
{
float depthFrom = util3d::transformPoint(pt3D, invLocalTransformFrom).z;
const cv::KeyPoint & kpt = fromSignature.getWords().find(wordId)->second;
const cv::KeyPoint & kpt = fromSignature.getWordsKpts()[indexFrom];
ptMap.insert(std::make_pair(1,FeatureBA(kpt, depthFrom)));
}
if(toSignature.getWords().size() && cameraModelTo.isValidForProjection())
if(!toSignature.getWordsKpts().empty() && cameraModelTo.isValidForProjection())
{
int indexTo = toSignature.getWords().find(wordId)->second;
float depthTo = 0.0f;
if(toSignature.getWords3().find(wordId) != toSignature.getWords3().end())
if(!toSignature.getWords3().empty())
{
depthTo = util3d::transformPoint(toSignature.getWords3().find(wordId)->second, invLocalTransformTo).z;
depthTo = util3d::transformPoint(toSignature.getWords3()[indexTo], invLocalTransformTo).z;
}
const cv::KeyPoint & kpt = toSignature.getWords().find(wordId)->second;
const cv::KeyPoint & kpt = toSignature.getWordsKpts()[indexTo];
ptMap.insert(std::make_pair(2,FeatureBA(kpt, depthTo)));
}
@@ -1841,24 +1900,25 @@ Transform RegistrationVis::computeTransformationImpl(
{
if(_maxInliersMeanDistance>0.0f)
{
std::multimap<int, cv::Point3f>::const_iterator words3Iter = fromSignature.getWords3().find(allInliers[i]);
if(words3Iter != fromSignature.getWords3().end())
std::multimap<int, int>::const_iterator wordsIter = fromSignature.getWords().find(allInliers[i]);
if(wordsIter != fromSignature.getWords().end() && !fromSignature.getWords3().empty())
{
if(uIsFinite(words3Iter->second.x))
const cv::Point3f & pt = fromSignature.getWords3()[wordsIter->second];
if(uIsFinite(pt.x))
{
cv::Point3f pt = util3d::transformPoint(words3Iter->second, transformInv);
distances.push_back(pt.x);
distances.push_back(util3d::transformPoint(pt, transformInv).x);
}
}
}
if(!pcaData.empty())
{
std::multimap<int, cv::KeyPoint>::const_iterator wordsIter = fromSignature.getWords().find(allInliers[i]);
UASSERT(wordsIter != fromSignature.getWords().end());
std::multimap<int, int>::const_iterator wordsIter = fromSignature.getWords().find(allInliers[i]);
UASSERT(wordsIter != fromSignature.getWords().end() && !fromSignature.getWordsKpts().empty());
float * ptr = pcaData.ptr<float>(i, 0);
ptr[0] = (wordsIter->second.pt.x-cx) / w;
ptr[1] = (wordsIter->second.pt.y-cy) / h;
const cv::KeyPoint & kpt = fromSignature.getWordsKpts()[wordsIter->second];
ptr[0] = (kpt.pt.x-cx) / w;
ptr[1] = (kpt.pt.y-cy) / h;
}
}
@@ -1899,6 +1959,7 @@ Transform RegistrationVis::computeTransformationImpl(
}
info.inliers = inliersCount;
info.inliersRatio = !toSignature.getWords().empty()?float(inliersCount)/float(toSignature.getWords().size()):0;
info.matches = matchesCount;
info.rejectedMsg = msg;
info.covariance = covariance;
corelib/src/Rtabmap.cpp
+33 -40
View File
@@ -678,19 +678,6 @@ int Rtabmap::getTotalMemSize() const
return 0;
}
std::multimap<int, cv::KeyPoint> Rtabmap::getWords(int locationId) const
{
if(_memory)
{
const Signature * s = _memory->getSignature(locationId);
if(s)
{
return s->getWords();
}
}
return std::multimap<int, cv::KeyPoint>();
}
bool Rtabmap::isInSTM(int locationId) const
{
if(_memory)
@@ -713,25 +700,7 @@ const Statistics & Rtabmap::getStatistics() const
{
return statistics_;
}
/*
bool Rtabmap::getMetricData(int locationId, cv::Mat & rgb, cv::Mat & depth, float & depthConstant, Transform & pose, Transform & localTransform) const
{
if(_memory)
{
const Signature * s = _memory->getSignature(locationId);
if(s && _optimizedPoses.find(s->id()) != _optimizedPoses.end())
{
rgb = s->getImage();
depth = s->getDepth();
depthConstant = s->getDepthConstant();
pose = _optimizedPoses.at(s->id());
localTransform = s->getLocalTransform();
return true;
}
}
return false;
}
*/
Transform Rtabmap::getPose(int locationId) const
{
return uValue(_optimizedPoses, locationId, Transform());
@@ -2235,6 +2204,7 @@ bool Rtabmap::process(
//============================================================
std::list<std::pair<int, int> > loopClosureLinksAdded;
int loopClosureVisualInliers = 0; // for statistics
float loopClosureVisualInliersRatio = 0.0f;
int loopClosureVisualMatches = 0;
float loopClosureLinearVariance = 0.0f;
float loopClosureAngularVariance = 0.0f;
@@ -2386,6 +2356,7 @@ bool Rtabmap::process(
lastProximitySpaceClosureId = nearestId;
loopClosureVisualInliers = info.inliers;
loopClosureVisualInliersRatio = info.inliersRatio;
loopClosureVisualMatches = info.matches;
loopClosureLinearVariance = 1.0/information.at<double>(0,0);
@@ -2589,6 +2560,7 @@ bool Rtabmap::process(
loopClosureVisualInliersDistribution = info.inliersDistribution;
loopClosureVisualInliers = info.inliers;
loopClosureVisualInliersRatio = info.inliersRatio;
loopClosureVisualMatches = info.matches;
rejectedGlobalLoopClosure = transform.isNull();
if(rejectedGlobalLoopClosure)
@@ -3167,6 +3139,7 @@ bool Rtabmap::process(
statistics_.addStatistic(Statistics::kLoopReactivate_id(), retrievalId);
statistics_.addStatistic(Statistics::kLoopHypothesis_ratio(), hypothesisRatio);
statistics_.addStatistic(Statistics::kLoopVisual_inliers(), loopClosureVisualInliers);
statistics_.addStatistic(Statistics::kLoopVisual_inliers_ratio(), loopClosureVisualInliersRatio);
statistics_.addStatistic(Statistics::kLoopVisual_matches(), loopClosureVisualMatches);
statistics_.addStatistic(Statistics::kLoopLinear_variance(), loopClosureLinearVariance);
statistics_.addStatistic(Statistics::kLoopAngular_variance(), loopClosureAngularVariance);
@@ -3315,6 +3288,13 @@ bool Rtabmap::process(
if(_publishRAMUsage)
{
statistics_.addStatistic(Statistics::kMemoryRAM_usage(), UProcessInfo::getMemoryUsage()/(1024*1024));
long estimatedMemoryUsage = sizeof(Rtabmap);
estimatedMemoryUsage += _optimizedPoses.size() * (sizeof(int) + sizeof(Transform) + 12 * sizeof(float) + sizeof(std::_Rb_tree_node_base)) + sizeof(std::map<int, Transform>);
estimatedMemoryUsage += _constraints.size() * (sizeof(int) + sizeof(Transform) + 12 * sizeof(float) + sizeof(cv::Mat) + 36 * sizeof(double) + sizeof(std::_Rb_tree_node_base)) + sizeof(std::map<int, Link>);
estimatedMemoryUsage += _memory->getMemoryUsed();
estimatedMemoryUsage += _bayesFilter->getMemoryUsed();
estimatedMemoryUsage += _parameters.size()*(sizeof(std::string)*2+sizeof(std::_Rb_tree_node_base)) + sizeof(ParametersMap);
statistics_.addStatistic(Statistics::kMemoryRAM_estimated(), (float)(estimatedMemoryUsage/(1024*1024)));//MB
}
if(_publishLikelihood || _publishPdf)
@@ -4463,18 +4443,30 @@ Signature Rtabmap::getSignatureCopy(int id, bool images, bool scan, bool userDat
groundTruth,
data);
std::multimap<int, Link> links = _memory->getLinks(id, true, true);
for(std::multimap<int, Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
{
if(iter->second.type() == Link::kLandmark)
{
s.addLandmark(iter->second);
}
else
{
s.addLink(iter->second);
}
}
if(withWords || withGlobalDescriptors)
{
std::multimap<int, cv::KeyPoint> words;
std::multimap<int, cv::Point3f> words3;
std::multimap<int, cv::Mat> wordsDescriptors;
std::multimap<int, int> words;
std::vector<cv::KeyPoint> wordsKpts;
std::vector<cv::Point3f> words3;
cv::Mat wordsDescriptors;
std::vector<rtabmap::GlobalDescriptor> globalDescriptors;
_memory->getNodeWordsAndGlobalDescriptors(id, words, words3, wordsDescriptors, globalDescriptors);
_memory->getNodeWordsAndGlobalDescriptors(id, words, wordsKpts, words3, wordsDescriptors, globalDescriptors);
if(withWords)
{
s.setWords(words);
s.setWords3(words3);
s.setWordsDescriptors(wordsDescriptors);
s.setWords(words, wordsKpts, words3, wordsDescriptors);
}
if(withGlobalDescriptors)
{
@@ -4620,7 +4612,7 @@ int Rtabmap::detectMoreLoopClosures(
std::map<int, Transform> posesToCheckLoopClosures;
std::map<int, Transform> poses;
std::multimap<int, Link> links;
std::map<int, Signature> signatures;
std::map<int, Signature> signatures; // some signatures may be in LTM, get them all
this->getGraph(poses, links, true, true, &signatures);
std::map<int, int> mapIds;
@@ -4706,6 +4698,7 @@ int Rtabmap::detectMoreLoopClosures(
if(!t.isNull())
{
UWARN(t.prettyPrint().c_str());
bool updateConstraints = true;
if(_optimizationMaxError > 0.0f)
{
corelib/src/SensorData.cpp
+5 -4
View File
@@ -763,9 +763,10 @@ void SensorData::setFeatures(const std::vector<cv::KeyPoint> & keypoints, const
_descriptors = descriptors;
}
long SensorData::getMemoryUsed() const // Return memory usage in Bytes
unsigned long SensorData::getMemoryUsed() const // Return memory usage in Bytes
{
return _imageCompressed.total()*_imageCompressed.elemSize() +
return sizeof(SensorData) +
_imageCompressed.total()*_imageCompressed.elemSize() +
_imageRaw.total()*_imageRaw.elemSize() +
_depthOrRightCompressed.total()*_depthOrRightCompressed.elemSize() +
_depthOrRightRaw.total()*_depthOrRightRaw.elemSize() +
@@ -779,8 +780,8 @@ long SensorData::getMemoryUsed() const // Return memory usage in Bytes
_obstacleCellsRaw.total()*_obstacleCellsRaw.elemSize()+
_emptyCellsCompressed.total()*_emptyCellsCompressed.elemSize() +
_emptyCellsRaw.total()*_emptyCellsRaw.elemSize()+
_keypoints.size() * sizeof(float) * 7 +
_keypoints3D.size() * sizeof(float)*3 +
_keypoints.size() * sizeof(cv::KeyPoint) +
_keypoints3D.size() * sizeof(cv::Point3f) +
_descriptors.total()*_descriptors.elemSize();
}
corelib/src/Signature.cpp
+72 -47
View File
@@ -132,11 +132,24 @@ bool Signature::hasLink(int idTo, Link::Type type) const
{
return _links.find(idTo) != _links.end();
}
for(std::multimap<int, Link>::const_iterator iter=_links.find(idTo); iter!=_links.end() && iter->first == idTo; ++iter)
if(idTo==0)
{
if(type == iter->second.type())
for(std::multimap<int, Link>::const_iterator iter=_links.begin(); iter!=_links.end(); ++iter)
{
return true;
if(type == iter->second.type())
{
return true;
}
}
}
else
{
for(std::multimap<int, Link>::const_iterator iter=_links.find(idTo); iter!=_links.end() && iter->first == idTo; ++iter)
{
if(type == iter->second.type())
{
return true;
}
}
}
return false;
@@ -209,11 +222,11 @@ void Signature::removeVirtualLinks()
float Signature::compareTo(const Signature & s) const
{
float similarity = 0.0f;
const std::multimap<int, cv::KeyPoint> & words = s.getWords();
const std::multimap<int, int> & words = s.getWords();
if(!s.isBadSignature() && !this->isBadSignature())
{
std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > pairs;
std::list<std::pair<int, std::pair<int, int> > > pairs;
int totalWords = ((int)_words.size()-_invalidWordsCount)>((int)words.size()-s.getInvalidWordsCount())?((int)_words.size()-_invalidWordsCount):((int)words.size()-s.getInvalidWordsCount());
UASSERT(totalWords > 0);
EpipolarGeometry::findPairs(words, _words, pairs);
@@ -225,11 +238,9 @@ float Signature::compareTo(const Signature & s) const
void Signature::changeWordsRef(int oldWordId, int activeWordId)
{
std::list<cv::KeyPoint> kps = uValues(_words, oldWordId);
if(kps.size())
std::list<int> words = uValues(_words, oldWordId);
if(words.size())
{
std::list<cv::Point3f> pts = uValues(_words3, oldWordId);
std::list<cv::Mat> descriptors = uValues(_wordsDescriptors, oldWordId);
if(oldWordId<=0)
{
_invalidWordsCount-=(int)_words.erase(oldWordId);
@@ -239,37 +250,41 @@ void Signature::changeWordsRef(int oldWordId, int activeWordId)
{
_words.erase(oldWordId);
}
_words3.erase(oldWordId);
_wordsDescriptors.erase(oldWordId);
_wordsChanged.insert(std::make_pair(oldWordId, activeWordId));
for(std::list<cv::KeyPoint>::const_iterator iter=kps.begin(); iter!=kps.end(); ++iter)
for(std::list<int>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
{
_words.insert(std::pair<int, cv::KeyPoint>(activeWordId, (*iter)));
}
for(std::list<cv::Point3f>::const_iterator iter=pts.begin(); iter!=pts.end(); ++iter)
{
_words3.insert(std::pair<int, cv::Point3f>(activeWordId, (*iter)));
}
for(std::list<cv::Mat>::const_iterator iter=descriptors.begin(); iter!=descriptors.end(); ++iter)
{
_wordsDescriptors.insert(std::pair<int, cv::Mat>(activeWordId, (*iter)));
_words.insert(std::pair<int, int>(activeWordId, (*iter)));
}
}
}
void Signature::setWords(const std::multimap<int, cv::KeyPoint> & words)
void Signature::setWords(const std::multimap<int, int> & words,
const std::vector<cv::KeyPoint> & keypoints,
const std::vector<cv::Point3f> & points,
const cv::Mat & descriptors)
{
UASSERT_MSG(descriptors.empty() || descriptors.rows == (int)words.size(), uFormat("words=%d, descriptors=%d", (int)words.size(), descriptors.rows).c_str());
UASSERT_MSG(points.empty() || points.size() == words.size(), uFormat("words=%d, points=%d", (int)words.size(), (int)points.size()).c_str());
UASSERT_MSG(keypoints.empty() || keypoints.size() == words.size(), uFormat("words=%d, descriptors=%d", (int)words.size(), (int)keypoints.size()).c_str());
UASSERT(words.empty() || !keypoints.empty() || !points.empty() || !descriptors.empty());
_invalidWordsCount = 0;
for(std::multimap<int, int>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
{
if(iter->first<=0)
{
++_invalidWordsCount;
}
// make sure indexes are all valid!
UASSERT_MSG(iter->second >=0 && iter->second < (int)words.size(), uFormat("iter->second=%d words.size()=%d", iter->second, (int)words.size()).c_str());
}
_enabled = false;
_words = words;
_invalidWordsCount = 0;
for(std::multimap<int, cv::KeyPoint>::iterator iter=_words.begin(); iter!=_words.end(); ++iter)
{
if(iter->first>0)
{
break;
}
++_invalidWordsCount;
}
_wordsKpts = keypoints;
_words3 = points;
_wordsDescriptors = descriptors.clone();
}
bool Signature::isBadSignature() const
@@ -280,24 +295,30 @@ bool Signature::isBadSignature() const
void Signature::removeAllWords()
{
_words.clear();
_wordsKpts.clear();
_words3.clear();
_wordsDescriptors.clear();
_wordsDescriptors = cv::Mat();
_invalidWordsCount = 0;
}
void Signature::removeWord(int wordId)
void Signature::setWordsDescriptors(const cv::Mat & descriptors)
{
if(wordId<=0)
if(descriptors.empty())
{
_invalidWordsCount-=(int)_words.erase(wordId);
UASSERT(_invalidWordsCount>=0);
if(_wordsKpts.empty() && _words3.empty())
{
removeAllWords();
}
else
{
_wordsDescriptors = cv::Mat();
}
}
else
{
_words.erase(wordId);
UASSERT(descriptors.rows == (int)_words.size());
_wordsDescriptors = descriptors.clone();
}
_words3.erase(wordId);
_wordsDescriptors.clear();
}
cv::Mat Signature::getPoseCovariance() const
@@ -321,19 +342,23 @@ cv::Mat Signature::getPoseCovariance() const
return covariance;
}
long Signature::getMemoryUsed(bool withSensorData) const // Return memory usage in Bytes
unsigned long Signature::getMemoryUsed(bool withSensorData) const // Return memory usage in Bytes
{
long total = _words.size() * sizeof(float) * 8 +
_words3.size() * sizeof(float) * 4;
if(!_wordsDescriptors.empty())
{
total += _wordsDescriptors.size() * sizeof(int);
total += _wordsDescriptors.size() * _wordsDescriptors.begin()->second.total() * _wordsDescriptors.begin()->second.elemSize();
}
unsigned long total = sizeof(Signature);
total += _words.size() * (sizeof(int)*2+sizeof(std::_Rb_tree_node_base)) + sizeof(std::multimap<int, cv::KeyPoint>);
total += _wordsKpts.size() * sizeof(cv::KeyPoint) + sizeof(std::vector<cv::KeyPoint>);
total += _words3.size() * sizeof(cv::Point3f) + sizeof(std::vector<cv::Point3f>);
total += _wordsDescriptors.total() * _wordsDescriptors.elemSize() + sizeof(cv::Mat);
total += _wordsChanged.size() * (sizeof(int)*2+sizeof(std::_Rb_tree_node_base)) + sizeof(std::map<int, int>);
if(withSensorData)
{
total+=_sensorData.getMemoryUsed();
}
total += _pose.size() * (sizeof(Transform) + sizeof(float)*12);
total += _groundTruthPose.size() * (sizeof(Transform) + sizeof(float)*12);
total += _velocity.size() * sizeof(float);
total += _links.size() * (sizeof(int) + sizeof(Transform) + 12 * sizeof(float) + sizeof(cv::Mat) + 36 * sizeof(double)+sizeof(std::_Rb_tree_node_base)) + sizeof(std::multimap<int, Link>);
total += _landmarks.size() * (sizeof(int) + sizeof(Transform) + 12 * sizeof(float) + sizeof(cv::Mat) + 36 * sizeof(double)+sizeof(std::_Rb_tree_node_base)) + sizeof(std::map<int, Link>);
return total;
}
corelib/src/VWDictionary.cpp
+41 -5
View File
@@ -331,7 +331,10 @@ bool VWDictionary::setNNStrategy(NNStrategy strategy)
_strategy = strategy;
if(update)
{
UINFO("Nearest neighbor strategy has changed, re-initialize search tree.");
if(_notIndexedWords.size() != _visualWords.size() || !_dataTree.empty())
{
UINFO("Nearest neighbor strategy has changed, re-initialize search tree.");
}
_dataTree = cv::Mat();
_notIndexedWords = uKeysSet(_visualWords);
_removedIndexedWords.clear();
@@ -363,6 +366,38 @@ unsigned int VWDictionary::getIndexMemoryUsed() const
return _flannIndex->memoryUsed();
}
unsigned long VWDictionary::getMemoryUsed(bool estimate) const
{
long memoryUsage = sizeof(VWDictionary);
memoryUsage += getIndexMemoryUsed();
memoryUsage += _dataTree.total()*_dataTree.elemSize();
if(estimate)
{
if(!_visualWords.empty())
{
memoryUsage += _visualWords.size()*(sizeof(int) + _visualWords.begin()->second->getMemoryUsed()+sizeof(std::_Rb_tree_node_base)) + sizeof(std::map<int, VisualWord *>);
}
}
else
{
for(std::map<int, VisualWord *>::const_iterator iter=_visualWords.begin(); iter!=_visualWords.end(); ++iter)
{
memoryUsage += sizeof(int) + iter->second->getMemoryUsed();
}
memoryUsage += _visualWords.size()*(sizeof(std::_Rb_tree_node_base)) + sizeof(std::map<int, VisualWord *>);
}
if(!_unusedWords.empty())
{
// they are the same words than in _visualWords, so just add the pointer size
memoryUsage += _unusedWords.size()*(sizeof(int) + sizeof(VisualWord *)+sizeof(std::_Rb_tree_node_base)) + sizeof(std::map<int, VisualWord *>);
}
memoryUsage += _mapIndexId.size() * (sizeof(int)*2+sizeof(std::_Rb_tree_node_base)) + sizeof(std::map<int ,int>);
memoryUsage += _mapIdIndex.size() * (sizeof(int)*2+sizeof(std::_Rb_tree_node_base)) + sizeof(std::map<int ,int>);
memoryUsage += _notIndexedWords.size() * (sizeof(int)+sizeof(std::_Rb_tree_node_base)) + sizeof(std::set<int>);
memoryUsage += _removedIndexedWords.size() * (sizeof(int)+sizeof(std::_Rb_tree_node_base)) + sizeof(std::set<int>);
return memoryUsage;
}
cv::Mat VWDictionary::convertBinTo32F(const cv::Mat & descriptorsIn, bool byteToFloat)
{
if(byteToFloat)
@@ -521,7 +556,8 @@ void VWDictionary::update()
{
UASSERT(descriptor.cols == _flannIndex->featuresDim());
UASSERT(descriptor.type() == _flannIndex->featuresType());
index = _flannIndex->addPoints(descriptor);
UASSERT(descriptor.rows == 1);
index = _flannIndex->addPoints(descriptor).front();
}
std::pair<std::map<int, int>::iterator, bool> inserted;
inserted = _mapIndexId.insert(std::pair<int, int>(index, w->id()));
@@ -628,15 +664,15 @@ void VWDictionary::update()
switch(_strategy)
{
case kNNFlannNaive:
_flannIndex->buildLinearIndex(_dataTree, useDistanceL1_, _rebalancingFactor);
_flannIndex->buildLinearIndex(_dataTree, useDistanceL1_, _incrementalDictionary&&_incrementalFlann?_rebalancingFactor:1);
break;
case kNNFlannKdTree:
UASSERT_MSG(type == CV_32F, "To use KdTree dictionary, float descriptors are required!");
_flannIndex->buildKDTreeIndex(_dataTree, KDTREE_SIZE, useDistanceL1_, _rebalancingFactor);
_flannIndex->buildKDTreeIndex(_dataTree, KDTREE_SIZE, useDistanceL1_, _incrementalDictionary&&_incrementalFlann?_rebalancingFactor:1);
break;
case kNNFlannLSH:
UASSERT_MSG(type == CV_8U, "To use LSH dictionary, binary descriptors are required!");
_flannIndex->buildLSHIndex(_dataTree, 12, 20, 2, _rebalancingFactor);
_flannIndex->buildLSHIndex(_dataTree, 12, 20, 2, _incrementalDictionary&&_incrementalFlann?_rebalancingFactor:1);
break;
default:
break;
corelib/src/VisualWord.cpp
+9
View File
@@ -69,4 +69,13 @@ int VisualWord::removeAllRef(int signatureId)
return removed;
}
unsigned long VisualWord::getMemoryUsed() const
{
unsigned long memoryUsage = sizeof(VisualWord);
memoryUsage += _references.size() * (sizeof(int)*2+sizeof(std::_Rb_tree_node_base)) + sizeof(std::map<int ,int>);
memoryUsage += _oldReferences.size() * (sizeof(int)*2+sizeof(std::_Rb_tree_node_base)) + sizeof(std::map<int ,int>);
memoryUsage += _descriptor.total() * _descriptor.elemSize();
return memoryUsage;
}
} // namespace rtabmap
corelib/src/odometry/OdometryF2F.cpp
+19 -14
View File
@@ -137,9 +137,7 @@ Transform OdometryF2F::computeTransform(
{
tmpRefFrame = refFrame_;
// reset matches, but keep already extracted features in newFrame.sensorData()
newFrame.setWords(std::multimap<int, cv::KeyPoint>());
newFrame.setWords3(std::multimap<int, cv::Point3f>());
newFrame.setWordsDescriptors(std::multimap<int, cv::Mat>());
newFrame.removeAllWords();
UWARN("Failed to find a transformation with the provided guess (%s), trying again without a guess.", guess.prettyPrint().c_str());
// If optical flow is used, switch temporary to feature matching
int visCorTypeBackup = Parameters::defaultVisCorType();
@@ -176,18 +174,18 @@ Transform OdometryF2F::computeTransform(
if(info && this->isInfoDataFilled())
{
std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > pairs;
std::list<std::pair<int, std::pair<int, int> > > pairs;
EpipolarGeometry::findPairsUnique(tmpRefFrame.getWords(), newFrame.getWords(), pairs);
info->refCorners.resize(pairs.size());
info->newCorners.resize(pairs.size());
std::map<int, int> idToIndex;
int i=0;
for(std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > >::iterator iter=pairs.begin();
for(std::list<std::pair<int, std::pair<int, int> > >::iterator iter=pairs.begin();
iter!=pairs.end();
++iter)
{
info->refCorners[i] = iter->second.first.pt;
info->newCorners[i] = iter->second.second.pt;
info->refCorners[i] = tmpRefFrame.getWordsKpts()[iter->second.first].pt;
info->newCorners[i] = newFrame.getWordsKpts()[iter->second.second].pt;
idToIndex.insert(std::make_pair(iter->first, i));
++i;
}
@@ -199,12 +197,21 @@ Transform OdometryF2F::computeTransform(
}
Transform t = this->getPose()*motionSinceLastKeyFrame.inverse();
for(std::multimap<int, cv::Point3f>::const_iterator iter=tmpRefFrame.getWords3().begin(); iter!=tmpRefFrame.getWords3().end(); ++iter)
if(!tmpRefFrame.getWords3().empty())
{
info->localMap.insert(std::make_pair(iter->first, util3d::transformPoint(iter->second, t)));
for(std::multimap<int, int>::const_iterator iter=tmpRefFrame.getWords().begin(); iter!=tmpRefFrame.getWords().end(); ++iter)
{
info->localMap.insert(std::make_pair(iter->first, util3d::transformPoint(tmpRefFrame.getWords3()[iter->second], t)));
}
}
info->localMapSize = tmpRefFrame.getWords3().size();
info->words = newFrame.getWords();
if(!newFrame.getWordsKpts().empty())
{
for(std::multimap<int, int>::const_iterator iter=newFrame.getWords().begin(); iter!=newFrame.getWords().end(); ++iter)
{
info->words.insert(std::make_pair(iter->first, newFrame.getWordsKpts()[iter->second]));
}
}
info->localScanMapSize = tmpRefFrame.sensorData().laserScanRaw().size();
@@ -232,7 +239,7 @@ Transform OdometryF2F::computeTransform(
(registrationPipeline_->isScanRequired() && (scanKeyFrameThr_ == 0.0f || regInfo.icpInliersRatio <= scanKeyFrameThr_)))
{
UDEBUG("Update key frame");
int features = newFrame.getWordsDescriptors().size();
int features = newFrame.getWordsDescriptors().rows;
if(registrationPipeline_->isImageRequired() && features == 0)
{
newFrame = Signature(data);
@@ -251,9 +258,7 @@ Transform OdometryF2F::computeTransform(
{
refFrame_ = newFrame;
refFrame_.setWords(std::multimap<int, cv::KeyPoint>());
refFrame_.setWords3(std::multimap<int, cv::Point3f>());
refFrame_.setWordsDescriptors(std::multimap<int, cv::Mat>());
refFrame_.removeAllWords();
//reset motion
lastKeyFramePose_.setNull();
corelib/src/odometry/OdometryF2M.cpp
+160 -95
View File
@@ -133,6 +133,27 @@ OdometryF2M::OdometryF2M(const ParametersMap & parameters) :
}
uInsert(bundleParameters, ParametersPair(Parameters::kVisCorType(), uNumber2Str(corType)));
int estType = Parameters::defaultVisEstimationType();
Parameters::parse(parameters, Parameters::kVisEstimationType(), estType);
if(estType > 1)
{
UWARN("%s=%d is not supported by OdometryF2M, using 2D->3D approach instead (type=1).",
Parameters::kVisEstimationType().c_str(),
estType);
estType = 1;
}
uInsert(bundleParameters, ParametersPair(Parameters::kVisEstimationType(), uNumber2Str(estType)));
bool forwardEst = Parameters::defaultVisForwardEstOnly();
Parameters::parse(parameters, Parameters::kVisForwardEstOnly(), forwardEst);
if(!forwardEst)
{
UWARN("%s=false is not supported by OdometryF2M, setting to true.",
Parameters::kVisForwardEstOnly().c_str());
forwardEst = true;
}
uInsert(bundleParameters, ParametersPair(Parameters::kVisForwardEstOnly(), uBool2Str(forwardEst)));
regPipeline_ = Registration::create(bundleParameters);
if(bundleAdjustment_>0 && regPipeline_->isScanRequired())
{
@@ -272,9 +293,7 @@ Transform OdometryF2M::computeTransform(
{
tmpMap = *map_;
// reset matches, but keep already extracted features in lastFrame_->sensorData()
lastFrame_->setWords(std::multimap<int, cv::KeyPoint>());
lastFrame_->setWords3(std::multimap<int, cv::Point3f>());
lastFrame_->setWordsDescriptors(std::multimap<int, cv::Mat>());
lastFrame_->removeAllWords();
points3DMap.clear();
bundlePoses.clear();
@@ -393,11 +412,9 @@ Transform OdometryF2M::computeTransform(
int wordId =regInfo.inliersIDs[i];
// 3D point
std::multimap<int, cv::Point3f>::const_iterator iter3D = tmpMap.getWords3().find(wordId);
UASSERT(iter3D!=tmpMap.getWords3().end());
points3DMap.insert(*iter3D);
std::multimap<int, cv::KeyPoint>::const_iterator iter2D = lastFrame_->getWords().find(wordId);
std::multimap<int, int>::const_iterator iter3D = tmpMap.getWords().find(wordId);
UASSERT(iter3D!=tmpMap.getWords().end() && !tmpMap.getWords3().empty());
points3DMap.insert(std::make_pair(wordId, tmpMap.getWords3()[iter3D->second]));
// all other references
std::map<int, std::map<int, FeatureBA> >::iterator refIter = bundleWordReferences_.find(wordId);
@@ -427,12 +444,19 @@ Transform OdometryF2M::computeTransform(
}
}
std::multimap<int, int>::const_iterator iter2D = lastFrame_->getWords().find(wordId);
if(iter2D!=lastFrame_->getWords().end())
{
UASSERT(lastFrame_->getWords3().find(wordId) != lastFrame_->getWords3().end());
//move back point in camera frame (to get depth along z)
cv::Point3f pt3d = util3d::transformPoint(lastFrame_->getWords3().find(wordId)->second, invLocalTransform);
references.insert(std::make_pair(lastFrame_->id(), FeatureBA(iter2D->second, pt3d.z)));
UASSERT(!lastFrame_->getWordsKpts().empty());
//get depth
float d = 0.0f;
if( !lastFrame_->getWords3().empty() &&
util3d::isFinite(lastFrame_->getWords3()[iter2D->second]))
{
//move back point in camera frame (to get depth along z)
d = util3d::transformPoint(lastFrame_->getWords3()[iter2D->second], invLocalTransform).z;
}
references.insert(std::make_pair(lastFrame_->id(), FeatureBA(lastFrame_->getWordsKpts()[iter2D->second], d)));
}
wordReferences.insert(std::make_pair(wordId, references));
@@ -557,9 +581,10 @@ Transform OdometryF2M::computeTransform(
// fields to update
LaserScan mapScan = tmpMap.sensorData().laserScanRaw();
std::multimap<int, cv::KeyPoint> mapWords = tmpMap.getWords();
std::multimap<int, cv::Point3f> mapPoints = tmpMap.getWords3();
std::multimap<int, cv::Mat> mapDescriptors = tmpMap.getWordsDescriptors();
std::multimap<int, int> mapWords = tmpMap.getWords();
std::vector<cv::KeyPoint> mapWordsKpts = tmpMap.getWordsKpts();
std::vector<cv::Point3f> mapPoints = tmpMap.getWords3();
cv::Mat mapDescriptors = tmpMap.getWordsDescriptors();
bool addVisualKeyFrame = regPipeline_->isImageRequired() &&
(keyFrameThr_ == 0.0f ||
@@ -590,8 +615,9 @@ Transform OdometryF2M::computeTransform(
// update local map
UASSERT(mapWords.size() == mapPoints.size());
UASSERT(mapPoints.size() == mapDescriptors.size());
UASSERT_MSG(lastFrame_->getWordsDescriptors().size() == lastFrame_->getWords3().size(), uFormat("%d vs %d", lastFrame_->getWordsDescriptors().size(), lastFrame_->getWords3().size()).c_str());
UASSERT(mapWords.size() == mapWordsKpts.size());
UASSERT((int)mapPoints.size() == mapDescriptors.rows);
UASSERT_MSG(lastFrame_->getWordsDescriptors().rows == (int)lastFrame_->getWords3().size(), uFormat("%d vs %d", lastFrame_->getWordsDescriptors().rows, (int)lastFrame_->getWords3().size()).c_str());
std::map<int, int>::iterator iterBundlePosesRef = bundlePoseReferences_.end();
if(bundleAdjustment_>0)
@@ -613,17 +639,15 @@ Transform OdometryF2M::computeTransform(
// update local map 3D points (if bundle adjustment was done)
for(std::map<int, cv::Point3f>::iterator iter=points3DMap.begin(); iter!=points3DMap.end(); ++iter)
{
UASSERT(mapPoints.count(iter->first) == 1);
//UDEBUG("Updated %d (%f,%f,%f) -> (%f,%f,%f)", iter->first, mapPoints.find(origin)->second.x, mapPoints.find(origin)->second.y, mapPoints.find(origin)->second.z, iter->second.x, iter->second.y, iter->second.z);
mapPoints.find(iter->first)->second = iter->second;
UASSERT(mapWords.count(iter->first) == 1);
//UDEBUG("Updated %d (%f,%f,%f) -> (%f,%f,%f)", iter->first, mapPoints[mapWords.find(iter->first)->second].x, mapPoints[mapWords.find(iter->first)->second].y, mapPoints[mapWords.find(iter->first)->second].z, iter->second.x, iter->second.y, iter->second.z);
mapPoints[mapWords.find(iter->first)->second] = iter->second;
}
}
// sort by feature response
std::multimap<float, std::pair<int, std::pair<cv::KeyPoint, std::pair<cv::Point3f, cv::Mat> > > > newIds;
UASSERT(lastFrame_->getWords3().size() == lastFrame_->getWords().size());
std::multimap<int, cv::KeyPoint>::const_iterator iter2D = lastFrame_->getWords().begin();
std::multimap<int, cv::Mat>::const_iterator iterDesc = lastFrame_->getWordsDescriptors().begin();
UDEBUG("new frame words3=%d", (int)lastFrame_->getWords3().size());
std::set<int> seenStatusUpdated;
Transform invLocalTransform;
@@ -648,11 +672,11 @@ Transform OdometryF2M::computeTransform(
if(!visDepthAsMask && validDepthRatio_ < 1.0f)
{
int ptsWithDepth = 0;
for (std::multimap<int, cv::Point3f>::const_iterator iter = lastFrame_->getWords3().begin();
for (std::vector<cv::Point3f>::const_iterator iter = lastFrame_->getWords3().begin();
iter != lastFrame_->getWords3().end();
++iter)
{
if(util3d::isFinite(iter->second))
if(util3d::isFinite(*iter))
{
++ptsWithDepth;
}
@@ -666,27 +690,29 @@ Transform OdometryF2M::computeTransform(
}
}
for(std::multimap<int, cv::Point3f>::const_iterator iter = lastFrame_->getWords3().begin(); iter!=lastFrame_->getWords3().end(); ++iter, ++iter2D, ++iterDesc)
for(std::multimap<int, int>::const_iterator iter = lastFrame_->getWords().begin(); iter!=lastFrame_->getWords().end(); ++iter)
{
if(mapPoints.find(iter->first) == mapPoints.end()) // Point not in map
const cv::Point3f & pt = lastFrame_->getWords3()[iter->second];
const cv::KeyPoint & kpt = lastFrame_->getWordsKpts()[iter->second];
if(mapWords.find(iter->first) == mapWords.end()) // Point not in map
{
if(util3d::isFinite(iter->second) || addPointsWithoutDepth)
if(util3d::isFinite(pt) || addPointsWithoutDepth)
{
newIds.insert(
std::make_pair(iter2D->second.response>0?1.0f/iter2D->second.response:0.0f,
std::make_pair(kpt.response>0?1.0f/kpt.response:0.0f,
std::make_pair(iter->first,
std::make_pair(iter2D->second,
std::make_pair(iter->second, iterDesc->second)))));
std::make_pair(kpt,
std::make_pair(pt, lastFrame_->getWordsDescriptors().row(iter->second))))));
}
}
else if(bundleAdjustment_>0)
{
if(lastFrame_->getWords().count(iter->first) == 1)
{
std::multimap<int, cv::KeyPoint>::iterator iterKpts = mapWords.find(iter->first);
if(iterKpts!=mapWords.end())
std::multimap<int, int>::iterator iterKpts = mapWords.find(iter->first);
if(iterKpts!=mapWords.end() && !mapWordsKpts.empty())
{
iterKpts->second.octave = iter2D->second.octave;
mapWordsKpts[iterKpts->second].octave = kpt.octave;
}
UASSERT(iterBundlePosesRef!=bundlePoseReferences_.end());
@@ -694,19 +720,19 @@ Transform OdometryF2M::computeTransform(
//move back point in camera frame (to get depth along z)
float depth = 0.0f;
if(util3d::isFinite(iter->second))
if(util3d::isFinite(pt))
{
depth = util3d::transformPoint(iter->second, invLocalTransform).z;
depth = util3d::transformPoint(pt, invLocalTransform).z;
}
if(bundleWordReferences_.find(iter->first) == bundleWordReferences_.end())
{
std::map<int, FeatureBA> framePt;
framePt.insert(std::make_pair(lastFrame_->id(), FeatureBA(iter2D->second, depth)));
framePt.insert(std::make_pair(lastFrame_->id(), FeatureBA(kpt, depth)));
bundleWordReferences_.insert(std::make_pair(iter->first, framePt));
}
else
{
bundleWordReferences_.find(iter->first)->second.insert(std::make_pair(lastFrame_->id(), FeatureBA(iter2D->second, depth)));
bundleWordReferences_.find(iter->first)->second.insert(std::make_pair(lastFrame_->id(), FeatureBA(kpt, depth)));
}
}
}
@@ -747,7 +773,8 @@ Transform OdometryF2M::computeTransform(
}
}
mapWords.insert(std::make_pair(iter->second.first, iter->second.second.first));
mapWords.insert(mapWords.end(), std::make_pair(iter->second.first, mapWords.size()));
mapWordsKpts.push_back(iter->second.second.first);
cv::Point3f pt = iter->second.second.second.first;
if(!util3d::isFinite(pt))
{
@@ -783,8 +810,8 @@ Transform OdometryF2M::computeTransform(
float scaleInf = (0.05 * model.fx()) / 0.01;
pt = util3d::transformPoint(cv::Point3f(ray[0]*scaleInf, ray[1]*scaleInf, ray[2]*scaleInf), model.localTransform()); // in base_link frame
}
mapPoints.insert(std::make_pair(iter->second.first, util3d::transformPoint(pt, newFramePose)));
mapDescriptors.insert(std::make_pair(iter->second.first, iter->second.second.second.second));
mapPoints.push_back(util3d::transformPoint(pt, newFramePose));
mapDescriptors.push_back(iter->second.second.second.second);
if(lastFrameOldestNewId_ > iter->second.first)
{
lastFrameOldestNewId_ = iter->second.first;
@@ -794,7 +821,7 @@ Transform OdometryF2M::computeTransform(
}
// remove words in map if max size is reached
if((int)mapPoints.size() > maximumMapSize_)
if((int)mapWords.size() > maximumMapSize_)
{
// remove oldest outliers first
std::set<int> inliers(regInfo.inliersIDs.begin(), regInfo.inliersIDs.end());
@@ -813,7 +840,7 @@ Transform OdometryF2M::computeTransform(
ids.resize(regInfo.matchesIDs.size()+oi);
UDEBUG("projected added=%d/%d minLastFrameId=%d", oi, (int)regInfo.projectedIDs.size(), lastFrameOldestNewId);
}
for(unsigned int i=0; i<ids.size() && (int)mapPoints.size() > maximumMapSize_ && mapPoints.size() >= newIds.size(); ++i)
for(unsigned int i=0; i<ids.size() && (int)mapWords.size() > maximumMapSize_ && mapWords.size() >= newIds.size(); ++i)
{
int id = ids.at(i);
if(inliers.find(id) == inliers.end())
@@ -831,18 +858,14 @@ Transform OdometryF2M::computeTransform(
bundleWordReferences_.erase(iterRef);
}
mapPoints.erase(id);
mapDescriptors.erase(id);
mapWords.erase(id);
++removed;
}
}
// remove oldest first
std::multimap<int, cv::Mat>::iterator iterMapDescriptors = mapDescriptors.begin();
std::multimap<int, cv::KeyPoint>::iterator iterMapWords = mapWords.begin();
for(std::multimap<int, cv::Point3f>::iterator iter = mapPoints.begin();
iter!=mapPoints.end() && (int)mapPoints.size() > maximumMapSize_ && mapPoints.size() >= newIds.size();)
for(std::multimap<int, int>::iterator iter = mapWords.begin();
iter!=mapWords.end() && (int)mapWords.size() > maximumMapSize_ && mapWords.size() >= newIds.size();)
{
if(inliers.find(iter->first) == inliers.end())
{
@@ -859,19 +882,36 @@ Transform OdometryF2M::computeTransform(
bundleWordReferences_.erase(iterRef);
}
mapPoints.erase(iter++);
mapDescriptors.erase(iterMapDescriptors++);
mapWords.erase(iterMapWords++);
mapWords.erase(iter++);
++removed;
}
else
{
++iter;
++iterMapDescriptors;
++iterMapWords;
}
}
if(mapWords.size() != mapPoints.size())
{
UDEBUG("Remove points");
std::vector<cv::KeyPoint> mapWordsKptsClean(mapWords.size());
std::vector<cv::Point3f> mapPointsClean(mapWords.size());
cv::Mat mapDescriptorsClean(mapWords.size(), mapDescriptors.cols, mapDescriptors.type());
int index = 0;
for(std::multimap<int, int>::iterator iter = mapWords.begin(); iter!=mapWords.end(); ++iter, ++index)
{
mapWordsKptsClean[index] = mapWordsKpts[iter->second];
mapPointsClean[index] = mapPoints[iter->second];
mapDescriptors.row(iter->second).copyTo(mapDescriptorsClean.row(index));
iter->second = index;
}
mapWordsKpts = mapWordsKptsClean;
mapWordsKptsClean.clear();
mapPoints = mapPointsClean;
mapPointsClean.clear();
mapDescriptors = mapDescriptorsClean;
}
Link * previousLink = 0;
for(std::map<int, int>::iterator iter=bundlePoseReferences_.begin(); iter!=bundlePoseReferences_.end();)
{
@@ -1099,9 +1139,7 @@ Transform OdometryF2M::computeTransform(
newFramePose.translation()));
}
map_->setWords(mapWords);
map_->setWords3(mapPoints);
map_->setWordsDescriptors(mapDescriptors);
map_->setWords(mapWords, mapWordsKpts, mapPoints, mapDescriptors);
}
}
@@ -1112,7 +1150,14 @@ Transform OdometryF2M::computeTransform(
info->localScanMapSize = tmpMap.sensorData().laserScanRaw().size();
if(this->isInfoDataFilled())
{
info->localMap = uMultimapToMap(tmpMap.getWords3());
info->localMap.clear();
if(!tmpMap.getWords3().empty())
{
for(std::multimap<int, int>::const_iterator iter=tmpMap.getWords().begin(); iter!=tmpMap.getWords().end(); ++iter)
{
info->localMap.insert(std::make_pair(iter->first, tmpMap.getWords3()[iter->second]));
}
}
info->localScanMap = tmpMap.sensorData().laserScanRaw();
}
}
@@ -1139,11 +1184,12 @@ Transform OdometryF2M::computeTransform(
if(regPipeline_->isImageRequired())
{
int ptsWithDepth = 0;
for (std::multimap<int, cv::Point3f>::const_iterator iter = lastFrame_->getWords3().begin();
iter != lastFrame_->getWords3().end();
for (std::multimap<int, int>::const_iterator iter = lastFrame_->getWords().begin();
iter != lastFrame_->getWords().end();
++iter)
{
if(util3d::isFinite(iter->second))
if(!lastFrame_->getWords3().empty() &&
util3d::isFinite(lastFrame_->getWords3()[iter->second]))
{
++ptsWithDepth;
}
@@ -1153,26 +1199,29 @@ Transform OdometryF2M::computeTransform(
{
frameValid = true;
// update local map
UASSERT_MSG(lastFrame_->getWordsDescriptors().size() == lastFrame_->getWords3().size(), uFormat("%d vs %d", lastFrame_->getWordsDescriptors().size(), lastFrame_->getWords3().size()).c_str());
UASSERT_MSG(lastFrame_->getWordsDescriptors().rows == (int)lastFrame_->getWords3().size(), uFormat("%d vs %d", lastFrame_->getWordsDescriptors().size(), lastFrame_->getWords3().size()).c_str());
UASSERT(lastFrame_->getWords3().size() == lastFrame_->getWords().size());
std::multimap<int, cv::KeyPoint> words;
std::multimap<int, cv::Point3f> transformedPoints;
std::multimap<int, int> words;
std::vector<cv::KeyPoint> wordsKpts;
std::vector<cv::Point3f> transformedPoints;
std::multimap<int, int> mapPointWeights;
std::multimap<int, cv::Mat> descriptors;
UASSERT(lastFrame_->getWords3().size() == lastFrame_->getWordsDescriptors().size());
std::multimap<int, cv::KeyPoint>::const_iterator wordsIter = lastFrame_->getWords().begin();
std::multimap<int, cv::Mat>::const_iterator descIter = lastFrame_->getWordsDescriptors().begin();
for (std::multimap<int, cv::Point3f>::const_iterator iter = lastFrame_->getWords3().begin();
iter != lastFrame_->getWords3().end();
++iter, ++descIter, ++wordsIter)
cv::Mat descriptors;
if(!lastFrame_->getWords3().empty())
{
if (util3d::isFinite(iter->second))
for (std::multimap<int, int>::const_iterator iter = lastFrame_->getWords().begin();
iter != lastFrame_->getWords().end();
++iter)
{
words.insert(*wordsIter);
transformedPoints.insert(std::make_pair(iter->first, util3d::transformPoint(iter->second, newFramePose)));
mapPointWeights.insert(std::make_pair(iter->first, 0));
descriptors.insert(*descIter);
const cv::Point3f & pt = lastFrame_->getWords3()[iter->second];
if (util3d::isFinite(pt))
{
words.insert(words.end(), std::make_pair(iter->first, words.size()));
wordsKpts.push_back(lastFrame_->getWordsKpts()[iter->second]);
transformedPoints.push_back(util3d::transformPoint(pt, newFramePose));
mapPointWeights.insert(std::make_pair(iter->first, 0));
descriptors.push_back(lastFrame_->getWordsDescriptors().row(iter->second));
}
}
}
@@ -1193,25 +1242,29 @@ Transform OdometryF2M::computeTransform(
}
// update bundleWordReferences_: used for bundle adjustment
for(std::multimap<int, cv::KeyPoint>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
if(!wordsKpts.empty())
{
if(words.count(iter->first) == 1)
for(std::multimap<int, int>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
{
UASSERT(bundleWordReferences_.find(iter->first) == bundleWordReferences_.end());
std::map<int, FeatureBA> framePt;
//get depth
float d = 0.0f;
if(lastFrame_->getWords3().count(iter->first) == 1 &&
util3d::isFinite(lastFrame_->getWords3().find(iter->first)->second))
if(words.count(iter->first) == 1)
{
//move back point in camera frame (to get depth along z)
d = util3d::transformPoint(lastFrame_->getWords3().find(iter->first)->second, invLocalTransform).z;
UASSERT(bundleWordReferences_.find(iter->first) == bundleWordReferences_.end());
std::map<int, FeatureBA> framePt;
//get depth
float d = 0.0f;
if(lastFrame_->getWords().count(iter->first) == 1 &&
!lastFrame_->getWords3().empty() &&
util3d::isFinite(lastFrame_->getWords3()[lastFrame_->getWords().find(iter->first)->second]))
{
//move back point in camera frame (to get depth along z)
d = util3d::transformPoint(lastFrame_->getWords3()[lastFrame_->getWords().find(iter->first)->second], invLocalTransform).z;
}
framePt.insert(std::make_pair(lastFrame_->id(), FeatureBA(wordsKpts[iter->second], d)));
bundleWordReferences_.insert(std::make_pair(iter->first, framePt));
}
framePt.insert(std::make_pair(lastFrame_->id(), FeatureBA(iter->second, d)));
bundleWordReferences_.insert(std::make_pair(iter->first, framePt));
}
}
@@ -1246,9 +1299,7 @@ Transform OdometryF2M::computeTransform(
}
}
map_->setWords(words);
map_->setWords3(transformedPoints);
map_->setWordsDescriptors(descriptors);
map_->setWords(words, wordsKpts, transformedPoints, descriptors);
addKeyFrame = true;
}
else
@@ -1347,7 +1398,14 @@ Transform OdometryF2M::computeTransform(
if(this->isInfoDataFilled())
{
info->localMap = uMultimapToMap(map_->getWords3());
info->localMap.clear();
if(!map_->getWords3().empty())
{
for(std::multimap<int, int>::const_iterator iter=map_->getWords().begin(); iter!=map_->getWords().end(); ++iter)
{
info->localMap.insert(std::make_pair(iter->first, map_->getWords3()[iter->second]));
}
}
info->localScanMap = map_->sensorData().laserScanRaw();
}
}
@@ -1360,7 +1418,14 @@ Transform OdometryF2M::computeTransform(
{
if(regPipeline_->isImageRequired())
{
info->words = lastFrame_->getWords();
info->words.clear();
if(!lastFrame_->getWordsKpts().empty())
{
for(std::multimap<int, int>::const_iterator iter=lastFrame_->getWords().begin(); iter!=lastFrame_->getWords().end(); ++iter)
{
info->words.insert(std::make_pair(iter->first, lastFrame_->getWordsKpts()[iter->second]));
}
}
}
}
}
corelib/src/odometry/OdometryMono.cpp
+32 -18
View File
@@ -283,15 +283,15 @@ Transform OdometryMono::computeTransform(SensorData & data, const Transform & gu
newCorners[oi] = imagePoints[i];
if(localMap_.count(ids[i]) == 1)
{
if(prevS->getWords().count(ids[i]) == 1)
if(prevS->getWords().count(ids[i]) == 1 && !prevS->getWordsKpts().empty())
{
// set guess if unique
refCorners[oi] = prevS->getWords().find(ids[i])->second.pt;
refCorners[oi] = prevS->getWordsKpts()[prevS->getWords().find(ids[i])->second].pt;
}
if(newS->getWords().count(ids[i]) == 1)
if(newS->getWords().count(ids[i]) == 1 && !newS->getWordsKpts().empty())
{
// set guess if unique
newCorners[oi] = newS->getWords().find(ids[i])->second.pt;
newCorners[oi] = newS->getWordsKpts()[newS->getWords().find(ids[i])->second].pt;
}
}
objectPointsTmp[oi] = objectPoints[i];
@@ -338,9 +338,9 @@ Transform OdometryMono::computeTransform(SensorData & data, const Transform & gu
if(this->isInfoDataFilled() && info)
{
cv::KeyPoint kpt;
if(newS->getWords().count(matches[i]) == 1)
if(newS->getWords().count(matches[i]) == 1 && !newS->getWordsKpts().empty())
{
kpt = newS->getWords().find(matches[i])->second;
kpt = newS->getWordsKpts()[newS->getWords().find(matches[i])->second];
}
kpt.pt = newCorners[i];
info->words.insert(std::make_pair(matches[i], kpt));
@@ -437,9 +437,9 @@ Transform OdometryMono::computeTransform(SensorData & data, const Transform & gu
for(std::set<int>::iterator iter = memory_->getStMem().begin(); iter!=memory_->getStMem().end(); ++iter)
{
const Signature * s = memory_->getSignature(*iter);
for(std::multimap<int, cv::KeyPoint>::const_iterator jter=s->getWords().begin(); jter!=s->getWords().end(); ++jter)
for(std::multimap<int, int>::const_iterator jter=s->getWords().begin(); jter!=s->getWords().end(); ++jter)
{
if(s->getWords().count(jter->first) == 1 && localMap_.find(jter->first)!=localMap_.end())
if(s->getWords().count(jter->first) == 1 && localMap_.find(jter->first)!=localMap_.end() && !s->getWordsKpts().empty())
{
if(wordReferences.find(jter->first)==wordReferences.end())
{
@@ -451,7 +451,8 @@ Transform OdometryMono::computeTransform(SensorData & data, const Transform & gu
{
depth = keyFrameWords3D_.at(s->id()).at(jter->first).x;
}
wordReferences.at(jter->first).insert(std::make_pair(s->id(), FeatureBA(jter->second, depth, cv::Mat())));
const cv::KeyPoint & kpts = s->getWordsKpts()[jter->second];
wordReferences.at(jter->first).insert(std::make_pair(s->id(), FeatureBA(kpts, depth, cv::Mat())));
}
}
}
@@ -502,9 +503,21 @@ Transform OdometryMono::computeTransform(SensorData & data, const Transform & gu
}
else if(float(inliers)/float(imagePoints.size()) < keyFrameThr_)
{
std::map<int, int> uniqueWordsPrevious = uMultimapToMapUnique(previousS->getWords());
std::map<int, int> uniqueWordsNew = uMultimapToMapUnique(newS->getWords());
std::map<int, cv::KeyPoint> wordsPrevious;
std::map<int, cv::KeyPoint> wordsNew;
for(std::map<int, int>::iterator iter=uniqueWordsPrevious.begin(); iter!=uniqueWordsPrevious.end(); ++iter)
{
wordsPrevious.insert(std::make_pair(iter->first, previousS->getWordsKpts()[iter->second]));
}
for(std::map<int, int>::iterator iter=uniqueWordsNew.begin(); iter!=uniqueWordsNew.end(); ++iter)
{
wordsNew.insert(std::make_pair(iter->first, newS->getWordsKpts()[iter->second]));
}
std::map<int, cv::Point3f> inliers3D = util3d::generateWords3DMono(
uMultimapToMapUnique(previousS->getWords()),
uMultimapToMapUnique(newS->getWords()),
wordsPrevious,
wordsNew,
cameraModel,
cameraTransform,
fundMatrixReprojError_,
@@ -626,9 +639,9 @@ Transform OdometryMono::computeTransform(SensorData & data, const Transform & gu
int ii=0;
for(std::map<int, cv::Point2f>::iterator iter=firstFrameGuessCorners_.begin(); iter!=firstFrameGuessCorners_.end(); ++iter)
{
std::multimap<int, cv::KeyPoint>::const_iterator jter=refS->getWords().find(iter->first);
UASSERT(jter != refS->getWords().end());
refCorners[ii] = jter->second.pt;
std::multimap<int, int>::const_iterator jter=refS->getWords().find(iter->first);
UASSERT(jter != refS->getWords().end() && !refS->getWordsKpts().empty());
refCorners[ii] = refS->getWordsKpts()[jter->second].pt;
refCornersGuess[ii] = iter->second;
cornerIds[ii] = iter->first;
++ii;
@@ -800,14 +813,15 @@ Transform OdometryMono::computeTransform(SensorData & data, const Transform & gu
// generate kpts
if(memory_->update(SensorData(data)))
{
const std::multimap<int, cv::KeyPoint> & words = memory_->getLastWorkingSignature()->getWords();
if((int)words.size() > minInliers_)
const Signature * s = memory_->getLastWorkingSignature();
const std::multimap<int, int> & words = s->getWords();
if((int)words.size() > minInliers_ && !s->getWordsKpts().empty())
{
for(std::multimap<int, cv::KeyPoint>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
for(std::multimap<int, int>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
{
if(words.count(iter->first) == 1)
{
firstFrameGuessCorners_.insert(std::make_pair(iter->first, iter->second.pt));
firstFrameGuessCorners_.insert(std::make_pair(iter->first, s->getWordsKpts()[iter->second].pt));
}
}
}