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

Rtabmap: Refactoring... removed all threading/events related stuff from Rtabmap object to RtabmapThread object (a thread wrapper of the rtabmap object)

Browse Source 
git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@803 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2013-02-03 22:12:02 +00:00
parent 916bafeba1
commit e8e75a9d40
11 changed files with 660 additions and 650 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -22,14 +22,14 @@
#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 <utilite/USemaphore.h>
#include <utilite/UMutex.h>
#include <utilite/UVariant.h>
#include "rtabmap/core/Parameters.h"
#include "rtabmap/core/Image.h"
#include "rtabmap/core/Statistics.h"
#include <opencv2/core/core.hpp>
#include <list>
#include <stack>
@@ -42,26 +42,10 @@ class EpipolarGeometry;
class Memory;
class BayesFilter;
class RTABMAP_EXP Rtabmap :
public UThreadNode,
public UEventsHandler
class RTABMAP_EXP Rtabmap
{
public:
enum State {
kStateIdle,
kStateDetecting,
kStateReseting,
kStateChangingParameters,
kStateDumpingMemory,
kStateDumpingPrediction,
kStateGeneratingGraph,
kStateGeneratingLocalGraph,
kStateDeletingMemory,
kStateCleanSensorsBuffer
};
enum VhStrategy {kVhNone, kVhEpipolar, kVhUndef};
static const char * kDefaultDatabaseName;
public:
@@ -78,34 +62,33 @@ public:
void init(const ParametersMap & param, bool deleteMemory = true);
void init(const std::string & configFile = "", bool deleteMemory = true);
void clearBufferedSensors();
void close();
const std::string & getWorkingDir() const {return _wDir;}
int getLoopClosureId() const;
int getRetrievedId() const;
int getLastLocationId();
int getLastLocationId() const;
float getLcHypValue() const {return _lcHypothesisValue;}
std::list<int> getWM(); // working memory
std::set<int> getSTM(); // short-term memory
int getWMSize(); // working memory size
int getSTMSize(); // short-term memory size
std::map<int, int> getWeights();
int getTotalMemSize();
std::list<int> getWM() const; // working memory
std::set<int> getSTM() const; // short-term memory
int getWMSize() const; // working memory size
int getSTMSize() const; // short-term memory size
std::map<int, int> getWeights() const;
int getTotalMemSize() const;
double getLastProcessTime() const {return _lastProcessTime;};
std::multimap<int, cv::KeyPoint> getWords(int locationId);
std::map<int, int> getNeighbors(int nodeId, int margin, bool lookInLTM = false);
bool isInSTM(int locationId);
Statistics getStatistics();
std::multimap<int, cv::KeyPoint> getWords(int locationId) const;
std::map<int, int> getNeighbors(int nodeId, int margin, bool lookInLTM = false) const;
bool isInSTM(int locationId) const;
const Statistics & getStatistics() const;
void setTimeThreshold(float maxTimeAllowed); // in ms
void generateGraph(const std::string & path);
void generateGraph(const std::string & path, int id=0, int margin=5);
void resetMemory(bool dbOverwritten = false);
void dumpPrediction();
void dumpData();
void updateParameters(const ParametersMap & parameters);
void dumpPrediction() const;
void dumpData() const;
void parseParameters(const ParametersMap & parameters);
void setWorkingDirectory(std::string path);
void deleteLastLocation();
void rejectLastLoopClosure();
@@ -114,23 +97,9 @@ public:
std::pair<int, float> selectHypothesis(const std::map<int, float> & posterior,
const std::map<int, float> & likelihood) const;
protected:
virtual void handleEvent(UEvent * anEvent);
private:
virtual void mainLoop();
virtual void mainLoopKill();
virtual void mainLoopBegin();
void process();
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 generateLocalGraph(const std::string & path, int id, int margin);
void parseParameters(const ParametersMap & parameters);
void setDataBufferSize(int size);
private:
// Modifiable parameters
@@ -141,7 +110,6 @@ private:
bool _publishKeypoints;
float _maxTimeAllowed; // in ms
unsigned int _maxMemoryAllowed; // signatures count in WM
int _imageBufferMaxSize;
float _loopThr;
float _loopRatio;
unsigned int _maxRetrieved;
@@ -153,16 +121,6 @@ private:
int _retrievedId;
double _lastProcessTime;
UMutex _stateMutex;
std::stack<State> _state;
std::stack<ParametersMap> _stateParam;
std::list<Image> _imageBuffer;
UMutex _imageMutex;
USemaphore _imageAdded;
UMutex _threadMutex;
// Abstract classes containing all loop closure
// strategies for a type of signature or configuration.
EpipolarGeometry * _epipolarGeometry;

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

@@ -20,131 +20,14 @@
#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>
#include "rtabmap/core/Statistics.h"
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, RejectedHypothesis,);
RTABMAP_STATS(Loop, Highest_hypothesis_id,);
RTABMAP_STATS(Loop, Highest_hypothesis_value,);
RTABMAP_STATS(Loop, Vp_hypothesis,);
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, Signatures_removed,);
RTABMAP_STATS(Memory, Signatures_retrieved,);
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, 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, Statistics_creation, ms);
RTABMAP_STATS(Timing, Memory_cleanup, ms);
RTABMAP_STATS(Timing, Total, ms);
RTABMAP_STATS(Timing, Forgetting, ms);
RTABMAP_STATS(Timing, Joining_trash, ms);
RTABMAP_STATS(Timing, Emptying_trash, ms);
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();
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 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;}
// getters
bool extended() const {return _extended;}
int refImageId() const {return _refImageId;}
int loopClosureId() const {return _loopClosureId;}
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::map<std::string, float> & data() const {return _data;}
private:
int _extended; // 0 -> only loop closure and last signature ID fields are filled
int _refImageId;
int _loopClosureId;
// extended data start here...
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;
// 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
{

89
corelib/include/rtabmap/core/RtabmapThread.h Normal file
View File

@@ -0,0 +1,89 @@
/*
* 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 RTABMAPTHREAD_H_
#define RTABMAPTHREAD_H_
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
#include <utilite/UThreadNode.h>
#include <utilite/UEventsHandler.h>
#include "rtabmap/core/RtabmapEvent.h"
#include "rtabmap/core/Image.h"
#include "rtabmap/core/Parameters.h"
#include <stack>
namespace rtabmap {
class Rtabmap;
class RTABMAP_EXP RtabmapThread :
public UThreadNode,
public UEventsHandler
{
public:
enum State {
kStateIdle,
kStateDetecting,
kStateReseting,
kStateChangingParameters,
kStateDumpingMemory,
kStateDumpingPrediction,
kStateGeneratingGraph,
kStateGeneratingLocalGraph,
kStateDeletingMemory,
kStateCleanSensorsBuffer
};
public:
RtabmapThread();
virtual ~RtabmapThread();
void setWorkingDirectory(const std::string & path);
void clearBufferedSensors();
protected:
virtual void handleEvent(UEvent * anEvent);
private:
virtual void mainLoop();
virtual void mainLoopKill();
void process();
void addImage(const Image & image);
void getImage(Image & image);
void pushNewState(State newState, const ParametersMap & parameters = ParametersMap());
void setDataBufferSize(int size);
private:
UMutex _stateMutex;
std::stack<State> _state;
std::stack<ParametersMap> _stateParam;
std::list<Image> _imageBuffer;
UMutex _imageMutex;
USemaphore _imageAdded;
int _imageBufferMaxSize;
Rtabmap * _rtabmap;
};
} /* namespace rtabmap */
#endif /* RTABMAPTHREAD_H_ */

146
corelib/include/rtabmap/core/Statistics.h Normal file
View File

@@ -0,0 +1,146 @@
/*
* 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 STATISTICS_H_
#define STATISTICS_H_
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#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, RejectedHypothesis,);
RTABMAP_STATS(Loop, Highest_hypothesis_id,);
RTABMAP_STATS(Loop, Highest_hypothesis_value,);
RTABMAP_STATS(Loop, Vp_hypothesis,);
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, Signatures_removed,);
RTABMAP_STATS(Memory, Signatures_retrieved,);
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, 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, Statistics_creation, ms);
RTABMAP_STATS(Timing, Memory_cleanup, ms);
RTABMAP_STATS(Timing, Total, ms);
RTABMAP_STATS(Timing, Forgetting, ms);
RTABMAP_STATS(Timing, Joining_trash, ms);
RTABMAP_STATS(Timing, Emptying_trash, ms);
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();
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 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;}
// getters
bool extended() const {return _extended;}
int refImageId() const {return _refImageId;}
int loopClosureId() const {return _loopClosureId;}
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::map<std::string, float> & data() const {return _data;}
private:
int _extended; // 0 -> only loop closure and last signature ID fields are filled
int _refImageId;
int _loopClosureId;
// extended data start here...
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;
// 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
};
}// end namespace rtabmap
#endif /* STATISTICS_H_ */