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

merged attention branch to trunk

Browse Source 
git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@657 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2012-12-11 18:05:05 +00:00
parent f9033809a2
commit 2836d4c48c
216 changed files with 7983 additions and 94891 deletions
Download Patch File Download Diff File Expand all files Collapse all files

53
corelib/include/rtabmap/core/Actuator.h
View File

@@ -1,53 +0,0 @@
/*
* Copyright (C) 2010-2011, Mathieu Labbe and IntRoLab - Universite de Sherbrooke
*
* This file is part of RTAB-Map.
*
* RTAB-Map is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* RTAB-Map is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with RTAB-Map. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef ACTUATOR_H_
#define ACTUATOR_H_
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
#include <opencv2/core/core.hpp>
#include <utilite/UEvent.h>
#include <list>
namespace rtabmap {
class Actuator
{
public:
enum Type{kTypeTwist=0, kTypeNotSpecified};
public:
Actuator(const cv::Mat & data, Type type, int num = 0) :
_data(data),
_type(type),
_num(num)
{}
const cv::Mat & data() const {return _data;}
int type() const {return _type;}
int num() const {return _num;}
virtual ~Actuator() {};
private:
cv::Mat _data;
int _type;
int _num;
};
}
#endif /* ACTUATOR_H_ */

16
corelib/include/rtabmap/core/Camera.h
View File

@@ -29,8 +29,8 @@
#include <utilite/UDirectory.h>
#include <utilite/UTimer.h>
#include "rtabmap/core/Parameters.h"
#include "rtabmap/core/KeypointDetector.h"
#include "rtabmap/core/KeypointDescriptor.h"
#include "rtabmap/core/Features2d.h"
#include "rtabmap/core/Image.h"
#include <set>
#include <stack>
#include <list>
@@ -59,9 +59,7 @@ public:
CameraEvent(const cv::Mat & descriptors, const std::vector<cv::KeyPoint> & keypoints, const cv::Mat & image = cv::Mat(), int cameraId = 0) :
UEvent(kCodeFeatures),
_cameraId(cameraId),
_image(image),
_descriptors(descriptors),
_keypoints(keypoints)
_image(image, descriptors, keypoints)
{
}
CameraEvent(int cameraId = 0) :
@@ -73,18 +71,14 @@ public:
int cameraId() const {return _cameraId;}
// Image or descriptors
const cv::Mat & image() const {return _image;}
const cv::Mat & descriptors() const {return _descriptors;}
const std::vector<cv::KeyPoint> & keypoints() const {return _keypoints;}
const Image & image() const {return _image;}
virtual ~CameraEvent() {}
virtual std::string getClassName() const {return std::string("CameraEvent");}
private:
int _cameraId;
cv::Mat _image;
cv::Mat _descriptors;
std::vector<cv::KeyPoint> _keypoints;
Image _image;
};
/**

52
corelib/include/rtabmap/core/ColorTable.h
View File

@@ -1,52 +0,0 @@
#ifndef COLORTABLE_H
#define COLORTABLE_H
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
#include <vector>
namespace rtabmap
{
class RTABMAP_EXP ColorTable
{
public:
enum Size{kSize8 = 8,
kSize16 = 16,
kSize32 = 32,
kSize64 = 64,
kSize128 = 128,
kSize256 = 256,
kSize512 = 512,
kSize1024 = 1024,
kSize65536 = 65536};
public:
ColorTable(int size);
virtual ~ColorTable() {}
static unsigned char INDEXED_TABLE_8[24];
static unsigned char INDEXED_TABLE_16[48];
static unsigned char INDEXED_TABLE_32[96];
static unsigned char INDEXED_TABLE_64[192];
static unsigned char INDEXED_TABLE_128[384];
static unsigned char INDEXED_TABLE_256[768];
static unsigned char INDEXED_TABLE_512[1536];
static unsigned char INDEXED_TABLE_1024[3076];
static unsigned char INDEXED_TABLE_65536[196608];
int size() const {return _size;}
unsigned short getIndex(unsigned char r, unsigned char g, unsigned char b) const;
void getRgb(unsigned short index, unsigned char & r, unsigned char & g, unsigned char & b) const;
unsigned short getNNIndex(unsigned char r, unsigned char g, unsigned char b) const;
void getNNRgb(unsigned short index, unsigned char & r, unsigned char & g, unsigned char & b) const;
private:
int _size;
std::vector<unsigned short> _rgb2indexed;
unsigned char * _indexedTable;
};
} // namespace rtabmap
#endif // COLORTABLE_H

97
corelib/include/rtabmap/core/DBDriver.h
View File

@@ -30,11 +30,10 @@
#include "utilite/UMutex.h"
#include "utilite/UThreadNode.h"
#include "rtabmap/core/Parameters.h"
#include "rtabmap/core/Signature.h"
namespace rtabmap {
class KeypointSignature;
class Signature;
class SMSignature;
class VWDictionary;
class VisualWord;
@@ -53,26 +52,21 @@ class RTABMAP_EXP DBDriver : public UThreadNode
public:
virtual ~DBDriver();
virtual std::string getDriverName() const = 0;
virtual void parseParameters(const ParametersMap & parameters);
const std::string & getUrl() const {return _url;}
void beginTransaction() const;
void commit() const;
void asyncSave(Signature * s);
void asyncSave(VisualWord * s);
void asyncSave(Signature * s); //ownership transferred
void asyncSave(VisualWord * vw); //ownership transferred
void emptyTrashes(bool async = false);
double getEmptyTrashesTime() const {return _emptyTrashesTime;}
bool isImagesCompressed() const {return _imagesCompressed;}
public:
bool addStatisticsAfterRun(int stMemSize, int lastSignAdded, int processMemUsed, int databaseMemUsed) const;
bool addStatisticsAfterRunSurf(int dictionarySize) const;
bool deleteAllVisualWords() const;
bool deleteAllObsoleteSSVWLinks() const;
bool deleteUnreferencedWords() const;
void addStatisticsAfterRun(int stMemSize, int lastSignAdded, int processMemUsed, int databaseMemUsed) const;
void addStatisticsAfterRunSurf(int dictionarySize) const;
public:
// Mutex-protected methods of abstract versions below
@@ -84,31 +78,24 @@ public:
bool isConnected() const;
long getMemoryUsed() const; // In bytes
bool executeNoResult(const std::string & sql) const;
// Update
bool changeWordsRef(const std::map<int, int> & refsToChange); // <oldWordId, activeWordId>
bool deleteWords(const std::vector<int> & ids);
void executeNoResult(const std::string & sql) const;
// Load objects
bool load(VWDictionary * dictionary) const;
bool loadLastNodes(std::list<Signature *> & signatures) const;
bool loadKeypointSignatures(const std::list<int> & ids, std::list<Signature *> & signatures);
bool loadSMSignatures(const std::list<int> & ids, std::list<Signature *> & signatures);
bool loadWords(const std::list<int> & wordIds, std::list<VisualWord *> & vws);
void load(VWDictionary * dictionary) const;
void loadLastNodes(std::list<Signature *> & signatures) const;
void loadSignatures(const std::list<int> & ids, std::list<Signature *> & signatures);
void loadWords(const std::set<int> & wordIds, std::list<VisualWord *> & vws);
// Specific queries...
bool getRawData(int id, std::list<Sensor> & data) const;
bool getActuatorData(int id, std::list<Actuator> & data) const;
bool getNeighborIds(int signatureId, std::set<int> & neighbors, bool onlyWithActions = false) const;
bool loadNeighbors(int signatureId, NeighborsMultiMap & neighbors) const;
bool getWeight(int signatureId, int & weight) const;
bool getLoopClosureIds(int signatureId, std::set<int> & loopIds, std::set<int> & childIds) const;
bool getAllNodeIds(std::set<int> & ids) const;
bool getLastNodeId(int & id) const;
bool getLastWordId(int & id) const;
bool getInvertedIndexNi(int signatureId, int & ni) const;
bool getHighestWeightedNodeIds(unsigned int count, std::multimap<int, int> & ids) const;
void getImage(int id, cv::Mat & image) const;
void getNeighborIds(int signatureId, std::set<int> & neighbors, bool onlyWithActions = false) const;
void loadNeighbors(int signatureId, std::set<int> & neighbors) const;
void getWeight(int signatureId, int & weight) const;
void getLoopClosureIds(int signatureId, std::set<int> & loopIds, std::set<int> & childIds) const;
void getAllNodeIds(std::set<int> & ids) const;
void getLastNodeId(int & id) const;
void getLastWordId(int & id) const;
void getInvertedIndexNi(int signatureId, int & ni) const;
protected:
DBDriver(const ParametersMap & parameters = ParametersMap());
@@ -119,41 +106,31 @@ private:
virtual bool isConnectedQuery() const = 0;
virtual long getMemoryUsedQuery() const = 0; // In bytes
virtual bool executeNoResultQuery(const std::string & sql) const = 0;
virtual void executeNoResultQuery(const std::string & sql) const = 0;
virtual bool changeWordsRefQuery(const std::map<int, int> & refsToChange) const = 0; // <oldWordId, activeWordId>
virtual bool deleteWordsQuery(const std::vector<int> & ids) const = 0;
virtual bool getNeighborIdsQuery(int signatureId, std::set<int> & neighbors, bool onlyWithActions = false) const = 0;
virtual bool getWeightQuery(int signatureId, int & weight) const = 0;
virtual bool getLoopClosureIdsQuery(int signatureId, std::set<int> & loopIds, std::set<int> & childIds) const = 0;
virtual void getNeighborIdsQuery(int signatureId, std::set<int> & neighbors, bool onlyWithActions = false) const = 0;
virtual void getWeightQuery(int signatureId, int & weight) const = 0;
virtual void getLoopClosureIdsQuery(int signatureId, std::set<int> & loopIds, std::set<int> & childIds) const = 0;
virtual bool saveQuery(const std::vector<VisualWord *> & visualWords) const = 0;
virtual bool updateQuery(const std::list<Signature *> & signatures) const = 0;
virtual bool saveQuery(const std::list<Signature *> & signatures) const = 0;
virtual void saveQuery(const std::vector<VisualWord *> & visualWords) const = 0;
virtual void updateQuery(const std::list<Signature *> & signatures) const = 0;
virtual void saveQuery(const std::list<Signature *> & signatures) const = 0;
// Load objects
virtual bool loadQuery(VWDictionary * dictionary) const = 0;
virtual bool loadLastNodesQuery(std::list<Signature *> & signatures) const = 0;
virtual bool loadQuery(int signatureId, Signature ** s) const = 0;
virtual bool loadQuery(int wordId, VisualWord ** vw) const = 0;
virtual bool loadQuery(int signatureId, KeypointSignature * ss) const = 0;
virtual bool loadQuery(int signatureId, SMSignature * ss) const = 0;
virtual bool loadKeypointSignaturesQuery(const std::list<int> & ids, std::list<Signature *> & signatures) const = 0;
virtual bool loadSMSignaturesQuery(const std::list<int> & ids, std::list<Signature *> & signatures) const = 0;
virtual bool loadWordsQuery(const std::list<int> & wordIds, std::list<VisualWord *> & vws) const = 0;
virtual bool loadNeighborsQuery(int signatureId, NeighborsMultiMap & neighbors) const = 0;
virtual void loadQuery(VWDictionary * dictionary) const = 0;
virtual void loadLastNodesQuery(std::list<Signature *> & signatures) const = 0;
virtual void loadSignaturesQuery(const std::list<int> & ids, std::list<Signature *> & signatures) const = 0;
virtual void loadWordsQuery(const std::set<int> & wordIds, std::list<VisualWord *> & vws) const = 0;
virtual void loadNeighborsQuery(int signatureId, std::set<int> & neighbors) const = 0;
virtual bool getRawDataQuery(int id, std::list<Sensor> & rawData) const = 0;
virtual bool getActuatorDataQuery(int id, std::list<Actuator> & rawData) const = 0;
virtual bool getAllNodeIdsQuery(std::set<int> & ids) const = 0;
virtual bool getLastNodeIdQuery(int & id) const = 0;
virtual bool getLastWordIdQuery(int & id) const = 0;
virtual bool getInvertedIndexNiQuery(int signatureId, int & ni) const = 0;
virtual bool getHighestWeightedNodeIdsQuery(unsigned int count, std::multimap<int,int> & signatures) const = 0;
virtual void getImageQuery(int id, cv::Mat & rawData) const = 0;
virtual void getAllNodeIdsQuery(std::set<int> & ids) const = 0;
virtual void getLastIdQuery(const std::string & tableName, int & id) const = 0;
virtual void getInvertedIndexNiQuery(int signatureId, int & ni) const = 0;
private:
//non-abstract methods
bool saveOrUpdate(const std::vector<Signature *> & signatures) const;
void saveOrUpdate(const std::vector<Signature *> & signatures) const;
//thread stuff
virtual void mainLoop();
@@ -166,8 +143,6 @@ private:
UMutex _trashesMutex;
UMutex _dbSafeAccessMutex;
USemaphore _addSem;
unsigned int _minSignaturesToSave;
unsigned int _minWordsToSave;
bool _imagesCompressed;
double _emptyTrashesTime;
std::string _url;

42
corelib/include/rtabmap/core/DBDriverFactory.h
View File

@@ -1,42 +0,0 @@
/*
* Copyright (C) 2010-2011, Mathieu Labbe and IntRoLab - Universite de Sherbrooke
*
* This file is part of RTAB-Map.
*
* RTAB-Map is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* RTAB-Map is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with RTAB-Map. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef DBDRIVERFACTORY_H_
#define DBDRIVERFACTORY_H_
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
#include "rtabmap/core/Parameters.h"
#include <string>
namespace rtabmap {
class DBDriver;
class RTABMAP_EXP DBDriverFactory
{
public:
static DBDriver * createDBDriver(const std::string & dbDriverName, const ParametersMap & parameters = ParametersMap());
public:
DBDriverFactory();
virtual ~DBDriverFactory();
};
}
#endif /* DBDRIVERFACTORY_H_ */

15
corelib/include/rtabmap/core/DBReader.h
View File

@@ -10,12 +10,11 @@
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
#include "rtabmap/core/Sensor.h"
#include "rtabmap/core/Actuator.h"
#include <utilite/UThreadNode.h>
#include <utilite/UTimer.h>
#include <opencv2/core/core.hpp>
#include <set>
namespace rtabmap {
@@ -25,14 +24,12 @@ class DBDriver;
class RTABMAP_EXP DBReader : public UThreadNode {
public:
DBReader(const std::string & databasePath,
float frameRate = 0.0f,
const std::set<Sensor::Type> & sensorTypes = std::set<Sensor::Type>(),
const std::set<Actuator::Type> & actuatorTypes = std::set<Actuator::Type>());
float frameRate = 0.0f);
virtual ~DBReader();
bool init();
bool init(int startIndex=0);
void setFrameRate(float frameRate);
void getNextSensorimotorState(std::list<Sensor> & sensors, std::list<Actuator> & actuators);
void getNextImage(cv::Mat & sensors);
protected:
virtual void mainLoopBegin();
@@ -41,8 +38,6 @@ protected:
private:
std::string _path;
float _frameRate;
std::set<Sensor::Type> _sensorTypes;
std::set<Actuator::Type> _actuatorTypes;
DBDriver * _dbDriver;
UTimer _timer;

118
corelib/include/rtabmap/core/EpipolarGeometry.h
View File

@@ -20,7 +20,7 @@
#pragma once
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
#include "rtabmap/core/Parameters.h"
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <list>
@@ -29,56 +29,82 @@
namespace rtabmap
{
//epipolar geometry
void RTABMAP_EXP findEpipolesFromF(
const cv::Mat & fundamentalMatrix,
cv::Vec3d & e1,
cv::Vec3d & e2);
class Signature;
cv::Mat RTABMAP_EXP findPFromF(
const cv::Mat & fundamentalMatrix,
const cv::Mat & x1,
const cv::Mat & x2);
class RTABMAP_EXP EpipolarGeometry
{
public:
EpipolarGeometry(const ParametersMap & parameters = ParametersMap());
virtual ~EpipolarGeometry();
bool check(const Signature * ssA, const Signature * ssB);
void parseParameters(const ParametersMap & parameters);
// return fundamental matrix
// status -> inliers = 1, outliers = 0
cv::Mat RTABMAP_EXP findFFromWords(
const std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs, // id, kpt1, kpt2
std::vector<uchar> & status,
double ransacParam1 = 3.0,
double ransacParam2 = 0.99);
int getMatchCountMinAccepted() const {return _matchCountMinAccepted;}
double getRansacParam1() const {return _ransacParam1;}
double getRansacParam2() const {return _ransacParam2;}
// assume a canonical camera (without K)
void RTABMAP_EXP findRTFromP(
const cv::Mat & p,
cv::Mat & r,
cv::Mat & t);
void setMatchCountMinAccepted(int matchCountMinAccepted) {_matchCountMinAccepted = matchCountMinAccepted;}
void setRansacParam1(double ransacParam1) {_ransacParam1 = ransacParam1;}
void setRansacParam2(double ransacParam2) {_ransacParam2 = ransacParam2;}
/**
* 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 RTABMAP_EXP 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);
/**
* if a=[1 2 3 4 6 6], b=[1 1 2 4 5 6 6], results= [(2,2) (4,4)]
* realPairsCount = 5
*/
int RTABMAP_EXP 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);
// STATIC STUFF
//epipolar geometry
static void RTABMAP_EXP findEpipolesFromF(
const cv::Mat & fundamentalMatrix,
cv::Vec3d & e1,
cv::Vec3d & e2);
/**
* 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 RTABMAP_EXP 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);
static cv::Mat RTABMAP_EXP findPFromF(
const cv::Mat & fundamentalMatrix,
const cv::Mat & x1,
const cv::Mat & x2);
// return fundamental matrix
// status -> inliers = 1, outliers = 0
static cv::Mat RTABMAP_EXP findFFromWords(
const std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs, // id, kpt1, kpt2
std::vector<uchar> & status,
double ransacParam1 = 3.0,
double ransacParam2 = 0.99);
// assume a canonical camera (without K)
static void RTABMAP_EXP findRTFromP(
const cv::Mat & p,
cv::Mat & r,
cv::Mat & t);
/**
* 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
*/
static int RTABMAP_EXP 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);
/**
* if a=[1 2 3 4 6 6], b=[1 1 2 4 5 6 6], results= [(2,2) (4,4)]
* realPairsCount = 5
*/
static int RTABMAP_EXP 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);
/**
* 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
*/
static int RTABMAP_EXP 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);
private:
int _matchCountMinAccepted;
double _ransacParam1;
double _ransacParam2;
};
} // namespace rtabmap

274
corelib/include/rtabmap/core/KeypointDetector.h → corelib/include/rtabmap/core/Features2d.h
View File

@@ -1,126 +1,148 @@
/*
* Copyright (C) 2010-2011, Mathieu Labbe and IntRoLab - Universite de Sherbrooke
*
* This file is part of RTAB-Map.
*
* RTAB-Map is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* RTAB-Map is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with RTAB-Map. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef KEYPOINTDETECTOR_H_
#define KEYPOINTDETECTOR_H_
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <list>
#include "rtabmap/core/Parameters.h"
namespace rtabmap
{
class VWDictionary;
class RTABMAP_EXP KeypointDetector
{
public:
enum DetectorType {kDetectorSurf, kDetectorStar, kDetectorSift, kDetectorFast, kDetectorUndef};
public:
virtual ~KeypointDetector() {}
std::vector<cv::KeyPoint> generateKeypoints(const cv::Mat & image);
virtual void parseParameters(const ParametersMap & parameters);
unsigned int getWordsPerImageTarget() const {return _wordsPerImageTarget;}
void setRoi(const std::string & roi);
cv::Rect computeRoi(const cv::Mat & image) const;
protected:
KeypointDetector(const ParametersMap & parameters = ParametersMap());
private:
virtual std::vector<cv::KeyPoint> _generateKeypoints(const cv::Mat & image, const cv::Rect & roi) const = 0;
private:
unsigned int _wordsPerImageTarget;
std::vector<float> _roiRatios; // size 4
};
//SURFDetector
class RTABMAP_EXP SURFDetector : public KeypointDetector
{
public:
SURFDetector(const ParametersMap & parameters = ParametersMap());
virtual ~SURFDetector();
virtual void parseParameters(const ParametersMap & parameters);
private:
virtual std::vector<cv::KeyPoint> _generateKeypoints(const cv::Mat & image, const cv::Rect & roi) const;
private:
double _hessianThreshold;
int _nOctaves;
int _nOctaveLayers;
bool _extended;
bool _upright;
bool _gpuVersion;
};
//SIFTDetector
class RTABMAP_EXP SIFTDetector : public KeypointDetector
{
public:
SIFTDetector(const ParametersMap & parameters = ParametersMap());
virtual ~SIFTDetector();
virtual void parseParameters(const ParametersMap & parameters);
private:
virtual std::vector<cv::KeyPoint> _generateKeypoints(const cv::Mat & image, const cv::Rect & roi) const;
private:
int _nfeatures;
int _nOctaveLayers;
double _contrastThreshold;
double _edgeThreshold;
double _sigma;
};
//StarDetector
class RTABMAP_EXP StarDetector : public KeypointDetector
{
public:
StarDetector(const ParametersMap & parameters = ParametersMap());
virtual ~StarDetector();
virtual void parseParameters(const ParametersMap & parameters);
private:
virtual std::vector<cv::KeyPoint> _generateKeypoints(const cv::Mat & image, const cv::Rect & roi) const;
private:
int _maxSize;
int _responseThreshold;
int _lineThresholdProjected;
int _lineThresholdBinarized;
int _suppressNonmaxSize;
};
//FASTDetector
class RTABMAP_EXP FASTDetector : public KeypointDetector
{
public:
FASTDetector(const ParametersMap & parameters = ParametersMap());
virtual ~FASTDetector();
virtual void parseParameters(const ParametersMap & parameters);
private:
virtual std::vector<cv::KeyPoint> _generateKeypoints(const cv::Mat & image, const cv::Rect & roi) const;
private:
int _threshold;
bool _nonmaxSuppression;
};
}
#endif /* KEYPOINTDETECTOR_H_ */
/*
* Copyright (C) 2010-2011, Mathieu Labbe and IntRoLab - Universite de Sherbrooke
*
* This file is part of RTAB-Map.
*
* RTAB-Map is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* RTAB-Map is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with RTAB-Map. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef KEYPOINTDESCRIPTOR_H_
#define KEYPOINTDESCRIPTOR_H_
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <list>
#include "rtabmap/core/Parameters.h"
namespace rtabmap {
/////////////////////
// KeypointDescriptor
/////////////////////
class RTABMAP_EXP KeypointDescriptor {
public:
enum DescriptorType {kDescriptorSurf, kDescriptorSift, kDescriptorUndef};
public:
virtual ~KeypointDescriptor();
virtual void parseParameters(const ParametersMap & parameters);
virtual cv::Mat generateDescriptors(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const = 0;
protected:
KeypointDescriptor(const ParametersMap & parameters = ParametersMap());
};
//SURFDescriptor
class RTABMAP_EXP SURFDescriptor : public KeypointDescriptor
{
public:
SURFDescriptor(const ParametersMap & parameters = ParametersMap());
virtual ~SURFDescriptor();
virtual void parseParameters(const ParametersMap & parameters);
virtual cv::Mat generateDescriptors(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
private:
double _hessianThreshold;
int _nOctaves;
int _nOctaveLayers;
bool _extended;
bool _upright;
bool _gpuVersion;
};
//SIFTDescriptor
class RTABMAP_EXP SIFTDescriptor : public KeypointDescriptor
{
public:
SIFTDescriptor(const ParametersMap & parameters = ParametersMap());
virtual ~SIFTDescriptor();
virtual void parseParameters(const ParametersMap & parameters);
virtual cv::Mat generateDescriptors(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
private:
int _nfeatures;
int _nOctaveLayers;
double _contrastThreshold;
double _edgeThreshold;
double _sigma;
};
/////////////////////
// KeypointDetector
/////////////////////
class RTABMAP_EXP KeypointDetector
{
public:
enum DetectorType {kDetectorSurf, kDetectorSift, kDetectorUndef};
public:
virtual ~KeypointDetector() {}
std::vector<cv::KeyPoint> generateKeypoints(const cv::Mat & image);
virtual void parseParameters(const ParametersMap & parameters);
unsigned int getWordsPerImageTarget() const {return _wordsPerImageTarget;}
void setRoi(const std::string & roi);
cv::Rect computeRoi(const cv::Mat & image) const;
protected:
KeypointDetector(const ParametersMap & parameters = ParametersMap());
private:
virtual std::vector<cv::KeyPoint> _generateKeypoints(const cv::Mat & image, const cv::Rect & roi) const = 0;
private:
unsigned int _wordsPerImageTarget;
std::vector<float> _roiRatios; // size 4
};
//SURFDetector
class RTABMAP_EXP SURFDetector : public KeypointDetector
{
public:
SURFDetector(const ParametersMap & parameters = ParametersMap());
virtual ~SURFDetector();
virtual void parseParameters(const ParametersMap & parameters);
private:
virtual std::vector<cv::KeyPoint> _generateKeypoints(const cv::Mat & image, const cv::Rect & roi) const;
private:
double _hessianThreshold;
int _nOctaves;
int _nOctaveLayers;
bool _extended;
bool _upright;
bool _gpuVersion;
};
//SIFTDetector
class RTABMAP_EXP SIFTDetector : public KeypointDetector
{
public:
SIFTDetector(const ParametersMap & parameters = ParametersMap());
virtual ~SIFTDetector();
virtual void parseParameters(const ParametersMap & parameters);
private:
virtual std::vector<cv::KeyPoint> _generateKeypoints(const cv::Mat & image, const cv::Rect & roi) const;
private:
int _nfeatures;
int _nOctaveLayers;
double _contrastThreshold;
double _edgeThreshold;
double _sigma;
};
}
#endif /* KEYPOINTDESCRIPTOR_H_ */

45
corelib/include/rtabmap/core/Image.h Normal file
View File

@@ -0,0 +1,45 @@
/*
* Image.h
*
* Created on: 2012-12-08
* Author: mathieu
*/
#ifndef IMAGE_H_
#define IMAGE_H_
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
namespace rtabmap
{
class Image
{
public:
Image(const cv::Mat & image = cv::Mat(),
const cv::Mat & descriptors = cv::Mat(),
const std::vector<cv::KeyPoint> & keypoints = std::vector<cv::KeyPoint>()) :
_image(image),
_descriptors(descriptors),
_keypoints(keypoints)
{
}
bool empty() const {return _image.empty() && _descriptors.empty() && _keypoints.size() == 0;}
const cv::Mat & image() const {return _image;}
const cv::Mat & descriptors() const {return _descriptors;}
const std::vector<cv::KeyPoint> & keypoints() const {return _keypoints;}
private:
cv::Mat _image;
cv::Mat _descriptors;
std::vector<cv::KeyPoint> _keypoints;
};
}
#endif /* IMAGE_H_ */

139
corelib/include/rtabmap/core/KeypointDescriptor.h
View File

@@ -1,139 +0,0 @@
/*
* Copyright (C) 2010-2011, Mathieu Labbe and IntRoLab - Universite de Sherbrooke
*
* This file is part of RTAB-Map.
*
* RTAB-Map is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* RTAB-Map is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with RTAB-Map. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef KEYPOINTDESCRIPTOR_H_
#define KEYPOINTDESCRIPTOR_H_
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <list>
#include "rtabmap/core/Parameters.h"
namespace rtabmap {
class RTABMAP_EXP KeypointDescriptor {
public:
enum DescriptorType {kDescriptorSurf, kDescriptorSift, kDescriptorBrief, kDescriptorColor, kDescriptorHue, kDescriptorUndef};
public:
virtual ~KeypointDescriptor();
virtual void parseParameters(const ParametersMap & parameters);
virtual cv::Mat generateDescriptors(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const = 0;
protected:
KeypointDescriptor(const ParametersMap & parameters = ParametersMap());
};
//SURFDescriptor
class RTABMAP_EXP SURFDescriptor : public KeypointDescriptor
{
public:
SURFDescriptor(const ParametersMap & parameters = ParametersMap());
virtual ~SURFDescriptor();
virtual void parseParameters(const ParametersMap & parameters);
virtual cv::Mat generateDescriptors(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
private:
double _hessianThreshold;
int _nOctaves;
int _nOctaveLayers;
bool _extended;
bool _upright;
bool _gpuVersion;
};
//SIFTDescriptor
class RTABMAP_EXP SIFTDescriptor : public KeypointDescriptor
{
public:
SIFTDescriptor(const ParametersMap & parameters = ParametersMap());
virtual ~SIFTDescriptor();
virtual void parseParameters(const ParametersMap & parameters);
virtual cv::Mat generateDescriptors(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
private:
int _nfeatures;
int _nOctaveLayers;
double _contrastThreshold;
double _edgeThreshold;
double _sigma;
};
//BRIEFDescriptor
class RTABMAP_EXP BRIEFDescriptor : public KeypointDescriptor
{
public:
BRIEFDescriptor(const ParametersMap & parameters = ParametersMap());
virtual ~BRIEFDescriptor();
virtual void parseParameters(const ParametersMap & parameters);
virtual cv::Mat generateDescriptors(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
private:
int _size;
};
//MinMax ColorDescriptor
class RTABMAP_EXP ColorDescriptor : public KeypointDescriptor
{
public:
ColorDescriptor(const ParametersMap & parameters = ParametersMap());
virtual ~ColorDescriptor();
virtual void parseParameters(const ParametersMap & parameters);
virtual cv::Mat generateDescriptors(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
protected:
void getCircularROI(int R, std::vector<int> & RxV) const;
};
//MinMax HueDescriptor
class RTABMAP_EXP HueDescriptor : public ColorDescriptor
{
public:
HueDescriptor(const ParametersMap & parameters = ParametersMap());
virtual ~HueDescriptor();
virtual void parseParameters(const ParametersMap & parameters);
virtual cv::Mat generateDescriptors(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
private:
// assuming that rgb values are normalized [0,1]
float rgb2hue(float r, float g, float b) const;
// assuming that rgb values are normalized [0,1]
inline float rgb2saturation(float r, float g, float b) const
{
float min = r;
min<g?min=g:min;
min<b?min=b:min;
float eps = 0.00001f;
return 1-(3*min)/(r+g+b+eps);
}
// assuming that rgb values are normalized [0,1]
inline float rgb2intensity(float r, float g, float b) const
{
return (r+g+b)/3;
}
};
}
#endif /* KEYPOINTDESCRIPTOR_H_ */

86
corelib/include/rtabmap/core/KeypointMemory.h
View File

@@ -1,86 +0,0 @@
/*
* Copyright (C) 2010-2011, Mathieu Labbe and IntRoLab - Universite de Sherbrooke
*
* This file is part of RTAB-Map.
*
* RTAB-Map is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* RTAB-Map is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with RTAB-Map. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef KEYPOINTMEMORY_H_
#define KEYPOINTMEMORY_H_
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
#include "rtabmap/core/Memory.h"
#include <opencv2/features2d/features2d.hpp>
namespace rtabmap {
class VWDictionary;
class VisualWord;
class KeypointDetector;
class KeypointDescriptor;
class RTABMAP_EXP KeypointMemory : public Memory
{
public:
KeypointMemory(const ParametersMap & parameters = ParametersMap());
virtual ~KeypointMemory();
virtual void parseParameters(const ParametersMap & parameters);
virtual bool init(const std::string & dbDriverName, const std::string & dbUrl, bool dbOverwritten = false, const ParametersMap & parameters = ParametersMap());
virtual std::map<int, float> computeLikelihood(const Signature * signature, const std::list<int> & ids, float & maximumScore);
virtual int forget(const std::set<int> & ignoredIds = std::set<int>());
virtual std::set<int> reactivateSignatures(const std::list<int> & ids, unsigned int maxLoaded, double & timeDbAccess);
virtual void dumpMemory(std::string directory) const;
virtual void dumpSignatures(const char * fileNameSign) const;
void dumpDictionary(const char * fileNameRef, const char * fileNameDesc) const;
const KeypointDetector * getKeypointDetector() const {return _keypointDetector;}
const KeypointDescriptor * getKeypointDescriptor() const {return _keypointDescriptor;}
const VWDictionary * getVWD() const {return _vwd;}
std::multimap<int, cv::KeyPoint> getWords(int signatureId) const;
protected:
virtual Signature * getSignatureLtMem(int id);
virtual void addSignatureToStm(Signature * signature, const std::list<Actuator> & actions = std::list<Actuator>());
virtual void clear();
virtual void moveToTrash(Signature * s);
virtual void preUpdate();
private:
virtual void copyData(const Signature * from, Signature * to);
virtual Signature * createSignature(int id, const std::list<Sensor> & sensors, bool keepRawData=false);
void disableWordsRef(int signatureId);
void enableWordsRef(const std::list<int> & signatureIds);
void cleanUnusedWords();
int getNi(int signatureId) const;
private:
VWDictionary * _vwd;
KeypointDetector * _keypointDetector;
KeypointDescriptor * _keypointDescriptor;
//std::map<int, int> _wordRefsToChange;
bool _reactivatedWordsComparedToNewWords;
float _badSignRatio;;
bool _tfIdfLikelihoodUsed;
bool _parallelized;
bool _tfIdfNormalized;
};
}
#endif /* KEYPOINTMEMORY_H_ */

118
corelib/include/rtabmap/core/Memory.h
View File

@@ -24,22 +24,24 @@
#include "utilite/UEventsHandler.h"
#include "rtabmap/core/Parameters.h"
#include "utilite/UVariant.h"
#include "rtabmap/core/Image.h"
#include <typeinfo>
#include <list>
#include <map>
#include <set>
#include "utilite/UStl.h"
#include <opencv2/core/core.hpp>
#include "rtabmap/core/Sensor.h"
#include "rtabmap/core/Actuator.h"
#include <opencv2/features2d/features2d.hpp>
namespace rtabmap {
class Signature;
class NeighborLink;
class DBDriver;
class Node;
class GraphNode;
class VWDictionary;
class VisualWord;
class KeypointDetector;
class KeypointDescriptor;
class RTABMAP_EXP Memory
{
@@ -53,60 +55,48 @@ public:
virtual ~Memory();
virtual void parseParameters(const ParametersMap & parameters);
bool update(const std::list<Sensor> & sensors,
const std::list<Actuator> & actuators,
std::map<std::string, float> & stats);
virtual bool init(const std::string & dbDriverName,
const std::string & dbUrl,
bool update(const Image & image, std::map<std::string, float> & stats);
bool init(const std::string & dbUrl,
bool dbOverwritten = false,
const ParametersMap & parameters = ParametersMap());
virtual std::map<int, float> computeLikelihood(const Signature * signature,
const std::list<int> & ids,
float & maximumScore);
virtual int forget(const std::set<int> & ignoredIds = std::set<int>());
virtual std::set<int> reactivateSignatures(const std::list<int> & ids,
unsigned int maxLoaded,
double & timeDbAccess);
std::map<int, float> computeLikelihood(const Signature * signature,
const std::list<int> & ids);
int forget(const std::set<int> & ignoredIds = std::set<int>());
std::set<int> reactivateSignatures(const std::list<int> & ids, unsigned int maxLoaded, double & timeDbAccess);
int cleanup(const std::list<int> & ignoredIds = std::list<int>());
void emptyTrash();
void joinTrashThread();
bool addLoopClosureLink(int oldId, int newId);
std::map<int, int> getNeighborsId(double & dbAccessTime,
int signatureId,
std::map<int, int> getNeighborsId(int signatureId,
unsigned int margin,
int maxCheckedInDatabase = -1,
bool onlyWithActions = false,
bool incrementMarginOnLoop = false,
bool ignoreSTM = true,
bool ignoreLoopIds = false) const;
float compareOneToOne(const std::vector<int> & idsA, const std::vector<int> & idsB);
bool ignoreLoopIds = false,
double * dbAccessTime = 0) const;
//getters
unsigned int getWorkingMemSize() const {return _workingMem.size();}
unsigned int getStMemSize() const {return _stMem.size();};
const std::set<int> & getWorkingMem() const {return _workingMem;}
const std::set<int> & getStMem() const {return _stMem;}
std::list<NeighborLink> getNeighborLinks(int signatureId,
int getMaxStMemSize() const {return _maxStMemSize;}
std::set<int> getNeighborLinks(int signatureId,
bool ignoreNeighborByLoopClosure = false,
bool lookInDatabase = false,
bool onlyWithActions = false) const;
bool lookInDatabase = false) const;
void getLoopClosureIds(int signatureId,
std::set<int> & loopClosureIds,
std::set<int> & childLoopClosureIds,
bool lookInDatabase = false) const;
bool isRawDataKept() const {return _rawDataKept;}
float getSimilarityThr() const {return _similarityThreshold;}
float getSimilarityThreshold() const {return _similarityThreshold;}
std::map<int, int> getWeights() const;
int getWeight(int id) const;
const std::vector<int> & getLastBaseIds() const {return _lastBaseIds;}
float getSimilarityOnlyLast() const {return _similarityOnlyLast;}
const std::map<int, std::map<int, float> > & getSimilaritiesMap() const {return _similaritiesMap;}
float getSimilarityOnlyWithLast() const {return _rehearsalOnlyWithLast;}
const Signature * getLastSignature() const;
int getDatabaseMemoryUsed() const; // in bytes
double getDbSavingTime() const;
std::list<Sensor> getRawData(int id) const;
bool isCommonSignatureUsed() const {return _commonSignatureUsed;}
cv::Mat getImage(int signatureId) const;
std::set<int> getAllSignatureIds() const;
bool memoryChanged() const {return _memoryChanged;}
const Signature * getSignature(int id) const;
@@ -115,33 +105,32 @@ public:
bool isInLTM(int signatureId) const {return !this->isInSTM(signatureId) && !this->isInWM(signatureId);}
//setters
void setSimilarityThreshold(float similarityThreshold);
void setSimilarityOnlyLast(int similarityOnlyLast) {_similarityOnlyLast = similarityOnlyLast;}
void setSimilarityThreshold(float similarity);
void setSimilarityOnlyLast(int rehearsalOnlyWithLast) {_rehearsalOnlyWithLast = rehearsalOnlyWithLast;}
void setOldSignatureRatio(float oldSignatureRatio);
void setMaxStMemSize(unsigned int maxStMemSize);
void setRecentWmRatio(float recentWmRatio);
void setCommonSignatureUsed(bool commonSignatureUsed);
void setRawDataKept(bool rawDataKept) {_rawDataKept = rawDataKept;}
void dumpMemoryTree(const char * fileNameTree) const;
virtual void dumpMemory(std::string directory) const;
virtual void dumpSignatures(const char * fileNameSign) const {}
virtual void dumpSignatures(const char * fileNameSign) const;
void dumpDictionary(const char * fileNameRef, const char * fileNameDesc) const;
void generateGraph(const std::string & fileName, std::set<int> ids = std::set<int>());
void cleanLocalGraph(int id, unsigned int margin);
void cleanLTM(int maxDepth = 10);
void createGraph(Node * parent,
void createGraph(GraphNode * parent,
unsigned int maxDepth,
const std::set<int> & endIds = std::set<int>());
protected:
virtual void preUpdate();
virtual void postUpdate() {}
//keypoint stuff
int getVWDictionarySize() const;
std::multimap<int, cv::KeyPoint> getWords(int signatureId) const;
virtual void addSignatureToStm(Signature * signature,
const std::list<Actuator> & actuators = std::list<Actuator>());
virtual void clear();
virtual void moveToTrash(Signature * s);
virtual Signature * getSignatureLtMem(int id);
protected:
void preUpdate();
void addSignatureToStm(Signature * signature);
void clear();
void moveToTrash(Signature * s);
void addSignatureToWm(Signature * signature);
Signature * _getSignature(int id) const;
@@ -154,24 +143,27 @@ protected:
const std::map<int, Signature*> & getSignatures() const {return _signatures;}
private:
virtual void copyData(const Signature * from, Signature * to) = 0;
virtual Signature * createSignature(int id,
const std::list<Sensor> & sensors,
bool keepRawData=false) = 0;
void copyData(const Signature * from, Signature * to);
Signature * createSignature(int id,
const Image & image,
bool keepRawData=false);
//keypoint stuff
void disableWordsRef(int signatureId);
void enableWordsRef(const std::list<int> & signatureIds);
void cleanUnusedWords();
int getNi(int signatureId) const;
void createVirtualSignature(Signature ** signature);
void cleanGraph(const Node * root);
protected:
DBDriver * _dbDriver;
private:
// parameters
float _similarityThreshold;
bool _similarityOnlyLast;
bool _rehearsalOnlyWithLast;
bool _rawDataKept;
bool _incrementalMemory;
unsigned int _maxStMemSize;
bool _commonSignatureUsed;
int _maxStMemSize;
float _recentWmRatio;
bool _dataMergedOnRehearsal;
@@ -179,14 +171,20 @@ private:
Signature * _lastSignature;
int _lastLoopClosureId;
bool _memoryChanged; // False by default, become true when Memory::update() is called.
bool _merging;
int _signaturesAdded;
std::map<int, Signature *> _signatures; // TODO : check if a signature is already added? although it is not supposed to occur...
std::set<int> _stMem; // id
std::set<int> _workingMem; // id,age
std::vector<int> _lastBaseIds;
std::map<int, std::map<int, float> > _similaritiesMap;
//Heypoint stuff
VWDictionary * _vwd;
KeypointDetector * _keypointDetector;
KeypointDescriptor * _keypointDescriptor;
bool _reactivatedWordsComparedToNewWords;
float _badSignRatio;;
bool _tfIdfLikelihoodUsed;
bool _parallelized;
};
} // namespace rtabmap

134
corelib/include/rtabmap/core/Micro.h
View File

@@ -1,134 +0,0 @@
/*
* Micro.h
*
* Created on: Mar 5, 2012
* Author: MatLab
*/
#ifndef MICRO_H_
#define MICRO_H_
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
#include <utilite/UThreadNode.h>
#include <utilite/UTimer.h>
#include <utilite/UEvent.h>
#include <utilite/ULogger.h>
#include <string>
#include <vector>
#include <opencv2/core/core.hpp>
class UAudioRecorder;
namespace rtabmap {
class MicroEvent :
public UEvent
{
public:
enum Type {
kTypeFrame,
kTypeFrameFreq,
kTypeFrameFreqSqrdMagn,
kTypeNoMoreFrames
};
public:
// kTypeNoMoreFrames constructor
MicroEvent(int microId = 0) :
UEvent(kTypeNoMoreFrames),
_sampleSize(0),
_microId(microId)
{
}
// kTypeFrame constructor
MicroEvent(const cv::Mat & frame,
int sampleSize,
int fs,
int channels,
int microId = 0) :
UEvent(kTypeFrame),
_frame(frame),
_sampleSize(sampleSize),
_microId(microId)
{
}
// kTypeFrameFreq and kTypeFrameFreqSqrdMagn constructors
MicroEvent(Type frameType,
const cv::Mat & frameFreq,
int fs,
int channels,
int microId = 0) :
UEvent(frameType),
_frame(frameFreq),
_sampleSize(sizeof(float)),
_microId(microId)
{
UASSERT(frameType == kTypeFrameFreqSqrdMagn || frameType == kTypeFrameFreq);
}
int type() const {return this->getCode();}
const cv::Mat & frame() const {return _frame;}
int sampleSize() const {return _sampleSize;}
int microId() const {return _microId;}
virtual ~MicroEvent() {}
virtual std::string getClassName() const {return std::string("MicroEvent");}
private:
cv::Mat _frame;
int _sampleSize; // bytes
int _fs; //sampling rate
int _microId;
};
class RTABMAP_EXP Micro : public UThreadNode
{
typedef float fftwf_complex[2];
public:
Micro(MicroEvent::Type eventType,
int deviceId,
int fs,
int frameLength,
int channels,
int bytesPerSample,
int id = 0);
Micro(MicroEvent::Type eventType,
const std::string & path,
bool simulateFrameRate,
int frameLength,
int id = 0,
bool playWhileRecording = false);
virtual ~Micro();
bool init();
void stop(); // same as kill() but handle the case where underlying recorder is running and not the micro.
void startRecorder(); // must only be used if Micro::start() is not used
cv::Mat getFrame();
cv::Mat getFrame(cv::Mat & frameFreq, bool sqrdMagn = false);
int fs();
int bytesPerSample();
int channels();
int nfft();
protected:
virtual void mainLoopBegin();
virtual void mainLoop();
virtual void mainLoopKill();
private:
MicroEvent::Type _eventType;
UAudioRecorder* _recorder;
bool _simulateFreq;
UTimer _timer;
std::vector<float> _window;
std::vector<float> _in;
fftwf_complex * _out; // fftwf_complex
void * _p; // fftwf_plan
int _id;
};
}
#endif /* MICRO_H_ */

123
corelib/include/rtabmap/core/Parameters.h
View File

@@ -120,74 +120,55 @@ typedef std::pair<const std::string, std::string> ParametersPair;
class RTABMAP_EXP Parameters
{
// Rtabmap parameters
RTABMAP_PARAM(Rtabmap, VhStrategy, int, 0); // None 0, Similarity 1, Epipolar 2
RTABMAP_PARAM(Rtabmap, PublishStats, bool, true); // Publishing statistics
RTABMAP_PARAM(Rtabmap, PublishRawData, bool, true); // Publishing raw data
RTABMAP_PARAM(Rtabmap, PublishPdf, bool, true); // Publishing pdf
RTABMAP_PARAM(Rtabmap, PublishLikelihood, bool, true); // Publishing likelihood
RTABMAP_PARAM(Rtabmap, RetrievalThr, float, 0.0); // Reactivation threshold
RTABMAP_PARAM(Rtabmap, TimeThr, float, 700.0); // Maximum time allowed for the detector (ms) (0 means infinity)
RTABMAP_PARAM(Rtabmap, MemoryThr, int, 0); // Maximum signatures in the Working Memory (ms) (0 means infinity)
RTABMAP_PARAM(Rtabmap, SMStateBufferSize, int, 0); // Data buffer size (0 min inf)
RTABMAP_PARAM(Rtabmap, VhStrategy, int, 0); // None 0, Similarity 1, Epipolar 2
RTABMAP_PARAM(Rtabmap, PublishStats, bool, true); // Publishing statistics
RTABMAP_PARAM(Rtabmap, PublishImage, bool, true); // Publishing image
RTABMAP_PARAM(Rtabmap, PublishPdf, bool, true); // Publishing pdf
RTABMAP_PARAM(Rtabmap, PublishLikelihood, bool, true); // Publishing likelihood
RTABMAP_PARAM(Rtabmap, TimeThr, float, 700.0); // Maximum time allowed for the detector (ms) (0 means infinity)
RTABMAP_PARAM(Rtabmap, MemoryThr, int, 0); // Maximum signatures in the Working Memory (ms) (0 means infinity)
RTABMAP_PARAM(Rtabmap, ImageBufferSize, int, 0); // Data buffer size (0 min inf)
RTABMAP_PARAM_STR(Rtabmap, WorkingDirectory, Parameters::getDefaultWorkingDirectory()); // Working directory
RTABMAP_PARAM(Rtabmap, MaxRetrieved, unsigned int, 2); // Maximum locations retrieved at the same time from LTM
RTABMAP_PARAM(Rtabmap, SelectionNeighborhoodSummationUsed, bool, false); // Neighborhood summation for hypothesis selection
RTABMAP_PARAM(Rtabmap, SelectionLikelihoodUsed, bool, false); // Neighborhood likelihood for hypothesis selection
RTABMAP_PARAM(Rtabmap, ActionsSentRejectHyp, bool, true); // Actions sent also on rejected hypotheses (on decreasing hypotheses)
RTABMAP_PARAM(Rtabmap, ConfidenceThr, float, 0.0); // Actions are not sent when the loop closure hypothesis is under the confidence threshold
RTABMAP_PARAM(Rtabmap, LikelihoodStdDevRemoved, bool, true); // Remove std dev on likelihood normalization.
RTABMAP_PARAM(Rtabmap, LikelihoodNullValuesIgnored, bool, true); // Ignore null values on likelihood normalization
RTABMAP_PARAM(Rtabmap, MaxRetrieved, unsigned int, 2); // Maximum locations retrieved at the same time from LTM
RTABMAP_PARAM(Rtabmap, LikelihoodNullValuesIgnored, bool, true); // Ignore null values on likelihood normalization
RTABMAP_PARAM(Rtabmap, StatisticLogsBufferedInRAM, bool, true); // Statistic logs buffered in RAM instead of written to hard drive after each iteration.
// Hypotheses selection
RTABMAP_PARAM(Rtabmap, LoopThr, float, 0.15); // Loop closing threshold
RTABMAP_PARAM(Rtabmap, LoopRatio, float, 0.9); // The loop closure hypothesis must be over LoopRatio x lastHypothesisValue
// Memory
RTABMAP_PARAM(Mem, SimilarityThr, float, 0.20); // Similarity between the last signature and neighbor
RTABMAP_PARAM(Mem, SimilarityOnlyLast, bool, false); // Only compare to the last signature in STM, otherwise it compares to all signatures in STM
RTABMAP_PARAM(Mem, RawDataKept, bool, true); // Keep raw data
RTABMAP_PARAM(Mem, MaxStMemSize, unsigned int, 30); // Short-time memory size
RTABMAP_PARAM(Mem, CommonSignatureUsed, bool, true); // A common signature/virtual place is automatically updated with id -1
RTABMAP_PARAM(Mem, IncrementalMemory, bool, true);
RTABMAP_PARAM(Mem, RecentWmRatio, float, 0.2); // Ratio of locations after the last loop closure in WM that cannot be transferred
RTABMAP_PARAM(Mem, DataMergedOnRehearsal, bool, true); // Merge data on rehearsal
RTABMAP_PARAM(Mem, SignatureType, int, 0); // Keypoint 0, Sensorimotor 1
RTABMAP_PARAM(Mem, RehearsalSimilarity, float, 0.2); // Rehearsal mean for each sensor
RTABMAP_PARAM(Mem, RehearsalOnlyWithLast, bool, true); // Only compare to the last signature in STM, otherwise it compares to all signatures in STM
RTABMAP_PARAM(Mem, ImageKept, bool, true); // Keep image
RTABMAP_PARAM(Mem, STMSize, unsigned int, 30); // Short-term memory size
RTABMAP_PARAM(Mem, IncrementalMemory, bool, true);
RTABMAP_PARAM(Mem, RecentWmRatio, float, 0.2); // Ratio of locations after the last loop closure in WM that cannot be transferred
RTABMAP_PARAM(Mem, DataMergedOnRehearsal, bool, true); // Merge data on rehearsal
// KeypointMemory (Keypoint-based)
RTABMAP_PARAM(Kp, PublishKeypoints, bool, true); // Publishing keypoints
RTABMAP_PARAM(Kp, NNStrategy, int, 2); // Naive 0, kdTree 1, kdForest 2
RTABMAP_PARAM(Kp, PublishKeypoints, bool, true); // Publishing keypoints
RTABMAP_PARAM(Kp, NNStrategy, int, 1); // Naive 0, kdForest 1
RTABMAP_PARAM(Kp, IncrementalDictionary, bool, true);
RTABMAP_PARAM(Kp, WordsPerImage, int, 400);
RTABMAP_PARAM(Kp, BadSignRatio, float, 0.2); //Bad signature ratio (less than Ratio x AverageWordsPerImage = bad)
RTABMAP_PARAM(Kp, MinDistUsed, bool, false); // The nearest neighbor must have a distance < minDist
RTABMAP_PARAM(Kp, MinDist, float, 0.05); // Matching a descriptor with a word (euclidean distance ^ 2)
RTABMAP_PARAM(Kp, NndrUsed, bool, true); // If NNDR ratio is used
RTABMAP_PARAM(Kp, NndrRatio, float, 0.8); // NNDR ratio (A matching pair is detected, if its distance is closer than X times the distance of the second nearest neighbor.)
RTABMAP_PARAM(Kp, MaxLeafs, int, 64); // Maximum number of leafs checked (when using kd-trees)
RTABMAP_PARAM(Kp, DetectorStrategy, int, 0); // Surf detector 0, Star detector 1, SIFT detector 2, FAST detector 3
RTABMAP_PARAM(Kp, DescriptorStrategy, int, 0); // kDescriptorSurf=0, kDescriptorSift, kDescriptorBrief, kDescriptorColor, kDescriptorHue, kDescriptorUndef
RTABMAP_PARAM(Kp, WordsPerImage, int, 400);
RTABMAP_PARAM(Kp, BadSignRatio, float, 0.2); //Bad signature ratio (less than Ratio x AverageWordsPerImage = bad)
RTABMAP_PARAM(Kp, MinDistUsed, bool, false); // The nearest neighbor must have a distance < minDist
RTABMAP_PARAM(Kp, MinDist, float, 0.05); // Matching a descriptor with a word (euclidean distance ^ 2)
RTABMAP_PARAM(Kp, NndrUsed, bool, true); // If NNDR ratio is used
RTABMAP_PARAM(Kp, NndrRatio, float, 0.8); // NNDR ratio (A matching pair is detected, if its distance is closer than X times the distance of the second nearest neighbor.)
RTABMAP_PARAM(Kp, MaxLeafs, int, 64); // Maximum number of leafs checked (when using kd-trees)
RTABMAP_PARAM(Kp, DetectorStrategy, int, 0); // Surf detector 0, SIFT detector 1, undef 2
RTABMAP_PARAM(Kp, DescriptorStrategy, int, 0); // kDescriptorSurf=0, kDescriptorSift, kDescriptorUndef
RTABMAP_PARAM(Kp, ReactivatedWordsComparedToNewWords, bool, true); //Reactivated words are compared to the last words added in the dictionary (which are not indexed)
RTABMAP_PARAM(Kp, TfIdfLikelihoodUsed, bool, false); // Use of the td-idf strategy to compute the likelihood
RTABMAP_PARAM(Kp, Parallelized, bool, true); // If the dictionary update and signature creation were parallelized
RTABMAP_PARAM(Kp, TfIdfNormalized, bool, false); // If tf-idf weighting is normalized by the words count ratio between compared signatures
RTABMAP_PARAM_STR(Kp, RoiRatios, "0.0 0.0 0.0 0.0"); // Region of interest ratios [left, right, top, bottom]
RTABMAP_PARAM_STR(Kp, DictionaryPath, ""); // Path of the pre-computed dictionary
// SM memory
RTABMAP_PARAM(SM, PublishMasks, bool, false); // Publishing motion masks
RTABMAP_PARAM(SM, MotionMaskUsed, bool, false); // Use motion mask
RTABMAP_PARAM(SM, LogPolarUsed, bool, false); // Use log-polar images
RTABMAP_PARAM(SM, ColorTable, int, 8); // Color table size 0=8, 1=16, 2=32, 3=64, 4=128, 5=256, 6=512, 7=1024, 8=65536
RTABMAP_PARAM(SM, AudioDBThreshold, float, 0.0f); // Audio dB threshold
RTABMAP_PARAM(SM, AudioDBIndexing, bool, true); // dB (decibel) indexing (otherwise it's squared magnitude indexing)
RTABMAP_PARAM(SM, MagnitudeInvariant, bool, false); // Make audio signature magnitude-invariant
RTABMAP_PARAM(Kp, TfIdfLikelihoodUsed, bool, false); // Use of the td-idf strategy to compute the likelihood
RTABMAP_PARAM(Kp, Parallelized, bool, true); // If the dictionary update and signature creation were parallelized
RTABMAP_PARAM_STR(Kp, RoiRatios, "0.0 0.0 0.0 0.0"); // Region of interest ratios [left, right, top, bottom]
RTABMAP_PARAM_STR(Kp, DictionaryPath, ""); // Path of the pre-computed dictionary
//Database
RTABMAP_PARAM(Db, MinSignaturesToSave, int, 20); // Minimum signatures needed in the trash to save them (empty trash thread)
RTABMAP_PARAM(Db, MinWordsToSave, int, 4000); // Minimum visual words needed in the trash to save them (empty trash thread)
RTABMAP_PARAM(Db, ImagesCompressed, bool, true); // Images are compressed when save to database
RTABMAP_PARAM(Db, ImagesCompressed, bool, true); // Images are compressed when saving to database
RTABMAP_PARAM(DbSqlite3, InMemory, bool, false); // Using database in the memory instead of a file on the hard disk
RTABMAP_PARAM(DbSqlite3, CacheSize, unsigned int, 2000); // Sqlite cache size (default is 2000)
RTABMAP_PARAM(DbSqlite3, CacheSize, unsigned int, 10000); // Sqlite cache size (default is 2000)
RTABMAP_PARAM(DbSqlite3, JournalMode, int, 3); // 0=DELETE, 1=TRUNCATE, 2=PERSIST, 3=MEMORY, 4=OFF (see sqlite3 doc : "PRAGMA journal_mode")
RTABMAP_PARAM(DbSqlite3, Synchronous, int, 0); // 0=OFF, 1=NORMAL, 2=FULL (see sqlite3 doc : "PRAGMA synchronous")
RTABMAP_PARAM(DbSqlite3, TempStore, int, 2); // 0=DEFAULT, 1=FILE, 2=MEMORY (see sqlite3 doc : "PRAGMA temp_store")
@@ -200,37 +181,33 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(SURF, Upright, bool, false); // U-SURF
RTABMAP_PARAM(SURF, GpuVersion, bool, false);
RTABMAP_PARAM(SIFT, NFeatures, int, 0);
RTABMAP_PARAM(SIFT, NOctaveLayers, int, 3);
RTABMAP_PARAM(SIFT, NFeatures, int, 0);
RTABMAP_PARAM(SIFT, NOctaveLayers, int, 3);
RTABMAP_PARAM(SIFT, ContrastThreshold, double, 0.04);
RTABMAP_PARAM(SIFT, EdgeThreshold, double, 10.0);
RTABMAP_PARAM(SIFT, Sigma, double, 1.6);
RTABMAP_PARAM(FAST, Threshold, int, 10);
RTABMAP_PARAM(FAST, NonmaxSuppression, bool, true);
RTABMAP_PARAM(BRIEF, Size, int, 32); // 16, 32, 64
RTABMAP_PARAM(Star, MaxSize, int, 45);
RTABMAP_PARAM(Star, ResponseThreshold, int, 30);
RTABMAP_PARAM(Star, LineThresholdProjected, int, 10);
RTABMAP_PARAM(Star, LineThresholdBinarized, int, 8);
RTABMAP_PARAM(Star, SuppressNonmaxSize, int, 5);
RTABMAP_PARAM(SIFT, EdgeThreshold, double, 10.0);
RTABMAP_PARAM(SIFT, Sigma, double, 1.6);
// BayesFilter
RTABMAP_PARAM(Bayes, VirtualPlacePriorThr, float, 0.9); // Virtual place prior
RTABMAP_PARAM(Bayes, VirtualPlacePriorThr, float, 0.9); // Virtual place prior
RTABMAP_PARAM_STR(Bayes, PredictionLC, "0.1 0.36 0.30 0.16 0.062 0.0151 0.00255 0.000324 2.5e-05 1.3e-06 4.8e-08 1.2e-09 1.9e-11 2.2e-13 1.7e-15 8.5e-18 2.9e-20 6.9e-23"); // Prediction of loop closures (Gaussian-like, here with sigma=1.6) - Format: {VirtualPlaceProb, LoopClosureProb, NeighborLvl1, NeighborLvl2, ...}
RTABMAP_PARAM(Bayes, PredictionOnNonNullActionsOnly, bool, false); // Make prediction on non-null action neighbors only
RTABMAP_PARAM(Bayes, FullPredictionUpdate, bool, true); // Regenerate all the prediction matrix on each iteration (otherwise only removed/added ids are updated).
// Verify hypotheses
RTABMAP_PARAM(Vh, Similarity, float, 0.5); // Minimum similarity to accept an hypothesis
RTABMAP_PARAM(VhEp, MatchCountMin, int, 8); // Minimum of matching visual words pairs to accept the loop hypothesis
RTABMAP_PARAM(VhEp, RansacParam1, float, 3.0); // Fundamental matrix (see cvFindFundamentalMat()): Max distance (in pixels) from the epipolar line for a point to be inlier
RTABMAP_PARAM(VhEp, RansacParam2, float, 0.99); // Fundamental matrix (see cvFindFundamentalMat()): Performance of the RANSAC
public:
virtual ~Parameters();
static const ParametersMap & getDefaultParameters();
/**
* Get default parameters
*
*/
static const ParametersMap & getDefaultParameters()
{
return parameters_;
}
private:
Parameters();

54
corelib/include/rtabmap/core/Rtabmap.h
View File

@@ -29,8 +29,7 @@
#include "utilite/UVariant.h"
#include "rtabmap/core/RtabmapEvent.h"
#include "rtabmap/core/Parameters.h"
#include "rtabmap/core/Sensor.h"
#include "rtabmap/core/Actuator.h"
#include "rtabmap/core/Image.h"
#include <opencv2/core/core.hpp>
#include <list>
#include <stack>
@@ -39,9 +38,7 @@
namespace rtabmap
{
class Signature;
class HypVerificator;
class EpipolarGeometry;
class Memory;
class BayesFilter;
@@ -58,11 +55,12 @@ public:
kStateDumpingMemory,
kStateDumpingPrediction,
kStateGeneratingGraph,
kStateGeneratingLocalGraph,
kStateDeletingMemory,
kStateCleanSensorsBuffer
};
enum VhStrategy {kVhNone, kVhSim, kVhEpipolar, kVhUndef};
enum VhStrategy {kVhNone, kVhEpipolar, kVhUndef};
static const char * kDefaultIniFileName;
static const char * kDefaultIniFilePath;
@@ -78,9 +76,10 @@ public:
Rtabmap();
virtual ~Rtabmap();
void process(const std::list<Sensor> & data);
void process(const Sensor & data); // for convenience when only one sensor is used
void process(const cv::Mat & image); // for convenience
void process(const Image & image); // for convenience
void dumpData();
void generateLocalGraph(const std::string & path, int id, int margin);
void init(const ParametersMap & param);
void init(const char * configFile = 0);
@@ -90,23 +89,19 @@ public:
int getLoopClosureId() const;
int getReactivatedId() const;
int getLastSignatureId() const;
const std::list<Actuator> & getActuator() const {return _actuators;}
float getLcHypValue() const {return _lastLcHypothesisValue;}
std::list<int> getWorkingMem() const;
std::set<int> getStMem() const;
std::map<int, int> getWeights() const;
int getTotalMemSize() const;
const std::string & getGraphFileName() const {return _graphFileName;}
void setMaxTimeAllowed(float maxTimeAllowed); // in ms
void setDataBufferSize(int size);
void setWorkingDirectory(std::string path);
void setGraphFileName(const std::string & fileName) {_graphFileName = fileName;}
void adjustLikelihood(std::map<int, float> & likelihood) const;
std::pair<int, float> selectHypothesis(const std::map<int, float> & posterior,
const std::map<int, float> & likelihood,
bool neighborSumUsed,
bool likelihoodUsed) const;
const std::map<int, float> & likelihood) const;
protected:
virtual void handleEvent(UEvent * anEvent);
@@ -117,9 +112,10 @@ private:
virtual void mainLoopBegin();
void process();
void resetMemory(bool dbOverwritten = false);
void addSensorimotor(const std::list<Sensor> & sensors, const std::list<Actuator> & actuators);
void getSensorimotor(std::list<Sensor> & sensors, std::list<Actuator> & actuators);
void addImage(const Image & image);
void getImage(Image & image);
void setupLogFiles(bool overwrite = false);
void flushStatisticLogs();
void releaseAllStrategies();
void pushNewState(State newState, const ParametersMap & parameters = ParametersMap());
void dumpPrediction() const;
@@ -128,51 +124,45 @@ private:
private:
// Modifiable parameters
bool _publishStats;
bool _publishRawData;
bool _publishImage;
bool _publishPdf;
bool _publishLikelihood;
bool _publishKeypoints;
bool _publishMasks;
float _maxTimeAllowed; // in ms
unsigned int _maxMemoryAllowed; // signatures count in WM
int _sensorsBufferMaxSize;
int _imageBufferMaxSize;
float _loopThr;
float _loopRatio;
float _retrievalThr;
unsigned int _maxRetrieved;
bool _selectionNeighborhoodSummationUsed;
bool _selectionLikelihoodUsed;
bool _actionsSentRejectHyp;
float _confidenceThr;
bool _likelihoodStdDevRemoved;
bool _likelihoodNullValuesIgnored;
bool _statisticLogsBufferedInRAM;
int _lcHypothesisId;
int _reactivateId;
int _retrievedId;
float _lastLcHypothesisValue;
int _lastLoopClosureId;
std::list<Actuator> _actuators;
UMutex _stateMutex;
std::stack<State> _state;
std::stack<ParametersMap> _stateParam;
std::list<std::pair<std::list<Sensor>, std::list<Actuator> > > _sensorimotorBuffer;
UMutex _sensorimotorMutex;
USemaphore _sensorimotorAdded;
std::list<Image> _imageBuffer;
UMutex _imageMutex;
USemaphore _imageAdded;
// Abstract classes containing all loop closure
// strategies for a type of signature or configuration.
HypVerificator * _vhStrategy;
EpipolarGeometry * _epipolarGeometry;
BayesFilter * _bayesFilter;
Memory * _memory;
FILE* _foutFloat;
FILE* _foutInt;
std::list<std::string> _bufferedLogsF;
std::list<std::string> _bufferedLogsI;
std::string _wDir;
std::string _graphFileName;
};
#endif /* RTABMAP_H_ */

54
corelib/include/rtabmap/core/RtabmapEvent.h
View File

@@ -22,8 +22,6 @@
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
#include "rtabmap/core/Sensor.h"
#include "rtabmap/core/Actuator.h"
#include <utilite/UEvent.h>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
@@ -51,30 +49,25 @@ class RTABMAP_EXP Statistics
RTABMAP_STATS(Loop, Highest_hypothesis_id,);
RTABMAP_STATS(Loop, Highest_hypothesis_value,);
RTABMAP_STATS(Loop, Vp_hypothesis,);
RTABMAP_STATS(Loop, Vp_likelihood,);
RTABMAP_STATS(Loop, ReactivateId,);
RTABMAP_STATS(Loop, Hypothesis_ratio,);
RTABMAP_STATS(Loop, Actions,);
RTABMAP_STATS(Loop, Actions_of,);
RTABMAP_STATS(Loop, Actions_chosen,);
RTABMAP_STATS(Memory, Working_memory_size,);
RTABMAP_STATS(Memory, Short_time_memory_size,);
RTABMAP_STATS(Memory, Database_size, MB);
RTABMAP_STATS(Memory, Process_memory_used, MB);
RTABMAP_STATS(Memory, Signatures_removed,);
RTABMAP_STATS(Memory, Signatures_retrieved,);
RTABMAP_STATS(Memory, Images_buffered,);
RTABMAP_STATS(Memory, Similarities_map,);
RTABMAP_STATS(Memory, Loop_closures_map,);
RTABMAP_STATS(Timing, Memory_update, ms);
RTABMAP_STATS(Timing, Cleaning_neighbors, ms);
RTABMAP_STATS(Timing, Reactivation, ms);
RTABMAP_STATS(Timing, Add_loop_closure_link, ms);
RTABMAP_STATS(Timing, Likelihood_computation, ms);
RTABMAP_STATS(Timing, Posterior_computation, ms);
RTABMAP_STATS(Timing, Hypotheses_creation, ms);
RTABMAP_STATS(Timing, Hypotheses_validation, ms);
RTABMAP_STATS(Timing, Action_selection, ms);
RTABMAP_STATS(Timing, Statistics_creation, ms);
RTABMAP_STATS(Timing, Memory_cleanup, ms);
RTABMAP_STATS(Timing, Total, ms);
@@ -82,7 +75,6 @@ class RTABMAP_EXP Statistics
RTABMAP_STATS(Timing, Joining_trash, ms);
RTABMAP_STATS(Timing, Emptying_trash, ms);
RTABMAP_STATS(, Parent_id,);
RTABMAP_STATS(, Hypothesis_reactivated,);
RTABMAP_STATS(Keypoint, Dictionary_size, words);
@@ -102,31 +94,27 @@ public:
void setExtended(bool extended) {_extended = extended;}
void setRefImageId(int refImageId) {_refImageId = refImageId;}
void setLoopClosureId(int loopClosureId) {_loopClosureId = loopClosureId;}
void setActuators(const std::list<Actuator> & actuators) {_actuators = actuators;}
void setRefRawData(const std::list<Sensor> & refRawData);
void setLoopClosureRawData(const std::list<Sensor> & loopClosureRawData);
void setRefImage(const cv::Mat & image);
void setLoopImage(const cv::Mat & image);
void setWeights(const std::map<int, int> & weights) {_weights = weights;}
void setPosterior(const std::map<int, float> & posterior) {_posterior = posterior;}
void setLikelihood(const std::map<int, float> & likelihood) {_likelihood = likelihood;}
void setRawLikelihood(const std::map<int, float> & rawLikelihood) {_rawLikelihood = rawLikelihood;}
void setRefWords(const std::multimap<int, cv::KeyPoint> & refWords) {_refWords = refWords;}
void setLoopWords(const std::multimap<int, cv::KeyPoint> & loopWords) {_loopWords = loopWords;}
void setRefMotionMask(const std::vector<unsigned char> & mask) {_refMotionMask = mask;}
void setLoopMotionMask(const std::vector<unsigned char> & mask) {_loopMotionMask = mask;}
// getters
bool extended() const {return _extended;}
int refImageId() const {return _refImageId;}
int loopClosureId() const {return _loopClosureId;}
const std::list<Actuator> & getActuators() const {return _actuators;}
const std::list<Sensor> & refRawData() const {return _refRawData;}
const std::list<Sensor> & loopClosureRawData() const {return _loopClosureRawData;}
const cv::Mat & refImage() const {return _refImage;}
const cv::Mat & loopImage() const {return _loopImage;}
const std::map<int, int> & weights() const {return _weights;}
const std::map<int, float> & posterior() const {return _posterior;}
const std::map<int, float> & likelihood() const {return _likelihood;}
const std::map<int, float> & rawLikelihood() const {return _rawLikelihood;}
const std::multimap<int, cv::KeyPoint> & refWords() const {return _refWords;}
const std::multimap<int, cv::KeyPoint> & loopWords() const {return _loopWords;}
const std::vector<unsigned char> & refMotionMask() const {return _refMotionMask;}
const std::vector<unsigned char> & loopMotionMask() const {return _loopMotionMask;}
const std::map<std::string, float> & data() const {return _data;}
@@ -136,24 +124,19 @@ private:
int _refImageId;
int _loopClosureId;
std::list<Actuator> _actuators;
// extended data start here...
std::list<Sensor> _refRawData;
std::list<Sensor> _loopClosureRawData;
cv::Mat _refImage;
cv::Mat _loopImage;
std::map<int, int> _weights;
std::map<int, float> _posterior;
std::map<int, float> _likelihood;
std::map<int, float> _rawLikelihood;
//keypoint memory
std::multimap<int, cv::KeyPoint> _refWords;
std::multimap<int, cv::KeyPoint> _loopWords;
//sm memory
std::vector<unsigned char> _refMotionMask;
std::vector<unsigned char> _loopMotionMask;
// Format for statistics (Plottable statistics must go in that map) :
// {"Group/Name/Unit", value}
// Example : {"Timing/Total time/ms", 500.0f}
@@ -183,13 +166,15 @@ private:
class RtabmapEventCmd : public UEvent
{
public:
enum dummy {d}; // Hack, to fix Eclipse complaining about not defined Cmd enum ?!
enum Cmd {
kCmdResetMemory,
kCmdDumpMemory,
kCmdDumpPrediction,
kCmdGenerateGraph,
kCmdDeleteMemory,
kCmdCleanSensorsBuffer};
kCmdResetMemory,
kCmdDumpMemory,
kCmdDumpPrediction,
kCmdGenerateGraph,
kCmdGenerateLocalGraph,
kCmdDeleteMemory,
kCmdCleanSensorsBuffer};
public:
RtabmapEventCmd(Cmd cmd) :
UEvent(0),
@@ -209,6 +194,7 @@ private:
class RtabmapEventInit : public UEvent
{
public:
enum dummy {d}; // Hack, to fix Eclipse complaining about not defined Status enum ?!
enum Status {
kInitializing,
kInitialized,

57
corelib/include/rtabmap/core/SMMemory.h
View File

@@ -1,57 +0,0 @@
/*
* Copyright (C) 2010-2011, Mathieu Labbe and IntRoLab - Universite de Sherbrooke
*
* This file is part of RTAB-Map.
*
* RTAB-Map is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* RTAB-Map is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with RTAB-Map. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef SIMPLEMEMORY_H_
#define SIMPLEMEMORY_H_
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
#include "rtabmap/core/Memory.h"
namespace rtabmap {
class ColorTable;
class SMSignature;
class RTABMAP_EXP SMMemory : public Memory
{
public:
SMMemory(const ParametersMap & parameters = ParametersMap());
virtual ~SMMemory();
virtual void parseParameters(const ParametersMap & parameters);
virtual std::set<int> reactivateSignatures(const std::list<int> & ids, unsigned int maxLoaded, double & timeDbAccess);
void setRoi(const std::string & roi);
void setColorTable(int size);
private:
virtual void copyData(const Signature * from, Signature * to);
virtual Signature * createSignature(int id, const std::list<Sensor> & sensors, bool keepRawData=false);
private:
bool _useLogPolar;
ColorTable * _colorTable;
bool _useMotionMask;
float _dBThreshold;
bool _dBIndexing;
bool _magnitudeInvariant;
};
}
#endif /* KEYPOINTMEMORY_H_ */

64
corelib/include/rtabmap/core/Sensor.h
View File

@@ -1,64 +0,0 @@
/*
* Copyright (C) 2010-2011, Mathieu Labbe and IntRoLab - Universite de Sherbrooke
*
* This file is part of RTAB-Map.
*
* RTAB-Map is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* RTAB-Map is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with RTAB-Map. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef SENSOR_H_
#define SENSOR_H_
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <vector>
#include <list>
#include <utilite/UEvent.h>
namespace rtabmap {
class Sensor
{
public:
enum Type{kTypeImage=0, kTypeImageFeatures2d, kTypeAudio, kTypeAudioFreq, kTypeAudioFreqSqrdMagn, kTypeJointState, kTypeTwist, kTypeNotSpecified};
public:
Sensor(const cv::Mat & data, Type type, int num = 0) :
_data(data),
_type(type),
_num(num)
{}
Sensor(const cv::Mat & descriptors, const std::vector<cv::KeyPoint> & keypoints, int num = 0) :
_data(descriptors),
_type(kTypeImageFeatures2d),
_num(num),
_keypoints(keypoints)
{}
const cv::Mat & data() const {return _data;}
int type() const {return _type;}
int num() const {return _num;}
virtual ~Sensor() {};
void setKeypoints(const std::vector<cv::KeyPoint> & keypoints) {_keypoints = keypoints;}
const std::vector<cv::KeyPoint> & getKeypoints() const {return _keypoints;}
private:
cv::Mat _data;
int _type;
int _num; // sensor number
std::vector<cv::KeyPoint> _keypoints; // for convenience with kTypeImageFeatures
};
}
#endif /* SENSOR_H_ */

47
corelib/include/rtabmap/core/SensorimotorEvent.h
View File

@@ -1,47 +0,0 @@
/*
* SensorimotorEvent.h
*
* Created on: 2012-05-27
* Author: mathieu
*/
#ifndef SENSORIMOTOREVENT_H_
#define SENSORIMOTOREVENT_H_
#include "rtabmap/core/Sensor.h"
#include "rtabmap/core/Actuator.h"
#include <utilite/UEvent.h>
namespace rtabmap
{
class SensorimotorEvent : public UEvent
{
public:
enum Type {
kTypeData,
kTypeNoMoreData
};
public:
SensorimotorEvent() :
UEvent(kTypeNoMoreData) {}
SensorimotorEvent(const std::list<Sensor> & sensors,
const std::list<Actuator> & actuators) :
UEvent(kTypeData),
sensors_(sensors),
actuators_(actuators) {}
virtual ~SensorimotorEvent() {}
int type() const {return this->getCode();}
virtual std::string getClassName() const {return "SensorimotorEvent";}
const std::list<Sensor> & sensors() const {return sensors_;}
const std::list<Actuator> & actuators() const {return actuators_;}
private:
std::list<Sensor> sensors_;
std::list<Actuator> actuators_;
};
}
#endif /* SENSORIMOTOREVENT_H_ */

6
corelib/include/rtabmap/core/VWDictionary.h
View File

@@ -30,14 +30,14 @@
namespace rtabmap
{
class NearestNeighbor;
class FlannNN;
class DBDriver;
class VisualWord;
class RTABMAP_EXP VWDictionary
{
public:
enum NNStrategy{kNNNaive, kNNKdTree, kNNFlannKdTree, kNNUndef};
enum NNStrategy{kNNNaive, kNNFlannKdTree, kNNUndef};
static const int ID_START;
static const int ID_INVALID;
@@ -106,7 +106,7 @@ private:
std::string _dictionaryPath; // a pre-computed dictionary (.txt)
int _dim;
int _lastWordId;
NearestNeighbor * _nn;
FlannNN * _nn;
cv::Mat _dataTree;
std::map<int ,int> _mapIndexId;
std::map<int, VisualWord*> _unusedWords; //<id,VisualWord*>, note that these words stay in _visualWords

96
corelib/include/rtabmap/core/VerifyHypotheses.h
View File

@@ -1,96 +0,0 @@
/*
* Copyright (C) 2010-2011, Mathieu Labbe and IntRoLab - Universite de Sherbrooke
*
* This file is part of RTAB-Map.
*
* RTAB-Map is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* RTAB-Map is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with RTAB-Map. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef VERIFYHYPOTHESES_H_
#define VERIFYHYPOTHESES_H_
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
#include <list>
#include "rtabmap/core/Parameters.h"
#include "utilite/UEventsHandler.h"
#include <map>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
namespace rtabmap
{
class Signature;
// return always true, i.e, there is no verification
class RTABMAP_EXP HypVerificator
{
public:
HypVerificator(const ParametersMap & parameters = ParametersMap());
virtual ~HypVerificator() {}
virtual bool verify(const Signature * ref, const Signature * hyp);
virtual void parseParameters(const ParametersMap & parameters);
};
/////////////////////////
// HypVerificatorSim
/////////////////////////
class HypVerificatorSim : public HypVerificator {
public:
HypVerificatorSim(const ParametersMap & parameters = ParametersMap());
virtual ~HypVerificatorSim();
virtual bool verify(const Signature * ref, const Signature * hyp);
virtual void parseParameters(const ParametersMap & parameters);
private:
float _similarity;
};
/////////////////////////
// HypVerificatorEpipolarGeo
/////////////////////////
class KeypointSignature;
class RTABMAP_EXP HypVerificatorEpipolarGeo : public HypVerificator
{
public:
HypVerificatorEpipolarGeo(const ParametersMap & parameters = ParametersMap());
virtual ~HypVerificatorEpipolarGeo();
virtual bool verify(const Signature * ref, const Signature * hyp);
virtual void parseParameters(const ParametersMap & parameters);
int getMatchCountMinAccepted() const {return _matchCountMinAccepted;}
double getRansacParam1() const {return _ransacParam1;}
double getRansacParam2() const {return _ransacParam2;}
void setMatchCountMinAccepted(int matchCountMinAccepted) {_matchCountMinAccepted = matchCountMinAccepted;}
void setRansacParam1(double ransacParam1) {_ransacParam1 = ransacParam1;}
void setRansacParam2(double ransacParam2) {_ransacParam2 = ransacParam2;}
private:
bool doEpipolarGeometry(const KeypointSignature * ssA, const KeypointSignature * ssB);
private:
int _matchCountMinAccepted;
double _ransacParam1;
double _ransacParam2;
};
} // namespace rtabmap
#endif /* VERIFYHYPOTHESES_H_ */

524
corelib/src/BayesFilter.cpp
View File

@@ -17,9 +17,9 @@
* along with RTAB-Map. If not, see <http://www.gnu.org/licenses/>.
*/
#include "rtabmap/core/BayesFilter.h"
#include "BayesFilter.h"
#include "rtabmap/core/Memory.h"
#include "rtabmap/core/Signature.h"
#include "Signature.h"
#include "rtabmap/core/Parameters.h"
#include <iostream>
@@ -29,7 +29,8 @@ namespace rtabmap {
BayesFilter::BayesFilter(const ParametersMap & parameters) :
_virtualPlacePrior(Parameters::defaultBayesVirtualPlacePriorThr()),
_predictionOnNonNullActionsOnly(Parameters::defaultBayesPredictionOnNonNullActionsOnly())
_fullPredictionUpdate(Parameters::defaultBayesFullPredictionUpdate()),
_totalPredictionLCValues(0.0f)
{
this->setPredictionLC(Parameters::defaultBayesPredictionLC());
this->parseParameters(parameters);
@@ -49,9 +50,9 @@ void BayesFilter::parseParameters(const ParametersMap & parameters)
{
this->setPredictionLC((*iter).second);
}
if((iter=parameters.find(Parameters::kBayesPredictionOnNonNullActionsOnly())) != parameters.end())
if((iter=parameters.find(Parameters::kBayesFullPredictionUpdate())) != parameters.end())
{
_predictionOnNonNullActionsOnly = uStr2Bool((*iter).second.c_str());
_fullPredictionUpdate = uStr2Bool((*iter).second.c_str());
}
}
@@ -108,6 +109,11 @@ void BayesFilter::setPredictionLC(const std::string & prediction)
_predictionLC = tmpValues;
}
}
_totalPredictionLCValues = 0.0f;
for(unsigned int j=0; j<_predictionLC.size(); ++j)
{
_totalPredictionLCValues += _predictionLC[j];
}
}
const std::vector<double> & BayesFilter::getPredictionLC() const
@@ -133,6 +139,7 @@ std::string BayesFilter::getPredictionLCStr() const
void BayesFilter::reset()
{
_posterior.clear();
_prediction = cv::Mat();
}
const std::map<int, float> & BayesFilter::computePosterior(const Memory * memory, const std::map<int, float> & likelihood)
@@ -160,7 +167,6 @@ const std::map<int, float> & BayesFilter::computePosterior(const Memory * memory
UTimer timer;
timer.start();
cv::Mat prediction;
cv::Mat prior;
cv::Mat posterior;
@@ -168,211 +174,176 @@ const std::map<int, float> & BayesFilter::computePosterior(const Memory * memory
int j=0;
// Recursive Bayes estimation...
// STEP 1 - Prediction : Prior*lastPosterior
prediction = cv::Mat(likelihood.size(), likelihood.size(), CV_32FC1);
if(this->generatePrediction(prediction, memory, uKeys(likelihood)))
_prediction = this->generatePrediction(memory, uKeys(likelihood));
ULOGGER_DEBUG("STEP1-generate prior=%fs, rows=%d, cols=%d", timer.ticks(), _prediction.rows, _prediction.cols);
//std::cout << "Prediction=" << _prediction << std::endl;
// Adjust the last posterior if some images were
// reactivated or removed from the working memory
posterior = cv::Mat(likelihood.size(), 1, CV_32FC1);
this->updatePosterior(memory, uKeys(likelihood));
j=0;
for(std::map<int, float>::const_iterator i=_posterior.begin(); i!= _posterior.end(); ++i)
{
ULOGGER_DEBUG("STEP1-generate prior=%fs, rows=%d, cols=%d", timer.ticks(), prediction.rows, prediction.cols);
//std::cout << "Prediction=" << prediction << std::endl;
// Adjust the last posterior if some images were
// reactivated or removed from the working memory
posterior = cv::Mat(likelihood.size(), 1, CV_32FC1);
this->updatePosterior(memory, uKeys(likelihood));
j=0;
for(std::map<int, float>::const_iterator i=_posterior.begin(); i!= _posterior.end(); ++i)
{
((float*)posterior.data)[j++] = (*i).second;
}
ULOGGER_DEBUG("STEP1-update posterior=%fs, posterior=%d, _posterior size=%d", posterior.rows, _posterior.size());
//std::cout << "LastPosterior=" << posterior << std::endl;
// Multiply prediction matrix with the last posterior
// (m,m) X (m,1) = (m,1)
prior = prediction * posterior;
ULOGGER_DEBUG("STEP1-matrix mult time=%fs", timer.ticks());
//std::cout << "ResultingPrior=" << prior << std::endl;
ULOGGER_DEBUG("STEP1-matrix mult time=%fs", timer.ticks());
std::vector<float> likelihoodValues = uValues(likelihood);
//std::cout << "Likelihood=" << cv::Mat(likelihoodValues) << std::endl;
// STEP 2 - Update : Multiply with observations (likelihood)
j=0;
for(std::map<int, float>::const_iterator i=likelihood.begin(); i!= likelihood.end(); ++i)
{
std::map<int, float>::iterator p =_posterior.find((*i).first);
if(p!= _posterior.end())
{
(*p).second = (*i).second * ((float*)prior.data)[j++];
sum+=(*p).second;
}
else
{
ULOGGER_ERROR("Problem1! can't find id=%d", (*i).first);
}
}
ULOGGER_DEBUG("STEP2-likelihood time=%fs", timer.ticks());
// Normalize
ULOGGER_DEBUG("sum=%f", sum);
if(sum != 0)
{
for(std::map<int, float>::iterator i=_posterior.begin(); i!= _posterior.end(); ++i)
{
(*i).second /= sum;
}
}
ULOGGER_DEBUG("normalize time=%fs", timer.ticks());
((float*)posterior.data)[j++] = (*i).second;
}
ULOGGER_DEBUG("STEP1-update posterior=%fs, posterior=%d, _posterior size=%d", posterior.rows, _posterior.size());
//std::cout << "LastPosterior=" << posterior << std::endl;
// Multiply prediction matrix with the last posterior
// (m,m) X (m,1) = (m,1)
prior = _prediction * posterior;
ULOGGER_DEBUG("STEP1-matrix mult time=%fs", timer.ticks());
//std::cout << "ResultingPrior=" << prior << std::endl;
ULOGGER_DEBUG("STEP1-matrix mult time=%fs", timer.ticks());
std::vector<float> likelihoodValues = uValues(likelihood);
//std::cout << "Likelihood=" << cv::Mat(likelihoodValues) << std::endl;
// STEP 2 - Update : Multiply with observations (likelihood)
j=0;
for(std::map<int, float>::const_iterator i=likelihood.begin(); i!= likelihood.end(); ++i)
{
std::map<int, float>::iterator p =_posterior.find((*i).first);
if(p!= _posterior.end())
{
(*p).second = (*i).second * ((float*)prior.data)[j++];
sum+=(*p).second;
}
else
{
ULOGGER_ERROR("Problem1! can't find id=%d", (*i).first);
}
}
ULOGGER_DEBUG("STEP2-likelihood time=%fs", timer.ticks());
//std::cout << "Posterior (before normalization)=" << _posterior << std::endl;
// Normalize
ULOGGER_DEBUG("sum=%f", sum);
if(sum != 0)
{
for(std::map<int, float>::iterator i=_posterior.begin(); i!= _posterior.end(); ++i)
{
(*i).second /= sum;
}
}
ULOGGER_DEBUG("normalize time=%fs", timer.ticks());
//std::cout << "Posterior=" << _posterior << std::endl;
return _posterior;
}
bool BayesFilter::generatePrediction(cv::Mat & prediction, const Memory * memory, const std::vector<int> & ids) const
cv::Mat BayesFilter::generatePrediction(const Memory * memory, const std::vector<int> & ids) const
{
ULOGGER_DEBUG("");
if(!_fullPredictionUpdate && !_prediction.empty())
{
return updatePrediction(_prediction, memory, uKeys(_posterior), ids);
}
UDEBUG("");
UASSERT(memory &&
_predictionLC.size() >= 2 &&
ids.size());
UTimer timer;
timer.start();
UTimer timerGlobal;
timerGlobal.start();
if(!memory ||
prediction.empty() ||
prediction.rows != prediction.cols ||
(unsigned int)prediction.rows != ids.size() ||
_predictionLC.size() < 2 ||
!ids.size())
{
ULOGGER_ERROR( "fail");
return false;
}
std::map<int, int> idToIndexMap;
for(unsigned int i=0; i<ids.size(); ++i)
{
if(ids[i] == 0)
{
UFATAL("Signature id is null ?!?");
}
UASSERT_MSG(ids[i] != 0, "Signature id is null ?!?");
idToIndexMap.insert(idToIndexMap.end(), std::make_pair(ids[i], i));
}
//int rows = prediction.rows;
prediction = cv::Mat::zeros(prediction.rows, prediction.cols, prediction.type());
cv::Mat prediction = cv::Mat::zeros(ids.size(), ids.size(), CV_32FC1);
int cols = prediction.cols;
// Each prior is a column vector
ULOGGER_DEBUG("_predictionLC.size()=%d",_predictionLC.size());
UDEBUG("_predictionLC.size()=%d",_predictionLC.size());
std::set<int> idsDone;
for(unsigned int i=0; i<ids.size(); ++i)
{
int loopSignId = ids[i];
if(loopSignId > 0)
if(idsDone.find(ids[i]) == idsDone.end())
{
// Set high values (gaussians curves) to loop closure neighbors
float sum = 0.0f; // sum values added
float totalModelValues = 0.0f;
for(unsigned int j=0; j<_predictionLC.size(); ++j)
if(ids[i] > 0)
{
totalModelValues += _predictionLC[j];
}
// Set high values (gaussians curves) to loop closure neighbors
// ADD prob for each neighbors
double dbAccessTime = 0.0;
std::map<int, int> neighbors = memory->getNeighborsId(dbAccessTime, loopSignId, _predictionLC.size()-1, 0, _predictionOnNonNullActionsOnly);
sum += this->addNeighborProb(prediction, i, neighbors, idToIndexMap);
// ADD values of not found neighbors to loop closure
if(sum < totalModelValues-_predictionLC[0])
{
float delta = totalModelValues-_predictionLC[0]-sum;
((float*)prediction.data)[i + i*cols] += delta;
sum+=delta;
}
float allOtherPlacesValue = 0;
if(totalModelValues < 1)
{
allOtherPlacesValue = 1.0f - totalModelValues;
}
// Set all loop events to small values according to the model
if(allOtherPlacesValue > 0 && cols>1)
{
float value = allOtherPlacesValue / float(cols - 1);
for(int j=ids[0] < 0?1:0; j<cols; ++j)
// ADD prob for each neighbors
std::map<int, int> neighbors = memory->getNeighborsId(ids[i], _predictionLC.size()-1, 0);
std::list<int> idsLoopMargin;
//filter neighbors in STM
for(std::map<int, int>::iterator iter=neighbors.begin(); iter!=neighbors.end();)
{
if(((float*)prediction.data)[i + j*cols] == 0)
if(memory->isInSTM(iter->first))
{
((float*)prediction.data)[i + j*cols] = value;
sum += ((float*)prediction.data)[i + j*cols];
neighbors.erase(iter++);
}
else
{
if(iter->second == 0)
{
idsLoopMargin.push_back(iter->second);
}
++iter;
}
}
}
//normalize this row
float maxNorm = 1 - (ids[0]<0?_predictionLC[0]:0); // 1 - virtual place probability
if(sum<maxNorm-0.0001 || sum>maxNorm+0.0001)
{
for(int j=ids[0] < 0?1:0; j<cols; ++j)
// should at least have 1 id in idsMarginLoop
if(idsLoopMargin.size() == 0)
{
((float*)prediction.data)[i + j*cols] *= maxNorm / sum;
UFATAL("No 0 margin neighbor for signature %d !?!?", ids[i]);
}
sum = maxNorm;
}
// ADD virtual place prob
if(ids[0] < 0)
{
((float*)prediction.data)[i] = _predictionLC[0];
sum += ((float*)prediction.data)[i];
}
//debug
//for(int j=0; j<cols; ++j)
//{
// ULOGGER_DEBUG("test col=%d = %f", i, prediction.data.fl[i + j*cols]);
//}
if(sum<0.99 || sum > 1.01)
{
UWARN("Prediction is not normalized sum=%f", sum);
}
}
else
{
// Set the virtual place prior
if(_virtualPlacePrior > 0)
{
if(cols>1) // The first must be the virtual place
// same neighbor tree for loop signatures (margin = 0)
for(std::list<int>::iterator iter = idsLoopMargin.begin(); iter!=idsLoopMargin.end(); ++iter)
{
((float*)prediction.data)[i] = _virtualPlacePrior;
float val = (1.0-_virtualPlacePrior)/(cols-1);
for(int j=1; j<cols; j++)
{
((float*)prediction.data)[i + j*cols] = val;
}
}
else if(cols>0)
{
((float*)prediction.data)[i] = 1;
float sum = 0.0f; // sum values added
sum += this->addNeighborProb(prediction, i, neighbors, idToIndexMap);
idsDone.insert(*iter);
this->normalize(prediction, i, sum, ids[0]<0);
}
}
else
{
// Only for some tests...
// when _virtualPlacePrior=0, set all priors to the same value
if(cols>1)
// Set the virtual place prior
if(_virtualPlacePrior > 0)
{
float val = 1.0/cols;
for(int j=0; j<cols; j++)
if(cols>1) // The first must be the virtual place
{
((float*)prediction.data)[i + j*cols] = val;
((float*)prediction.data)[i] = _virtualPlacePrior;
float val = (1.0-_virtualPlacePrior)/(cols-1);
for(int j=1; j<cols; j++)
{
((float*)prediction.data)[i + j*cols] = val;
}
}
else if(cols>0)
{
((float*)prediction.data)[i] = 1;
}
}
else if(cols>0)
else
{
((float*)prediction.data)[i] = 1;
// Only for some tests...
// when _virtualPlacePrior=0, set all priors to the same value
if(cols>1)
{
float val = 1.0/cols;
for(int j=0; j<cols; j++)
{
((float*)prediction.data)[i + j*cols] = val;
}
}
else if(cols>0)
{
((float*)prediction.data)[i] = 1;
}
}
}
}
@@ -380,7 +351,217 @@ bool BayesFilter::generatePrediction(cv::Mat & prediction, const Memory * memory
ULOGGER_DEBUG("time = %fs", timerGlobal.ticks());
return true;
return prediction;
}
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);
int cols = prediction.cols;
// ADD values of not found neighbors to loop closure
if(addedProbabilitiesSum < _totalPredictionLCValues-_predictionLC[0])
{
float delta = _totalPredictionLCValues-_predictionLC[0]-addedProbabilitiesSum;
((float*)prediction.data)[index + index*cols] += delta;
addedProbabilitiesSum+=delta;
}
float allOtherPlacesValue = 0;
if(_totalPredictionLCValues < 1)
{
allOtherPlacesValue = 1.0f - _totalPredictionLCValues;
}
// Set all loop events to small values according to the model
if(allOtherPlacesValue > 0 && cols>1)
{
float value = allOtherPlacesValue / float(cols - 1);
for(int j=virtualPlaceUsed?1:0; j<cols; ++j)
{
if(((float*)prediction.data)[index + j*cols] == 0)
{
((float*)prediction.data)[index + j*cols] = value;
addedProbabilitiesSum += ((float*)prediction.data)[index + j*cols];
}
}
}
//normalize this row
float maxNorm = 1 - (virtualPlaceUsed?_predictionLC[0]:0); // 1 - virtual place probability
if(addedProbabilitiesSum<maxNorm-0.0001 || addedProbabilitiesSum>maxNorm+0.0001)
{
for(int j=virtualPlaceUsed?1:0; j<cols; ++j)
{
((float*)prediction.data)[index + j*cols] *= maxNorm / addedProbabilitiesSum;
}
addedProbabilitiesSum = maxNorm;
}
// ADD virtual place prob
if(virtualPlaceUsed)
{
((float*)prediction.data)[index] = _predictionLC[0];
addedProbabilitiesSum += ((float*)prediction.data)[index];
}
//debug
//for(int j=0; j<cols; ++j)
//{
// ULOGGER_DEBUG("test col=%d = %f", i, prediction.data.fl[i + j*cols]);
//}
if(addedProbabilitiesSum<0.99 || addedProbabilitiesSum > 1.01)
{
UWARN("Prediction is not normalized sum=%f", addedProbabilitiesSum);
}
}
cv::Mat BayesFilter::updatePrediction(const cv::Mat & oldPrediction,
const Memory * memory,
const std::vector<int> & oldIds,
const std::vector<int> & newIds) const
{
UTimer timer;
UDEBUG("");
UASSERT(memory &&
oldIds.size() &&
newIds.size() &&
oldIds.size() == (unsigned int)oldPrediction.cols &&
oldIds.size() == (unsigned int)oldPrediction.rows);
cv::Mat prediction = cv::Mat::zeros(newIds.size(), newIds.size(), CV_32FC1);
// Create id to index maps
std::map<int, int> oldIdToIndexMap;
std::map<int, int> newIdToIndexMap;
for(unsigned int i=0; i<oldIds.size() || i<newIds.size(); ++i)
{
if(i<oldIds.size())
{
UASSERT(oldIds[i]);
oldIdToIndexMap.insert(oldIdToIndexMap.end(), std::make_pair(oldIds[i], i));
//UDEBUG("oldIdToIndexMap[%d] = %d", oldIds[i], i);
}
if(i<newIds.size())
{
UASSERT(newIds[i]);
newIdToIndexMap.insert(newIdToIndexMap.end(), std::make_pair(newIds[i], i));
//UDEBUG("newIdToIndexMap[%d] = %d", newIds[i], i);
}
}
UDEBUG("time creating id-index maps = %fs", timer.restart());
//Get removed ids
std::set<int> removedIds;
for(unsigned int i=0; i<oldIds.size(); ++i)
{
if(!uContains(newIdToIndexMap, oldIds[i]))
{
removedIds.insert(removedIds.end(), oldIds[i]);
UDEBUG("removed id=%d at oldIndex=%d", oldIds[i], i);
}
}
UDEBUG("time getting removed ids = %fs", timer.restart());
int added = 0;
// get ids to update
std::set<int> idsToUpdate;
for(unsigned int i=0; i<oldIds.size() || i<newIds.size(); ++i)
{
if(i<oldIds.size())
{
if(removedIds.find(oldIds[i]) != removedIds.end())
{
unsigned int cols = oldPrediction.cols;
for(unsigned int j=0; j<cols; ++j)
{
if(((const float *)oldPrediction.data)[i + j*cols] != 0.0f &&
j!=i &&
removedIds.find(oldIds[j]) == removedIds.end())
{
//UDEBUG("to update id=%d from id=%d removed (value=%f)", oldIds[j], oldIds[i], ((const float *)oldPrediction.data)[i + j*cols]);
idsToUpdate.insert(oldIds[j]);
}
}
}
}
if(i<newIds.size() && !uContains(oldIdToIndexMap,newIds[i]))
{
std::map<int, int> neighbors = memory->getNeighborsId(newIds[i], _predictionLC.size()-1, 0);
float sum = this->addNeighborProb(prediction, i, neighbors, newIdToIndexMap);
this->normalize(prediction, i, sum, newIds[0]<0);
++added;
for(std::map<int,int>::iterator iter=neighbors.begin(); iter!=neighbors.end(); ++iter)
{
if(uContains(oldIdToIndexMap, iter->first) &&
removedIds.find(iter->first) == removedIds.end())
{
idsToUpdate.insert(iter->first);
}
}
}
}
UDEBUG("time getting ids to update = %fs", timer.restart());
// update modified/added ids
int modified = 0;
for(std::set<int>::iterator iter = idsToUpdate.begin(); iter!=idsToUpdate.end(); ++iter)
{
std::map<int, int> neighbors = memory->getNeighborsId(*iter, _predictionLC.size()-1, 0);
int index = newIdToIndexMap.at(*iter);
float sum = this->addNeighborProb(prediction, index, neighbors, newIdToIndexMap);
this->normalize(prediction, index, sum, newIds[0]<0);
++modified;
}
UDEBUG("time updating modified/added ids = %fs", timer.restart());
//UDEBUG("oldIds.size()=%d, oldPrediction.cols=%d, oldPrediction.rows=%d", oldIds.size(), oldPrediction.cols, oldPrediction.rows);
//UDEBUG("newIdToIndexMap.size()=%d, prediction.cols=%d, prediction.rows=%d", newIdToIndexMap.size(), prediction.cols, prediction.rows);
// copy not changed probabilities
int copied = 0;
for(unsigned int i=0; i<oldIds.size(); ++i)
{
if(oldIds[i]>0 && removedIds.find(oldIds[i]) == removedIds.end() && idsToUpdate.find(oldIds[i]) == idsToUpdate.end())
{
for(int j=0; j<oldPrediction.cols; ++j)
{
if(removedIds.find(oldIds[j]) == removedIds.end() && ((const float *)oldPrediction.data)[i + j*oldPrediction.cols] != 0.0f)
{
//UDEBUG("i=%d, j=%d", i, j);
//UDEBUG("oldIds[i]=%d, oldIds[j]=%d", oldIds[i], oldIds[j]);
//UDEBUG("newIdToIndexMap.at(oldIds[i])=%d", newIdToIndexMap.at(oldIds[i]));
//UDEBUG("newIdToIndexMap.at(oldIds[j])=%d", newIdToIndexMap.at(oldIds[j]));
((float *)prediction.data)[newIdToIndexMap.at(oldIds[i]) + newIdToIndexMap.at(oldIds[j])*prediction.cols] = ((const float *)oldPrediction.data)[i + j*oldPrediction.cols];
}
}
++copied;
}
}
UDEBUG("time copying = %fs", timer.restart());
//update virtual place
if(newIds[0] < 0)
{
if(prediction.cols>1) // The first must be the virtual place
{
((float*)prediction.data)[0] = _virtualPlacePrior;
float val = (1.0-_virtualPlacePrior)/(prediction.cols-1);
for(int j=1; j<prediction.cols; j++)
{
((float*)prediction.data)[j*prediction.cols] = val;
}
}
else if(prediction.cols>0)
{
((float*)prediction.data)[0] = 1;
}
}
UDEBUG("time updating virtual place = %fs", timer.restart());
UDEBUG("Modified=%d, Added=%d, Copied=%d", modified, added, copied);
return prediction;
}
void BayesFilter::updatePosterior(const Memory * memory, const std::vector<int> & likelihoodIds)
@@ -411,11 +592,10 @@ void BayesFilter::updatePosterior(const Memory * memory, const std::vector<int>
float BayesFilter::addNeighborProb(cv::Mat & prediction, unsigned int col, const std::map<int, int> & neighbors, const std::map<int, int> & idToIndexMap) const
{
if((unsigned int)prediction.cols != idToIndexMap.size() ||
(unsigned int)prediction.rows != idToIndexMap.size())
{
UFATAL("Requirements no met");
}
UASSERT((unsigned int)prediction.cols == idToIndexMap.size() &&
(unsigned int)prediction.rows == idToIndexMap.size() &&
col < (unsigned int)prediction.cols &&
col < (unsigned int)prediction.rows);
float sum=0;
for(std::map<int, int>::const_iterator iter=neighbors.begin(); iter!=neighbors.end(); ++iter)

17
corelib/include/rtabmap/core/BayesFilter.h → corelib/src/BayesFilter.h
View File

@@ -51,19 +51,28 @@ public:
float getVirtualPlacePrior() const {return _virtualPlacePrior;}
const std::vector<double> & getPredictionLC() const; // {Vp, Lc, l1, l2, l3, l4...}
std::string getPredictionLCStr() const; // for convenience {Vp, Lc, l1, l2, l3, l4...}
bool isPredictionOnNonNullActionsOnly() const {return _predictionOnNonNullActionsOnly;}
bool generatePrediction(cv::Mat & prediction, const Memory * memory, const std::vector<int> & ids) const;
cv::Mat generatePrediction(const Memory * memory, const std::vector<int> & ids) const;
private:
cv::Mat updatePrediction(const cv::Mat & oldPrediction,
const Memory * memory,
const std::vector<int> & oldIds,
const std::vector<int> & newIds) const;
void updatePosterior(const Memory * memory, const std::vector<int> & likelihoodIds);
float addNeighborProb(cv::Mat & prediction, unsigned int col, const std::map<int, int> & neighbors, const std::map<int, int> & idToIndexMap) const;
float addNeighborProb(cv::Mat & prediction,
unsigned int col,
const std::map<int, int> & neighbors,
const std::map<int, int> & idToIndexMap) const;
void normalize(cv::Mat & prediction, unsigned int index, float addedProbabilitiesSum, bool virtualPlaceUsed) const;
private:
std::map<int, float> _posterior;
cv::Mat _prediction;
float _virtualPlacePrior;
std::vector<double> _predictionLC; // {Vp, Lc, l1, l2, l3, l4...}
bool _predictionOnNonNullActionsOnly;
bool _fullPredictionUpdate;
float _totalPredictionLCValues;
};
} // namespace rtabmap

100
corelib/src/CMakeLists.txt
View File

@@ -4,28 +4,20 @@ SET(SRC_FILES
RtabmapEvent.cpp
Memory.cpp
KeypointMemory.cpp
SMMemory.cpp
DBDriverFactory.cpp
DBDriver.cpp
DBDriverSqlite3.cpp
DBReader.cpp
Camera.cpp
Micro.cpp
EpipolarGeometry.cpp
VisualWord.cpp
VWDictionary.cpp
BayesFilter.cpp
Parameters.cpp
Signature.cpp
KeypointDetector.cpp
KeypointDescriptor.cpp
VerifyHypotheses.cpp
Features2d.cpp
NearestNeighbor.cpp
ColorTable.cpp
)
SET(INCLUDE_DIRS
@@ -35,93 +27,35 @@ SET(INCLUDE_DIRS
${UTILITE_INCLUDE_DIRS}
${OpenCV_INCLUDE_DIRS}
${SQLITE3_INCLUDE_DIR}
${ZLIB_INCLUDE_DIRS}
${FFTW3F_INCLUDE_DIRS}
)
SET(LIBRARIES
${UTILITE_LIBRARIES}
${OpenCV_LIBS}
${SQLITE3_LIBRARY}
${ZLIB_LIBRARIES}
${FFTW3F_LIBRARIES}
)
####################################
# Generate resources files
####################################
ADD_CUSTOM_COMMAND(
OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/DatabaseSchema_sql.h
COMMAND ${URESOURCEGENERATOR_EXEC} -n rtabmap -p ${CMAKE_CURRENT_BINARY_DIR} ${CMAKE_CURRENT_SOURCE_DIR}/resources/DatabaseSchema.sql
COMMENT "[Creating database resource]"
DEPENDS ${CMAKE_CURRENT_SOURCE_DIR}/resources/DatabaseSchema.sql
SET(R
${CMAKE_CURRENT_SOURCE_DIR}/resources/DatabaseSchema.sql
)
#replace semicolons by spaces
foreach(arg ${R})
set(RESOURCES "${RESOURCES}" "${arg}")
endforeach(arg ${R})
SET(RESOURCES_HEADERS
${CMAKE_CURRENT_BINARY_DIR}/DatabaseSchema_sql.h
)
ADD_CUSTOM_COMMAND(
OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/ColorIndexes65536_bin_zip.h
COMMAND ${URESOURCEGENERATOR_EXEC} -n rtabmap -p ${CMAKE_CURRENT_BINARY_DIR} ${CMAKE_CURRENT_SOURCE_DIR}/resources/ColorIndexes65536.bin.zip
COMMENT "[Creating color table resource]"
DEPENDS ${CMAKE_CURRENT_SOURCE_DIR}/resources/ColorIndexes65536.bin.zip
)
ADD_CUSTOM_COMMAND(
OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/ColorIndexes1024_bin_zip.h
COMMAND ${URESOURCEGENERATOR_EXEC} -n rtabmap -p ${CMAKE_CURRENT_BINARY_DIR} ${CMAKE_CURRENT_SOURCE_DIR}/resources/ColorIndexes1024.bin.zip
COMMENT "[Creating color table resource]"
DEPENDS ${CMAKE_CURRENT_SOURCE_DIR}/resources/ColorIndexes1024.bin.zip
)
ADD_CUSTOM_COMMAND(
OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/ColorIndexes512_bin_zip.h
COMMAND ${URESOURCEGENERATOR_EXEC} -n rtabmap -p ${CMAKE_CURRENT_BINARY_DIR} ${CMAKE_CURRENT_SOURCE_DIR}/resources/ColorIndexes512.bin.zip
COMMENT "[Creating color table resource]"
DEPENDS ${CMAKE_CURRENT_SOURCE_DIR}/resources/ColorIndexes512.bin.zip
)
ADD_CUSTOM_COMMAND(
OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/ColorIndexes256_bin_zip.h
COMMAND ${URESOURCEGENERATOR_EXEC} -n rtabmap -p ${CMAKE_CURRENT_BINARY_DIR} ${CMAKE_CURRENT_SOURCE_DIR}/resources/ColorIndexes256.bin.zip
COMMENT "[Creating color table resource]"
DEPENDS ${CMAKE_CURRENT_SOURCE_DIR}/resources/ColorIndexes256.bin.zip
)
ADD_CUSTOM_COMMAND(
OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/ColorIndexes128_bin_zip.h
COMMAND ${URESOURCEGENERATOR_EXEC} -n rtabmap -p ${CMAKE_CURRENT_BINARY_DIR} ${CMAKE_CURRENT_SOURCE_DIR}/resources/ColorIndexes128.bin.zip
COMMENT "[Creating color table resource]"
DEPENDS ${CMAKE_CURRENT_SOURCE_DIR}/resources/ColorIndexes128.bin.zip
)
ADD_CUSTOM_COMMAND(
OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/ColorIndexes64_bin_zip.h
COMMAND ${URESOURCEGENERATOR_EXEC} -n rtabmap -p ${CMAKE_CURRENT_BINARY_DIR} ${CMAKE_CURRENT_SOURCE_DIR}/resources/ColorIndexes64.bin.zip
COMMENT "[Creating color table resource]"
DEPENDS ${CMAKE_CURRENT_SOURCE_DIR}/resources/ColorIndexes64.bin.zip
)
ADD_CUSTOM_COMMAND(
OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/ColorIndexes32_bin_zip.h
COMMAND ${URESOURCEGENERATOR_EXEC} -n rtabmap -p ${CMAKE_CURRENT_BINARY_DIR} ${CMAKE_CURRENT_SOURCE_DIR}/resources/ColorIndexes32.bin.zip
COMMENT "[Creating color table resource]"
DEPENDS ${CMAKE_CURRENT_SOURCE_DIR}/resources/ColorIndexes32.bin.zip
)
ADD_CUSTOM_COMMAND(
OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/ColorIndexes16_bin_zip.h
COMMAND ${URESOURCEGENERATOR_EXEC} -n rtabmap -p ${CMAKE_CURRENT_BINARY_DIR} ${CMAKE_CURRENT_SOURCE_DIR}/resources/ColorIndexes16.bin.zip
COMMENT "[Creating color table resource]"
DEPENDS ${CMAKE_CURRENT_SOURCE_DIR}/resources/ColorIndexes16.bin.zip
)
ADD_CUSTOM_COMMAND(
OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/ColorIndexes8_bin_zip.h
COMMAND ${URESOURCEGENERATOR_EXEC} -n rtabmap -p ${CMAKE_CURRENT_BINARY_DIR} ${CMAKE_CURRENT_SOURCE_DIR}/resources/ColorIndexes8.bin.zip
COMMENT "[Creating color table resource]"
DEPENDS ${CMAKE_CURRENT_SOURCE_DIR}/resources/ColorIndexes8.bin.zip
)
SET(RESOURCES
${CMAKE_CURRENT_BINARY_DIR}/DatabaseSchema_sql.h
${CMAKE_CURRENT_BINARY_DIR}/ColorIndexes65536_bin_zip.h
${CMAKE_CURRENT_BINARY_DIR}/ColorIndexes1024_bin_zip.h
${CMAKE_CURRENT_BINARY_DIR}/ColorIndexes512_bin_zip.h
${CMAKE_CURRENT_BINARY_DIR}/ColorIndexes256_bin_zip.h
${CMAKE_CURRENT_BINARY_DIR}/ColorIndexes128_bin_zip.h
${CMAKE_CURRENT_BINARY_DIR}/ColorIndexes64_bin_zip.h
${CMAKE_CURRENT_BINARY_DIR}/ColorIndexes32_bin_zip.h
${CMAKE_CURRENT_BINARY_DIR}/ColorIndexes16_bin_zip.h
${CMAKE_CURRENT_BINARY_DIR}/ColorIndexes8_bin_zip.h
OUTPUT ${RESOURCES_HEADERS}
COMMAND ${URESOURCEGENERATOR_EXEC} -n rtabmap -p ${CMAKE_CURRENT_BINARY_DIR} ${RESOURCES}
COMMENT "[Creating resources]"
DEPENDS ${R}
)
####################################
@@ -134,7 +68,7 @@ INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
# Add binary that is built from the source file "main.cpp".
# The extension is automatically found.
ADD_LIBRARY(rtabmap_corelib ${SRC_FILES} ${RESOURCES})
ADD_LIBRARY(rtabmap_corelib ${SRC_FILES} ${RESOURCES_HEADERS})
TARGET_LINK_LIBRARIES(rtabmap_corelib ${LIBRARIES})
SET_TARGET_PROPERTIES(

22
corelib/src/Camera.cpp
View File

@@ -21,9 +21,7 @@
#include "utilite/UEventsManager.h"
#include "utilite/UConversion.h"
#include "rtabmap/core/DBDriver.h"
#include "rtabmap/core/DBDriverFactory.h"
#include "rtabmap/core/KeypointDescriptor.h"
#include "rtabmap/core/KeypointDetector.h"
#include "rtabmap/core/Features2d.h"
#include "utilite/UStl.h"
#include "utilite/UConversion.h"
#include "utilite/UFile.h"
@@ -125,15 +123,9 @@ void Camera::parseParameters(const ParametersMap & parameters)
}
switch(detector)
{
case KeypointDetector::kDetectorStar:
_keypointDetector = new StarDetector(parameters);
break;
case KeypointDetector::kDetectorSift:
_keypointDetector = new SIFTDetector(parameters);
break;
case KeypointDetector::kDetectorFast:
_keypointDetector = new FASTDetector(parameters);
break;
case KeypointDetector::kDetectorSurf:
default:
_keypointDetector = new SURFDetector(parameters);
@@ -158,15 +150,6 @@ void Camera::parseParameters(const ParametersMap & parameters)
case KeypointDescriptor::kDescriptorSift:
_keypointDescriptor = new SIFTDescriptor(parameters);
break;
case KeypointDescriptor::kDescriptorBrief:
_keypointDescriptor = new BRIEFDescriptor(parameters);
break;
case KeypointDescriptor::kDescriptorColor:
_keypointDescriptor = new ColorDescriptor(parameters);
break;
case KeypointDescriptor::kDescriptorHue:
_keypointDescriptor = new HueDescriptor(parameters);
break;
case KeypointDescriptor::kDescriptorSurf:
default:
_keypointDescriptor = new SURFDescriptor(parameters);
@@ -511,7 +494,8 @@ CameraVideo::CameraVideo(const std::string & filePath,
int id) :
Camera(imageRate, autoRestart, imageWidth, imageHeight, framesDropped, id),
_filePath(filePath),
_src(kVideoFile)
_src(kVideoFile),
_usbDevice(0)
{
}

67721
corelib/src/ColorTable.cpp
View File

File diff suppressed because it is too large Load Diff

399
corelib/src/DBDriver.cpp
View File

@@ -19,9 +19,8 @@
#include "rtabmap/core/DBDriver.h"
#include "rtabmap/core/Signature.h"
#include "rtabmap/core/VWDictionary.h"
#include "rtabmap/core/VisualWord.h"
#include "Signature.h"
#include "VisualWord.h"
#include "utilite/UConversion.h"
#include "utilite/UMath.h"
#include "utilite/ULogger.h"
@@ -31,8 +30,6 @@
namespace rtabmap {
DBDriver::DBDriver(const ParametersMap & parameters) :
_minSignaturesToSave(Parameters::defaultDbMinSignaturesToSave()),
_minWordsToSave(Parameters::defaultDbMinWordsToSave()),
_imagesCompressed(Parameters::defaultDbImagesCompressed()),
_emptyTrashesTime(0)
{
@@ -48,14 +45,6 @@ DBDriver::~DBDriver()
void DBDriver::parseParameters(const ParametersMap & parameters)
{
ParametersMap::const_iterator iter;
if((iter=parameters.find(Parameters::kDbMinSignaturesToSave())) != parameters.end())
{
_minSignaturesToSave = std::atoi((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kDbMinWordsToSave())) != parameters.end())
{
_minWordsToSave = std::atoi((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kDbImagesCompressed())) != parameters.end())
{
_imagesCompressed = uStr2Bool((*iter).second.c_str());
@@ -131,18 +120,15 @@ void DBDriver::commit() const
_transactionMutex.unlock();
}
bool DBDriver::executeNoResult(const std::string & sql) const
void DBDriver::executeNoResult(const std::string & sql) const
{
bool r;
_dbSafeAccessMutex.lock();
r = this->executeNoResultQuery(sql);
this->executeNoResultQuery(sql);
_dbSafeAccessMutex.unlock();
return r;
}
void DBDriver::emptyTrashes(bool async)
{
ULOGGER_DEBUG("");
if(async)
{
ULOGGER_DEBUG("Async emptying, start the trash thread");
@@ -153,11 +139,12 @@ void DBDriver::emptyTrashes(bool async)
UTimer totalTime;
totalTime.start();
std::vector<Signature*> signatures;
std::map<int, Signature*> signatures;
std::map<int, VisualWord*> visualWords;
_trashesMutex.lock();
{
signatures = uValues(_trashSignatures);
ULOGGER_DEBUG("signatures=%d, visualWords=%d", _trashSignatures.size(), _trashVisualWords.size());
signatures = _trashSignatures;
visualWords = _trashVisualWords;
_trashSignatures.clear();
_trashVisualWords.clear();
@@ -168,7 +155,6 @@ void DBDriver::emptyTrashes(bool async)
if(signatures.size() || visualWords.size())
{
ULOGGER_DEBUG("trashSignatures size = %d, trashVisualWords size = %d", signatures.size(), visualWords.size());
this->beginTransaction();
UTimer timer;
timer.start();
@@ -177,16 +163,16 @@ void DBDriver::emptyTrashes(bool async)
if(this->isConnected())
{
//Only one query to the database
this->saveOrUpdate(signatures);
this->saveOrUpdate(uValues(signatures));
}
for(std::vector<Signature *>::iterator iter=signatures.begin(); iter!=signatures.end(); ++iter)
for(std::map<int, Signature *>::iterator iter=signatures.begin(); iter!=signatures.end(); ++iter)
{
delete *iter;
delete iter->second;
}
signatures.clear();
ULOGGER_DEBUG("Time emptying memory signatures trash = %f...", timer.ticks());
}
ULOGGER_DEBUG("Time emptying memory signatures trash = %f...", timer.ticks());
if(visualWords.size())
{
if(this->isConnected())
@@ -200,8 +186,9 @@ void DBDriver::emptyTrashes(bool async)
delete (*iter).second;
}
visualWords.clear();
ULOGGER_DEBUG("Time emptying memory visualWords trash = %f...", timer.ticks());
}
ULOGGER_DEBUG("Time emptying memory visualWords trash = %f...", timer.ticks());
this->commit();
}
@@ -219,10 +206,6 @@ void DBDriver::asyncSave(Signature * s)
_trashesMutex.lock();
{
_trashSignatures.insert(std::pair<int, Signature*>(s->id(), s));
if(_trashSignatures.size() > _minSignaturesToSave && this->isIdle())
{
this->start();
}
}
_trashesMutex.unlock();
}
@@ -235,79 +218,13 @@ void DBDriver::asyncSave(VisualWord * vw)
_trashesMutex.lock();
{
_trashVisualWords.insert(std::pair<int, VisualWord*>(vw->id(), vw));
if(_trashVisualWords.size() > _minWordsToSave && this->isIdle())
{
this->start();
}
}
_trashesMutex.unlock();
}
}
bool DBDriver::getSignature(int signatureId, Signature ** s)
{
*s = 0;
_trashesMutex.lock();
{
if(_trashSignatures.size())
{
_dbSafeAccessMutex.lock();
_dbSafeAccessMutex.unlock();
std::map<int, Signature*>::iterator iter =_trashSignatures.find(signatureId);
if(iter != _trashSignatures.end())
{
*s = iter->second;
_trashSignatures.erase(iter);
}
}
}
_trashesMutex.unlock();
if(*s == 0)
{
bool r;
_dbSafeAccessMutex.lock();
r = this->loadQuery(signatureId, s);
_dbSafeAccessMutex.unlock();
return r;
}
return true;
}
bool DBDriver::getVisualWord(int wordId, VisualWord ** vw)
{
*vw = 0;
_trashesMutex.lock();
{
if(_trashVisualWords.size())
{
_dbSafeAccessMutex.lock();
_dbSafeAccessMutex.unlock();
std::map<int, VisualWord*>::iterator iter = _trashVisualWords.find(wordId);
if(iter != _trashVisualWords.end())
{
*vw = iter->second;
_trashVisualWords.erase(iter);
}
}
}
_trashesMutex.unlock();
if(*vw == 0)
{
bool r;
_dbSafeAccessMutex.lock();
r = this->loadQuery(wordId, vw);
_dbSafeAccessMutex.unlock();
return r;
}
return true;
}
//Automatically begin and commit a transaction
bool DBDriver::saveOrUpdate(const std::vector<Signature *> & signatures) const
void DBDriver::saveOrUpdate(const std::vector<Signature *> & signatures) const
{
ULOGGER_DEBUG("");
std::list<Signature *> toSave;
@@ -335,28 +252,23 @@ bool DBDriver::saveOrUpdate(const std::vector<Signature *> & signatures) const
this->saveQuery(toSave);
}
}
return false;
}
bool DBDriver::load(VWDictionary * dictionary) const
void DBDriver::load(VWDictionary * dictionary) const
{
bool r;
_dbSafeAccessMutex.lock();
r = this->loadQuery(dictionary);
this->loadQuery(dictionary);
_dbSafeAccessMutex.unlock();
return r;
}
bool DBDriver::loadLastNodes(std::list<Signature *> & signatures) const
void DBDriver::loadLastNodes(std::list<Signature *> & signatures) const
{
bool r;
_dbSafeAccessMutex.lock();
r = this->loadLastNodesQuery(signatures);
this->loadLastNodesQuery(signatures);
_dbSafeAccessMutex.unlock();
return r;
}
bool DBDriver::loadKeypointSignatures(const std::list<int> & signIds, std::list<Signature *> & signatures)
void DBDriver::loadSignatures(const std::list<int> & signIds, std::list<Signature *> & signatures)
{
UDEBUG("");
// look up in the trash before the database
@@ -399,84 +311,16 @@ bool DBDriver::loadKeypointSignatures(const std::list<int> & signIds, std::list<
UDEBUG("");
if(ids.size())
{
bool r;
_dbSafeAccessMutex.lock();
r = this->loadKeypointSignaturesQuery(ids, signatures);
this->loadSignaturesQuery(ids, signatures);
_dbSafeAccessMutex.unlock();
return r;
}
else if(signatures.size())
{
return true;
}
return false;
}
// TODO the same code of method loadKeypointSignatures() above is used here
bool DBDriver::loadSMSignatures(const std::list<int> & signIds, std::list<Signature *> & signatures)
void DBDriver::loadWords(const std::set<int> & wordIds, std::list<VisualWord *> & vws)
{
UDEBUG("");
// look up in the trash before the database
std::list<int> ids = signIds;
std::list<Signature*>::iterator sIter;
bool valueFound = false;
_trashesMutex.lock();
{
_dbSafeAccessMutex.lock();
_dbSafeAccessMutex.unlock();
for(std::list<int>::iterator iter = ids.begin(); iter != ids.end();)
{
valueFound = false;
for(std::map<int, Signature*>::iterator sIter = _trashSignatures.begin(); sIter!=_trashSignatures.end();)
{
if(sIter->first == *iter)
{
signatures.push_back(sIter->second);
_trashSignatures.erase(sIter++);
valueFound = true;
break;
}
else
{
++sIter;
}
}
if(valueFound)
{
iter = ids.erase(iter);
}
else
{
++iter;
}
}
}
_trashesMutex.unlock();
UDEBUG("");
if(ids.size())
{
bool r;
_dbSafeAccessMutex.lock();
r = this->loadSMSignaturesQuery(ids, signatures);
_dbSafeAccessMutex.unlock();
return r;
}
else if(signatures.size())
{
return true;
}
return false;
}
bool DBDriver::loadWords(const std::list<int> & wordIds, std::list<VisualWord *> & vws)
{
if(!wordIds.size())
{
return false;
}
// look up in the trash before the database
std::list<int> ids = wordIds;
std::set<int> ids = wordIds;
std::map<int, VisualWord*>::iterator wIter;
std::list<VisualWord *> puttedBack;
_trashesMutex.lock();
@@ -485,7 +329,7 @@ bool DBDriver::loadWords(const std::list<int> & wordIds, std::list<VisualWord *>
{
_dbSafeAccessMutex.lock();
_dbSafeAccessMutex.unlock();
for(std::list<int>::iterator iter = ids.begin(); iter != ids.end();)
for(std::set<int>::iterator iter = ids.begin(); iter != ids.end();)
{
wIter = _trashVisualWords.find(*iter);
if(wIter != _trashVisualWords.end())
@@ -493,7 +337,7 @@ bool DBDriver::loadWords(const std::list<int> & wordIds, std::list<VisualWord *>
UDEBUG("put back word %d from trash", *iter);
puttedBack.push_back(wIter->second);
_trashVisualWords.erase(wIter);
iter = ids.erase(iter);
ids.erase(iter++);
}
else
{
@@ -505,235 +349,90 @@ bool DBDriver::loadWords(const std::list<int> & wordIds, std::list<VisualWord *>
_trashesMutex.unlock();
if(ids.size())
{
bool r;
_dbSafeAccessMutex.lock();
r = this->loadWordsQuery(ids, vws);
this->loadWordsQuery(ids, vws);
_dbSafeAccessMutex.unlock();
uAppend(vws, puttedBack);
return r;
}
else if(puttedBack.size())
{
uAppend(vws, puttedBack);
return true;
}
return false;
}
// <oldWordId, activeWordId>
bool DBDriver::changeWordsRef(const std::map<int, int> & refsToChange)
{
//Change references in the trash
KeypointSignature * s = 0;
UTimer timer;
_trashesMutex.lock();
{
_dbSafeAccessMutex.lock();
_dbSafeAccessMutex.unlock();
timer.start();
for(std::map<int, Signature *>::iterator iter = _trashSignatures.begin(); iter!=_trashSignatures.end(); ++iter)
{
s = dynamic_cast<KeypointSignature*>(iter->second);
if(s)
{
for(std::map<int, int>::const_iterator jter = refsToChange.begin(); jter!=refsToChange.end(); ++jter)
{
s->changeWordsRef((*jter).first, (*jter).second);
}
}
}
ULOGGER_DEBUG("Trash changing words references time=%fs", timer.ticks());
}
_trashesMutex.unlock();
bool r;
_dbSafeAccessMutex.lock();
r = this->changeWordsRefQuery(refsToChange);
_dbSafeAccessMutex.unlock();
return r;
}
bool DBDriver::deleteWords(const std::vector<int> & ids)
{
//Delete words in the trash
std::map<int, VisualWord*>::iterator iter;
_trashesMutex.lock();
{
_dbSafeAccessMutex.lock();
_dbSafeAccessMutex.unlock();
for(unsigned int i=0; i<ids.size(); ++i)
{
iter = _trashVisualWords.find(ids[i]);
if(iter != _trashVisualWords.end())
{
_trashVisualWords.erase(iter);
delete (*iter).second;
}
}
}
_trashesMutex.unlock();
bool r;
_dbSafeAccessMutex.lock();
r = this->deleteWordsQuery(ids);
_dbSafeAccessMutex.unlock();
return r;
}
bool DBDriver::deleteAllVisualWords() const
{
ULOGGER_DEBUG("");
if(this->isConnected())
{
std::string query;
query += "DELETE FROM VisualWord;";
_dbSafeAccessMutex.lock();
bool r = this->executeNoResultQuery(query);
_dbSafeAccessMutex.unlock();
return r;
}
return false;
}
bool DBDriver::deleteAllObsoleteSSVWLinks() const
{
ULOGGER_DEBUG("");
if(this->isConnected())
{
std::string query;
query += "DELETE FROM Map_Node_Word WHERE NOT EXISTS (SELECT id FROM Word WHERE id = Map_Node_Word.word_id);";
_dbSafeAccessMutex.lock();
bool r = this->executeNoResultQuery(query);
_dbSafeAccessMutex.unlock();
return r;
}
return false;
}
bool DBDriver::deleteUnreferencedWords() const
{
ULOGGER_DEBUG("");
if(this->isConnected())
{
std::string query = "DELETE FROM Word WHERE id NOT IN (SELECT word_id FROM Map_Node_Word);";
_dbSafeAccessMutex.lock();
bool r = this->executeNoResultQuery(query);
_dbSafeAccessMutex.unlock();
return r;
}
return false;
}
//TODO Check also in the trash ?
bool DBDriver::getRawData(int id, std::list<Sensor> & rawData) const
void DBDriver::getImage(int signatureId, cv::Mat & rawData) const
{
_dbSafeAccessMutex.lock();
bool result = this->getRawDataQuery(id, rawData);
this->getImageQuery(signatureId, rawData);
_dbSafeAccessMutex.unlock();
return result;
}
//TODO Check also in the trash ?
bool DBDriver::getActuatorData(int id, std::list<Actuator> & data) const
void DBDriver::getNeighborIds(int signatureId, std::set<int> & neighbors, bool onlyWithActions) const
{
_dbSafeAccessMutex.lock();
bool result = this->getActuatorDataQuery(id, data);
this->getNeighborIdsQuery(signatureId, neighbors, onlyWithActions);
_dbSafeAccessMutex.unlock();
return result;
}
//TODO Check also in the trash ?
bool DBDriver::getNeighborIds(int signatureId, std::set<int> & neighbors, bool onlyWithActions) const
void DBDriver::loadNeighbors(int signatureId, std::set<int> & neighbors) const
{
bool r;
_dbSafeAccessMutex.lock();
r = this->getNeighborIdsQuery(signatureId, neighbors, onlyWithActions);
this->loadNeighborsQuery(signatureId, neighbors);
_dbSafeAccessMutex.unlock();
return r;
}
//TODO Check also in the trash ?
bool DBDriver::loadNeighbors(int signatureId, NeighborsMultiMap & neighbors) const
void DBDriver::getWeight(int signatureId, int & weight) const
{
bool r;
_dbSafeAccessMutex.lock();
r = this->loadNeighborsQuery(signatureId, neighbors);
this->getWeightQuery(signatureId, weight);
_dbSafeAccessMutex.unlock();
return r;
}
//TODO Check also in the trash ?
bool DBDriver::getWeight(int signatureId, int & weight) const
void DBDriver::getLoopClosureIds(int signatureId, std::set<int> & loopIds, std::set<int> & childIds) const
{
bool r;
_dbSafeAccessMutex.lock();
r = this->getWeightQuery(signatureId, weight);
this->getLoopClosureIdsQuery(signatureId, loopIds, childIds);
_dbSafeAccessMutex.unlock();
return r;
}
//TODO Check also in the trash ?
bool DBDriver::getLoopClosureIds(int signatureId, std::set<int> & loopIds, std::set<int> & childIds) const
void DBDriver::getAllNodeIds(std::set<int> & ids) const
{
bool r;
_dbSafeAccessMutex.lock();
r = this->getLoopClosureIdsQuery(signatureId, loopIds, childIds);
this->getAllNodeIdsQuery(ids);
_dbSafeAccessMutex.unlock();
return r;
}
//TODO Check also in the trash ?
bool DBDriver::getAllNodeIds(std::set<int> & ids) const
void DBDriver::getLastNodeId(int & id) const
{
bool r;
_dbSafeAccessMutex.lock();
r = this->getAllNodeIdsQuery(ids);
this->getLastIdQuery("Node", id);
_dbSafeAccessMutex.unlock();
return r;
}
//TODO Check also in the trash ?
bool DBDriver::getLastNodeId(int & id) const
void DBDriver::getLastWordId(int & id) const
{
bool r;
_dbSafeAccessMutex.lock();
r = this->getLastNodeIdQuery(id);
this->getLastIdQuery("Word", id);
_dbSafeAccessMutex.unlock();
return r;
}
//TODO Check also in the trash ?
bool DBDriver::getLastWordId(int & id) const
void DBDriver::getInvertedIndexNi(int signatureId, int & ni) const
{
bool r;
_dbSafeAccessMutex.lock();
r = this->getLastWordIdQuery(id);
this->getInvertedIndexNiQuery(signatureId, ni);
_dbSafeAccessMutex.unlock();
return r;
}
//TODO Check also in the trash ?
bool DBDriver::getInvertedIndexNi(int signatureId, int & ni) const
{
bool r;
_dbSafeAccessMutex.lock();
r = this->getInvertedIndexNiQuery(signatureId, ni);
_dbSafeAccessMutex.unlock();
return r;
}
bool DBDriver::getHighestWeightedNodeIds(unsigned int count, std::multimap<int, int> & ids) const
{
bool r;
_dbSafeAccessMutex.lock();
r = this->getHighestWeightedNodeIdsQuery(count, ids);
_dbSafeAccessMutex.unlock();
return r;
}
bool DBDriver::addStatisticsAfterRun(int stMemSize, int lastSignAdded, int processMemUsed, int databaseMemUsed) const
void DBDriver::addStatisticsAfterRun(int stMemSize, int lastSignAdded, int processMemUsed, int databaseMemUsed) const
{
ULOGGER_DEBUG("");
if(this->isConnected())
@@ -745,13 +444,11 @@ bool DBDriver::addStatisticsAfterRun(int stMemSize, int lastSignAdded, int proce
<< processMemUsed << ","
<< databaseMemUsed << ");";
bool r = this->executeNoResultQuery(query.str());
return r;
this->executeNoResultQuery(query.str());
}
return false;
}
bool DBDriver::addStatisticsAfterRunSurf(int dictionarySize) const
void DBDriver::addStatisticsAfterRunSurf(int dictionarySize) const
{
ULOGGER_DEBUG("");
if(this->isConnected())
@@ -759,10 +456,8 @@ bool DBDriver::addStatisticsAfterRunSurf(int dictionarySize) const
std::stringstream query;
query << "INSERT INTO StatisticsDictionary(dictionary_size) values(" << dictionarySize << ");";
bool r = this->executeNoResultQuery(query.str());
return r;
this->executeNoResultQuery(query.str());
}
return false;
}
} // namespace rtabmap

70
corelib/src/DBDriverFactory.cpp
View File

@@ -1,70 +0,0 @@
/*
* Copyright (C) 2010-2011, Mathieu Labbe and IntRoLab - Universite de Sherbrooke
*
* This file is part of RTAB-Map.
*
* RTAB-Map is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* RTAB-Map is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with RTAB-Map. If not, see <http://www.gnu.org/licenses/>.
*/
#include "rtabmap/core/DBDriverFactory.h"
#include "DBDriverSqlite3.h"
#include "utilite/ULogger.h"
namespace rtabmap {
DBDriver * DBDriverFactory::createDBDriver(const std::string & dbDriverName, const ParametersMap & parameters)
{
// TODO Do it with dynamic link libraries...
// Find the driver...
// Link dynamically to the driver...
DBDriver * driver = 0;
// Static link
if(dbDriverName.compare("sqlite3") == 0)
{
driver = new DBDriverSqlite3(parameters);
}
else if(dbDriverName.compare("mysql") == 0)
{
// TODO mysql driver
ULOGGER_ERROR("mysql driver is not implemented!");
}
else if(dbDriverName.compare("postgresql") == 0)
{
// TODO postgresql driver
ULOGGER_ERROR("postgresql driver is not implemented!");
}
else if(dbDriverName.compare("oracle") == 0)
{
// TODO oracle driver
ULOGGER_ERROR("oracle driver is not implemented!");
}
else
{
ULOGGER_ERROR("Unknown driver \"%s\"", dbDriverName.c_str());
}
return driver;
}
DBDriverFactory::DBDriverFactory() {
}
DBDriverFactory::~DBDriverFactory() {
}
}

2305
corelib/src/DBDriverSqlite3.cpp
View File

File diff suppressed because it is too large Load Diff

64
corelib/src/DBDriverSqlite3.h
View File

@@ -22,7 +22,7 @@
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
#include "rtabmap/core/DBDriver.h"
#include <opencv2/features2d/features2d.hpp>
#include <sqlite3.h>
namespace rtabmap {
@@ -32,7 +32,6 @@ public:
DBDriverSqlite3(const ParametersMap & parameters = ParametersMap());
virtual ~DBDriverSqlite3();
virtual std::string getDriverName() const {return "sqlite3";}
virtual void parseParameters(const ParametersMap & parameters);
void setDbInMemory(bool dbInMemory);
void setJournalMode(int journalMode);
@@ -46,55 +45,44 @@ private:
virtual bool isConnectedQuery() const;
virtual long getMemoryUsedQuery() const; // In bytes
virtual bool executeNoResultQuery(const std::string & sql) const;
virtual void executeNoResultQuery(const std::string & sql) const;
virtual bool changeWordsRefQuery(const std::map<int, int> & refsToChange) const; // <oldWordId, activeWordId>
virtual bool deleteWordsQuery(const std::vector<int> & ids) const;
virtual bool getNeighborIdsQuery(int signatureId, std::set<int> & neighbors, bool onlyWithActions = false) const;
virtual bool getWeightQuery(int signatureId, int & weight) const;
virtual bool getLoopClosureIdsQuery(int signatureId, std::set<int> & loopIds, std::set<int> & childIds) const;
virtual void getNeighborIdsQuery(int signatureId, std::set<int> & neighbors, bool onlyWithActions = false) const;
virtual void getWeightQuery(int signatureId, int & weight) const;
virtual void getLoopClosureIdsQuery(int signatureId, std::set<int> & loopIds, std::set<int> & childIds) const;
virtual bool saveQuery(const std::vector<VisualWord *> & visualWords) const;
virtual bool updateQuery(const std::list<Signature *> & signatures) const;
virtual bool saveQuery(const std::list<Signature *> & signatures) const;
virtual void saveQuery(const std::vector<VisualWord *> & visualWords) const;
virtual void updateQuery(const std::list<Signature *> & signatures) const;
virtual void saveQuery(const std::list<Signature *> & signatures) const;
// Load objects
virtual bool loadQuery(VWDictionary * dictionary) const;
virtual bool loadLastNodesQuery(std::list<Signature *> & signatures) const;
virtual bool loadQuery(int signatureId, Signature ** s) const;
virtual bool loadQuery(int wordId, VisualWord ** vw) const;
virtual bool loadQuery(int signatureId, KeypointSignature * ss) const;
virtual bool loadQuery(int signatureId, SMSignature * ss) const;
virtual bool loadKeypointSignaturesQuery(const std::list<int> & ids, std::list<Signature *> & signatures) const;
virtual bool loadSMSignaturesQuery(const std::list<int> & ids, std::list<Signature *> & signatures) const;
virtual bool loadWordsQuery(const std::list<int> & wordIds, std::list<VisualWord *> & vws) const;
virtual bool loadNeighborsQuery(int signatureId, NeighborsMultiMap & neighbors) const;
bool loadLinksQuery(std::list<Signature *> & signatures) const;
virtual void loadQuery(VWDictionary * dictionary) const;
virtual void loadLastNodesQuery(std::list<Signature *> & signatures) const;
virtual void loadSignaturesQuery(const std::list<int> & ids, std::list<Signature *> & signatures) const;
virtual void loadWordsQuery(const std::set<int> & wordIds, std::list<VisualWord *> & vws) const;
virtual void loadNeighborsQuery(int signatureId, std::set<int> & neighbors) const;
virtual bool getRawDataQuery(int id, std::list<Sensor> & rawData) const;
virtual bool getActuatorDataQuery(int id, std::list<Actuator> & data) const;
virtual bool getAllNodeIdsQuery(std::set<int> & ids) const;
virtual bool getLastNodeIdQuery(int & id) const;
virtual bool getLastWordIdQuery(int & id) const;
virtual bool getInvertedIndexNiQuery(int signatureId, int & ni) const;
virtual bool getHighestWeightedNodeIdsQuery(unsigned int count, std::multimap<int, int> & ids) const;
virtual void getImageQuery(int nodeId, cv::Mat & image) const;
virtual void getAllNodeIdsQuery(std::set<int> & ids) const;
virtual void getLastIdQuery(const std::string & tableName, int & id) const;
virtual void getInvertedIndexNiQuery(int signatureId, int & ni) const;
private:
std::string queryStepNode() const;
std::string queryStepSensor() const;
std::string queryStepNodeToSensor() const;
std::string queryStepImage() const;
std::string queryStepLink() const;
std::string queryStepActuator() const;
std::string queryStepWordsChanged() const;
std::string queryStepKeypoint() const;
std::string queryStepSensors() const;
int stepNode(sqlite3_stmt * ppStmt, const Signature * s) const;
int stepSensor(sqlite3_stmt * ppStmt, int id, int num, const std::vector<int> & data, const Sensor & sensor) const;
int stepLink(sqlite3_stmt * ppStmt, int fromId, int toId, int type, int actuator_id, const std::vector<int> & baseIds) const;
int stepActuator(sqlite3_stmt * ppStmt, int id, int num, const Actuator & actuator) const;
int stepWordsChanged(sqlite3_stmt * ppStmt, int signatureId, int oldWordId, int newWordId) const;
int stepKeypoint(sqlite3_stmt * ppStmt, int signatureId, int wordId, const cv::KeyPoint & kp) const;
void stepNode(sqlite3_stmt * ppStmt, const Signature * s) const;
void stepNodeToSensor(sqlite3_stmt * ppStmt, int nodeId, int sensorId, int num) const;
void stepImage(sqlite3_stmt * ppStmt, int id, const cv::Mat & image) const;
void stepLink(sqlite3_stmt * ppStmt, int fromId, int toId, int type) const;
void stepWordsChanged(sqlite3_stmt * ppStmt, int signatureId, int oldWordId, int newWordId) const;
void stepKeypoint(sqlite3_stmt * ppStmt, int signatureId, int wordId, const cv::KeyPoint & kp) const;
private:
void loadLinksQuery(std::list<Signature *> & signatures) const;
int loadOrSaveDb(sqlite3 *pInMemory, const std::string & fileName, int isSave) const;
private:

89
corelib/src/DBReader.cpp
View File

@@ -7,24 +7,20 @@
#include "rtabmap/core/DBReader.h"
#include "rtabmap/core/DBDriver.h"
#include "rtabmap/core/SensorimotorEvent.h"
#include "rtabmap/core/DBDriverFactory.h"
#include "DBDriverSqlite3.h"
#include <utilite/ULogger.h>
#include <utilite/UEventsManager.h>
#include <utilite/UFile.h>
#include "rtabmap/core/Camera.h"
namespace rtabmap {
DBReader::DBReader(const std::string & databasePath,
float frameRate,
const std::set<Sensor::Type> & sensorTypes,
const std::set<Actuator::Type> & actuatorTypes) :
float frameRate) :
_path(databasePath),
_frameRate(frameRate),
_sensorTypes(sensorTypes),
_actuatorTypes(actuatorTypes),
_dbDriver(0),
_currentId(_ids.end())
{
@@ -40,7 +36,7 @@ DBReader::~DBReader()
}
}
bool DBReader::init()
bool DBReader::init(int startIndex)
{
if(_dbDriver)
{
@@ -59,7 +55,7 @@ bool DBReader::init()
rtabmap::ParametersMap parameters;
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kDbSqlite3InMemory(), "false"));
_dbDriver = DBDriverFactory::createDBDriver("sqlite3", parameters);
_dbDriver = new DBDriverSqlite3(parameters);
if(!_dbDriver)
{
UERROR("Driver doesn't exist.");
@@ -75,6 +71,18 @@ bool DBReader::init()
_dbDriver->getAllNodeIds(_ids);
_currentId = _ids.begin();
if(startIndex>0 && _ids.size())
{
std::set<int>::iterator iter = _ids.lower_bound(startIndex);
if(iter == _ids.end())
{
UWARN("Start index is too high (%d), the last in database is %d. Starting from beginning...", startIndex, *_ids.rbegin());
}
else
{
_currentId = iter;
}
}
return true;
}
@@ -94,27 +102,23 @@ void DBReader::mainLoopBegin()
void DBReader::mainLoop()
{
std::list<Sensor> sensors;
std::list<Actuator> actuators;
this->getNextSensorimotorState(sensors, actuators);
if(!sensors.empty() || !actuators.empty())
cv::Mat image;
this->getNextImage(image);
if(!image.empty())
{
UEventsManager::post(new SensorimotorEvent(sensors, actuators));
UEventsManager::post(new CameraEvent(image));
}
else if(!this->isKilled())
{
UDEBUG("no more sensorimotor states...");
UDEBUG("no more images...");
this->kill();
UEventsManager::post(new SensorimotorEvent());
UEventsManager::post(new CameraEvent());
}
}
void DBReader::getNextSensorimotorState(std::list<Sensor> & sensors, std::list<Actuator> & actuators)
void DBReader::getNextImage(cv::Mat & image)
{
sensors.clear();
actuators.clear();
if(_dbDriver)
{
float frameRate = _frameRate;
@@ -140,49 +144,12 @@ void DBReader::getNextSensorimotorState(std::list<Sensor> & sensors, std::list<A
if(!this->isKilled() && _currentId != _ids.end())
{
//sensors
_dbDriver->getRawData(*_currentId, sensors);
//actuators
NeighborsMultiMap neighbors;
_dbDriver->getImage(*_currentId, image);
++_currentId;
if(_currentId != _ids.end())
if(image.empty())
{
_dbDriver->getActuatorData(*_currentId, actuators);
UWARN("No image loaded from the database!");
}
UDEBUG("sensors.size=%d actuators.size=%d", sensors.size(), actuators.size());
//filtering for types wanted
if(_sensorTypes.size())
{
for(std::list<Sensor>::iterator jter=sensors.begin(); jter!=sensors.end();)
{
if(_sensorTypes.find((Sensor::Type)jter->type()) == _sensorTypes.end())
{
jter = sensors.erase(jter);
}
else
{
++jter;
}
}
}
if(_actuatorTypes.size())
{
for(std::list<Actuator>::iterator jter=actuators.begin(); jter!=actuators.end();)
{
if(_actuatorTypes.find((Actuator::Type)jter->type()) == _actuatorTypes.end())
{
jter = actuators.erase(jter);
}
else
{
++jter;
}
}
}
UDEBUG("after filtering sensors.size=%d actuators.size=%d", sensors.size(), actuators.size());
}
}
else

82
corelib/src/EpipolarGeometry.cpp
View File

@@ -18,9 +18,11 @@
*/
#include "rtabmap/core/EpipolarGeometry.h"
#include "Signature.h"
#include "utilite/ULogger.h"
#include "utilite/UTimer.h"
#include "utilite/UStl.h"
#include "utilite/UMath.h"
#include <opencv2/core/core.hpp>
#include <opencv2/core/core_c.h>
@@ -30,8 +32,74 @@
namespace rtabmap
{
/////////////////////////
// HypVerificatorEpipolarGeo
/////////////////////////
EpipolarGeometry::EpipolarGeometry(const ParametersMap & parameters) :
_matchCountMinAccepted(Parameters::defaultVhEpMatchCountMin()),
_ransacParam1(Parameters::defaultVhEpRansacParam1()),
_ransacParam2(Parameters::defaultVhEpRansacParam2())
{
this->parseParameters(parameters);
}
EpipolarGeometry::~EpipolarGeometry() {
}
void EpipolarGeometry::parseParameters(const ParametersMap & parameters)
{
ParametersMap::const_iterator iter;
if((iter=parameters.find(Parameters::kVhEpMatchCountMin())) != parameters.end())
{
_matchCountMinAccepted = std::atoi((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kVhEpRansacParam1())) != parameters.end())
{
_ransacParam1 = std::atof((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kVhEpRansacParam2())) != parameters.end())
{
_ransacParam2 = std::atof((*iter).second.c_str());
}
}
bool EpipolarGeometry::check(const Signature * ssA, const Signature * ssB)
{
if(ssA == 0 || ssB == 0)
{
return false;
}
ULOGGER_DEBUG("id(%d,%d)", ssA->id(), ssB->id());
std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > pairs;
findPairsUnique(ssA->getWords(), ssB->getWords(), pairs);
if((int)pairs.size()<_matchCountMinAccepted)
{
return false;
}
std::vector<uchar> status;
cv::Mat f = findFFromWords(pairs, status, _ransacParam1, _ransacParam2);
int inliers = uSum(status);
if(inliers < _matchCountMinAccepted)
{
ULOGGER_DEBUG("Epipolar constraint failed A : not enough inliers (%d/%d), min is %d", inliers, pairs.size(), _matchCountMinAccepted);
return false;
}
else
{
UDEBUG("inliers = %d/%d", inliers, pairs.size());
return true;
}
}
//STATIC STUFF
//Epipolar geometry
void findEpipolesFromF(const cv::Mat & fundamentalMatrix, cv::Vec3d & e1, cv::Vec3d & e2)
void EpipolarGeometry::findEpipolesFromF(const cv::Mat & fundamentalMatrix, cv::Vec3d & e1, cv::Vec3d & e2)
{
if(fundamentalMatrix.rows != 3 || fundamentalMatrix.cols != 3)
{
@@ -66,7 +134,7 @@ void findEpipolesFromF(const cv::Mat & fundamentalMatrix, cv::Vec3d & e1, cv::Ve
//Assuming P0 = [eye(3) zeros(3,1)]
// x1 and x2 are 2D points
// return camera matrix P (3x4) matrix
cv::Mat findPFromF(const cv::Mat & fundamentalMatrix, const cv::Mat & x1, const cv::Mat & x2)
cv::Mat EpipolarGeometry::findPFromF(const cv::Mat & fundamentalMatrix, const cv::Mat & x1, const cv::Mat & x2)
{
if(fundamentalMatrix.rows != 3 || fundamentalMatrix.cols != 3)
@@ -229,7 +297,7 @@ cv::Mat findPFromF(const cv::Mat & fundamentalMatrix, const cv::Mat & x1, const
return p;
}
cv::Mat findFFromWords(
cv::Mat EpipolarGeometry::findFFromWords(
const std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs, // id, kpt1, kpt2
std::vector<uchar> & status,
double ransacParam1,
@@ -312,7 +380,7 @@ cv::Mat findFFromWords(
return fundamentalMatrix;
}
void findRTFromP(
void EpipolarGeometry::findRTFromP(
const cv::Mat & p,
cv::Mat & r,
cv::Mat & t)
@@ -331,7 +399,7 @@ void findRTFromP(
* 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 findPairs(const std::multimap<int, cv::KeyPoint> & wordsA,
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)
{
@@ -359,7 +427,7 @@ int findPairs(const std::multimap<int, cv::KeyPoint> & wordsA,
* if a=[1 2 3 4 6 6], b=[1 1 2 4 5 6 6], results= [(2,2) (4,4)]
* realPairsCount = 5
*/
int findPairsUnique(const std::multimap<int, cv::KeyPoint> & wordsA,
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)
{
@@ -388,7 +456,7 @@ int findPairsUnique(const std::multimap<int, cv::KeyPoint> & wordsA,
* 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 findPairsAll(const std::multimap<int, cv::KeyPoint> & wordsA,
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)
{

619
corelib/src/Features2d.cpp Normal file
View File

@@ -0,0 +1,619 @@
/*
* Copyright (C) 2010-2011, Mathieu Labbe and IntRoLab - Universite de Sherbrooke
*
* This file is part of RTAB-Map.
*
* RTAB-Map is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* RTAB-Map is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with RTAB-Map. If not, see <http://www.gnu.org/licenses/>.
*/
#include "rtabmap/core/Features2d.h"
#include "utilite/UStl.h"
#include "utilite/UConversion.h"
#include "utilite/ULogger.h"
#include "utilite/UMath.h"
#include "utilite/ULogger.h"
#include "utilite/UTimer.h"
#include <opencv2/imgproc/imgproc_c.h>
#include <opencv2/gpu/gpu.hpp>
#include <opencv2/core/version.hpp>
#if CV_MAJOR_VERSION >=2 and CV_MINOR_VERSION >=4
#include <opencv2/nonfree/features2d.hpp>
#endif
#define OPENCV_SURF_GPU CV_MAJOR_VERSION >= 2 and CV_MINOR_VERSION >=2 and CV_SUBMINOR_VERSION>=1
namespace rtabmap {
/////////////////////
// KeypointDescriptor
/////////////////////
KeypointDescriptor::KeypointDescriptor(const ParametersMap & parameters)
{
this->parseParameters(parameters);
}
KeypointDescriptor::~KeypointDescriptor()
{
}
void KeypointDescriptor::parseParameters(const ParametersMap & parameters)
{
}
//////////////////////////
//SURFDescriptor
//////////////////////////
SURFDescriptor::SURFDescriptor(const ParametersMap & parameters) :
KeypointDescriptor(parameters),
_hessianThreshold(Parameters::defaultSURFHessianThreshold()),
_nOctaves(Parameters::defaultSURFOctaves()),
_nOctaveLayers(Parameters::defaultSURFOctaveLayers()),
_extended(Parameters::defaultSURFExtended()),
_upright(Parameters::defaultSURFUpright()),
_gpuVersion(Parameters::defaultSURFGpuVersion())
{
this->parseParameters(parameters);
}
SURFDescriptor::~SURFDescriptor()
{
}
void SURFDescriptor::parseParameters(const ParametersMap & parameters)
{
ParametersMap::const_iterator iter;
if((iter=parameters.find(Parameters::kSURFExtended())) != parameters.end())
{
_extended = uStr2Bool((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSURFHessianThreshold())) != parameters.end())
{
_hessianThreshold = std::atof((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSURFOctaveLayers())) != parameters.end())
{
_nOctaveLayers = std::atoi((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSURFOctaves())) != parameters.end())
{
_nOctaves = std::atoi((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSURFOctaves())) != parameters.end())
{
_nOctaves = std::atoi((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSURFUpright())) != parameters.end())
{
_upright = uStr2Bool((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSURFGpuVersion())) != parameters.end())
{
_gpuVersion = uStr2Bool((*iter).second.c_str());
}
KeypointDescriptor::parseParameters(parameters);
}
cv::Mat SURFDescriptor::generateDescriptors(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
{
ULOGGER_DEBUG("");
cv::Mat descriptors;
if(image.empty())
{
ULOGGER_ERROR("Image is null ?!?");
return descriptors;
}
// SURF support only grayscale images
cv::Mat imageGrayScale;
if(image.channels() != 1 || image.depth() != CV_8U)
{
cv::cvtColor(image, imageGrayScale, CV_BGR2GRAY);
}
cv::Mat img;
if(!imageGrayScale.empty())
{
img = imageGrayScale;
}
else
{
img = image;
}
/*#if OPENCV_SURF_GPU
if(_gpuVersion)
{
std::vector<float> d;
cv::gpu::GpuMat imgGpu(img);
cv::gpu::GpuMat descriptorsGpu;
cv::gpu::GpuMat keypointsGpu;
cv::gpu::SURF_GPU surfGpu(_params.hessianThreshold, _params.nOctaves, _params.nOctaveLayers, _params.extended, 0.01f, _params.upright);
surfGpu.uploadKeypoints(keypoints, keypointsGpu);
surfGpu(imgGpu, cv::gpu::GpuMat(), keypointsGpu, descriptorsGpu, true);
surfGpu.downloadDescriptors(descriptorsGpu, d);
unsigned int dim = _params.extended?128:64;
descriptors = cv::Mat(d.size()/dim, dim, CV_32F);
for(int i=0; i<descriptors.rows; ++i)
{
float * rowFl = descriptors.ptr<float>(i);
memcpy(rowFl, &d[i*dim], dim*sizeof(float));
}
}
else
{
cv::SurfDescriptorExtractor extractor(_params.nOctaves, _params.nOctaveLayers, _params.extended, _params.upright);
extractor.compute(img, keypoints, descriptors);
}
#else*/
#if CV_MAJOR_VERSION >=2 and CV_MINOR_VERSION >=4
cv::SURF extractor(_hessianThreshold, _nOctaves, _nOctaveLayers, _extended, _upright);
extractor.compute(img, keypoints, descriptors);
#else
cv::SurfDescriptorExtractor extractor(_nOctaves, _nOctaveLayers, _extended, _upright);
extractor.compute(img, keypoints, descriptors);
#endif
//#endif
return descriptors;
}
//////////////////////////
//SIFTDescriptor
//////////////////////////
SIFTDescriptor::SIFTDescriptor(const ParametersMap & parameters) :
KeypointDescriptor(parameters),
_nfeatures(Parameters::defaultSIFTNFeatures()),
_nOctaveLayers(Parameters::defaultSIFTNOctaveLayers()),
_contrastThreshold(Parameters::defaultSIFTContrastThreshold()),
_edgeThreshold(Parameters::defaultSIFTEdgeThreshold()),
_sigma(Parameters::defaultSIFTSigma())
{
this->parseParameters(parameters);
}
SIFTDescriptor::~SIFTDescriptor()
{
}
void SIFTDescriptor::parseParameters(const ParametersMap & parameters)
{
ParametersMap::const_iterator iter;
if((iter=parameters.find(Parameters::kSIFTContrastThreshold())) != parameters.end())
{
_contrastThreshold = std::atof((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSIFTEdgeThreshold())) != parameters.end())
{
_edgeThreshold = std::atof((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSIFTNFeatures())) != parameters.end())
{
_nfeatures = std::atoi((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSIFTNOctaveLayers())) != parameters.end())
{
_nOctaveLayers = std::atoi((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSIFTSigma())) != parameters.end())
{
_sigma = std::atof((*iter).second.c_str());
}
KeypointDescriptor::parseParameters(parameters);
}
cv::Mat SIFTDescriptor::generateDescriptors(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
{
ULOGGER_DEBUG("");
cv::Mat descriptors;
if(image.empty())
{
ULOGGER_ERROR("Image is null ?!?");
return descriptors;
}
// SURF support only grayscale images
cv::Mat imageGrayScale;
if(image.channels() != 1 || image.depth() != CV_8U)
{
cv::cvtColor(image, imageGrayScale, CV_BGR2GRAY);
}
cv::Mat img;
if(!imageGrayScale.empty())
{
img = imageGrayScale;
}
else
{
img = image;
}
#if CV_MAJOR_VERSION >=2 and CV_MINOR_VERSION >=4
cv::SIFT extractor(_nfeatures, _nOctaveLayers, _contrastThreshold, _edgeThreshold, _sigma);
extractor.compute(img, keypoints, descriptors);
#else
cv::SIFT extractor(cv::SIFT::DescriptorParams::GET_DEFAULT_MAGNIFICATION(),
cv::SIFT::DescriptorParams::DEFAULT_IS_NORMALIZE,
true,
cv::SIFT::CommonParams::DEFAULT_NOCTAVES,
_nOctaveLayers);
extractor(img, cv::Mat(), keypoints, descriptors, true);
#endif
return descriptors;
}
/////////////////////
// KeypointDetector
/////////////////////
KeypointDetector::KeypointDetector(const ParametersMap & parameters) :
_wordsPerImageTarget(Parameters::defaultKpWordsPerImage()),
_roiRatios(std::vector<float>(4, 0.0f))
{
this->setRoi(Parameters::defaultKpRoiRatios());
this->parseParameters(parameters);
}
void KeypointDetector::parseParameters(const ParametersMap & parameters)
{
ParametersMap::const_iterator iter;
if((iter=parameters.find(Parameters::kKpWordsPerImage())) != parameters.end())
{
_wordsPerImageTarget = std::atoi((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kKpRoiRatios())) != parameters.end())
{
this->setRoi((*iter).second);
}
}
std::vector<cv::KeyPoint> KeypointDetector::generateKeypoints(const cv::Mat & image)
{
ULOGGER_DEBUG("");
std::vector<cv::KeyPoint> keypoints;
if(!image.empty())
{
UTimer timer;
timer.start();
cv::Rect roi = computeRoi(image);
// Get keypoints
keypoints = this->_generateKeypoints(image, roi);
ULOGGER_DEBUG("Keypoints extraction time = %f s, keypoints extracted = %d", timer.ticks(), keypoints.size());
//clip the number of words... to _wordsPerImageTarget
// Variable hessian threshold
if(_wordsPerImageTarget > 0)
{
if(keypoints.size() > 0)
{
// 10% margin...
if(keypoints.size() > 1.1 * _wordsPerImageTarget)
{
ULOGGER_DEBUG("too much words (%d), removing words under the new hessian threshold", keypoints.size());
// Remove words under the new hessian threshold
// Sort words by hessian
std::multimap<float, std::vector<cv::KeyPoint>::iterator> hessianMap; // <hessian,id>
for(std::vector<cv::KeyPoint>::iterator itKey = keypoints.begin(); itKey != keypoints.end(); ++itKey)
{
//Keep track of the data, to be easier to manage the data in the next step
hessianMap.insert(std::pair<float, std::vector<cv::KeyPoint>::iterator>(fabs(itKey->response), itKey));
}
// Remove them from the signature
int removed = hessianMap.size()-_wordsPerImageTarget;
std::multimap<float, std::vector<cv::KeyPoint>::iterator>::reverse_iterator iter = hessianMap.rbegin();
std::vector<cv::KeyPoint> kptsTmp(_wordsPerImageTarget);
for(unsigned int k=0; k < kptsTmp.size() && iter!=hessianMap.rend(); ++k, ++iter)
{
kptsTmp[k] = *iter->second;
// Adjust keypoint position to raw image
kptsTmp[k].pt.x += roi.x;
kptsTmp[k].pt.y += roi.y;
}
keypoints = kptsTmp;
ULOGGER_DEBUG("%d keypoints removed, (kept %d), minimum response=%f", removed, keypoints.size(), kptsTmp.size()?kptsTmp.back().response:0.0f);
}
else if(roi.x || roi.y)
{
// Adjust keypoint position to raw image
for(std::vector<cv::KeyPoint>::iterator iter=keypoints.begin(); iter!=keypoints.end(); ++iter)
{
iter->pt.x += roi.x;
iter->pt.y += roi.y;
}
}
}
ULOGGER_DEBUG("removing words time = %f s", timer.ticks());
}
else if(roi.x || roi.y)
{
// Adjust keypoint position to raw image
for(std::vector<cv::KeyPoint>::iterator iter=keypoints.begin(); iter!=keypoints.end(); ++iter)
{
iter->pt.x += roi.x;
iter->pt.y += roi.y;
}
}
}
else
{
ULOGGER_ERROR("Image is null!");
}
return keypoints;
}
void KeypointDetector::setRoi(const std::string & roi)
{
std::list<std::string> strValues = uSplit(roi, ' ');
if(strValues.size() != 4)
{
ULOGGER_ERROR("The number of values must be 4 (roi=\"%s\")", roi.c_str());
}
else
{
std::vector<float> tmpValues(4);
unsigned int i=0;
for(std::list<std::string>::iterator iter = strValues.begin(); iter!=strValues.end(); ++iter)
{
tmpValues[i] = std::atof((*iter).c_str());
++i;
}
if(tmpValues[0] >= 0 && tmpValues[0] < 1 && tmpValues[0] < 1.0f-tmpValues[1] &&
tmpValues[1] >= 0 && tmpValues[1] < 1 && tmpValues[1] < 1.0f-tmpValues[0] &&
tmpValues[2] >= 0 && tmpValues[2] < 1 && tmpValues[2] < 1.0f-tmpValues[3] &&
tmpValues[3] >= 0 && tmpValues[3] < 1 && tmpValues[3] < 1.0f-tmpValues[2])
{
_roiRatios = tmpValues;
}
else
{
ULOGGER_ERROR("The roi ratios are not valid (roi=\"%s\")", roi.c_str());
}
}
}
cv::Rect KeypointDetector::computeRoi(const cv::Mat & image) const
{
if(!image.empty() && _roiRatios.size() == 4)
{
float width = image.cols;
float height = image.rows;
cv::Rect roi(0, 0, width, height);
UDEBUG("roi ratios = %f, %f, %f, %f", _roiRatios[0],_roiRatios[1],_roiRatios[2],_roiRatios[3]);
UDEBUG("roi = %d, %d, %d, %d", roi.x, roi.y, roi.width, roi.height);
//left roi
if(_roiRatios[0] > 0 && _roiRatios[0] < 1 - _roiRatios[1])
{
roi.x = width * _roiRatios[0];
}
//right roi
roi.width = width - roi.x;
if(_roiRatios[1] > 0 && _roiRatios[1] < 1 - _roiRatios[0])
{
roi.width -= width * _roiRatios[1];
}
//top roi
if(_roiRatios[2] > 0 && _roiRatios[2] < 1 - _roiRatios[3])
{
roi.y = height * _roiRatios[2];
}
//bottom roi
roi.height = height - roi.y;
if(_roiRatios[3] > 0 && _roiRatios[3] < 1 - _roiRatios[2])
{
roi.height -= height * _roiRatios[3];
}
UDEBUG("roi = %d, %d, %d, %d", roi.x, roi.y, roi.width, roi.height);
return roi;
}
else
{
UERROR("Image is null or _roiRatios(=%d) != 4", _roiRatios.size());
return cv::Rect();
}
}
//////////////////////////
//SURFDetector
//////////////////////////
SURFDetector::SURFDetector(const ParametersMap & parameters) :
KeypointDetector(parameters),
_hessianThreshold(Parameters::defaultSURFHessianThreshold()),
_nOctaves(Parameters::defaultSURFOctaves()),
_nOctaveLayers(Parameters::defaultSURFOctaveLayers()),
_extended(Parameters::defaultSURFExtended()),
_upright(Parameters::defaultSURFUpright()),
_gpuVersion(Parameters::defaultSURFGpuVersion())
{
this->parseParameters(parameters);
}
SURFDetector::~SURFDetector()
{
}
void SURFDetector::parseParameters(const ParametersMap & parameters)
{
ParametersMap::const_iterator iter;
if((iter=parameters.find(Parameters::kSURFExtended())) != parameters.end())
{
_extended = uStr2Bool((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSURFHessianThreshold())) != parameters.end())
{
_hessianThreshold = std::atof((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSURFOctaveLayers())) != parameters.end())
{
_nOctaveLayers = std::atoi((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSURFOctaves())) != parameters.end())
{
_nOctaves = std::atoi((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSURFOctaves())) != parameters.end())
{
_nOctaves = std::atoi((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSURFUpright())) != parameters.end())
{
_upright = uStr2Bool((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSURFGpuVersion())) != parameters.end())
{
_gpuVersion = uStr2Bool((*iter).second.c_str());
}
KeypointDetector::parseParameters(parameters);
}
std::vector<cv::KeyPoint> SURFDetector::_generateKeypoints(const cv::Mat & image, const cv::Rect & roi) const
{
ULOGGER_DEBUG("");
std::vector<cv::KeyPoint> keypoints;
if(image.empty())
{
ULOGGER_ERROR("Image is null ?!?");
return keypoints;
}
// SURF support only grayscale images
cv::Mat imageGrayScale;
if(image.channels() != 1 || image.depth() != CV_8U)
{
cv::cvtColor(image, imageGrayScale, CV_BGR2GRAY);
}
cv::Mat img;
if(!imageGrayScale.empty())
{
img = imageGrayScale;
}
else
{
img = image;
}
cv::Mat imgRoi(img, roi);
/*#if OPENCV_SURF_GPU
if(_gpuVersion )
{
cv::gpu::GpuMat imgGpu(imgRoi);
cv::gpu::GpuMat keypointsGpu;
cv::gpu::SURF_GPU surfGpu(params.hessianThreshold, params.nOctaves, params.nOctaveLayers, params.extended, 0.01f, params.upright);
surfGpu(imgGpu, cv::gpu::GpuMat(), keypointsGpu);
surfGpu.downloadKeypoints(keypointsGpu, keypoints);
}
else
{
cv::SurfFeatureDetector detector(params.hessianThreshold, params.nOctaves, params.nOctaveLayers, params.upright);
detector.detect(imgRoi, keypoints);
}
#else*/
cv::SURF detector(_hessianThreshold, _nOctaves, _nOctaveLayers, _extended, _upright);
#if CV_MAJOR_VERSION >=2 and CV_MINOR_VERSION >=4
detector.detect(imgRoi, keypoints);
#else
detector(imgRoi, cv::Mat(), keypoints);
#endif
//#endif
return keypoints;
}
//////////////////////////
//SIFTDetector
//////////////////////////
SIFTDetector::SIFTDetector(const ParametersMap & parameters) :
KeypointDetector(parameters),
_nfeatures(Parameters::defaultSIFTNFeatures()),
_nOctaveLayers(Parameters::defaultSIFTNOctaveLayers()),
_contrastThreshold(Parameters::defaultSIFTContrastThreshold()),
_edgeThreshold(Parameters::defaultSIFTEdgeThreshold()),
_sigma(Parameters::defaultSIFTSigma())
{
this->parseParameters(parameters);
}
SIFTDetector::~SIFTDetector()
{
}
void SIFTDetector::parseParameters(const ParametersMap & parameters)
{
ParametersMap::const_iterator iter;
if((iter=parameters.find(Parameters::kSIFTContrastThreshold())) != parameters.end())
{
_contrastThreshold = std::atof((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSIFTEdgeThreshold())) != parameters.end())
{
_edgeThreshold = std::atof((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSIFTNFeatures())) != parameters.end())
{
_nfeatures = std::atoi((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSIFTNOctaveLayers())) != parameters.end())
{
_nOctaveLayers = std::atoi((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSIFTSigma())) != parameters.end())
{
_sigma = std::atof((*iter).second.c_str());
}
KeypointDetector::parseParameters(parameters);
}
std::vector<cv::KeyPoint> SIFTDetector::_generateKeypoints(const cv::Mat & image, const cv::Rect & roi) const
{
ULOGGER_DEBUG("");
std::vector<cv::KeyPoint> keypoints;
if(image.empty())
{
ULOGGER_ERROR("Image is null ?!?");
return keypoints;
}
// SURF support only grayscale images
cv::Mat imageGrayScale;
if(image.channels() != 1 || image.depth() != CV_8U)
{
cv::cvtColor(image, imageGrayScale, CV_BGR2GRAY);
}
cv::Mat img;
if(!imageGrayScale.empty())
{
img = imageGrayScale;
}
else
{
img = image;
}
cv::Mat imgRoi(img, roi);
#if CV_MAJOR_VERSION >=2 and CV_MINOR_VERSION >=4
cv::SIFT detector(_nfeatures, _nOctaveLayers, _contrastThreshold, _edgeThreshold, _sigma);
detector.detect(imgRoi, keypoints); // Opencv surf keypoints
#else
cv::SIFT detector(_contrastThreshold, _edgeThreshold, cv::SIFT::CommonParams::DEFAULT_NOCTAVES, _nOctaveLayers);
detector(imgRoi, cv::Mat(), keypoints); // Opencv surf keypoints
#endif
return keypoints;
}
}

539
corelib/src/KeypointDescriptor.cpp
View File

@@ -1,539 +0,0 @@
/*
* Copyright (C) 2010-2011, Mathieu Labbe and IntRoLab - Universite de Sherbrooke
*
* This file is part of RTAB-Map.
*
* RTAB-Map is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* RTAB-Map is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with RTAB-Map. If not, see <http://www.gnu.org/licenses/>.
*/
#include "rtabmap/core/KeypointDescriptor.h"
#include "utilite/UStl.h"
#include "utilite/UConversion.h"
#include "utilite/ULogger.h"
#include "utilite/UMath.h"
#include "utilite/ULogger.h"
#include <opencv2/imgproc/imgproc_c.h>
#include <opencv2/gpu/gpu.hpp>
#include <opencv2/core/version.hpp>
#if CV_MAJOR_VERSION >=2 and CV_MINOR_VERSION >=4
#include <opencv2/nonfree/features2d.hpp>
#endif
#define OPENCV_SURF_GPU CV_MAJOR_VERSION >= 2 and CV_MINOR_VERSION >=2 and CV_SUBMINOR_VERSION>=1
namespace rtabmap {
KeypointDescriptor::KeypointDescriptor(const ParametersMap & parameters)
{
this->parseParameters(parameters);
}
KeypointDescriptor::~KeypointDescriptor()
{
}
void KeypointDescriptor::parseParameters(const ParametersMap & parameters)
{
}
//////////////////////////
//SURFDescriptor
//////////////////////////
SURFDescriptor::SURFDescriptor(const ParametersMap & parameters) :
KeypointDescriptor(parameters),
_hessianThreshold(Parameters::defaultSURFHessianThreshold()),
_nOctaves(Parameters::defaultSURFOctaves()),
_nOctaveLayers(Parameters::defaultSURFOctaveLayers()),
_extended(Parameters::defaultSURFExtended()),
_upright(Parameters::defaultSURFUpright()),
_gpuVersion(Parameters::defaultSURFGpuVersion())
{
this->parseParameters(parameters);
}
SURFDescriptor::~SURFDescriptor()
{
}
void SURFDescriptor::parseParameters(const ParametersMap & parameters)
{
ParametersMap::const_iterator iter;
if((iter=parameters.find(Parameters::kSURFExtended())) != parameters.end())
{
_extended = uStr2Bool((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSURFHessianThreshold())) != parameters.end())
{
_hessianThreshold = std::atof((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSURFOctaveLayers())) != parameters.end())
{
_nOctaveLayers = std::atoi((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSURFOctaves())) != parameters.end())
{
_nOctaves = std::atoi((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSURFOctaves())) != parameters.end())
{
_nOctaves = std::atoi((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSURFUpright())) != parameters.end())
{
_upright = uStr2Bool((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSURFGpuVersion())) != parameters.end())
{
_gpuVersion = uStr2Bool((*iter).second.c_str());
}
KeypointDescriptor::parseParameters(parameters);
}
cv::Mat SURFDescriptor::generateDescriptors(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
{
ULOGGER_DEBUG("");
cv::Mat descriptors;
if(image.empty())
{
ULOGGER_ERROR("Image is null ?!?");
return descriptors;
}
// SURF support only grayscale images
cv::Mat imageGrayScale;
if(image.channels() != 1 || image.depth() != CV_8U)
{
cv::cvtColor(image, imageGrayScale, CV_BGR2GRAY);
}
cv::Mat img;
if(!imageGrayScale.empty())
{
img = imageGrayScale;
}
else
{
img = image;
}
/*#if OPENCV_SURF_GPU
if(_gpuVersion)
{
std::vector<float> d;
cv::gpu::GpuMat imgGpu(img);
cv::gpu::GpuMat descriptorsGpu;
cv::gpu::GpuMat keypointsGpu;
cv::gpu::SURF_GPU surfGpu(_params.hessianThreshold, _params.nOctaves, _params.nOctaveLayers, _params.extended, 0.01f, _params.upright);
surfGpu.uploadKeypoints(keypoints, keypointsGpu);
surfGpu(imgGpu, cv::gpu::GpuMat(), keypointsGpu, descriptorsGpu, true);
surfGpu.downloadDescriptors(descriptorsGpu, d);
unsigned int dim = _params.extended?128:64;
descriptors = cv::Mat(d.size()/dim, dim, CV_32F);
for(int i=0; i<descriptors.rows; ++i)
{
float * rowFl = descriptors.ptr<float>(i);
memcpy(rowFl, &d[i*dim], dim*sizeof(float));
}
}
else
{
cv::SurfDescriptorExtractor extractor(_params.nOctaves, _params.nOctaveLayers, _params.extended, _params.upright);
extractor.compute(img, keypoints, descriptors);
}
#else*/
#if CV_MAJOR_VERSION >=2 and CV_MINOR_VERSION >=4
cv::SURF extractor(_hessianThreshold, _nOctaves, _nOctaveLayers, _extended, _upright);
extractor.compute(img, keypoints, descriptors);
#else
cv::SurfDescriptorExtractor extractor(_nOctaves, _nOctaveLayers, _extended, _upright);
extractor.compute(img, keypoints, descriptors);
#endif
//#endif
return descriptors;
}
//////////////////////////
//SIFTDescriptor
//////////////////////////
SIFTDescriptor::SIFTDescriptor(const ParametersMap & parameters) :
KeypointDescriptor(parameters),
_nfeatures(Parameters::defaultSIFTNFeatures()),
_nOctaveLayers(Parameters::defaultSIFTNOctaveLayers()),
_contrastThreshold(Parameters::defaultSIFTContrastThreshold()),
_edgeThreshold(Parameters::defaultSIFTEdgeThreshold()),
_sigma(Parameters::defaultSIFTSigma())
{
this->parseParameters(parameters);
}
SIFTDescriptor::~SIFTDescriptor()
{
}
void SIFTDescriptor::parseParameters(const ParametersMap & parameters)
{
ParametersMap::const_iterator iter;
if((iter=parameters.find(Parameters::kSIFTContrastThreshold())) != parameters.end())
{
_contrastThreshold = std::atof((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSIFTEdgeThreshold())) != parameters.end())
{
_edgeThreshold = std::atof((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSIFTNFeatures())) != parameters.end())
{
_nfeatures = std::atoi((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSIFTNOctaveLayers())) != parameters.end())
{
_nOctaveLayers = std::atoi((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSIFTSigma())) != parameters.end())
{
_sigma = std::atof((*iter).second.c_str());
}
KeypointDescriptor::parseParameters(parameters);
}
cv::Mat SIFTDescriptor::generateDescriptors(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
{
ULOGGER_DEBUG("");
cv::Mat descriptors;
if(image.empty())
{
ULOGGER_ERROR("Image is null ?!?");
return descriptors;
}
// SURF support only grayscale images
cv::Mat imageGrayScale;
if(image.channels() != 1 || image.depth() != CV_8U)
{
cv::cvtColor(image, imageGrayScale, CV_BGR2GRAY);
}
cv::Mat img;
if(!imageGrayScale.empty())
{
img = imageGrayScale;
}
else
{
img = image;
}
#if CV_MAJOR_VERSION >=2 and CV_MINOR_VERSION >=4
cv::SIFT extractor(_nfeatures, _nOctaveLayers, _contrastThreshold, _edgeThreshold, _sigma);
extractor.compute(img, keypoints, descriptors);
#else
cv::SIFT extractor(cv::SIFT::DescriptorParams::GET_DEFAULT_MAGNIFICATION(),
cv::SIFT::DescriptorParams::DEFAULT_IS_NORMALIZE,
true,
cv::SIFT::CommonParams::DEFAULT_NOCTAVES,
_nOctaveLayers);
extractor(img, cv::Mat(), keypoints, descriptors, true);
#endif
return descriptors;
}
//////////////////////////
//BRIEFDescriptor
//////////////////////////
BRIEFDescriptor::BRIEFDescriptor(const ParametersMap & parameters) :
KeypointDescriptor(parameters),
_size(Parameters::defaultBRIEFSize())
{
this->parseParameters(parameters);
}
BRIEFDescriptor::~BRIEFDescriptor()
{
}
void BRIEFDescriptor::parseParameters(const ParametersMap & parameters)
{
ParametersMap::const_iterator iter;
if((iter=parameters.find(Parameters::kBRIEFSize())) != parameters.end())
{
_size = std::atoi((*iter).second.c_str());
}
KeypointDescriptor::parseParameters(parameters);
}
cv::Mat BRIEFDescriptor::generateDescriptors(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
{
ULOGGER_DEBUG("");
cv::Mat descriptors;
if(image.empty())
{
ULOGGER_ERROR("Image is null ?!?");
return descriptors;
}
// BRIEF support only grayscale images ?
cv::Mat imageGrayScale;
if(image.channels() != 1 || image.depth() != CV_8U)
{
cv::cvtColor(image, imageGrayScale, CV_BGR2GRAY);
}
cv::Mat img;
if(!imageGrayScale.empty())
{
img = imageGrayScale;
}
else
{
img = image;
}
cv::BriefDescriptorExtractor brief(_size);
brief.compute(img, keypoints, descriptors);
return descriptors;
}
//////////////////////////
//ColorDescriptor
//////////////////////////
ColorDescriptor::ColorDescriptor(const ParametersMap & parameters) :
KeypointDescriptor(parameters)
{
this->parseParameters(parameters);
}
ColorDescriptor::~ColorDescriptor()
{
}
void ColorDescriptor::parseParameters(const ParametersMap & parameters)
{
// No parameter...
KeypointDescriptor::parseParameters(parameters);
}
cv::Mat ColorDescriptor::generateDescriptors(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
{
ULOGGER_DEBUG("");
cv::Mat descriptors;
if(image.empty())
{
ULOGGER_ERROR("Image is null ?!?");
return descriptors;
}
cv::Mat imageConverted;
if(image.channels() != 3 || image.depth() != CV_8U)
{
cv::cvtColor(image, imageConverted, CV_GRAY2BGR);
}
cv::Mat imgMat;
if(!imageConverted.empty())
{
imgMat = imageConverted;
}
else
{
imgMat = image;
}
//create descriptors...
descriptors = cv::Mat(keypoints.size(), 6, CV_32F);
int i=0;
for(std::vector<cv::KeyPoint>::const_iterator key=keypoints.begin(); key!=keypoints.end(); ++key)
{
int grayMax = -1; // grayValue
int grayMin = -1; // grayValue
float d[6] = {0};
std::vector<int> RxV;
cv::Point center = cv::Point(cvRound(key->pt.x), cvRound(key->pt.y));
int R = cvRound(key->size*1.2/9.*2);
this->getCircularROI(R, RxV);
cv::Mat_<cv::Vec3b>& img = (cv::Mat_<cv::Vec3b>&)imgMat; //3 channel pointer to image
// find the brighter and darker pixels
for( int dy = -R; dy <= R; ++dy )
{
int Rx = RxV[abs(dy)];
for( int dx = -Rx; dx <= Rx; ++dx )
{
if(center.y+dy < img.rows && center.y+dy >= 0 && center.x+dx < img.cols && center.x+dx >= 0)
{
//bgr
uchar b = img(center.y+dy, center.x+dx)[0];
uchar g = img(center.y+dy, center.x+dx)[1];
uchar r = img(center.y+dy, center.x+dx)[2];
int gray = b*0.114 + g*0.587 + r*0.299;
if(grayMax<0 || gray > grayMax)
{
grayMax = gray;
d[0] = b;
d[1] = g;
d[2] = r;
}
if(grayMin<0 || gray < grayMin)
{
grayMin = gray;
d[3] = b;
d[4] = g;
d[5] = r;
}
}
else
{
//ULOGGER_WARN("The keypoint size is outside of the image ranges (x,y)=(%d,%d) radius=%d", center.y+dy, center.x+dx, R);
}
}
}
for(int j=0; j<6; ++j)
{
descriptors.at<float>(i,j) = d[j] / 255; // Normalize between 0 and 1
}
++i;
}
return descriptors;
}
// the function returns x boundary coordinates of
// the circle for each y. RxV[y1] = x1 means that
// when y=y1, -x1 <=x<=x1 is inside the circle
// (from OpenCv doc, C++ Cheatsheet)
void ColorDescriptor::getCircularROI(int R, std::vector<int> & RxV) const
{
RxV.resize(R+1);
for( int y = 0; y <= R; y++ )
RxV[y] = cvRound(sqrt(double(R*R - y*y)));
}
//////////////////////////
//HueDescriptor
//////////////////////////
HueDescriptor::HueDescriptor(const ParametersMap & parameters) :
ColorDescriptor(parameters)
{
this->parseParameters(parameters);
}
HueDescriptor::~HueDescriptor()
{
}
void HueDescriptor::parseParameters(const ParametersMap & parameters)
{
// No parameter...
KeypointDescriptor::parseParameters(parameters);
}
cv::Mat HueDescriptor::generateDescriptors(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
{
ULOGGER_DEBUG("");
cv::Mat descriptors;
if(image.empty())
{
ULOGGER_ERROR("Image is null ?!?");
return descriptors;
}
cv::Mat imageConverted;
if(image.channels() != 3 || image.depth() != CV_8U)
{
cv::cvtColor(image, imageConverted, CV_GRAY2BGR);
}
cv::Mat imgMat;
if(!imageConverted.empty())
{
imgMat = imageConverted;
}
else
{
imgMat = image;
}
//create descriptors...
descriptors = cv::Mat(keypoints.size(), 2, CV_32F);
int i=0;
for(std::vector<cv::KeyPoint>::const_iterator key=keypoints.begin(); key!=keypoints.end(); ++key)
{
int intensityMax = -1;
int intensityMin = -1;
float d[2] = {0};
std::vector<int> RxV;
cv::Point center = cv::Point(cvRound(key->pt.x), cvRound(key->pt.y));
int R = cvRound(key->size*1.2/9.*2);
this->getCircularROI(R, RxV);
cv::Mat_<cv::Vec3b>& img = (cv::Mat_<cv::Vec3b>&)imgMat; //3 channel pointer to image
// find the brighter and darker pixels using the intensity
int dxb=0;
int dyb=0;
int dxd=0;
int dyd=0;
for( int dy = -R; dy <= R; ++dy )
{
int Rx = RxV[abs(dy)];
for( int dx = -Rx; dx <= Rx; ++dx )
{
if(center.y+dy < img.rows && center.y+dy >= 0 && center.x+dx < img.cols && center.x+dx >= 0)
{
//bgr
float b = float(img(center.y+dy, center.x+dx)[0]) / 255.0f;
float g = float(img(center.y+dy, center.x+dx)[1]) / 255.0f;
float r = float(img(center.y+dy, center.x+dx)[2]) / 255.0f;
int intensity = rgb2intensity(r, g, b);
if(intensityMax<0 || intensity > intensityMax)
{
intensityMax = intensity;
dxb = dx;
dyb = dy;
}
if(intensityMin<0 || intensity < intensityMin)
{
intensityMin = intensity;
dxd = dx;
dyd = dy;
}
}
else
{
//ULOGGER_WARN("The keypoint size is outside of the image ranges (x,y)=(%d,%d) radius=%d", center.y+dy, center.x+dx, R);
}
}
}
// brighter
float b = float(img(center.y+dyb, center.x+dxb)[0]) / 255.0f;
float g = float(img(center.y+dyb, center.x+dxb)[1]) / 255.0f;
float r = float(img(center.y+dyb, center.x+dxb)[2]) / 255.0f;
d[0] = rgb2hue(r, g, b);
// darker
b = float(img(center.y+dyd, center.x+dxd)[0]) / 255.0f;
g = float(img(center.y+dyd, center.x+dxd)[1]) / 255.0f;
r = float(img(center.y+dyd, center.x+dxd)[2]) / 255.0f;
d[1] = rgb2hue(r, g, b);
float * rowFl = descriptors.ptr<float>(i);
memcpy(rowFl, &d[i*2], 2*sizeof(float));
++i;
}
return descriptors;
}
// assuming that rgb values are normalized [0,1]
float HueDescriptor::rgb2hue(float r, float g, float b) const
{
double pi = 3.14159265359;
if(b<=g)
{
return acos(((r-g)+(r-b))/(2*sqrt((r-g)*(r-g)+(r-b)*(g-b))))/pi;
}
else
{
return (pi-acos(((r-g)+(r-b))/(2*sqrt((r-g)*(r-g)+(r-b)*(g-b)))))/pi;
}
}
}

479
corelib/src/KeypointDetector.cpp
View File

@@ -36,485 +36,6 @@
namespace rtabmap
{
KeypointDetector::KeypointDetector(const ParametersMap & parameters) :
_wordsPerImageTarget(Parameters::defaultKpWordsPerImage()),
_roiRatios(std::vector<float>(4, 0.0f))
{
this->setRoi(Parameters::defaultKpRoiRatios());
this->parseParameters(parameters);
}
void KeypointDetector::parseParameters(const ParametersMap & parameters)
{
ParametersMap::const_iterator iter;
if((iter=parameters.find(Parameters::kKpWordsPerImage())) != parameters.end())
{
_wordsPerImageTarget = std::atoi((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kKpRoiRatios())) != parameters.end())
{
this->setRoi((*iter).second);
}
}
std::vector<cv::KeyPoint> KeypointDetector::generateKeypoints(const cv::Mat & image)
{
ULOGGER_DEBUG("");
std::vector<cv::KeyPoint> keypoints;
if(!image.empty())
{
UTimer timer;
timer.start();
cv::Rect roi = computeRoi(image);
// Get keypoints
keypoints = this->_generateKeypoints(image, roi);
ULOGGER_DEBUG("Keypoints extraction time = %f s, keypoints extracted = %d", timer.ticks(), keypoints.size());
//clip the number of words... to _wordsPerImageTarget
// Variable hessian threshold
if(_wordsPerImageTarget > 0)
{
if(keypoints.size() > 0)
{
// 10% margin...
if(keypoints.size() > 1.1 * _wordsPerImageTarget)
{
ULOGGER_DEBUG("too much words (%d), removing words under the new hessian threshold", keypoints.size());
// Remove words under the new hessian threshold
// Sort words by hessian
std::multimap<float, std::vector<cv::KeyPoint>::iterator> hessianMap; // <hessian,id>
for(std::vector<cv::KeyPoint>::iterator itKey = keypoints.begin(); itKey != keypoints.end(); ++itKey)
{
//Keep track of the data, to be easier to manage the data in the next step
hessianMap.insert(std::pair<float, std::vector<cv::KeyPoint>::iterator>(fabs(itKey->response), itKey));
}
// Remove them from the signature
int removed = hessianMap.size()-_wordsPerImageTarget;
std::multimap<float, std::vector<cv::KeyPoint>::iterator>::reverse_iterator iter = hessianMap.rbegin();
std::vector<cv::KeyPoint> kptsTmp(_wordsPerImageTarget);
for(unsigned int k=0; k < kptsTmp.size() && iter!=hessianMap.rend(); ++k, ++iter)
{
kptsTmp[k] = *iter->second;
// Adjust keypoint position to raw image
kptsTmp[k].pt.x += roi.x;
kptsTmp[k].pt.y += roi.y;
}
keypoints = kptsTmp;
ULOGGER_DEBUG("%d keypoints removed, (kept %d), minimum response=%f", removed, keypoints.size(), kptsTmp.size()?kptsTmp.back().response:0.0f);
}
else if(roi.x || roi.y)
{
// Adjust keypoint position to raw image
for(std::vector<cv::KeyPoint>::iterator iter=keypoints.begin(); iter!=keypoints.end(); ++iter)
{
iter->pt.x += roi.x;
iter->pt.y += roi.y;
}
}
}
ULOGGER_DEBUG("removing words time = %f s", timer.ticks());
}
else if(roi.x || roi.y)
{
// Adjust keypoint position to raw image
for(std::vector<cv::KeyPoint>::iterator iter=keypoints.begin(); iter!=keypoints.end(); ++iter)
{
iter->pt.x += roi.x;
iter->pt.y += roi.y;
}
}
}
else
{
ULOGGER_ERROR("Image is null!");
}
return keypoints;
}
void KeypointDetector::setRoi(const std::string & roi)
{
std::list<std::string> strValues = uSplit(roi, ' ');
if(strValues.size() != 4)
{
ULOGGER_ERROR("The number of values must be 4 (roi=\"%s\")", roi.c_str());
}
else
{
std::vector<float> tmpValues(4);
unsigned int i=0;
for(std::list<std::string>::iterator iter = strValues.begin(); iter!=strValues.end(); ++iter)
{
tmpValues[i] = std::atof((*iter).c_str());
++i;
}
if(tmpValues[0] >= 0 && tmpValues[0] < 1 && tmpValues[0] < 1.0f-tmpValues[1] &&
tmpValues[1] >= 0 && tmpValues[1] < 1 && tmpValues[1] < 1.0f-tmpValues[0] &&
tmpValues[2] >= 0 && tmpValues[2] < 1 && tmpValues[2] < 1.0f-tmpValues[3] &&
tmpValues[3] >= 0 && tmpValues[3] < 1 && tmpValues[3] < 1.0f-tmpValues[2])
{
_roiRatios = tmpValues;
}
else
{
ULOGGER_ERROR("The roi ratios are not valid (roi=\"%s\")", roi.c_str());
}
}
}
cv::Rect KeypointDetector::computeRoi(const cv::Mat & image) const
{
if(!image.empty() && _roiRatios.size() == 4)
{
float width = image.cols;
float height = image.rows;
cv::Rect roi(0, 0, width, height);
UDEBUG("roi ratios = %f, %f, %f, %f", _roiRatios[0],_roiRatios[1],_roiRatios[2],_roiRatios[3]);
UDEBUG("roi = %d, %d, %d, %d", roi.x, roi.y, roi.width, roi.height);
//left roi
if(_roiRatios[0] > 0 && _roiRatios[0] < 1 - _roiRatios[1])
{
roi.x = width * _roiRatios[0];
}
//right roi
roi.width = width - roi.x;
if(_roiRatios[1] > 0 && _roiRatios[1] < 1 - _roiRatios[0])
{
roi.width -= width * _roiRatios[1];
}
//top roi
if(_roiRatios[2] > 0 && _roiRatios[2] < 1 - _roiRatios[3])
{
roi.y = height * _roiRatios[2];
}
//bottom roi
roi.height = height - roi.y;
if(_roiRatios[3] > 0 && _roiRatios[3] < 1 - _roiRatios[2])
{
roi.height -= height * _roiRatios[3];
}
UDEBUG("roi = %d, %d, %d, %d", roi.x, roi.y, roi.width, roi.height);
return roi;
}
else
{
UERROR("Image is null or _roiRatios(=%d) != 4", _roiRatios.size());
return cv::Rect();
}
}
//////////////////////////
//SURFDetector
//////////////////////////
SURFDetector::SURFDetector(const ParametersMap & parameters) :
KeypointDetector(parameters),
_hessianThreshold(Parameters::defaultSURFHessianThreshold()),
_nOctaves(Parameters::defaultSURFOctaves()),
_nOctaveLayers(Parameters::defaultSURFOctaveLayers()),
_extended(Parameters::defaultSURFExtended()),
_upright(Parameters::defaultSURFUpright()),
_gpuVersion(Parameters::defaultSURFGpuVersion())
{
this->parseParameters(parameters);
}
SURFDetector::~SURFDetector()
{
}
void SURFDetector::parseParameters(const ParametersMap & parameters)
{
ParametersMap::const_iterator iter;
if((iter=parameters.find(Parameters::kSURFExtended())) != parameters.end())
{
_extended = uStr2Bool((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSURFHessianThreshold())) != parameters.end())
{
_hessianThreshold = std::atof((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSURFOctaveLayers())) != parameters.end())
{
_nOctaveLayers = std::atoi((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSURFOctaves())) != parameters.end())
{
_nOctaves = std::atoi((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSURFOctaves())) != parameters.end())
{
_nOctaves = std::atoi((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSURFUpright())) != parameters.end())
{
_upright = uStr2Bool((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSURFGpuVersion())) != parameters.end())
{
_gpuVersion = uStr2Bool((*iter).second.c_str());
}
KeypointDetector::parseParameters(parameters);
}
std::vector<cv::KeyPoint> SURFDetector::_generateKeypoints(const cv::Mat & image, const cv::Rect & roi) const
{
ULOGGER_DEBUG("");
std::vector<cv::KeyPoint> keypoints;
if(image.empty())
{
ULOGGER_ERROR("Image is null ?!?");
return keypoints;
}
// SURF support only grayscale images
cv::Mat imageGrayScale;
if(image.channels() != 1 || image.depth() != CV_8U)
{
cv::cvtColor(image, imageGrayScale, CV_BGR2GRAY);
}
cv::Mat img;
if(!imageGrayScale.empty())
{
img = imageGrayScale;
}
else
{
img = image;
}
cv::Mat imgRoi(img, roi);
/*#if OPENCV_SURF_GPU
if(_gpuVersion )
{
cv::gpu::GpuMat imgGpu(imgRoi);
cv::gpu::GpuMat keypointsGpu;
cv::gpu::SURF_GPU surfGpu(params.hessianThreshold, params.nOctaves, params.nOctaveLayers, params.extended, 0.01f, params.upright);
surfGpu(imgGpu, cv::gpu::GpuMat(), keypointsGpu);
surfGpu.downloadKeypoints(keypointsGpu, keypoints);
}
else
{
cv::SurfFeatureDetector detector(params.hessianThreshold, params.nOctaves, params.nOctaveLayers, params.upright);
detector.detect(imgRoi, keypoints);
}
#else*/
cv::SURF detector(_hessianThreshold, _nOctaves, _nOctaveLayers, _extended, _upright);
#if CV_MAJOR_VERSION >=2 and CV_MINOR_VERSION >=4
detector.detect(imgRoi, keypoints);
#else
detector(imgRoi, cv::Mat(), keypoints);
#endif
//#endif
return keypoints;
}
//////////////////////////
//SIFTDetector
//////////////////////////
SIFTDetector::SIFTDetector(const ParametersMap & parameters) :
KeypointDetector(parameters),
_nfeatures(Parameters::defaultSIFTNFeatures()),
_nOctaveLayers(Parameters::defaultSIFTNOctaveLayers()),
_contrastThreshold(Parameters::defaultSIFTContrastThreshold()),
_edgeThreshold(Parameters::defaultSIFTEdgeThreshold()),
_sigma(Parameters::defaultSIFTSigma())
{
this->parseParameters(parameters);
}
SIFTDetector::~SIFTDetector()
{
}
void SIFTDetector::parseParameters(const ParametersMap & parameters)
{
ParametersMap::const_iterator iter;
if((iter=parameters.find(Parameters::kSIFTContrastThreshold())) != parameters.end())
{
_contrastThreshold = std::atof((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSIFTEdgeThreshold())) != parameters.end())
{
_edgeThreshold = std::atof((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSIFTNFeatures())) != parameters.end())
{
_nfeatures = std::atoi((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSIFTNOctaveLayers())) != parameters.end())
{
_nOctaveLayers = std::atoi((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSIFTSigma())) != parameters.end())
{
_sigma = std::atof((*iter).second.c_str());
}
KeypointDetector::parseParameters(parameters);
}
std::vector<cv::KeyPoint> SIFTDetector::_generateKeypoints(const cv::Mat & image, const cv::Rect & roi) const
{
ULOGGER_DEBUG("");
std::vector<cv::KeyPoint> keypoints;
if(image.empty())
{
ULOGGER_ERROR("Image is null ?!?");
return keypoints;
}
// SURF support only grayscale images
cv::Mat imageGrayScale;
if(image.channels() != 1 || image.depth() != CV_8U)
{
cv::cvtColor(image, imageGrayScale, CV_BGR2GRAY);
}
cv::Mat img;
if(!imageGrayScale.empty())
{
img = imageGrayScale;
}
else
{
img = image;
}
cv::Mat imgRoi(img, roi);
#if CV_MAJOR_VERSION >=2 and CV_MINOR_VERSION >=4
cv::SIFT detector(_nfeatures, _nOctaveLayers, _contrastThreshold, _edgeThreshold, _sigma);
detector.detect(imgRoi, keypoints); // Opencv surf keypoints
#else
cv::SIFT detector(_contrastThreshold, _edgeThreshold, cv::SIFT::CommonParams::DEFAULT_NOCTAVES, _nOctaveLayers);
detector(imgRoi, cv::Mat(), keypoints); // Opencv surf keypoints
#endif
return keypoints;
}
//////////////////////////
//StarDetector
//////////////////////////
StarDetector::StarDetector(const ParametersMap & parameters) :
KeypointDetector(parameters),
_maxSize(Parameters::defaultStarMaxSize()),
_responseThreshold(Parameters::defaultStarResponseThreshold()),
_lineThresholdProjected(Parameters::defaultStarLineThresholdProjected()),
_lineThresholdBinarized(Parameters::defaultStarLineThresholdBinarized()),
_suppressNonmaxSize(Parameters::defaultStarSuppressNonmaxSize())
{
this->parseParameters(parameters);
}
StarDetector::~StarDetector()
{
}
void StarDetector::parseParameters(const ParametersMap & parameters)
{
ULOGGER_WARN("The StarDetector parameters can't be changed on ROS (this is an issue with the default (and too old) opencv revision used in ROS)");
ParametersMap::const_iterator iter;
if((iter=parameters.find(Parameters::kStarLineThresholdBinarized())) != parameters.end())
{
_lineThresholdBinarized = std::atoi((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kStarLineThresholdProjected())) != parameters.end())
{
_lineThresholdProjected = std::atoi((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kStarMaxSize())) != parameters.end())
{
_maxSize = std::atoi((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kStarResponseThreshold())) != parameters.end())
{
_responseThreshold = int(std::atof((*iter).second.c_str()));
}
if((iter=parameters.find(Parameters::kStarSuppressNonmaxSize())) != parameters.end())
{
_suppressNonmaxSize = std::atoi((*iter).second.c_str());
}
KeypointDetector::parseParameters(parameters);
}
std::vector<cv::KeyPoint> StarDetector::_generateKeypoints(const cv::Mat & image, const cv::Rect & roi) const
{
ULOGGER_DEBUG("");
std::vector<cv::KeyPoint> keypoints;
if(image.empty())
{
ULOGGER_ERROR("Image is null ?!?");
return keypoints;
}
cv::Mat img(image);
// Get keypoints with the star detector
cv::Mat imgRoi(img, roi);
cv::StarDetector detector(_maxSize, _responseThreshold, _lineThresholdProjected, _lineThresholdBinarized, _suppressNonmaxSize);
#if CV_MAJOR_VERSION >=2 and CV_MINOR_VERSION >=4
detector.detect(imgRoi, keypoints);
#else
detector(imgRoi, keypoints);
#endif
return keypoints;
}
//////////////////////////
//FastDetector
//////////////////////////
FASTDetector::FASTDetector(const ParametersMap & parameters) :
KeypointDetector(parameters),
_threshold(Parameters::defaultFASTThreshold()),
_nonmaxSuppression(Parameters::defaultFASTNonmaxSuppression())
{
this->parseParameters(parameters);
}
FASTDetector::~FASTDetector()
{
}
void FASTDetector::parseParameters(const ParametersMap & parameters)
{
ParametersMap::const_iterator iter;
if((iter=parameters.find(Parameters::kFASTThreshold())) != parameters.end())
{
_threshold = std::atoi((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kFASTNonmaxSuppression())) != parameters.end())
{
_nonmaxSuppression = uStr2Bool((*iter).second.c_str());
}
KeypointDetector::parseParameters(parameters);
}
std::vector<cv::KeyPoint> FASTDetector::_generateKeypoints(const cv::Mat & image, const cv::Rect & roi) const
{
ULOGGER_DEBUG("");
std::vector<cv::KeyPoint> keypoints;
if(image.empty())
{
ULOGGER_ERROR("Image is null ?!?");
return keypoints;
}
cv::Mat img(image);
cv::Mat imgRoi(img, roi);
cv::FastFeatureDetector fast(_threshold, _nonmaxSuppression);
// Get keypoints with the fast detector
fast.detect(imgRoi, keypoints);
return keypoints;
}
}

1003
corelib/src/KeypointMemory.cpp
View File

File diff suppressed because it is too large Load Diff

1952
corelib/src/Memory.cpp
View File

File diff suppressed because it is too large Load Diff

383
corelib/src/Micro.cpp
View File

@@ -1,383 +0,0 @@
/*
* Micro.cpp
*
* Created on: Mar 5, 2012
* Author: MatLab
*/
#include "rtabmap/core/Micro.h"
#include "utilite/UAudioRecorderMic.h"
#include "utilite/UAudioRecorderFile.h"
#include <utilite/UEventsManager.h>
#include <utilite/UFile.h>
#include <utilite/UMath.h>
#include <fftw3.h>
namespace rtabmap {
Micro::Micro(MicroEvent::Type eventType,
int deviceId,
int fs,
int frameLength,
int channels,
int bytesPerSample,
int id) :
_eventType(eventType),
_recorder(0),
_simulateFreq(false),
_out(0),
_id(id)
{
UASSERT(eventType == MicroEvent::kTypeFrame || eventType == MicroEvent::kTypeFrameFreq || eventType == MicroEvent::kTypeFrameFreqSqrdMagn);
UASSERT(deviceId >= 0);
UASSERT(frameLength > 0 && frameLength % 2 == 0);
_recorder = new UAudioRecorderMic(deviceId, fs, frameLength, bytesPerSample, channels);
}
Micro::Micro(MicroEvent::Type eventType,
const std::string & path,
bool simulateFrameRate,
int frameLength,
int id,
bool playWhileRecording) :
_eventType(eventType),
_recorder(0),
_simulateFreq(simulateFrameRate),
_out(0),
_id(id)
{
UASSERT(eventType == MicroEvent::kTypeFrame || eventType == MicroEvent::kTypeFrameFreq || eventType == MicroEvent::kTypeFrameFreqSqrdMagn);
UASSERT(frameLength > 0 && frameLength % 2 == 0);
if(playWhileRecording)
{
simulateFrameRate = false;
}
_recorder = new UAudioRecorderFile(path, playWhileRecording, frameLength);
}
Micro::~Micro()
{
UDEBUG("");
join(true);
if(_recorder)
{
delete _recorder;
}
if(_out)
{
fftwf_destroy_plan((fftwf_plan)_p);
fftwf_free(_out);
_out = 0;
}
}
bool Micro::init()
{
if(!_recorder->init())
{
UERROR("Recorder initialization failed!");
return false;
}
// init FFTW stuff
if(_out)
{
fftwf_destroy_plan((fftwf_plan)_p);
fftwf_free(_out);
_out = 0;
_in.clear();
}
int N = _recorder->frameLength();
_in.resize(N);
_out = (fftwf_complex*) fftwf_malloc(sizeof(fftwf_complex) * N);
_p = fftwf_plan_dft_r2c_1d(N, _in.data(), _out, 0);
_window = uHamming(N);
return true;
}
void Micro::stop()
{
if(this->isRunning())
{
this->kill();
}
else if(_recorder && _recorder->isRunning())
{
_recorder->join(true);
}
}
void Micro::startRecorder()
{
if(_recorder)
{
_recorder->start();
_timer.start();
}
}
void Micro::mainLoopBegin()
{
this->startRecorder();
}
void Micro::mainLoop()
{
if(!_recorder)
{
UERROR("Recorder not initialized");
this->kill();
return;
}
if(this->isRunning())
{
bool noMoreFrames = true;
if(_eventType == MicroEvent::kTypeFrame)
{
UDEBUG("");
cv::Mat data = this->getFrame();
if(!data.empty())
{
noMoreFrames = false;
UEventsManager::post(new MicroEvent(data, 2, _recorder->fs(), _recorder->channels(), _id));
}
}
else if(_eventType == MicroEvent::kTypeFrameFreq)
{
UDEBUG("");
cv::Mat freq;
cv::Mat data = this->getFrame(freq, false);
if(!data.empty())
{
noMoreFrames = false;
UEventsManager::post(new MicroEvent(MicroEvent::kTypeFrameFreq, freq, _recorder->fs(), _recorder->channels(), _id));
}
}
else if(_eventType == MicroEvent::kTypeFrameFreqSqrdMagn)
{
UDEBUG("");
cv::Mat freq;
cv::Mat data = this->getFrame(freq, true);
if(!data.empty())
{
noMoreFrames = false;
UEventsManager::post(new MicroEvent(MicroEvent::kTypeFrameFreqSqrdMagn, freq, _recorder->fs(), _recorder->channels(), _id));
}
}
else
{
UFATAL("Not supposed to be here...");
}
if(noMoreFrames)
{
if(this->isRunning())
{
UEventsManager::post(new MicroEvent(_id));
}
this->kill();
}
}
}
void Micro::mainLoopKill()
{
if(_recorder)
{
_recorder->join(true);
}
}
cv::Mat Micro::getFrame()
{
cv::Mat data;
std::vector<char> frame;
if(!_recorder)
{
UERROR("Micro is not initialized...");
return data;
}
int frameLength = _recorder->frameLength();
int fs = _recorder->fs();
int channels = _recorder->channels();
int bytesPerSample = _recorder->bytesPerSample();
if(_simulateFreq && fs)
{
int sleepTime = ((double(frameLength)/double(fs) - _timer.getElapsedTime()) * 1000.0) + 0.5;
if(sleepTime > 2)
{
uSleep(sleepTime-2);
}
// Add precision at the cost of a small overhead
while(_timer.getElapsedTime() < double(frameLength)/double(fs)-0.000001)
{
//
}
double slept = _timer.getElapsedTime();
_timer.start();
UDEBUG("slept=%fs vs target=%fs", slept, double(frameLength)/double(fs));
}
if(_recorder->getNextFrame(frame, true) && int(frame.size()) == frameLength * channels * bytesPerSample)
{
UASSERT(bytesPerSample == 1 || bytesPerSample == 2 || bytesPerSample == 4);
if(bytesPerSample == 1)
{
data = cv::Mat(channels, frameLength, CV_8S);
// Split channels in rows
for(unsigned int i = 0; i<frame.size(); i+=channels*bytesPerSample)
{
for(unsigned int j=0; j<(unsigned int)channels; ++j)
{
data.at<char>(j, i/(channels*bytesPerSample)) = *((char*)&frame[i + j*bytesPerSample]);
}
}
}
else if(bytesPerSample == 2)
{
data = cv::Mat(channels, frameLength, CV_16S);
// Split channels in rows
for(unsigned int i = 0; i<frame.size(); i+=channels*bytesPerSample)
{
for(unsigned int j=0; j<(unsigned int)channels; ++j)
{
data.at<short>(j, i/(channels*bytesPerSample)) = *((short*)&frame[i + j*bytesPerSample]);
}
}
}
else if(bytesPerSample == 4)
{
data = cv::Mat(channels, frameLength, CV_32S);
// Split channels in rows
for(unsigned int i = 0; i<frame.size(); i+=channels*bytesPerSample)
{
for(unsigned int j=0; j<(unsigned int)channels; ++j)
{
data.at<int>(j, i/(channels*bytesPerSample)) = *((int*)&frame[i + j*bytesPerSample]);
}
}
}
}
else
{
UDEBUG("No more frames...");
}
return data;
}
cv::Mat Micro::getFrame(cv::Mat & frameFreq, bool sqrdMagn)
{
cv::Mat frame = this->getFrame();
if(!frame.empty())
{
UASSERT(frame.depth() == CV_8S || frame.depth() == CV_16S || frame.depth() == CV_32S);
cv::Mat timeSample(frame.rows, frame.cols, CV_32F);
for(int i=0; i<frame.cols; ++i)
{
// for each channels
for(int j=0; j<frame.rows; ++j)
{
if(frame.depth() == CV_8S)
{
timeSample.at<float>(j, i) = _window[i] * (float)(frame.at<char>(j, i)) / float(1<<7); // between 0 and 1
}
else if(frame.depth() == CV_16S)
{
timeSample.at<float>(j, i) = _window[i] * (float)(frame.at<short>(j, i)) / float(1<<15); // between 0 and 1
}
else if(frame.depth() == CV_32S)
{
timeSample.at<float>(j, i) = _window[i] * (float)(frame.at<int>(j, i)) / float(1<<31); // between 0 and 1
}
}
}
int size = timeSample.cols/2+1;
if(sqrdMagn)
{
frameFreq = cv::Mat(timeSample.rows, size, CV_32F);
}
else
{
frameFreq = cv::Mat(timeSample.rows, size * 2, CV_32F); // [re, im, re, im, ...]
}
// for each channels
for(int j=0; j<timeSample.rows; ++j)
{
cv::Mat row = timeSample.row(j);
cv::Mat rowFreq = frameFreq.row(j);
memcpy(_in.data(), row.data, row.cols*sizeof(float));
fftwf_execute((fftwf_plan)_p); /* repeat as needed */
float re;
float im;
for(int i=0; i<size; ++i)
{
re = float(_out[i][0]);
im = float(_out[i][1]);
if(sqrdMagn)
{
frameFreq.at<float>(0, i) = re*re+im*im; // squared magnitude
}
else
{
frameFreq.at<float>(0, i*2) = re;
frameFreq.at<float>(0, i*2+1) = im;
}
}
}
}
return frame;
}
int Micro::fs()
{
int fs = 0;
if(_recorder)
{
fs = _recorder->fs();
}
return fs;
}
int Micro::bytesPerSample()
{
int bytes = 0;
if(_recorder)
{
bytes = _recorder->bytesPerSample();
}
return bytes;
}
int Micro::channels()
{
int channels = 0;
if(_recorder)
{
channels = _recorder->channels();
}
return channels;
}
int Micro::nfft()
{
int n = 0;
if(_recorder)
{
n = _recorder->frameLength();
}
return n?n/2+1:0;
}
}

60
corelib/src/NearestNeighbor.cpp
View File

@@ -17,7 +17,7 @@
* along with RTAB-Map. If not, see <http://www.gnu.org/licenses/>.
*/
#include "rtabmap/core/NearestNeighbor.h"
#include "NearestNeighbor.h"
#include "utilite/ULogger.h"
#include <opencv2/core/core.hpp>
@@ -25,61 +25,24 @@ namespace rtabmap
{
/////////////////////////
// KdTreeNN
// FlannNN
/////////////////////////
KdTreeNN::KdTreeNN(const ParametersMap & parameters)
{
this->parseParameters(parameters);
}
KdTreeNN::~KdTreeNN()
{
}
void KdTreeNN::setData(const cv::Mat & data)
{
//(data is not copied)
_tree.build(data);
}
void KdTreeNN::search(const cv::Mat & queries, cv::Mat & indices, cv::Mat & dists, int knn, int emax)
{
_tree.findNearest(queries, knn, emax, indices, cv::noArray(), dists);
}
void KdTreeNN::search(const cv::Mat & data, const cv::Mat & queries, cv::Mat & indices, cv::Mat & dists, int knn, int emax) const
{
cv::KDTree tree(data);
tree.findNearest(queries, knn, emax, indices, cv::noArray(), dists);
}
void KdTreeNN::parseParameters(const ParametersMap & parameters)
{
NearestNeighbor::parseParameters(parameters);
}
/////////////////////////
// FlannKdTreeNN
/////////////////////////
FlannKdTreeNN::FlannKdTreeNN(const ParametersMap & parameters) :
FlannNN::FlannNN(Strategy strategy, const ParametersMap & parameters) :
_treeFlannIndex(0),
_strategy(kKDTree)
_strategy(strategy)
{
ULOGGER_DEBUG("");
this->parseParameters(parameters);
}
FlannKdTreeNN::~FlannKdTreeNN() {
FlannNN::~FlannNN() {
if(_treeFlannIndex)
{
delete _treeFlannIndex;
}
}
void FlannKdTreeNN::setData(const cv::Mat & data)
void FlannNN::setData(const cv::Mat & data)
{
if(_treeFlannIndex)
{
@@ -88,10 +51,9 @@ void FlannKdTreeNN::setData(const cv::Mat & data)
}
_treeFlannIndex = createIndex(data, _strategy); // using 4 randomized trees
//_treeFlannIndex = new cv::flann::Index(_dataTree, cv::flann::AutotunedIndexParams(0.9, 0.01, 0, 0.1)); // use autotuned parameters
}
void FlannKdTreeNN::search(const cv::Mat & queries, cv::Mat & indices, cv::Mat & dists, int knn, int emax)
void FlannNN::search(const cv::Mat & queries, cv::Mat & indices, cv::Mat & dists, int knn, int emax)
{
ULOGGER_DEBUG("");
if(_treeFlannIndex)
@@ -105,7 +67,7 @@ void FlannKdTreeNN::search(const cv::Mat & queries, cv::Mat & indices, cv::Mat &
}
}
void FlannKdTreeNN::search(const cv::Mat & data, const cv::Mat & queries, cv::Mat & indices, cv::Mat & dists, int knn, int emax) const
void FlannNN::search(const cv::Mat & data, const cv::Mat & queries, cv::Mat & indices, cv::Mat & dists, int knn, int emax) const
{
ULOGGER_DEBUG("");
cv::flann::Index * index = createIndex(data, _strategy);
@@ -114,13 +76,13 @@ void FlannKdTreeNN::search(const cv::Mat & data, const cv::Mat & queries, cv::Ma
delete index;
}
void FlannKdTreeNN::parseParameters(const ParametersMap & parameters)
void FlannNN::parseParameters(const ParametersMap & parameters)
{
NearestNeighbor::parseParameters(parameters);
}
enum Strategy{kLinear, kKDTree, kMeans, kComposite, kAutoTuned, kUndefined};
cv::flann::Index * FlannKdTreeNN::createIndex(const cv::Mat & data, Strategy s) const
cv::flann::Index * FlannNN::createIndex(const cv::Mat & data, Strategy s) const
{
cv::flann::Index * index = 0;
switch(s)

90
corelib/include/rtabmap/core/NearestNeighbor.h → corelib/src/NearestNeighbor.h
View File

@@ -31,92 +31,18 @@
namespace rtabmap
{
class VisualWord;
class RTABMAP_EXP NearestNeighbor
{
public:
public:
virtual ~NearestNeighbor() {}
virtual void setData(const cv::Mat & data) = 0;
virtual void search(
const cv::Mat & queries,
cv::Mat & indices,
cv::Mat & dists,
int knn = 1,
int emax = 64) = 0;
virtual void search(
const cv::Mat & data,
const cv::Mat & queries,
cv::Mat & indices,
cv::Mat & dists,
int knn = 1,
int emax = 64) const = 0;
virtual bool isDist64F() const = 0;
virtual bool isDistSquared() const = 0;
virtual void parseParameters(const ParametersMap & parameters) {}
protected:
NearestNeighbor() {}
};
/////////////////////////
// KdTreeNN
// FIXME KdTreeNN seems broken, it does not give same results as naive and FLANN
/////////////////////////
class RTABMAP_EXP KdTreeNN : public NearestNeighbor
{
public:
KdTreeNN(const ParametersMap & parameters = ParametersMap());
virtual ~KdTreeNN();
virtual void setData(const cv::Mat & data);
virtual void search(const cv::Mat & queries,
cv::Mat & indices,
cv::Mat & dists,
int knn = 1,
int emax = 64);
virtual void search(const cv::Mat & data,
const cv::Mat & queries,
cv::Mat & indices,
cv::Mat & dists,
int knn = 1,
int emax = 64) const;
virtual bool isDist64F() const {return false;}
virtual bool isDistSquared() const {return false;}
virtual void parseParameters(const ParametersMap & parameters);
private:
cv::KDTree _tree;
};
/////////////////////////
// FlannKdTreeNN
/////////////////////////
class RTABMAP_EXP FlannKdTreeNN : public NearestNeighbor
/////////////////////////
// FlannNN
/////////////////////////
class RTABMAP_EXP FlannNN
{
public:
enum dummy {d}; // Hack, to fix Eclipse complaining about not defined Strategy enum ?!
enum Strategy{kLinear, kKDTree, kMeans, kComposite, kAutoTuned, kUndefined};
public:
FlannKdTreeNN(const ParametersMap & parameters = ParametersMap());
FlannKdTreeNN(Strategy s, const ParametersMap & parameters = ParametersMap());
virtual ~FlannKdTreeNN();
FlannNN(Strategy s = kKDTree, const ParametersMap & parameters = ParametersMap());
virtual ~FlannNN();
void setStrategy(Strategy s) {if(_strategy!=kUndefined) _strategy = s;}
@@ -135,8 +61,6 @@ public:
int knn = 1,
int emax = 64) const;
virtual bool isDist64F() const {return false;}
virtual bool isDistSquared() const {return true;}
virtual void parseParameters(const ParametersMap & parameters);
private:

98
corelib/src/Node.h
View File

@@ -1,98 +0,0 @@
/*
* Copyright (C) 2010-2011, Mathieu Labbe and IntRoLab - Universite de Sherbrooke
*
* This file is part of RTAB-Map.
*
* RTAB-Map is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* RTAB-Map is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with RTAB-Map. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef NODE_H_
#define NODE_H_
namespace rtabmap {
class Node
{
public:
Node(int id, Node * parent = 0) :
_parent(parent),
_id(id)
{
if(_parent)
{
_parent->addChild(this);
}
}
virtual ~Node()
{
//We copy the set because when a child is destroyed, it is removed from its parent.
std::set<Node*> children = _children;
_children.clear();
for(std::set<Node*>::iterator iter=children.begin(); iter!=children.end(); ++iter)
{
delete *iter;
}
children.clear();
if(_parent)
{
_parent->removeChild(this);
}
}
int id() const {return _id;}
bool isAncestor(int id) const
{
if(_parent)
{
if(_parent->id() == id)
{
return true;
}
return _parent->isAncestor(id);
}
return false;
}
void expand(std::list<std::list<int> > & paths, std::list<int> currentPath = std::list<int>()) const
{
currentPath.push_back(_id);
if(_children.size() == 0)
{
paths.push_back(currentPath);
return;
}
for(std::set<Node*>::const_iterator iter=_children.begin(); iter!=_children.end(); ++iter)
{
(*iter)->expand(paths, currentPath);
}
}
private:
void addChild(Node * child)
{
_children.insert(child);
}
void removeChild(Node * child)
{
_children.erase(child);
}
private:
std::set<Node*> _children;
Node * _parent;
int _id;
};
}
#endif /* NODE_H_ */

5
corelib/src/Parameters.cpp
View File

@@ -35,11 +35,6 @@ Parameters::~Parameters()
{
}
const ParametersMap & Parameters::getDefaultParameters()
{
return parameters_;
}
std::string Parameters::getDefaultWorkingDirectory()
{
std::string path = UDirectory::homeDir();

1008
corelib/src/Rtabmap.cpp
View File

File diff suppressed because it is too large Load Diff

8
corelib/src/RtabmapEvent.cpp
View File

@@ -47,14 +47,14 @@ void Statistics::addStatistic(const std::string & name, float value)
_data.insert(std::pair<std::string, float>(name, value));
}
void Statistics::setRefRawData(const std::list<Sensor> & refRawData)
void Statistics::setRefImage(const cv::Mat & image)
{
_refRawData = refRawData;
_refImage = image;
}
void Statistics::setLoopClosureRawData(const std::list<Sensor> & loopClosureRawData)
void Statistics::setLoopImage(const cv::Mat & image)
{
_loopClosureRawData = loopClosureRawData;
_loopImage = image;
}
}

367
corelib/src/SMMemory.cpp
View File

@@ -1,367 +0,0 @@
/*
* Copyright (C) 2010-2011, Mathieu Labbe and IntRoLab - Universite de Sherbrooke
*
* This file is part of RTAB-Map.
*
* RTAB-Map is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* RTAB-Map is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with RTAB-Map. If not, see <http://www.gnu.org/licenses/>.
*/
#include "rtabmap/core/SMMemory.h"
#include "rtabmap/core/Signature.h"
#include "rtabmap/core/DBDriver.h"
#include "utilite/UtiLite.h"
#include "rtabmap/core/Parameters.h"
#include "rtabmap/core/RtabmapEvent.h"
#include "utilite/UStl.h"
#include "utilite/UConversion.h"
#include <opencv2/imgproc/imgproc_c.h>
#include <opencv2/core/core.hpp>
#include <set>
#include <iostream>
#include <sstream>
#include <string>
#include "rtabmap/core/ColorTable.h"
namespace rtabmap {
SMMemory::SMMemory(const ParametersMap & parameters) :
Memory(parameters),
_useLogPolar(Parameters::defaultSMLogPolarUsed()),
_colorTable(0),
_useMotionMask(Parameters::defaultSMMotionMaskUsed()),
_dBThreshold(Parameters::defaultSMAudioDBThreshold()),
_dBIndexing(Parameters::defaultSMAudioDBIndexing()),
_magnitudeInvariant(Parameters::defaultSMMagnitudeInvariant())
{
this->parseParameters(parameters);
if(!_colorTable)
{
// index 0 = 8, index 1 = 16...
if(Parameters::defaultSMColorTable() == 8)
{
setColorTable(65536);
}
else
{
int i=1;
setColorTable(i<<(Parameters::defaultSMColorTable() + 3));
}
}
}
SMMemory::~SMMemory()
{
ULOGGER_DEBUG("");
if(this->memoryChanged())
{
this->clear();
}
delete _colorTable;
}
void SMMemory::parseParameters(const ParametersMap & parameters)
{
ParametersMap::const_iterator iter;
if((iter=parameters.find(Parameters::kSMLogPolarUsed())) != parameters.end())
{
_useLogPolar = uStr2Bool((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSMMotionMaskUsed())) != parameters.end())
{
_useMotionMask = uStr2Bool((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSMAudioDBThreshold())) != parameters.end())
{
_dBThreshold = atof((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSMAudioDBIndexing())) != parameters.end())
{
_dBIndexing = uStr2Bool((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSMMagnitudeInvariant())) != parameters.end())
{
_magnitudeInvariant = uStr2Bool((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kSMColorTable())) != parameters.end())
{
// index 0 = 8, index 1 = 16...
if(atoi((*iter).second.c_str()) == 8)
{
setColorTable(65536);
}
else
{
int i=1;
setColorTable(i<<(atoi((*iter).second.c_str()) + 3));
}
}
Memory::parseParameters(parameters);
}
void SMMemory::setColorTable(int size)
{
if(_colorTable)
{
if(_colorTable->size() != size)
{
delete _colorTable;
_colorTable = new ColorTable(size);
}
}
else
{
_colorTable = new ColorTable(size);
}
}
void SMMemory::copyData(const Signature * from, Signature * to)
{
// The signatures must be SMSignature
const SMSignature * sFrom = dynamic_cast<const SMSignature *>(from);
SMSignature * sTo = dynamic_cast<SMSignature *>(to);
UTimer timer;
timer.start();
if(sFrom && sTo)
{
sTo->setSensors(sFrom->getData());
}
else
{
ULOGGER_ERROR("Can't merge the signatures because there are not same type.");
}
ULOGGER_DEBUG("Merging time = %fs", timer.ticks());
}
Signature * SMMemory::createSignature(int id, const std::list<Sensor> & rawSensors, bool keepRawData)
{
if(_useMotionMask)
{
UWARN("Using motion mask TODO");
}
UDEBUG("");
UTimer timer;
timer.start();
UTimer timerDetails;
timerDetails.start();
std::list<std::vector<int> > postData;
//const SMSignature * previousSignature = dynamic_cast<const SMSignature *>(this->getLastSignature());
// Process all sensors
for(std::list<Sensor>::const_iterator iter = rawSensors.begin(); iter!=rawSensors.end(); ++iter)
{
if(iter->type() == Sensor::kTypeImage)
{
UASSERT(iter->data().type() == CV_8UC3 && iter->data().channels() == 3);
const cv::Mat & image = iter->data();
UDEBUG("depth=%d, width=%d, height=%d, nChannels=%d, imageSize=%d,", image.type(), image.cols, image.rows, image.channels(), image.total());
if(_useLogPolar)
{
// Log-polar transform
int radius = image.rows < image.cols ? image.rows/2: image.cols/2;
CvSize polarSize = cvSize(64, 128);
float M = polarSize.width/std::log(radius);
IplImage * polar = cvCreateImage( polarSize, 8, 3 );
IplImage iplImg = image;
cvLogPolar(&iplImg, polar, cvPoint2D32f(image.cols/2,image.rows/2), double(M), CV_INTER_LINEAR+CV_WARP_FILL_OUTLIERS );
UDEBUG("polar size= %d, %d, time=%fs", polar->width, polar->height, timerDetails.ticks());
// IND transform
unsigned char * data = (unsigned char *)polar->imageData;
int k=0;
std::vector<int> sensors(polar->width*polar->height);
for(int i=0; i<polar->height; ++i)
{
for(int j=0; j<polar->width; ++j)
{
unsigned char & b = data[i*polar->widthStep+j*3+0];
unsigned char & g = data[i*polar->widthStep+j*3+1];
unsigned char & r = data[i*polar->widthStep+j*3+2];
int index = (int)_colorTable->getIndex(r, g, b);
sensors[k] = index;
++k;
}
}
postData.push_back(sensors);
cvReleaseImage(&polar);
UDEBUG("indexing time = %fs", timerDetails.ticks());
}
else
{
// IND transform
int k=0;
std::vector<int> sensors(image.cols*image.rows);
int sum=0;
for(int i=0; i<image.rows; ++i)
{
cv::Mat row = image.row(i); // DON'T modify row! (it refers to const data)
for(int j=0; j<row.cols; j+=3)
{
unsigned char b = row.at<unsigned char>(j+0);
unsigned char g = row.at<unsigned char>(j+1);
unsigned char r = row.at<unsigned char>(j+2);
if(b && g && r)
{
sensors[k] = (int)_colorTable->getIndex(r, g, b); // index
}
else
{
sensors[k] = 0; // null, will be ignored on likelihood computation
}
++k;
}
}
postData.push_back(sensors);
UDEBUG("sum=%d, indexing time = %fs", sum, timerDetails.ticks());
}
} // end kTypeImage
else if(iter->type() == Sensor::kTypeAudioFreqSqrdMagn)
{
UASSERT(iter->data().type() == CV_32FC1);
const cv::Mat & data = iter->data();
int k = 0;
std::vector<int> sensors(data.cols, 0);
unsigned int index;
float max = uMax((float*)data.data, data.cols, index);
int maxLimit = -1; // FIXME Must be not hard coded
float minDB = -1000;// FIXME Must be not hard coded
UDEBUG("data.rows=%d, data.cols=%d, data.type=%d, max=%f at %d", data.rows, data.cols, data.type(), max, index);
if(_dBThreshold > 0)
{
maxLimit = max / std::pow(10.0f, _dBThreshold/10);
}
for(int i=0; i<data.cols; ++i)
{
float val = data.at<float>(0, i);
if(_dBIndexing && max)
{
if(val>=0.001f)
{
val = 10*std::log(val/max);// transform to dB
}
else
{
val = minDB;
}
}
if(!_dBIndexing && val <= maxLimit)
{
val = 0;
}
else if(_dBIndexing)
{
if(val <= minDB || (_dBThreshold && val <= -_dBThreshold))
{
val = 0;
}
else if(max)
{
if(_magnitudeInvariant)
{
val = -1; // ignore magnitude, just set it not null to say this frequency is here
}
else
{
val -= 1; // make sure high values are not null
}
}
}
sensors[k] = int(val);
if((!_dBIndexing && sensors[k]<0) || (_dBIndexing && sensors[k]>0))
{
UERROR("sensors[%d]=%d %f", k, sensors[k], data.at<float>(0,i));
}
++k;
}
postData.push_back(sensors);
} // end kTypeAudioFreqSqrdMagn
else if(iter->type() == Sensor::kTypeTwist)
{
UASSERT(iter->data().type() == CV_32FC1);
const cv::Mat & data = iter->data();
std::vector<int> sensors(data.cols);
for(int i=0; i<data.cols; ++i)
{
sensors[i] = (int)(data.at<float>(0, i)*100.0f);
}
postData.push_back(sensors);
} //end kTypeTwist
else
{
UWARN("Sensor type (%d) not handled!", iter->type());
}
}
ULOGGER_DEBUG("time new signature (id=%d) %fs", id, timer.ticks());
if(keepRawData)
{
return new SMSignature(postData, id, rawSensors);
}
else
{
return new SMSignature(postData, id);
}
}
std::set<int> SMMemory::reactivateSignatures(const std::list<int> & ids, unsigned int maxLoaded, double & timeDbAccess)
{
// get the signatures, if not in the working memory, they
// will be loaded from the database in an more efficient way
// than how it is done in the Memory
ULOGGER_DEBUG("");
UTimer timer;
std::list<int> idsToLoad;
std::map<int, int>::iterator wmIter;
for(std::list<int>::const_iterator i=ids.begin(); i!=ids.end(); ++i)
{
if(!this->getSignature(*i) && !uContains(idsToLoad, *i))
{
if(!maxLoaded || idsToLoad.size() < maxLoaded)
{
idsToLoad.push_back(*i);
}
}
}
ULOGGER_DEBUG("idsToLoad = %d", idsToLoad.size());
std::list<Signature *> reactivatedSigns;
if(_dbDriver)
{
_dbDriver->loadSMSignatures(idsToLoad, reactivatedSigns);
}
timeDbAccess = timer.getElapsedTime();
for(std::list<Signature *>::iterator i=reactivatedSigns.begin(); i!=reactivatedSigns.end(); ++i)
{
//append to working memory
this->addSignatureToWm(*i);
}
ULOGGER_DEBUG("time = %fs", timer.ticks());
return std::set<int>(idsToLoad.begin(), idsToLoad.end());
}
} // namespace rtabmap

300
corelib/src/Signature.cpp
View File

@@ -17,176 +17,94 @@
* along with RTAB-Map. If not, see <http://www.gnu.org/licenses/>.
*/
#include "rtabmap/core/Signature.h"
#include "Signature.h"
#include "rtabmap/core/EpipolarGeometry.h"
#include "rtabmap/core/Memory.h"
#include <opencv2/highgui/highgui.hpp>
#include "rtabmap/core/VerifyHypotheses.h"
#include <utilite/UtiLite.h>
namespace rtabmap
{
bool NeighborLink::updateIds(int idFrom, int idTo)
{
bool modified = false;
if(_toId == idFrom)
{
_toId = idTo;
modified = true;
}
for(unsigned int i=0; i<_baseIds.size(); ++i)
{
if(_baseIds[i] == idFrom)
{
_baseIds[i] = idTo;
modified = true;
}
}
return modified;
}
Signature::~Signature()
{
ULOGGER_DEBUG("id=%d", _id);
}
Signature::Signature(int id) :
_id(id),
_weight(0),
_saved(false),
_modified(true)
Signature::Signature(
int id,
const std::multimap<int, cv::KeyPoint> & words,
const cv::Mat & image) :
_id(id),
_weight(0),
_saved(false),
_modified(true),
_neighborsModified(true),
_words(words),
_enabled(false),
_image(image)
{
}
Signature::Signature(int id, const std::list<Sensor> & rawData) :
_id(id),
_weight(0),
_rawData(rawData),
_saved(false),
_modified(true)
void Signature::addNeighbors(const std::set<int> & neighbors)
{
}
void Signature::addNeighbors(const NeighborsMultiMap & neighbors)
{
for(NeighborsMultiMap::const_iterator i=neighbors.begin(); i!=neighbors.end(); ++i)
for(std::set<int>::const_iterator i=neighbors.begin(); i!=neighbors.end(); ++i)
{
this->addNeighbor(i->second);
this->addNeighbor(*i);
}
}
void Signature::addNeighbor(const NeighborLink & neighbor)
void Signature::addNeighbor(int neighbor)
{
UDEBUG("Add neighbor %d to %d", neighbor.toId(), this->id());
if(ULogger::level() == ULogger::kDebug)
{
UTimer timer;
std::string baseIdsDebug;
const std::vector<int> & baseIds = neighbor.baseIds();
for(unsigned int i=0; i<baseIds.size(); ++i)
{
baseIdsDebug.append(uFormat("%d", baseIds[i]));
if(i+1 < baseIds.size())
{
baseIdsDebug.append(", ");
}
}
UDEBUG("Adding neighbor %d to %d with %d actions, %d baseIds = [%s] (time print=%fs)", neighbor.toId(), this->id(), neighbor.actuators().size(), neighbor.baseIds().size(), baseIdsDebug.c_str(), timer.getElapsedTime());
}
_neighbors.insert(std::pair<int, NeighborLink>(neighbor.toId(), neighbor));
if(neighbor.actuators().size())
{
_neighborsWithActuators.insert(neighbor.toId());
}
_neighborsAll.insert(neighbor.toId());
UDEBUG("Add neighbor %d to %d", neighbor, this->id());
_neighbors.insert(neighbor);
_neighborsModified = true;
}
void Signature::removeNeighbor(int neighborId)
{
int count = _neighbors.erase(neighborId);
if(count)
{
_neighborsModified = true;
}
}
void Signature::removeNeighbors()
{
if(_neighbors.size())
_neighborsModified = true;
_neighbors.clear();
}
void Signature::changeNeighborIds(int idFrom, int idTo)
{
std::pair<NeighborsMultiMap::iterator, NeighborsMultiMap::iterator> pair = _neighbors.equal_range(idFrom);
if(pair.first != _neighbors.end() && pair.first != pair.second)
if(_neighbors.find(idFrom) != _neighbors.end())
{
std::list<NeighborLink> linksToAdd;
for(NeighborsMultiMap::iterator iter = pair.first; iter!=pair.second; ++iter)
{
NeighborLink link = iter->second;
link.updateIds(idFrom, idTo);
linksToAdd.push_back(link);
}
_neighbors.erase(idFrom);
_neighborsWithActuators.erase(idFrom);
_neighborsAll.erase(idFrom);
for(std::list<NeighborLink>::iterator iter=linksToAdd.begin(); iter!=linksToAdd.end(); ++iter)
{
_neighbors.insert(std::pair<int, NeighborLink>(iter->toId(), *iter));
if(iter->actuators().size())
{
_neighborsWithActuators.insert(iter->toId());
}
_neighborsAll.insert(iter->toId());
}
_neighbors.insert(idTo);
_neighborsModified = true;
UDEBUG("(%d) neighbor ids changed from %d to %d", _id, idFrom, idTo);
}
UDEBUG("(%d) neighbor ids changed from %d to %d", _id, idFrom, idTo);
}
//KeypointSignature
KeypointSignature::KeypointSignature(int id) :
Signature(id),
_enabled(false)
float Signature::compareTo(const Signature * s) const
{
}
KeypointSignature::KeypointSignature(const std::multimap<int, cv::KeyPoint> & words,
int id) :
Signature(id),
_words(words),
_enabled(false)
{
}
KeypointSignature::KeypointSignature(
const std::multimap<int, cv::KeyPoint> & words,
int id,
const std::list<Sensor> & rawData) :
Signature(id, rawData),
_words(words),
_enabled(false)
{
}
KeypointSignature::~KeypointSignature()
{
}
float KeypointSignature::compareTo(const Signature * s) const
{
const KeypointSignature * ss = dynamic_cast<const KeypointSignature *>(s);
float similarity = 0;
if(ss) //Compatible
float similarity = 0.0f;
const std::multimap<int, cv::KeyPoint> & words = s->getWords();
if(words.size() != 0 && _words.size() != 0)
{
const std::multimap<int, cv::KeyPoint> & words = ss->getWords();
std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > pairs;
int totalWords = _words.size()>words.size()?_words.size():words.size();
EpipolarGeometry::findPairs(words, _words, pairs);
if(words.size() != 0 && _words.size() != 0)
{
std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > pairs;
int totalWords = _words.size()>words.size()?_words.size():words.size();
findPairs(words, _words, pairs);
similarity = float(pairs.size()) / float(totalWords);
}
similarity = float(pairs.size()) / float(totalWords);
}
return similarity;
}
void KeypointSignature::changeWordsRef(int oldWordId, int activeWordId)
void Signature::changeWordsRef(int oldWordId, int activeWordId)
{
std::list<cv::KeyPoint> kps = uValues(_words, oldWordId);
if(kps.size())
@@ -200,137 +118,19 @@ void KeypointSignature::changeWordsRef(int oldWordId, int activeWordId)
}
}
bool KeypointSignature::isBadSignature() const
bool Signature::isBadSignature() const
{
return !_words.size();
}
void KeypointSignature::removeAllWords()
void Signature::removeAllWords()
{
_words.clear();
}
void KeypointSignature::removeWord(int wordId)
void Signature::removeWord(int wordId)
{
_words.erase(wordId);
}
//SMSignature
SMSignature::SMSignature(
const std::list<std::vector<int> > & data,
int id) :
Signature(id),
_data(data)
{
UDEBUG("data=%d", (int)_data.size());
}
SMSignature::SMSignature(
const std::list<std::vector<int> > & data,
int id,
const std::list<Sensor> & rawData) :
Signature(id, rawData),
_data(data)
{
UDEBUG("data=%d", (int)_data.size());
}
SMSignature::SMSignature(int id) :
Signature(id)
{
}
SMSignature::~SMSignature()
{
}
float SMSignature::compareTo(const Signature * s) const
{
const SMSignature * sm = dynamic_cast<const SMSignature *>(s);
float similarity = 0;
if(sm)
{
const std::list<std::vector<int> > & dataB = sm->getData();
//const std::vector<unsigned char> & motionMaskB = sm->getMotionMask();
//if(_data.size() == sensorsB.size() && _data.size()) //Compatible
if(_data.size() == dataB.size()) //Compatible
{
std::vector<float> similarities(_data.size());
// compare sensors
std::list<std::vector<int> >::const_iterator iterA = _data.begin();
std::list<std::vector<int> >::const_iterator iterB = dataB.begin();
int j=0;
while(iterA != _data.end() && iterB != dataB.end())
{
if(iterA->size() == iterB->size())
{
int sum = 0;
int notNull = 0;
for(unsigned int i=0; i<iterA->size(); ++i)
{
sum += iterA->at(i) && iterA->at(i) == iterB->at(i) ? 1 : 0;
notNull += iterA->at(i) || iterB->at(i) ? 1 : 0;
}
if(notNull)
{
similarities[j] = float(sum)/float(notNull);
}
else
{
similarities[j] = 1.0f; // example, silence == 100% silence
}
}
else
{
UERROR("Data are not the same size (%d vs %d)", (int)iterA->size(), (int)iterB->size());
}
++iterA;
++iterB;
++j;
}
similarity = uMean(similarities);
if(ULogger::level() == ULogger::kDebug)
{
std::string str;
for(unsigned int i=0; i<similarities.size(); ++i)
{
str.append(uFormat("%f", similarities[i]));
if(i<similarities.size()-1)
{
str.append(", ");
}
}
UDEBUG("similarities (%d vs %d) = [%s]", this->id(), s->id(), str.c_str());
}
if(similarity<0 || similarity>1)
{
UERROR("Something wrong! similarity is not between 0 and 1 (%f)", similarity);
}
}
else if(!s->isBadSignature() && !this->isBadSignature())
{
UWARN("Not compatible nodes : nb sensors A=%d B=%d", (int)_data.size(), (int)dataB.size());
}
}
else if(s)
{
UWARN("Only SM signatures are compared. (type tested=%s)", s->nodeType().c_str());
}
return similarity;
}
bool SMSignature::isBadSignature() const
{
//return uSum(_data) == 0;
return !_data.size();
}
} //namespace rtabmap

140
corelib/include/rtabmap/core/Signature.h → corelib/src/Signature.h
View File

@@ -24,69 +24,36 @@
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include "rtabmap/core/Sensor.h"
#include "rtabmap/core/Actuator.h"
#include <map>
#include <list>
#include <vector>
#include <set>
//TODO : add copy constructor
namespace rtabmap
{
class RTABMAP_EXP NeighborLink
{
public:
NeighborLink(int toId, const std::vector<int> & baseIds = std::vector<int>(), const std::list<Actuator> & actuators = std::list<Actuator>(), int actuatorId = 0) :
_toId(toId),
_actuatorId(actuatorId),
_actuators(actuators),
_baseIds(baseIds)
{}
virtual ~NeighborLink() {}
int toId() const {return _toId;}
int actuatorId() const {return _actuatorId;}
const std::list<Actuator> & actuators() const {return _actuators;}
const std::vector<int> & baseIds() const {return _baseIds;}
bool updateIds(int idFrom, int idTo);
private:
int _toId;
int _actuatorId;
std::list<Actuator> _actuators;
std::vector<int> _baseIds; // first is the nearest
};
class Memory;
typedef std::multimap<int, NeighborLink> NeighborsMultiMap;
class RTABMAP_EXP Signature
{
public:
Signature(int id,
const std::multimap<int, cv::KeyPoint> & words,
const cv::Mat & image = cv::Mat());
virtual ~Signature();
/**
* Must return a value between >=0 and <=1 (1 means 100% similarity)
* Must return a value between >=0 and <=1 (1 means 100% similarity).
*/
virtual float compareTo(const Signature * signature) const = 0;
virtual bool isBadSignature() const = 0;
virtual std::string nodeType() const = 0;
void setRawData(const std::list<Sensor> & rawData) {_rawData = rawData;}
const std::list<Sensor> & getRawData() const {return _rawData;}
float compareTo(const Signature * signature) const;
bool isBadSignature() const;
int id() const {return _id;}
void addNeighbors(const NeighborsMultiMap & neighbors);
void addNeighbor(const NeighborLink & neighbor);
void removeNeighbor(int neighborId) {
if(_neighbors.erase(neighborId))
_neighborsModified = true;
_neighborsWithActuators.erase(neighborId);
_neighborsAll.erase(neighborId);}
void addNeighbors(const std::set<int> & neighbors);
void addNeighbor(int neighbor);
void removeNeighbor(int neighborId);
void removeNeighbors();
bool hasNeighbor(int neighborId) const {return _neighbors.find(neighborId) != _neighbors.end();}
void setWeight(int weight) {if(_weight!=weight)_modified=true;_weight = weight;}
void setLoopClosureIds(const std::set<int> & loopClosureIds) {_loopClosureIds = loopClosureIds;_neighborsModified=true;}
@@ -99,9 +66,7 @@ public:
void setModified(bool modified) {_modified = modified; _neighborsModified = modified;}
void changeNeighborIds(int idFrom, int idTo);
const NeighborsMultiMap & getNeighbors() const {return _neighbors;}
const std::set<int> & getNeighborsWithActuators() const {return _neighborsWithActuators;}
const std::set<int> & getNeighborsAll() const {return _neighborsAll;}
const std::set<int> & getNeighbors() const {return _neighbors;}
int getWeight() const {return _weight;}
const std::set<int> & getLoopClosureIds() const {return _loopClosureIds;}
const std::set<int> & getChildLoopClosureIds() const {return _childLoopClosureIds;}
@@ -109,92 +74,35 @@ public:
bool isModified() const {return _modified || _neighborsModified;}
bool isNeighborsModified() const {return _neighborsModified;}
protected:
Signature(int id);
Signature(int id, const std::list<Sensor> & rawData);
private:
int _id;
NeighborsMultiMap _neighbors; // id, neighborLink
std::set<int> _neighborsWithActuators; // Hack, to increase efficiency of Memory::getNeighborIds()
std::set<int> _neighborsAll; // Hack, to increase efficiency of Memory::getNeighborIds()
int _weight;
std::set<int> _loopClosureIds;
std::set<int> _childLoopClosureIds;
std::list<Sensor> _rawData;
bool _saved; // If it's saved to bd
bool _modified;
bool _neighborsModified; // Optimization when updating signatures in database
};
class KeypointDetector;
class VWDictionary;
class RTABMAP_EXP KeypointSignature :
public Signature
{
public:
KeypointSignature(int id);
KeypointSignature(
const std::multimap<int, cv::KeyPoint> & words,
int id);
KeypointSignature(
const std::multimap<int, cv::KeyPoint> & words,
int id,
const std::list<Sensor> & sensors);
virtual ~KeypointSignature();
virtual float compareTo(const Signature * signature) const;
virtual bool isBadSignature() const;
virtual std::string nodeType() const {return "KeypointSignature";};
//visual words stuff
void removeAllWords();
void removeWord(int wordId);
void changeWordsRef(int oldWordId, int activeWordId);
void setWords(const std::multimap<int, cv::KeyPoint> & words) {_enabled = false;_words = words;}
bool isEnabled() const {return _enabled;}
void setEnabled(bool enabled) {_enabled = enabled;}
const std::multimap<int, cv::KeyPoint> & getWords() const {return _words;}
const std::map<int, int> & getWordsChanged() const {return _wordsChanged;}
void setImage(const cv::Mat & image) {_image = image;}
const cv::Mat & getImage() const {return _image;}
private:
int _id;
std::set<int> _neighbors; // id
int _weight;
std::set<int> _loopClosureIds;
std::set<int> _childLoopClosureIds;
bool _saved; // If it's saved to bd
bool _modified;
bool _neighborsModified; // Optimization when updating signatures in database
// 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::map<int, int> _wordsChanged; // <oldId, newId>
bool _enabled;
cv::Mat _image;
};
class RTABMAP_EXP SMSignature :
public Signature
{
public:
SMSignature(
const std::list<std::vector<int> > & data,
int id);
SMSignature(
const std::list<std::vector<int> > & data,
int id,
const std::list<Sensor> & rawData);
SMSignature(int id);
virtual ~SMSignature();
virtual float compareTo(const Signature * signature) const;
virtual bool isBadSignature() const;
virtual std::string nodeType() const {return "SMSignature";};
void setSensors(const std::list<std::vector<int> > & data) {_data = data;}
const std::list<std::vector<int> > & getData() const {return _data;}
private:
std::list<std::vector<int> > _data;
};
} // namespace rtabmap

93
corelib/src/VWDictionary.cpp
View File

@@ -18,11 +18,11 @@
*/
#include "rtabmap/core/VWDictionary.h"
#include "rtabmap/core/VisualWord.h"
#include "VisualWord.h"
#include "rtabmap/core/Signature.h"
#include "Signature.h"
#include "rtabmap/core/DBDriver.h"
#include "rtabmap/core/NearestNeighbor.h"
#include "NearestNeighbor.h"
#include "rtabmap/core/Parameters.h"
#include "utilite/UtiLite.h"
@@ -231,15 +231,8 @@ void VWDictionary::setNNStrategy(NNStrategy strategy, const ParametersMap & para
}
switch(strategy)
{
case kNNKdTree:
//FIXME KdTreeNN is broken...
//_nn = new KdTreeNN(parameters);
//break;
UWARN("KdTree OpenCV is broken, setting nearest neighbor strategy to KdForest FLANN...");
_nn = new FlannKdTreeNN(parameters);
break;
case kNNFlannKdTree:
_nn = new FlannKdTreeNN(parameters);
_nn = new FlannNN(FlannNN::kKDTree, parameters);
break;
case kNNNaive:
default:
@@ -261,13 +254,7 @@ void VWDictionary::setNNStrategy(NNStrategy strategy, const ParametersMap & para
VWDictionary::NNStrategy VWDictionary::nnStrategy() const
{
NNStrategy strategy = kNNUndef;
KdTreeNN * kdTree = dynamic_cast<KdTreeNN*>(_nn);
FlannKdTreeNN * flannKdTree = dynamic_cast<FlannKdTreeNN*>(_nn);
if(kdTree)
{
strategy = kNNKdTree;
}
else if(flannKdTree)
if(_nn)
{
strategy = kNNFlannKdTree;
}
@@ -338,7 +325,7 @@ void VWDictionary::update()
}
// Create the kd-Tree
_dataTree = cv::Mat::zeros(_visualWords.size(), _dim, CV_32F); // SURF descriptors are CV_32F
_dataTree = cv::Mat(_visualWords.size(), _dim, CV_32F); // SURF descriptors are CV_32F
std::map<int, VisualWord*>::const_iterator iter = _visualWords.begin();
for(unsigned int i=0; i < _visualWords.size(); ++i, ++iter)
{
@@ -355,7 +342,7 @@ void VWDictionary::update()
}
}
ULOGGER_DEBUG("_mapIndexId.size() = %d, words.size()=%d",_mapIndexId.size(), _visualWords.size());
ULOGGER_DEBUG("_mapIndexId.size() = %d, words.size()=%d, _dim=%d",_mapIndexId.size(), _visualWords.size(), _dim);
ULOGGER_DEBUG("copying data = %f s", timer.ticks());
// Update the nearest neighbor algorithm
@@ -453,14 +440,8 @@ std::list<int> VWDictionary::addNewWords(const cv::Mat & descriptors,
cv::Mat results(descriptors.rows, k, CV_32SC1); // results index
cv::Mat dists;
if(_nn->isDist64F())
{
dists = cv::Mat(descriptors.rows, k, CV_64FC1); // Distance results are CV_64FC1;
}
else
{
dists = cv::Mat(descriptors.rows, k, CV_32FC1); // Distance results are CV_32FC1
}
dists = cv::Mat(descriptors.rows, k, CV_32FC1); // Distance results are CV_32FC1
cv::Mat newPts; // SURF descriptors are CV_32F
if(descriptors.type()!=CV_32F)
{
@@ -494,19 +475,7 @@ std::list<int> VWDictionary::addNewWords(const cv::Mat & descriptors,
for(unsigned int j=0; j<k; ++j)
{
float dist;
if(_nn->isDist64F())
{
dist = (float)dists.at<double>(i,j);
}
else
{
dist = dists.at<float>(i,j);
}
if(!_nn->isDistSquared())
{
dist*=dist;
}
dist = dists.at<float>(i,j);
fullResults.insert(std::pair<float, int>(dist, uValue(_mapIndexId, results.at<int>(i,j))));
}
}
@@ -664,16 +633,8 @@ std::vector<int> VWDictionary::findNN(const std::list<VisualWord *> & vws, bool
cv::Mat dists;
cv::Mat resultsNotIndexed(vws.size(), k, CV_32SC1);
cv::Mat distsNotIndexed;
if(_nn->isDist64F())
{
dists = cv::Mat(vws.size(), k, CV_64FC1); // Distance results are CV_64FC1;
distsNotIndexed = cv::Mat(vws.size(), k, CV_64FC1); // Distance results are CV_64FC1;
}
else
{
dists = cv::Mat(vws.size(), k, CV_32FC1); // Distance results are CV_32FC1
distsNotIndexed = cv::Mat(vws.size(), k, CV_32FC1); // Distance results are CV_32FC1
}
dists = cv::Mat(vws.size(), k, CV_32FC1); // Distance results are CV_32FC1
distsNotIndexed = cv::Mat(vws.size(), k, CV_32FC1); // Distance results are CV_32FC1
cv::Mat newPts(vws.size(), _dim, CV_32F); // SURF descriptors are CV_32F
// fill the request matrix
@@ -740,36 +701,12 @@ std::vector<int> VWDictionary::findNN(const std::list<VisualWord *> & vws, bool
float dist;
if(!_dataTree.empty())
{
if(_nn->isDist64F())
{
dist = (float)dists.at<double>(i,j);
}
else
{
dist = dists.at<float>(i,j);
}
if(!_nn->isDistSquared())
{
dist*=dist;
}
dist = dists.at<float>(i,j);
fullResults.insert(std::pair<float, int>(dist, uValue(_mapIndexId, results.at<int>(i,j))));
}
if(searchInNewlyAddedWords && unreferencedWordsCount)
{
if(_nn->isDist64F())
{
dist = (float)distsNotIndexed.at<double>(i,j);
}
else
{
dist = distsNotIndexed.at<float>(i,j);
}
if(!_nn->isDistSquared())
{
dist*=dist;
}
dist = distsNotIndexed.at<float>(i,j);
fullResults.insert(std::pair<float, int>(dist, uValue(mapIndexIdNotIndexed, resultsNotIndexed.at<int>(i,j))));
}
}
@@ -1038,7 +975,7 @@ void VWDictionary::getCommonWords(unsigned int nbCommonWords, int totalSign, std
}
else
{
commonWords = uValues(countMap);
commonWords = uValuesList(countMap);
}
ULOGGER_DEBUG("time = %f s", timer.ticks());
}

166
corelib/src/VerifyHypotheses.cpp
View File

@@ -1,166 +0,0 @@
/*
* Copyright (C) 2010-2011, Mathieu Labbe and IntRoLab - Universite de Sherbrooke
*
* This file is part of RTAB-Map.
*
* RTAB-Map is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* RTAB-Map is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with RTAB-Map. If not, see <http://www.gnu.org/licenses/>.
*/
#include "rtabmap/core/VerifyHypotheses.h"
#include "rtabmap/core/Parameters.h"
#include "rtabmap/core/Signature.h"
#include "rtabmap/core/EpipolarGeometry.h"
#include <cstdlib>
#include "utilite/UtiLite.h"
namespace rtabmap
{
HypVerificator::HypVerificator(const ParametersMap & parameters)
{
this->parseParameters(parameters);
}
void HypVerificator::parseParameters(const ParametersMap & parameters)
{
}
bool HypVerificator::verify(const Signature * ref, const Signature * hyp)
{
UDEBUG("");
return ref && hyp && !ref->isBadSignature() && !hyp->isBadSignature();
}
/////////////////////////
// HypVerificatorSim
/////////////////////////
HypVerificatorSim::HypVerificatorSim(const ParametersMap & parameters) :
HypVerificator(parameters),
_similarity(Parameters::defaultVhSimilarity())
{
this->parseParameters(parameters);
}
HypVerificatorSim::~HypVerificatorSim()
{
}
void HypVerificatorSim::parseParameters(const ParametersMap & parameters)
{
ParametersMap::const_iterator iter;
if((iter=parameters.find(Parameters::kVhSimilarity())) != parameters.end())
{
_similarity = std::atof((*iter).second.c_str());
}
HypVerificator::parseParameters(parameters);
}
bool HypVerificatorSim::verify(const Signature * ref, const Signature * hyp)
{
UDEBUG("");
if(ref && hyp)
{
return ref->compareTo(hyp) >= _similarity;
}
return false;
}
/////////////////////////
// HypVerificatorEpipolarGeo
/////////////////////////
HypVerificatorEpipolarGeo::HypVerificatorEpipolarGeo(const ParametersMap & parameters) :
HypVerificator(parameters),
_matchCountMinAccepted(Parameters::defaultVhEpMatchCountMin()),
_ransacParam1(Parameters::defaultVhEpRansacParam1()),
_ransacParam2(Parameters::defaultVhEpRansacParam2())
{
this->parseParameters(parameters);
}
HypVerificatorEpipolarGeo::~HypVerificatorEpipolarGeo() {
}
void HypVerificatorEpipolarGeo::parseParameters(const ParametersMap & parameters)
{
ParametersMap::const_iterator iter;
if((iter=parameters.find(Parameters::kVhEpMatchCountMin())) != parameters.end())
{
_matchCountMinAccepted = std::atoi((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kVhEpRansacParam1())) != parameters.end())
{
_ransacParam1 = std::atof((*iter).second.c_str());
}
if((iter=parameters.find(Parameters::kVhEpRansacParam2())) != parameters.end())
{
_ransacParam2 = std::atof((*iter).second.c_str());
}
HypVerificator::parseParameters(parameters);
}
bool HypVerificatorEpipolarGeo::verify(const Signature * ref, const Signature * hyp)
{
UDEBUG("");
const KeypointSignature * ssRef = dynamic_cast<const KeypointSignature *>(ref);
const KeypointSignature * ssHyp = dynamic_cast<const KeypointSignature *>(hyp);
if(ssRef && ssHyp)
{
return doEpipolarGeometry(ssHyp, ssRef);
}
return false;
}
bool HypVerificatorEpipolarGeo::doEpipolarGeometry(const KeypointSignature * ssA, const KeypointSignature * ssB)
{
if(ssA == 0 || ssB == 0)
{
return false;
}
ULOGGER_DEBUG("id(%d,%d)", ssA->id(), ssB->id());
std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > pairs;
findPairsUnique(ssA->getWords(), ssB->getWords(), pairs);
if((int)pairs.size()<_matchCountMinAccepted)
{
return false;
}
std::vector<uchar> status;
cv::Mat f = findFFromWords(pairs, status, _ransacParam1, _ransacParam2);
int inliers = uSum(status);
if(inliers < _matchCountMinAccepted)
{
ULOGGER_DEBUG("Epipolar constraint failed A : not enough inliers (%d/%d), min is %d", inliers, pairs.size(), _matchCountMinAccepted);
return false;
}
else
{
UDEBUG("inliers = %d/%d", inliers, pairs.size());
return true;
}
}
}

2
corelib/src/VisualWord.cpp
View File

@@ -17,7 +17,7 @@
* along with RTAB-Map. If not, see <http://www.gnu.org/licenses/>.
*/
#include "rtabmap/core/VisualWord.h"
#include "VisualWord.h"
#include "utilite/ULogger.h"
#include "utilite/UStl.h"

2
corelib/include/rtabmap/core/VisualWord.h → corelib/src/VisualWord.h
View File

@@ -50,7 +50,7 @@ private:
int _id;
float * _descriptor;
int _dim;
bool _saved; // If it's saved to bd
bool _saved; // If it's saved to db
int _totalReferences;
std::map<int, int> _references; // (signature id , occurrence in the signature)

BIN
corelib/src/resources/ColorIndexes1024.bin.zip
View File

Binary file not shown.

BIN
corelib/src/resources/ColorIndexes128.bin.zip
View File

Binary file not shown.

BIN
corelib/src/resources/ColorIndexes16.bin.zip
View File

Binary file not shown.

BIN
corelib/src/resources/ColorIndexes256.bin.zip
View File

Binary file not shown.

BIN
corelib/src/resources/ColorIndexes32.bin.zip
View File

Binary file not shown.

BIN
corelib/src/resources/ColorIndexes512.bin.zip
View File

Binary file not shown.

BIN
corelib/src/resources/ColorIndexes64.bin.zip
View File

Binary file not shown.

BIN
corelib/src/resources/ColorIndexes65536.bin.zip
View File

Binary file not shown.

BIN
corelib/src/resources/ColorIndexes8.bin.zip
View File

Binary file not shown.

47
corelib/src/resources/DatabaseSchema.sql
View File

@@ -1,5 +1,5 @@
-- *******************************************************************
-- construct_avpd_db: Script for creating the database
-- DatabaseSchema: Script for creating the database
-- Usage:
-- $ sqlite3 LTM.db < DatabaseSchema.sql
--
@@ -8,60 +8,37 @@
-- *******************************************************************
-- CLEAN
-- *******************************************************************
/*DROP TABLE Node;
DROP TABLE Link;
DROP TABLE Sensor;
DROP TABLE Actuator;
DROP TABLE Word;
DROP TABLE Map_Node_Word;
DROP TABLE Statistics;
DROP TABLE StatisticsSurf;*/
/*DROP TABLE Node;*/
-- *******************************************************************
-- CREATE
-- *******************************************************************
CREATE TABLE Node (
id INTEGER NOT NULL,
type INTEGER NOT NULL, -- 0=Keypoint, 1=Sensor
weight INTEGER,
time_enter DATE,
PRIMARY KEY (id)
);
CREATE TABLE Sensor (
CREATE TABLE Image (
id INTEGER NOT NULL,
num INTEGER NOT NULL,
type INTEGER NOT NULL, -- kTypeImage=0, kTypeImageFeatures2d, kTypeAudio, kTypeAudioFreq, kTypeAudioFreqSqrdMagn, kTypeJointState, kTypeNotSpecified
data BLOB, -- PostProcessed data (indexed integers)
raw_width INTEGER NOT NULL,
raw_height INTEGER NOT NULL,
raw_data_type INTEGER NOT NULL,
raw_compressed CHAR NOT NULL,
raw_data BLOB,
PRIMARY KEY (id, num)
time_enter DATE,
PRIMARY KEY (id)
);
CREATE TABLE Link (
from_id INTEGER NOT NULL,
to_id INTEGER NOT NULL,
type INTEGER NOT NULL, -- neighbor=0, loop=1, child=2
actuator_id INTEGER,
base_ids BLOB,
FOREIGN KEY (from_id) REFERENCES Node(id),
FOREIGN KEY (to_id) REFERENCES Node(id)
);
CREATE TABLE Actuator (
id INTEGER NOT NULL,
num INTEGER NOT NULL,
type INTEGER NOT NULL, -- kTypeTwist=0, kTypeNotSpecified
width INTEGER NOT NULL,
height INTEGER NOT NULL,
data_type INTEGER NOT NULL,
data BLOB,
PRIMARY KEY (id, num)
);
--
CREATE TABLE Word (
id INTEGER NOT NULL,
@@ -92,17 +69,17 @@ CREATE TABLE Statistics (
);
CREATE TABLE StatisticsDictionary (
dictionary_size INTEGER,
time_enter DATE
dictionary_size INTEGER,
time_enter DATE
);
-- *******************************************************************
-- TRIGGERS
-- *******************************************************************
CREATE TRIGGER insert_Map_Node_Word BEFORE INSERT ON Map_Node_Word
WHEN NOT EXISTS (SELECT type FROM Node WHERE Node.id = NEW.node_id AND type=0)
WHEN NOT EXISTS (SELECT Node.id FROM Node WHERE Node.id = NEW.node_id)
BEGIN
SELECT RAISE(ABORT, 'Keypoint type constraint failed');
SELECT RAISE(ABORT, 'Foreign key constraint failed in Map_Node_Word table');
END;
-- Creating a trigger for time_enter
@@ -121,16 +98,10 @@ BEGIN
UPDATE Statistics SET time_enter = DATETIME('NOW') WHERE rowid = new.rowid;
END;
CREATE TRIGGER insert_StatisticsDictionary_timeEnter AFTER INSERT ON StatisticsDictionary
BEGIN
UPDATE StatisticsDictionary SET time_enter = DATETIME('NOW') WHERE rowid = new.rowid;
END;
-- *******************************************************************
-- INDEXES
-- *******************************************************************
CREATE INDEX IDX_Map_Node_Word_node_id on Map_Node_Word (node_id);
CREATE INDEX IDX_Sensor_Id on Sensor (id);
CREATE INDEX IDX_Link_from_id on Link (from_id);