mirror of
https://github.com/introlab/rtabmap.git
synced 2026-09-02 01:20:25 +08:00
moved rtabmap-ros-pkg project in this project
git-svn-id: http://rtabmap.googlecode.com/svn/branches/0.3/rtabmap@52 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
237
corelib/include/rtabmap/core/Camera.h
Normal file
237
corelib/include/rtabmap/core/Camera.h
Normal file
@@ -0,0 +1,237 @@
|
||||
/*
|
||||
* 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/>.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
|
||||
|
||||
#include <opencv2/highgui/highgui.hpp>
|
||||
#include "utilite/UThreadNode.h"
|
||||
#include "utilite/UEventsHandler.h"
|
||||
#include "rtabmap/core/Parameters.h"
|
||||
#include <set>
|
||||
#include <stack>
|
||||
|
||||
class UDirectory;
|
||||
|
||||
namespace rtabmap
|
||||
{
|
||||
|
||||
class KeypointDetector;
|
||||
class KeypointDescriptor;
|
||||
class SMState;
|
||||
|
||||
/**
|
||||
* Only encapsulate the image in a newly created SMState.
|
||||
*/
|
||||
class RTABMAP_EXP CamPostTreatment
|
||||
{
|
||||
public:
|
||||
CamPostTreatment(const ParametersMap & parameters = ParametersMap()) {
|
||||
this->parseParameters(parameters);
|
||||
}
|
||||
virtual ~CamPostTreatment() {}
|
||||
virtual SMState * process(IplImage * image);
|
||||
virtual void parseParameters(const ParametersMap & parameters) {}
|
||||
};
|
||||
|
||||
/**
|
||||
* Extract keypoints from the image
|
||||
*/
|
||||
class RTABMAP_EXP CamKeypointTreatment : public CamPostTreatment
|
||||
{
|
||||
public:
|
||||
enum DetectorStrategy {kDetectorSurf, kDetectorStar, kDetectorSift, kDetectorUndef};
|
||||
enum DescriptorStrategy {kDescriptorSurf, kDescriptorColorSurf, kDescriptorLaplacianSurf, kDescriptorSift, kDescriptorHueSurf, kDescriptorUndef};
|
||||
|
||||
public:
|
||||
CamKeypointTreatment(const ParametersMap & parameters = ParametersMap()) :
|
||||
_keypointDetector(0),
|
||||
_keypointDescriptor(0)
|
||||
{
|
||||
this->parseParameters(parameters);
|
||||
}
|
||||
virtual ~CamKeypointTreatment();
|
||||
virtual SMState * process(IplImage * image);
|
||||
virtual void parseParameters(const ParametersMap & parameters);
|
||||
DetectorStrategy detectorStrategy() const;
|
||||
private:
|
||||
KeypointDetector * _keypointDetector;
|
||||
KeypointDescriptor * _keypointDescriptor;
|
||||
};
|
||||
|
||||
/**
|
||||
* Class Camera
|
||||
*
|
||||
*/
|
||||
class RTABMAP_EXP Camera :
|
||||
public UThreadNode,
|
||||
public UEventsHandler
|
||||
{
|
||||
public:
|
||||
enum State {kStateCapturing, kStateChangingParameters};
|
||||
|
||||
public:
|
||||
virtual ~Camera();
|
||||
|
||||
virtual IplImage * takeImage() = 0;
|
||||
virtual bool init() = 0;
|
||||
bool isPaused() const {return !this->isRunning();}
|
||||
bool isCapturing() const {return this->isRunning();}
|
||||
unsigned int getImageWidth() const {return _imageWidth;}
|
||||
unsigned int getImageHeight() const {return _imageHeight;}
|
||||
void setPostThreatement(CamPostTreatment * strategy); // ownership is transferred
|
||||
|
||||
protected:
|
||||
/**
|
||||
* Constructor
|
||||
*
|
||||
* @param imageRate : image/second , 0 for fast as the camera can
|
||||
*/
|
||||
Camera(float imageRate = 0, bool autoRestart = false, unsigned int imageWidth = 0, unsigned int imageHeight = 0);
|
||||
|
||||
virtual void handleEvent(UEvent* anEvent);
|
||||
|
||||
private:
|
||||
virtual void mainLoop();
|
||||
void process();
|
||||
void pushNewState(State newState, const ParametersMap & parameters = ParametersMap());
|
||||
virtual void parseParameters(const ParametersMap & parameters) {_postThreatement->parseParameters(parameters);}
|
||||
|
||||
private:
|
||||
float _imageRate;
|
||||
int _id;
|
||||
bool _autoRestart;
|
||||
unsigned int _imageWidth;
|
||||
unsigned int _imageHeight;
|
||||
CamPostTreatment * _postThreatement;
|
||||
|
||||
UMutex _stateMutex;
|
||||
std::stack<State> _state;
|
||||
std::stack<ParametersMap> _stateParam;
|
||||
};
|
||||
|
||||
|
||||
|
||||
/////////////////////////
|
||||
// CameraImages
|
||||
/////////////////////////
|
||||
class RTABMAP_EXP CameraImages :
|
||||
public Camera
|
||||
{
|
||||
public:
|
||||
CameraImages(const std::string & path,
|
||||
int startAt = 1,
|
||||
bool refreshDir = false,
|
||||
float imageRate = 0,
|
||||
bool autoRestart = false,
|
||||
unsigned int imageWidth = 0,
|
||||
unsigned int imageHeight = 0);
|
||||
virtual ~CameraImages();
|
||||
|
||||
virtual IplImage * takeImage();
|
||||
virtual bool init();
|
||||
|
||||
private:
|
||||
std::string _path;
|
||||
int _startAt;
|
||||
// If the list of files in the directory is refreshed
|
||||
// on each call of takeImage()
|
||||
bool _refreshDir;
|
||||
UDirectory * _dir;
|
||||
int _count;
|
||||
std::string _lastFileName;
|
||||
|
||||
|
||||
|
||||
};
|
||||
|
||||
|
||||
|
||||
|
||||
/////////////////////////
|
||||
// CameraVideo
|
||||
/////////////////////////
|
||||
class RTABMAP_EXP CameraVideo :
|
||||
public Camera
|
||||
{
|
||||
public:
|
||||
enum Source{kVideoFile, kUsbDevice};
|
||||
|
||||
public:
|
||||
CameraVideo(int usbDevice = 0,
|
||||
float imageRate = 0,
|
||||
bool autoRestart = false,
|
||||
unsigned int imageWidth = 0,
|
||||
unsigned int imageHeight = 0);
|
||||
CameraVideo(const std::string & fileName,
|
||||
float imageRate = 0,
|
||||
bool autoRestart = false,
|
||||
unsigned int imageWidth = 0,
|
||||
unsigned int imageHeight = 0);
|
||||
virtual ~CameraVideo();
|
||||
|
||||
virtual IplImage * takeImage();
|
||||
virtual bool init();
|
||||
|
||||
private:
|
||||
// File type
|
||||
std::string _fileName;
|
||||
|
||||
CvCapture* _capture;
|
||||
Source _src;
|
||||
|
||||
// Usb camera
|
||||
int _usbDevice;
|
||||
};
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
/////////////////////////
|
||||
// CameraDatabase
|
||||
/////////////////////////
|
||||
class DBDriver;
|
||||
class RTABMAP_EXP CameraDatabase :
|
||||
public Camera
|
||||
{
|
||||
public:
|
||||
CameraDatabase(const std::string & path,
|
||||
bool ignoreChildren,
|
||||
float imageRate = 0,
|
||||
bool autoRestart = false,
|
||||
unsigned int imageWidth = 0,
|
||||
unsigned int imageHeight = 0);
|
||||
virtual ~CameraDatabase();
|
||||
|
||||
virtual IplImage * takeImage();
|
||||
virtual bool init();
|
||||
|
||||
private:
|
||||
std::string _path;
|
||||
bool _ignoreChildren;
|
||||
std::set<int>::iterator _indexIter;
|
||||
DBDriver * _dbDriver;
|
||||
std::set<int> _ids;
|
||||
};
|
||||
|
||||
|
||||
} // namespace rtabmap
|
||||
79
corelib/include/rtabmap/core/CameraEvent.h
Normal file
79
corelib/include/rtabmap/core/CameraEvent.h
Normal file
@@ -0,0 +1,79 @@
|
||||
/*
|
||||
* 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 CAMERAEVENT_H_
|
||||
#define CAMERAEVENT_H_
|
||||
|
||||
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
|
||||
|
||||
#include "utilite/UEvent.h"
|
||||
#include <opencv2/core/core.hpp>
|
||||
|
||||
namespace rtabmap
|
||||
{
|
||||
|
||||
class RTABMAP_EXP CameraEvent :
|
||||
public UEvent
|
||||
{
|
||||
public:
|
||||
enum Code {
|
||||
kCodeCtrl,
|
||||
kCodeNoMoreImages
|
||||
};
|
||||
|
||||
enum Cmd {
|
||||
kCmdUndefined,
|
||||
kCmdPause,
|
||||
kCmdChangeParam
|
||||
};
|
||||
|
||||
public:
|
||||
CameraEvent(Cmd command, float imageRate = -1, bool autoRestart = false) :
|
||||
UEvent(kCodeCtrl),
|
||||
_command(command),
|
||||
_imageRate(imageRate),
|
||||
_autoRestart(autoRestart)
|
||||
{
|
||||
}
|
||||
|
||||
CameraEvent() :
|
||||
UEvent(kCodeNoMoreImages),
|
||||
_command(kCmdUndefined),
|
||||
_imageRate(-1)
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
virtual ~CameraEvent() {}
|
||||
|
||||
virtual std::string getClassName() const {return std::string("CameraEvent");}
|
||||
|
||||
const Cmd & getCommand() const {return _command;}
|
||||
float getImageRate() const {return _imageRate;}
|
||||
bool getAutoRestart() const {return _autoRestart;}
|
||||
|
||||
private:
|
||||
Cmd _command;
|
||||
float _imageRate;
|
||||
bool _autoRestart;
|
||||
};
|
||||
|
||||
} // namespace rtabmap
|
||||
|
||||
#endif /* CAMERAEVENT_H_ */
|
||||
178
corelib/include/rtabmap/core/DBDriver.h
Normal file
178
corelib/include/rtabmap/core/DBDriver.h
Normal file
@@ -0,0 +1,178 @@
|
||||
/*
|
||||
* 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 DBDRIVER_H_
|
||||
#define DBDRIVER_H_
|
||||
|
||||
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
|
||||
|
||||
#include <string>
|
||||
#include <list>
|
||||
#include <map>
|
||||
#include <set>
|
||||
#include <opencv2/core/core.hpp>
|
||||
#include "utilite/UMutex.h"
|
||||
#include "utilite/UThreadNode.h"
|
||||
#include "rtabmap/core/Parameters.h"
|
||||
|
||||
namespace rtabmap {
|
||||
|
||||
class Signature;
|
||||
class KeypointSignature;
|
||||
class VWDictionary;
|
||||
class VisualWord;
|
||||
|
||||
// Todo This class needs a refactoring, the _dbSafeAccessMutex problem when the trash is emptying (transaction)
|
||||
// "Of course, it has always been the case and probably always will be
|
||||
//that you cannot use the same sqlite3 connection in two or more
|
||||
//threads at the same time. You can use different sqlite3 connections
|
||||
//at the same time in different threads, or you can move the same
|
||||
//sqlite3 connection across threads (subject to the constraints above)
|
||||
//but never, never try to use the same connection simultaneously in
|
||||
//two or more threads."
|
||||
//
|
||||
class RTABMAP_EXP DBDriver : public UThreadNode
|
||||
{
|
||||
public:
|
||||
virtual ~DBDriver();
|
||||
|
||||
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 emptyTrashes(bool async = false);
|
||||
double getEmptyTrashesTime() const {return _emptyTrashesTime;}
|
||||
|
||||
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;
|
||||
|
||||
bool addNeighbor(int id, int neighbor, const std::list<std::vector<float> > & actuatorStates);
|
||||
bool removeNeighbor(int id, int neighbor);
|
||||
|
||||
public:
|
||||
// Mutex-protected methods of abstract versions below
|
||||
bool getSignature(int signatureId, Signature ** s);
|
||||
bool getVisualWord(int wordId, VisualWord ** vw);
|
||||
|
||||
bool openConnection(const std::string & url);
|
||||
void closeConnection();
|
||||
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);
|
||||
|
||||
// Load objects
|
||||
bool load(VWDictionary * dictionary) const;
|
||||
bool loadLastSignatures(std::list<Signature *> & signatures) const;
|
||||
bool loadKeypointSignatures(const std::list<int> & ids, std::list<Signature *> & signatures, bool onlyParents = false);
|
||||
bool loadWords(const std::list<int> & wordIds, std::list<VisualWord *> & vws);
|
||||
|
||||
// Specific queries...
|
||||
bool getImage(int id, IplImage ** img) const;
|
||||
bool getNeighborIds(int signatureId, std::set<int> & neighbors) const;
|
||||
bool loadNeighbors(int signatureId, std::map<int, std::list<std::vector<float> > > & neighbors) const;
|
||||
bool getWeight(int signatureId, int & weight) const;
|
||||
bool getLoopClosureId(int signatureId, int & loopId) const;
|
||||
bool getImageCompressed(int id, CvMat ** compressed) const;
|
||||
bool getAllSignatureIds(std::set<int> & ids) const;
|
||||
bool getLastSignatureId(int & id) const;
|
||||
bool getLastVisualWordId(int & id) const;
|
||||
bool getSurfNi(int signatureId, int & ni) const;
|
||||
bool getChildrenIds(int signatureId, std::list<int> & ids) const;
|
||||
bool getHighestWeightedSignatures(unsigned int count, std::multimap<int, int> & ids) const;
|
||||
|
||||
protected:
|
||||
DBDriver(const ParametersMap & parameters = ParametersMap());
|
||||
|
||||
private:
|
||||
virtual bool connectDatabaseQuery(const std::string & url) = 0;
|
||||
virtual void disconnectDatabaseQuery() = 0;
|
||||
virtual bool isConnectedQuery() const = 0;
|
||||
virtual long getMemoryUsedQuery() const = 0; // In bytes
|
||||
|
||||
virtual bool 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) const = 0;
|
||||
virtual bool getWeightQuery(int signatureId, int & weight) const = 0;
|
||||
virtual bool getLoopClosureIdQuery(int signatureId, int & loopId) const = 0;
|
||||
virtual bool addNeighborQuery(int id, int neighbor, const std::list<std::vector<float> > & actuatorStates) 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 KeypointSignature * ss) const = 0;
|
||||
virtual bool saveQuery(const std::list<KeypointSignature *> & signatures) const = 0;
|
||||
|
||||
// Load objects
|
||||
virtual bool loadQuery(VWDictionary * dictionary) const = 0;
|
||||
virtual bool loadLastSignaturesQuery(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 loadKeypointSignaturesQuery(const std::list<int> & ids, std::list<Signature *> & signatures, bool onlyParents = false) const = 0;
|
||||
virtual bool loadWordsQuery(const std::list<int> & wordIds, std::list<VisualWord *> & vws) const = 0;
|
||||
virtual bool loadNeighborsQuery(int signatureId, std::map<int, std::list<std::vector<float> > > & neighbors) const = 0;
|
||||
|
||||
virtual bool getImageCompressedQuery(int id, CvMat ** compressed) const = 0;
|
||||
virtual bool getAllSignatureIdsQuery(std::set<int> & ids) const = 0;
|
||||
virtual bool getLastSignatureIdQuery(int & id) const = 0;
|
||||
virtual bool getLastVisualWordIdQuery(int & id) const = 0;
|
||||
virtual bool getSurfNiQuery(int signatureId, int & ni) const = 0;
|
||||
virtual bool getChildrenIdsQuery(int signatureId, std::list<int> & ids) const = 0;
|
||||
virtual bool getHighestWeightedSignaturesQuery(unsigned int count, std::multimap<int,int> & signatures) const = 0;
|
||||
|
||||
private:
|
||||
//non-abstract methods
|
||||
bool saveOrUpdate(const std::vector<Signature *> & signatures) const;
|
||||
|
||||
//thread stuff
|
||||
virtual void mainLoop();
|
||||
virtual void killCleanup();
|
||||
|
||||
private:
|
||||
UMutex _transactionMutex;
|
||||
std::map<int, Signature *> _trashSignatures;//<id, Signature*>
|
||||
std::map<int, VisualWord *> _trashVisualWords; //<id, VisualWord*>
|
||||
UMutex _trashesMutex;
|
||||
UMutex _dbSafeAccessMutex;
|
||||
USemaphore _addSem;
|
||||
unsigned int _minSignaturesToSave;
|
||||
unsigned int _minWordsToSave;
|
||||
bool _asyncWaiting;
|
||||
double _emptyTrashesTime;
|
||||
std::string _url;
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#endif /* DBDRIVER_H_ */
|
||||
42
corelib/include/rtabmap/core/DBDriverFactory.h
Normal file
42
corelib/include/rtabmap/core/DBDriverFactory.h
Normal file
@@ -0,0 +1,42 @@
|
||||
/*
|
||||
* 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_ */
|
||||
33
corelib/include/rtabmap/core/EpipolarGeometry.h
Normal file
33
corelib/include/rtabmap/core/EpipolarGeometry.h
Normal file
@@ -0,0 +1,33 @@
|
||||
/*
|
||||
* 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/>.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
|
||||
|
||||
#include <opencv2/core/core.hpp>
|
||||
|
||||
namespace rtabmap
|
||||
{
|
||||
|
||||
//epipolar geometry
|
||||
void RTABMAP_EXP findEpipolesFromF(const cv::Mat & fundamentalMatrix, cv::Vec3d & e1, cv::Vec3d & e2);
|
||||
void RTABMAP_EXP findPFromF(const cv::Mat & fundamentalMatrix, cv::Mat & p2, cv::Vec3d e2 = cv::Vec3d());
|
||||
|
||||
} // namespace rtabmap
|
||||
142
corelib/include/rtabmap/core/KeypointDescriptor.h
Normal file
142
corelib/include/rtabmap/core/KeypointDescriptor.h
Normal file
@@ -0,0 +1,142 @@
|
||||
/*
|
||||
* 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:
|
||||
virtual ~KeypointDescriptor();
|
||||
|
||||
std::list<std::vector<float> > generateDescriptors(const IplImage * image, const std::list<cv::KeyPoint> & keypoints) const;
|
||||
void setChildDescriptor(KeypointDescriptor * childDescriptor);
|
||||
virtual void parseParameters(const ParametersMap & parameters);
|
||||
|
||||
protected:
|
||||
KeypointDescriptor(const ParametersMap & parameters = ParametersMap(), KeypointDescriptor * childDescriptor = 0);
|
||||
const KeypointDescriptor * getChildDescriptor() const {return _childDescriptor;}
|
||||
|
||||
private:
|
||||
virtual std::list<std::vector<float> > _generateDescriptors(
|
||||
const IplImage * image,
|
||||
const std::list<cv::KeyPoint> & keypoints) const = 0;
|
||||
|
||||
private:
|
||||
KeypointDescriptor * _childDescriptor;
|
||||
};
|
||||
|
||||
//SURFDescriptor
|
||||
class RTABMAP_EXP SURFDescriptor : public KeypointDescriptor
|
||||
{
|
||||
public:
|
||||
SURFDescriptor(const ParametersMap & parameters = ParametersMap(), KeypointDescriptor * childDescriptor = 0);
|
||||
virtual ~SURFDescriptor();
|
||||
virtual void parseParameters(const ParametersMap & parameters);
|
||||
private:
|
||||
virtual std::list<std::vector<float> > _generateDescriptors(const IplImage * image, const std::list<cv::KeyPoint> & keypoints) const;
|
||||
|
||||
private:
|
||||
cv::SURF _surf;
|
||||
bool _gpuVersion;
|
||||
bool _upright;
|
||||
};
|
||||
|
||||
//SIFTDescriptor
|
||||
class RTABMAP_EXP SIFTDescriptor : public KeypointDescriptor
|
||||
{
|
||||
public:
|
||||
SIFTDescriptor(const ParametersMap & parameters = ParametersMap(), KeypointDescriptor * childDescriptor = 0);
|
||||
virtual ~SIFTDescriptor();
|
||||
virtual void parseParameters(const ParametersMap & parameters);
|
||||
private:
|
||||
virtual std::list<std::vector<float> > _generateDescriptors(const IplImage * image, const std::list<cv::KeyPoint> & keypoints) const;
|
||||
|
||||
private:
|
||||
cv::SIFT::CommonParams _commonParams;
|
||||
cv::SIFT::DescriptorParams _descriptorParams;
|
||||
};
|
||||
|
||||
//LaplacianDescriptor
|
||||
class RTABMAP_EXP LaplacianDescriptor : public KeypointDescriptor
|
||||
{
|
||||
public:
|
||||
LaplacianDescriptor(const ParametersMap & parameters = ParametersMap(), KeypointDescriptor * childDescriptor = 0);
|
||||
virtual ~LaplacianDescriptor();
|
||||
virtual void parseParameters(const ParametersMap & parameters);
|
||||
private:
|
||||
virtual std::list<std::vector<float> > _generateDescriptors(const IplImage * image, const std::list<cv::KeyPoint> & keypoints) const;
|
||||
};
|
||||
|
||||
//MinMax ColorDescriptor
|
||||
class RTABMAP_EXP ColorDescriptor : public KeypointDescriptor
|
||||
{
|
||||
public:
|
||||
ColorDescriptor(const ParametersMap & parameters = ParametersMap(), KeypointDescriptor * childDescriptor = 0);
|
||||
virtual ~ColorDescriptor();
|
||||
virtual void parseParameters(const ParametersMap & parameters);
|
||||
protected:
|
||||
void getCircularROI(int R, std::vector<int> & RxV) const;
|
||||
private:
|
||||
virtual std::list<std::vector<float> > _generateDescriptors(const IplImage * image, const std::list<cv::KeyPoint> & keypoints) const;
|
||||
};
|
||||
|
||||
//MinMax HueDescriptor
|
||||
class RTABMAP_EXP HueDescriptor : public ColorDescriptor
|
||||
{
|
||||
public:
|
||||
HueDescriptor(const ParametersMap & parameters = ParametersMap(), KeypointDescriptor * childDescriptor = 0);
|
||||
virtual ~HueDescriptor();
|
||||
virtual void parseParameters(const ParametersMap & parameters);
|
||||
private:
|
||||
virtual std::list<std::vector<float> > _generateDescriptors(const IplImage * image, const std::list<cv::KeyPoint> & keypoints) const;
|
||||
|
||||
// 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_ */
|
||||
107
corelib/include/rtabmap/core/KeypointDetector.h
Normal file
107
corelib/include/rtabmap/core/KeypointDetector.h
Normal file
@@ -0,0 +1,107 @@
|
||||
/*
|
||||
* 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:
|
||||
virtual ~KeypointDetector() {}
|
||||
std::list<cv::KeyPoint> generateKeypoints(const IplImage * image);
|
||||
virtual void parseParameters(const ParametersMap & parameters);
|
||||
unsigned int getWordsPerImageTarget() const {return _wordsPerImageTarget;}
|
||||
double getAdaptiveResponseThr() const {return _adaptiveResponseThr;}
|
||||
virtual double getMinimumResponseThr() const = 0;
|
||||
bool isUsingAdaptiveResponseThr() const {return _usingAdaptiveResponseThr;}
|
||||
void setRoi(const std::string & roi);
|
||||
protected:
|
||||
KeypointDetector(const ParametersMap & parameters = ParametersMap());
|
||||
void setAdaptiveResponseThr(float adaptiveResponseThr) {_adaptiveResponseThr = adaptiveResponseThr;}
|
||||
private:
|
||||
virtual std::list<cv::KeyPoint> _generateKeypoints(const IplImage * image, const cv::Rect & roi) const = 0;
|
||||
cv::Rect computeRoi(const IplImage * image) const;
|
||||
private:
|
||||
unsigned int _wordsPerImageTarget;
|
||||
bool _usingAdaptiveResponseThr;
|
||||
double _adaptiveResponseThr;
|
||||
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);
|
||||
virtual double getMinimumResponseThr() const {return _surf.hessianThreshold;};
|
||||
private:
|
||||
virtual std::list<cv::KeyPoint> _generateKeypoints(const IplImage * image, const cv::Rect & roi) const;
|
||||
private:
|
||||
cv::SURF _surf;
|
||||
bool _gpuVersion;
|
||||
bool _upright;
|
||||
};
|
||||
|
||||
//SIFTDetector
|
||||
class RTABMAP_EXP SIFTDetector : public KeypointDetector
|
||||
{
|
||||
public:
|
||||
SIFTDetector(const ParametersMap & parameters = ParametersMap());
|
||||
virtual ~SIFTDetector();
|
||||
virtual void parseParameters(const ParametersMap & parameters);
|
||||
virtual double getMinimumResponseThr() const {return _detectorParams.threshold;};
|
||||
private:
|
||||
virtual std::list<cv::KeyPoint> _generateKeypoints(const IplImage * image, const cv::Rect & roi) const;
|
||||
private:
|
||||
cv::SIFT::CommonParams _commonParams;
|
||||
cv::SIFT::DetectorParams _detectorParams;
|
||||
};
|
||||
|
||||
//StarDetector
|
||||
class RTABMAP_EXP StarDetector : public KeypointDetector
|
||||
{
|
||||
public:
|
||||
StarDetector(const ParametersMap & parameters = ParametersMap());
|
||||
virtual ~StarDetector();
|
||||
virtual void parseParameters(const ParametersMap & parameters);
|
||||
virtual double getMinimumResponseThr() const {return (double)_star.responseThreshold;};
|
||||
private:
|
||||
virtual std::list<cv::KeyPoint> _generateKeypoints(const IplImage * image, const cv::Rect & roi) const;
|
||||
private:
|
||||
cv::StarDetector _star;
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#endif /* KEYPOINTDETECTOR_H_ */
|
||||
243
corelib/include/rtabmap/core/Parameters.h
Normal file
243
corelib/include/rtabmap/core/Parameters.h
Normal file
@@ -0,0 +1,243 @@
|
||||
/*
|
||||
* 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 PARAMETERS_H_
|
||||
#define PARAMETERS_H_
|
||||
|
||||
// default parameters
|
||||
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
|
||||
#include "utilite/UEvent.h"
|
||||
#include <string>
|
||||
#include <map>
|
||||
#include "utilite/UDestroyer.h"
|
||||
|
||||
namespace rtabmap
|
||||
{
|
||||
|
||||
typedef std::map<std::string, std::string> ParametersMap; // Key, value
|
||||
typedef std::pair<const std::string, std::string> ParametersPair;
|
||||
|
||||
/**
|
||||
* Macro used to create parameter's key and default value.
|
||||
* This macro must be used only in the Parameters class definition (in this file).
|
||||
* They are automatically added to the default parameters map of the class Parameters.
|
||||
* Example:
|
||||
* @code
|
||||
* //for PARAM(Video, ImageWidth, int, 640), the output will be :
|
||||
* public:
|
||||
* static std::string kVideoImageWidth() {return std::string("Video/ImageWidth");}
|
||||
* static int defaultVideoImageWidth() {return 640;}
|
||||
* private:
|
||||
* class DummyVideoImageWidth {
|
||||
* public:
|
||||
* DummyVideoImageWidth() {parameters_.insert(ParametersPair("Video/ImageWidth", "640"));}
|
||||
* };
|
||||
* DummyVideoImageWidth dummyVideoImageWidth;
|
||||
* @endcode
|
||||
*/
|
||||
#define RTABMAP_PARAM(PREFIX, NAME, TYPE, DEFAULT_VALUE) \
|
||||
public: \
|
||||
static std::string k##PREFIX##NAME() {return std::string(#PREFIX "/" #NAME);} \
|
||||
static TYPE default##PREFIX##NAME() {return DEFAULT_VALUE;} \
|
||||
private: \
|
||||
class Dummy##PREFIX##NAME { \
|
||||
public: \
|
||||
Dummy##PREFIX##NAME() {parameters_.insert(ParametersPair(#PREFIX "/" #NAME, #DEFAULT_VALUE));} \
|
||||
}; \
|
||||
Dummy##PREFIX##NAME dummy##PREFIX##NAME;
|
||||
// end define PARAM
|
||||
|
||||
/**
|
||||
* It's the same as the macro PARAM but it should be used for string parameters.
|
||||
* Macro used to create parameter's key and default value.
|
||||
* This macro must be used only in the Parameters class definition (in this file).
|
||||
* They are automatically added to the default parameters map of the class Parameters.
|
||||
* Example:
|
||||
* @code
|
||||
* //for PARAM_STR(Video, TextFileName, "Hello_world"), the output will be :
|
||||
* public:
|
||||
* static std::string kVideoFileName() {return std::string("Video/FileName");}
|
||||
* static std::string defaultVideoFileName() {return "Hello_world";}
|
||||
* private:
|
||||
* class DummyVideoFileName {
|
||||
* public:
|
||||
* DummyVideoFileName() {parameters_.insert(ParametersPair("Video/FileName", "Hello_world"));}
|
||||
* };
|
||||
* DummyVideoFileName dummyVideoFileName;
|
||||
* @endcode
|
||||
*/
|
||||
#define RTABMAP_PARAM_STR(PREFIX, NAME, DEFAULT_VALUE) \
|
||||
public: \
|
||||
static std::string k##PREFIX##NAME() {return std::string(#PREFIX "/" #NAME);} \
|
||||
static std::string default##PREFIX##NAME() {return DEFAULT_VALUE;} \
|
||||
private: \
|
||||
class Dummy##PREFIX##NAME { \
|
||||
public: \
|
||||
Dummy##PREFIX##NAME() {parameters_.insert(ParametersPair(#PREFIX "/" #NAME, DEFAULT_VALUE));} \
|
||||
}; \
|
||||
Dummy##PREFIX##NAME dummy##PREFIX##NAME;
|
||||
// end define PARAM
|
||||
|
||||
/**
|
||||
* Class Parameters.
|
||||
* This class is used to manage all custom parameters
|
||||
* we want in the application. It was designed to be very easy to add
|
||||
* a new parameter (just by adding one line of code).
|
||||
* The macro PARAM(PREFIX, NAME, TYPE, DEFAULT_VALUE) is
|
||||
* used to create a parameter in this class. A parameter can be accessed after by
|
||||
* Parameters::defaultPARAMETERNAME() for the default value, Parameters::kPARAMETERNAME for his key (parameter name).
|
||||
* The class provides also a general map containing all the parameter's key and
|
||||
* default value. This map can be accessed anywhere in the application by
|
||||
* Parameters::getDefaultParameters();
|
||||
* Example:
|
||||
* @code
|
||||
* //Defining a parameter in this class with the macro PARAM:
|
||||
* PARAM(Video, ImageWidth, int, 640);
|
||||
*
|
||||
* // Now from anywhere in the application (Parameters is a singleton)
|
||||
* int width = Parameters::defaultVideoImageWidth(); // theDefaultValue = 640
|
||||
* std::string theKey = Parameters::kVideoImageWidth(); // theKey = "Video/ImageWidth"
|
||||
* std::string strValue = Util::value(Parameters::getDefaultParameters(), theKey); // strValue = "640"
|
||||
* @endcode
|
||||
* @see getDefaultParameters()
|
||||
* TODO Add a detailed example with simple classes
|
||||
*/
|
||||
class RTABMAP_EXP Parameters
|
||||
{
|
||||
// Rtabmap parameters
|
||||
RTABMAP_PARAM(Rtabmap, VhStrategy, int, 0); // Simple 0, Epipolar 1
|
||||
RTABMAP_PARAM(Rtabmap, PublishStats, bool, true); // Publishing statistics
|
||||
RTABMAP_PARAM(Rtabmap, ReactivationThr, float, 0.0); // Reactivation threshold
|
||||
RTABMAP_PARAM(Rtabmap, TimeThr, float, 0.7); // Maximum time allowed for the detector (s) (0 means infinity)
|
||||
RTABMAP_PARAM(Rtabmap, DisableReactivation, bool, false); // Memory reactivation when a loop closure occurs : 0=enable, 1=disable
|
||||
RTABMAP_PARAM(Rtabmap, SMStateBufferSize, int, 1); // Data buffer size (0 min inf)
|
||||
RTABMAP_PARAM(Rtabmap, MinMemorySizeForLoopDetection, unsigned int, 15); //Minimum size of the memory to create loop closure hypotheses
|
||||
RTABMAP_PARAM_STR(Rtabmap, WorkingDirectory, Parameters::getDefaultWorkingDirectory()); // Working directory
|
||||
RTABMAP_PARAM(Rtabmap, LocalGraphCleaned, bool, false); // Clean the neighborhood of the retrieved id
|
||||
RTABMAP_PARAM(Rtabmap, MaxRetrieved, unsigned int, 2); // Maximum locations retrieved at the same time from LTM
|
||||
|
||||
// Hypotheses selection
|
||||
RTABMAP_PARAM(Rtabmap, LoopThr, float, 0.10); // Loop closing threshold
|
||||
RTABMAP_PARAM(Rtabmap, LoopRatio, float, 0.90); // 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, false); // Keep raw data
|
||||
RTABMAP_PARAM(Mem, MaxStMemSize, unsigned int, 25); // 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, DatabaseCleaned, bool, true); // Delete old signatures in the database (the ones which can't never be reactivated)
|
||||
RTABMAP_PARAM(Mem, DelayRequired, int, 10); // Delay (in iterations) required to transfer signatures
|
||||
RTABMAP_PARAM(Mem, RecentWmRatio, float, 0.2); // Ratio of locations after the last loop closure in WM that cannot be transferred
|
||||
|
||||
// KeypointMemory (Keypoint-based)
|
||||
RTABMAP_PARAM(Kp, NNStrategy, int, 2); // Naive 0, kdTree 1, kdForest 2
|
||||
RTABMAP_PARAM(Kp, IncrementalDictionary, bool, true);
|
||||
RTABMAP_PARAM(Kp, WordsPerImage, int, 400);
|
||||
RTABMAP_PARAM(Kp, BadSignRatio, float, 0.05); //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
|
||||
RTABMAP_PARAM(Kp, DescriptorStrategy, int, 0); // kDescriptorSurf=0, kDescriptorColorSurf, kDescriptorLaplacianSurf, kDescriptorSift, kDescriptorHueSurf, kDescriptorUndef
|
||||
RTABMAP_PARAM(Kp, UsingAdaptiveResponseThr, bool, false);
|
||||
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
|
||||
|
||||
//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(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, JournalMode, int, 0); // 0=DELETE, 1=TRUNCATE, 2=PERSIST, 3=MEMORY, 4=OFF (see sqlite3 doc : "PRAGMA journal_mode")
|
||||
|
||||
RTABMAP_PARAM(SURF, Extended, bool, false); // true=128, false=64
|
||||
RTABMAP_PARAM(SURF, HessianThreshold, float, 100.0);
|
||||
RTABMAP_PARAM(SURF, Octaves, int, 4);
|
||||
RTABMAP_PARAM(SURF, OctaveLayers, int, 2);
|
||||
RTABMAP_PARAM(SURF, GpuVersion, bool, false);
|
||||
RTABMAP_PARAM(SURF, Upright, bool, false); // U-SURF
|
||||
|
||||
RTABMAP_PARAM(SIFT, Threshold, double, 0.006667); // true=128, false=64
|
||||
RTABMAP_PARAM(SIFT, EdgeThreshold, double, 10.0);
|
||||
|
||||
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);
|
||||
|
||||
// BayesFilter
|
||||
RTABMAP_PARAM(Bayes, VirtualPlacePriorThr, float, 0.9); // Virtual place prior
|
||||
RTABMAP_PARAM_STR(Bayes, PredictionLC, "0.1 0.24 0.18 0.1 0.04 0.01"); // Prediction of loop closures (Gaussian-like, must be pair size) - Format: {VirtualPlaceProb, LoopClosureProb, BackwardNeighborLvl1, ForwardNeighborLvl1, BackwardNeighborLvl2, ForwardNeighborLvl2, ...}
|
||||
|
||||
// Verify hypotheses
|
||||
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();
|
||||
|
||||
private:
|
||||
Parameters();
|
||||
static Parameters * getInstance();
|
||||
const ParametersMap & getParameters() const;
|
||||
void addParameter(const std::string & key, const std::string & value);
|
||||
static std::string getDefaultWorkingDirectory();
|
||||
|
||||
private:
|
||||
static Parameters * instance_;
|
||||
static UDestroyer<Parameters> destroyer_;
|
||||
static ParametersMap parameters_;
|
||||
};
|
||||
|
||||
/**
|
||||
* The parameters event. This event is used to send
|
||||
* parameters across the threads.
|
||||
*/
|
||||
class ParamEvent : public UEvent
|
||||
{
|
||||
public:
|
||||
ParamEvent(const ParametersMap & parameters) : UEvent(0), parameters_(parameters) {}
|
||||
ParamEvent(const std::string & parameterKey, const std::string & parameterValue) : UEvent(0)
|
||||
{
|
||||
parameters_.insert(std::pair<std::string, std::string>(parameterKey, parameterValue));
|
||||
}
|
||||
~ParamEvent() {}
|
||||
virtual std::string getClassName() const {return "ParamEvent";}
|
||||
|
||||
const ParametersMap & getParameters() const {return parameters_;}
|
||||
|
||||
private:
|
||||
ParametersMap parameters_; /**< The parameters map (key,value). */
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#endif /* PARAMETERS_H_ */
|
||||
|
||||
165
corelib/include/rtabmap/core/Rtabmap.h
Normal file
165
corelib/include/rtabmap/core/Rtabmap.h
Normal file
@@ -0,0 +1,165 @@
|
||||
/*
|
||||
* 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 CTABMAP_H_
|
||||
#define CTABMAP_H_
|
||||
|
||||
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
|
||||
|
||||
#include "utilite/UThreadNode.h"
|
||||
#include "utilite/UEventsHandler.h"
|
||||
#include "utilite/USemaphore.h"
|
||||
#include "utilite/UMutex.h"
|
||||
#include "utilite/UVariant.h"
|
||||
#include "rtabmap/core/RtabmapEvent.h"
|
||||
#include "rtabmap/core/Parameters.h"
|
||||
#include <opencv2/core/core.hpp>
|
||||
#include <list>
|
||||
#include <stack>
|
||||
#include <set>
|
||||
|
||||
namespace rtabmap
|
||||
{
|
||||
|
||||
class Signature;
|
||||
|
||||
class VerifyHypotheses;
|
||||
class Memory;
|
||||
class BayesFilter;
|
||||
class SMState;
|
||||
|
||||
class RTABMAP_EXP Rtabmap :
|
||||
public UThreadNode,
|
||||
public UEventsHandler
|
||||
{
|
||||
public:
|
||||
enum State {
|
||||
kStateIdle,
|
||||
kStateDetecting,
|
||||
kStateReseting,
|
||||
kStateChangingParameters,
|
||||
kStateDumpingMemory,
|
||||
kStateDumpingPrediction,
|
||||
kStateGeneratingGraph,
|
||||
kStateDeletingMemory
|
||||
};
|
||||
|
||||
enum VhStrategy {kVhSimple, kVhEpipolar, kVhUndef};
|
||||
|
||||
static const char * kDefaultIniFileName;
|
||||
static const char * kDefaultIniFilePath;
|
||||
|
||||
public:
|
||||
static std::string getVersion();
|
||||
static std::string getIniFilePath();
|
||||
static void readParameters(const char * configFile, ParametersMap & parameters);
|
||||
static void writeParameters(const char * configFile, const ParametersMap & parameters);
|
||||
|
||||
public:
|
||||
Rtabmap();
|
||||
virtual ~Rtabmap();
|
||||
|
||||
void process(SMState * data);
|
||||
void dumpData();
|
||||
|
||||
void init(const ParametersMap & param);
|
||||
void init(const char * configFile = 0);
|
||||
|
||||
const std::string & getWorkingDir() const {return _wDir;}
|
||||
int getLoopClosureId() const;
|
||||
int getLastSignatureId() const;
|
||||
const std::list<std::vector<float> > & getActions() const {return _actions;}
|
||||
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 setReactivationDisabled(bool reactivationDisabled);
|
||||
void setMaxTimeAllowed(float maxTimeAllowed); // in sec
|
||||
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;
|
||||
void selectHypotheses(const std::map<int, float> & posterior,
|
||||
std::list<std::pair<int, float> > & hypotheses,
|
||||
bool useNeighborSum) const;
|
||||
|
||||
protected:
|
||||
virtual void handleEvent(UEvent * anEvent);
|
||||
|
||||
private:
|
||||
virtual void mainLoop();
|
||||
virtual void killCleanup();
|
||||
virtual void startInit();
|
||||
void process();
|
||||
void addSMState(SMState * data);
|
||||
SMState * getSMState();
|
||||
void setupLogFiles();
|
||||
void releaseAllStrategies();
|
||||
void pushNewState(State newState, const ParametersMap & parameters = ParametersMap());
|
||||
void dumpPrediction() const;
|
||||
void parseParameters(const ParametersMap & parameters);
|
||||
|
||||
private:
|
||||
// Modifiable parameters
|
||||
bool _publishStats;
|
||||
bool _reactivationDisabled;
|
||||
float _maxTimeAllowed; // in sec
|
||||
int _smStateBufferMaxSize;
|
||||
unsigned int _minMemorySizeForLoopDetection;
|
||||
float _loopThr;
|
||||
float _loopRatio;
|
||||
float _remThr;
|
||||
bool _localGraphCleaned;
|
||||
unsigned int _maxRetrieved;
|
||||
|
||||
int _lcHypothesisId;
|
||||
int _reactivateId;
|
||||
float _highestHypothesisValue;
|
||||
unsigned int _spreadMargin;
|
||||
int _lastLoopClosureId;
|
||||
std::list<std::vector<float> > _actions;
|
||||
|
||||
UMutex _stateMutex;
|
||||
std::stack<State> _state;
|
||||
std::stack<ParametersMap> _stateParam;
|
||||
|
||||
std::list<SMState *> _smStateBuffer;
|
||||
UMutex _smStateBufferMutex;
|
||||
USemaphore _newSMStateSem;
|
||||
|
||||
// Abstract classes containing all loop closure
|
||||
// strategies for a type of signature or configuration.
|
||||
VerifyHypotheses * _vhStrategy;
|
||||
BayesFilter * _bayesFilter;
|
||||
|
||||
Memory * _memory;
|
||||
|
||||
FILE* _foutFloat;
|
||||
FILE* _foutInt;
|
||||
|
||||
std::string _wDir;
|
||||
std::string _graphFileName;
|
||||
};
|
||||
|
||||
#endif /* CTABMAP_H_ */
|
||||
|
||||
} // namespace rtabmap
|
||||
233
corelib/include/rtabmap/core/RtabmapEvent.h
Normal file
233
corelib/include/rtabmap/core/RtabmapEvent.h
Normal file
@@ -0,0 +1,233 @@
|
||||
/*
|
||||
* 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 RTABMAPEVENT_H_
|
||||
#define RTABMAPEVENT_H_
|
||||
|
||||
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
|
||||
|
||||
#include "utilite/UEvent.h"
|
||||
#include <opencv2/core/core.hpp>
|
||||
#include <opencv2/features2d/features2d.hpp>
|
||||
#include <opencv2/imgproc/imgproc.hpp>
|
||||
#include "utilite/ULogger.h"
|
||||
#include <list>
|
||||
#include <vector>
|
||||
|
||||
namespace rtabmap
|
||||
{
|
||||
|
||||
#define RTABMAP_STATS(PREFIX, NAME, UNIT) \
|
||||
public: \
|
||||
static std::string k##PREFIX##NAME() {return #PREFIX "/" #NAME "/" #UNIT;} \
|
||||
private: \
|
||||
class Dummy##PREFIX##NAME { \
|
||||
public: \
|
||||
Dummy##PREFIX##NAME() {if(!_defaultDataInitialized)_defaultData.insert(std::pair<std::string, float>(#PREFIX "/" #NAME "/" #UNIT, 0.0f));} \
|
||||
}; \
|
||||
Dummy##PREFIX##NAME dummy##PREFIX##NAME;
|
||||
|
||||
class RTABMAP_EXP Statistics
|
||||
{
|
||||
RTABMAP_STATS(Loop, Closure_id,);
|
||||
RTABMAP_STATS(Loop, Rejected_reason,);
|
||||
RTABMAP_STATS(Loop, Highest_hypothesis_id,);
|
||||
RTABMAP_STATS(Loop, Highest_hypothesis_value,);
|
||||
RTABMAP_STATS(Loop, Vp_likelihood,);
|
||||
RTABMAP_STATS(Loop, ReactivateId,);
|
||||
RTABMAP_STATS(Loop, Hypothesis_ratio,);
|
||||
|
||||
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_reactivated,);
|
||||
RTABMAP_STATS(Memory, Images_buffered,);
|
||||
|
||||
RTABMAP_STATS(Timing, Memory_update, ms);
|
||||
RTABMAP_STATS(Timing, Cleaning_neighbors, ms);
|
||||
RTABMAP_STATS(Timing, Reactivation, 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, Statistics_creation, ms);
|
||||
RTABMAP_STATS(Timing, Memory_cleanup, ms);
|
||||
RTABMAP_STATS(Timing, Total, ms);
|
||||
RTABMAP_STATS(Timing, Forgetting, ms);
|
||||
RTABMAP_STATS(Timing, Emptying_memory_trash, ms);
|
||||
|
||||
RTABMAP_STATS(, Parent_id,);
|
||||
RTABMAP_STATS(, Hypothesis_reactivated,);
|
||||
|
||||
RTABMAP_STATS(Keypoint, Dictionary_size, words);
|
||||
RTABMAP_STATS(Keypoint, Response_threshold,);
|
||||
|
||||
public:
|
||||
static const std::map<std::string, float> & defaultData();
|
||||
|
||||
public:
|
||||
Statistics();
|
||||
Statistics(const Statistics & s);
|
||||
virtual ~Statistics();
|
||||
|
||||
// name format = "Grp/Name/unit"
|
||||
void addStatistic(const std::string & name, float value);
|
||||
|
||||
// setters
|
||||
void setExtended(bool extended) {_extended = extended;}
|
||||
void setRefImageId(int refImageId) {_refImageId = refImageId;}
|
||||
void setLoopClosureId(int loopClosureId) {_loopClosureId = loopClosureId;}
|
||||
void setActions(const std::list<std::vector<float> > & actions) {_actions = actions;}
|
||||
void setRefImage(IplImage ** refImage);
|
||||
void setRefImage(const IplImage * refImage);
|
||||
void setLoopClosureImage(IplImage ** loopClosureImage);
|
||||
void setLoopClosureImage(const IplImage * loopClosureImage);
|
||||
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 setRefWords(const std::multimap<int, cv::KeyPoint> & refWords) {_refWords = refWords;}
|
||||
void setLoopWords(const std::multimap<int, cv::KeyPoint> & loopWords) {_loopWords = loopWords;}
|
||||
|
||||
// getters
|
||||
bool extended() const {return _extended;}
|
||||
int refImageId() const {return _refImageId;}
|
||||
int loopClosureId() const {return _loopClosureId;}
|
||||
const std::list<std::vector<float> > & getActions() const {return _actions;}
|
||||
const IplImage * refImage() const {return _refImage;}
|
||||
const IplImage * loopClosureImage() const {return _loopClosureImage;}
|
||||
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::multimap<int, cv::KeyPoint> & refWords() const {return _refWords;}
|
||||
const std::multimap<int, cv::KeyPoint> & loopWords() const {return _loopWords;}
|
||||
const std::map<std::string, float> & data() const {return _data;}
|
||||
|
||||
|
||||
Statistics & operator=(const Statistics & s);
|
||||
|
||||
private:
|
||||
int _extended; // 0 -> only loop closure and last signature ID fields are filled
|
||||
|
||||
int _refImageId;
|
||||
int _loopClosureId;
|
||||
|
||||
std::list<std::vector<float> > _actions;
|
||||
|
||||
// extended data start here...
|
||||
IplImage * _refImage; // Released by the event destructor
|
||||
IplImage * _loopClosureImage; // Released by the event destructor
|
||||
|
||||
std::map<int, int> _weights;
|
||||
std::map<int, float> _posterior;
|
||||
std::map<int, float> _likelihood;
|
||||
|
||||
//surf
|
||||
std::multimap<int, cv::KeyPoint> _refWords;
|
||||
std::multimap<int, cv::KeyPoint> _loopWords;
|
||||
|
||||
// Format for statistics (Plottable statistics must go in that map) :
|
||||
// {"Group/Name/Unit", value}
|
||||
// Example : {"Timing/Total time/ms", 500.0f}
|
||||
std::map<std::string, float> _data;
|
||||
static std::map<std::string, float> _defaultData;
|
||||
static bool _defaultDataInitialized;
|
||||
// end extended data
|
||||
};
|
||||
|
||||
|
||||
////////// The RtabmapEvent class //////////////
|
||||
class RtabmapEvent : public UEvent
|
||||
{
|
||||
public:
|
||||
RtabmapEvent(Statistics ** stats) :
|
||||
UEvent(0),
|
||||
_stats(*stats) {}
|
||||
|
||||
virtual ~RtabmapEvent() {if(_stats) delete _stats;}
|
||||
const Statistics & getStats() const {return *_stats;}
|
||||
virtual std::string getClassName() const {return std::string("RtabmapEvent");}
|
||||
|
||||
private:
|
||||
Statistics * _stats;
|
||||
};
|
||||
|
||||
class RtabmapEventCmd : public UEvent
|
||||
{
|
||||
public:
|
||||
enum Cmd {
|
||||
kCmdResetMemory,
|
||||
kCmdDumpMemory,
|
||||
kCmdDumpPrediction,
|
||||
kCmdGenerateGraph,
|
||||
kCmdDeleteMemory};
|
||||
public:
|
||||
RtabmapEventCmd(Cmd cmd) :
|
||||
UEvent(0),
|
||||
_cmd(cmd) {}
|
||||
|
||||
virtual ~RtabmapEventCmd() {}
|
||||
Cmd getCmd() const {return _cmd;}
|
||||
void setStr(const std::string & str) {_str = str;}
|
||||
const std::string & getStr() const {return _str;}
|
||||
virtual std::string getClassName() const {return std::string("RtabmapEventCmd");}
|
||||
|
||||
private:
|
||||
Cmd _cmd;
|
||||
std::string _str;
|
||||
};
|
||||
|
||||
class RtabmapEventInit : public UEvent
|
||||
{
|
||||
public:
|
||||
enum Status {
|
||||
kInitializing,
|
||||
kInitialized,
|
||||
kInfo,
|
||||
kError
|
||||
};
|
||||
|
||||
public:
|
||||
RtabmapEventInit(Status status, const std::string & info = std::string()) :
|
||||
UEvent(0),
|
||||
_status(status),
|
||||
_info(info)
|
||||
{}
|
||||
|
||||
// for convenience
|
||||
RtabmapEventInit(const std::string & info) :
|
||||
UEvent(0),
|
||||
_status(kInfo),
|
||||
_info(info)
|
||||
{}
|
||||
|
||||
Status getStatus() const {return _status;}
|
||||
const std::string & getInfo() const {return _info;}
|
||||
|
||||
virtual ~RtabmapEventInit() {}
|
||||
virtual std::string getClassName() const {return std::string("RtabmapEventInit");}
|
||||
private:
|
||||
Status _status;
|
||||
std::string _info; // "Loading signatures", "Loading words" ...
|
||||
};
|
||||
|
||||
} // namespace rtabmap
|
||||
|
||||
#endif /* RTABMAPEVENT_H_ */
|
||||
37
corelib/include/rtabmap/core/RtabmapExp.h
Normal file
37
corelib/include/rtabmap/core/RtabmapExp.h
Normal file
@@ -0,0 +1,37 @@
|
||||
/*
|
||||
* 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 RTABMAPEXP_H
|
||||
#define RTABMAPEXP_H
|
||||
|
||||
#ifdef WIN32
|
||||
#ifdef RTABMAP_EXPORTS
|
||||
#define RTABMAP_EXP __declspec( dllexport )
|
||||
#else
|
||||
#ifdef RTABMAP_EXPORTS_STATIC
|
||||
#define RTABMAP_EXP
|
||||
#else
|
||||
#define RTABMAP_EXP __declspec( dllimport )
|
||||
#endif
|
||||
#endif
|
||||
#else
|
||||
#define RTABMAP_EXP
|
||||
#endif
|
||||
|
||||
#endif // RTABMAPEXP_H
|
||||
97
corelib/include/rtabmap/core/SMState.h
Normal file
97
corelib/include/rtabmap/core/SMState.h
Normal file
@@ -0,0 +1,97 @@
|
||||
/*
|
||||
* 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 SMPAIRVARIANT_H_
|
||||
#define SMPAIRVARIANT_H_
|
||||
|
||||
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
|
||||
|
||||
#include <opencv2/core/core.hpp>
|
||||
#include <opencv2/features2d/features2d.hpp>
|
||||
#include <list>
|
||||
#include <vector>
|
||||
|
||||
namespace rtabmap {
|
||||
|
||||
// SensoriMotor state
|
||||
class SMState
|
||||
{
|
||||
public:
|
||||
// Constructor 1
|
||||
// image and/or keypoints can be passed for debugging (rtabmap will not re-extract keypoints/descriptors from the image if not null, only for debug/visualization)
|
||||
// take image ownership
|
||||
SMState(const std::list<std::vector<float> > & sensorStates, const std::list<std::vector<float> > & actuatorStates, IplImage * image = 0, const std::list<cv::KeyPoint> & keypoints = std::list<cv::KeyPoint>()) :
|
||||
_sensorStates(sensorStates),
|
||||
_actuatorStates(actuatorStates),
|
||||
_image(image),
|
||||
_keypoints(keypoints),
|
||||
_descriptorsProvided(true)
|
||||
{}
|
||||
// Constructor 2 :
|
||||
// rtabmap will automatically extract keypoints and descriptors from the image...
|
||||
// take image ownership
|
||||
SMState(IplImage * image, const std::list<std::vector<float> > & actuatorStates = std::list<std::vector<float> >()) :
|
||||
_actuatorStates(actuatorStates),
|
||||
_image(image),
|
||||
_descriptorsProvided(false)
|
||||
{}
|
||||
virtual ~SMState()
|
||||
{
|
||||
if(_image)
|
||||
cvReleaseImage(&_image);
|
||||
}
|
||||
|
||||
bool isDescriptorsProvided() const {return _descriptorsProvided;}
|
||||
const IplImage * getImage() const {return _image;}
|
||||
const std::list<cv::KeyPoint> & getKeypoints() const {return _keypoints;}
|
||||
const std::list<std::vector<float> > & getSensorStates() const {return _sensorStates;}
|
||||
const std::list<std::vector<float> > & getActuatorStates() const {return _actuatorStates;}
|
||||
|
||||
void setSensorStates(const std::list<std::vector<float> > & sensorStates) {_sensorStates=sensorStates;}
|
||||
void setActuatorStates(const std::list<std::vector<float> > & actuatorStates) {_actuatorStates=actuatorStates;}
|
||||
|
||||
private:
|
||||
std::list<std::vector<float> > _sensorStates; // descriptors
|
||||
std::list<std::vector<float> > _actuatorStates;
|
||||
IplImage * _image;
|
||||
std::list<cv::KeyPoint> _keypoints;
|
||||
bool _descriptorsProvided;
|
||||
};
|
||||
|
||||
// Sensorimotor state event
|
||||
// Take ownership of the state
|
||||
class SMStateEvent : public UEvent
|
||||
{
|
||||
public:
|
||||
SMStateEvent(SMState * state) :
|
||||
UEvent(0),
|
||||
_state(state) {}
|
||||
|
||||
virtual ~SMStateEvent() {if(_state) delete _state;}
|
||||
const SMState * getData() const {return _state;}
|
||||
SMState * getDataOwnership() {SMState * state = _state; _state=0; return state;}
|
||||
virtual std::string getClassName() const {return "SMStateEvent";} // TODO : macro?
|
||||
|
||||
private:
|
||||
SMState * _state;
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#endif /* SMPAIRVARIANT_H_ */
|
||||
Reference in New Issue
Block a user