i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 01:20:25 +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_ */

4
corelib/src/CMakeLists.txt
View File

@@ -1,7 +1,9 @@
SET(SRC_FILES
Rtabmap.cpp
RtabmapEvent.cpp
RtabmapThread.cpp
Statistics.cpp
Memory.cpp

3
corelib/src/Memory.cpp
View File

@@ -68,6 +68,8 @@ Memory::Memory(const ParametersMap & parameters) :
bool Memory::init(const std::string & dbUrl, bool dbOverwritten, const ParametersMap & parameters)
{
UEventsManager::post(new RtabmapEventInit(RtabmapEventInit::kInitializing));
ULOGGER_DEBUG("");
this->parseParameters(parameters);
@@ -188,6 +190,7 @@ bool Memory::init(const std::string & dbUrl, bool dbOverwritten, const Parameter
}
ULOGGER_DEBUG("Total word references added = %d", _vwd->getTotalActiveReferences());
UEventsManager::post(new RtabmapEventInit(RtabmapEventInit::kInitialized));
return success;
}

561
corelib/src/Rtabmap.cpp
View File

@@ -18,11 +18,9 @@
*/
#include "rtabmap/core/Rtabmap.h"
#include "rtabmap/core/RtabmapEvent.h"
#include "rtabmap/core/Version.h"
#include "rtabmap/core/Features2d.h"
#include "rtabmap/core/Camera.h"
#include "Signature.h"
#include "rtabmap/core/EpipolarGeometry.h"
@@ -30,7 +28,11 @@
#include "rtabmap/core/Memory.h"
#include "BayesFilter.h"
#include <utilite/UtiLite.h>
#include <utilite/ULogger.h>
#include <utilite/UFile.h>
#include <utilite/UTimer.h>
#include <utilite/UConversion.h>
#include <utilite/UMath.h>
#include "SimpleIni.h"
@@ -64,7 +66,6 @@ Rtabmap::Rtabmap() :
_publishKeypoints(Parameters::defaultKpPublishKeypoints()),
_maxTimeAllowed(Parameters::defaultRtabmapTimeThr()), // 700 ms
_maxMemoryAllowed(Parameters::defaultRtabmapMemoryThr()), // 0=inf
_imageBufferMaxSize(Parameters::defaultRtabmapImageBufferSize()),
_loopThr(Parameters::defaultRtabmapLoopThr()),
_loopRatio(Parameters::defaultRtabmapLoopRatio()),
_maxRetrieved(Parameters::defaultRtabmapMaxRetrieved()),
@@ -200,9 +201,52 @@ void Rtabmap::flushStatisticLogs()
}
}
void Rtabmap::releaseAllStrategies()
void Rtabmap::init(const ParametersMap & parameters, bool deleteMemory)
{
ULOGGER_DEBUG("");
if(deleteMemory)
{
// Set the working directory before
ParametersMap::const_iterator iter;
if((iter=parameters.find(Parameters::kRtabmapWorkingDirectory())) != parameters.end())
{
this->setWorkingDirectory(iter->second.c_str());
}
this->resetMemory(true);
}
this->parseParameters(parameters);
setupLogFiles();
}
void Rtabmap::init(const std::string & configFile, bool deleteMemory)
{
// fill ctrl struct with values from the configuration file
ParametersMap param;// = Parameters::defaultParameters;
if(!configFile.empty())
{
ULOGGER_DEBUG("Read parameters from = %s", configFile.c_str());
this->readParameters(configFile, param);
}
this->init(param, deleteMemory);
}
void Rtabmap::close()
{
flushStatisticLogs();
if(_foutFloat)
{
fclose(_foutFloat);
_foutFloat = 0;
}
if(_foutInt)
{
fclose(_foutInt);
_foutInt = 0;
}
if(_epipolarGeometry)
{
delete _epipolarGeometry;
@@ -220,86 +264,6 @@ void Rtabmap::releaseAllStrategies()
}
}
void Rtabmap::pushNewState(State newState, const ParametersMap & parameters)
{
ULOGGER_DEBUG("to %d", newState);
_stateMutex.lock();
{
_state.push(newState);
_stateParam.push(parameters);
}
_stateMutex.unlock();
_imageAdded.release();
}
void Rtabmap::init(const ParametersMap & parameters, bool deleteMemory)
{
UScopeMutex s(&_threadMutex);
if(this->isRunning())
{
pushNewState(kStateChangingParameters, parameters);
if(deleteMemory)
{
pushNewState(kStateDeletingMemory);
}
}
else
{
if(deleteMemory)
{
// Set the working directory before
ParametersMap::const_iterator iter;
if((iter=parameters.find(Parameters::kRtabmapWorkingDirectory())) != parameters.end())
{
this->setWorkingDirectory(iter->second.c_str());
}
this->resetMemory(true);
}
this->parseParameters(parameters);
}
setupLogFiles();
}
void Rtabmap::init(const std::string & configFile, bool deleteMemory)
{
UScopeMutex s(&_threadMutex);
// fill ctrl struct with values from the configuration file
ParametersMap param;// = Parameters::defaultParameters;
if(!configFile.empty())
{
ULOGGER_DEBUG("Read parameters from = %s", configFile.c_str());
this->readParameters(configFile, param);
}
this->init(param, deleteMemory);
}
void Rtabmap::close()
{
UEventsManager::removeHandler(this);
// Stop the thread first
join(true);
flushStatisticLogs();
if(_foutFloat)
{
fclose(_foutFloat);
_foutFloat = 0;
}
if(_foutInt)
{
fclose(_foutInt);
_foutInt = 0;
}
this->releaseAllStrategies();
}
void Rtabmap::parseParameters(const ParametersMap & parameters)
{
ULOGGER_DEBUG("");
@@ -340,10 +304,6 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
{
_loopRatio = std::atof(iter->second.c_str());
}
if((iter=parameters.find(Parameters::kRtabmapImageBufferSize())) != parameters.end())
{
_imageBufferMaxSize = std::atoi(iter->second.c_str());
}
if((iter=parameters.find(Parameters::kRtabmapWorkingDirectory())) != parameters.end())
{
this->setWorkingDirectory(iter->second.c_str());
@@ -366,12 +326,8 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
if(!_memory)
{
UEventsManager::post(new RtabmapEventInit(RtabmapEventInit::kInitializing));
UEventsManager::post(new RtabmapEventInit("Creating memory..."));
_memory = new Memory(parameters);
UEventsManager::post(new RtabmapEventInit("Creating memory, done!"));
_memory->init(_wDir + kDefaultDatabaseName, false, parameters);
UEventsManager::post(new RtabmapEventInit(RtabmapEventInit::kInitialized));
}
else
{
@@ -419,9 +375,8 @@ int Rtabmap::getRetrievedId() const
return _retrievedId;
}
int Rtabmap::getLastLocationId()
int Rtabmap::getLastLocationId() const
{
UScopeMutex s(&_threadMutex);
int id = 0;
if(_memory && _memory->getLastSignature())
{
@@ -430,9 +385,8 @@ int Rtabmap::getLastLocationId()
return id;
}
std::list<int> Rtabmap::getWM()
std::list<int> Rtabmap::getWM() const
{
UScopeMutex s(&_threadMutex);
std::list<int> mem;
if(_memory)
{
@@ -442,9 +396,8 @@ std::list<int> Rtabmap::getWM()
return mem;
}
int Rtabmap::getWMSize()
int Rtabmap::getWMSize() const
{
UScopeMutex s(&_threadMutex);
if(_memory)
{
return _memory->getWorkingMem().size()-1; // remove virtual place
@@ -452,9 +405,8 @@ int Rtabmap::getWMSize()
return 0;
}
std::map<int, int> Rtabmap::getWeights()
std::map<int, int> Rtabmap::getWeights() const
{
UScopeMutex s(&_threadMutex);
std::map<int, int> weights;
if(_memory)
{
@@ -464,9 +416,8 @@ std::map<int, int> Rtabmap::getWeights()
return weights;
}
std::set<int> Rtabmap::getSTM()
std::set<int> Rtabmap::getSTM() const
{
UScopeMutex s(&_threadMutex);
if(_memory)
{
return _memory->getStMem();
@@ -474,9 +425,8 @@ std::set<int> Rtabmap::getSTM()
return std::set<int>();
}
int Rtabmap::getSTMSize()
int Rtabmap::getSTMSize() const
{
UScopeMutex s(&_threadMutex);
if(_memory)
{
return _memory->getStMem().size();
@@ -484,9 +434,8 @@ int Rtabmap::getSTMSize()
return 0;
}
int Rtabmap::getTotalMemSize()
int Rtabmap::getTotalMemSize() const
{
UScopeMutex s(&_threadMutex);
if(_memory)
{
const Signature * s =_memory->getLastSignature();
@@ -498,9 +447,8 @@ int Rtabmap::getTotalMemSize()
return 0;
}
std::multimap<int, cv::KeyPoint> Rtabmap::getWords(int locationId)
std::multimap<int, cv::KeyPoint> Rtabmap::getWords(int locationId) const
{
UScopeMutex s(&_threadMutex);
if(_memory)
{
const Signature * s = _memory->getSignature(locationId);
@@ -512,9 +460,8 @@ std::multimap<int, cv::KeyPoint> Rtabmap::getWords(int locationId)
return std::multimap<int, cv::KeyPoint>();
}
std::map<int, int> Rtabmap::getNeighbors(int nodeId, int margin, bool lookInLTM)
std::map<int, int> Rtabmap::getNeighbors(int nodeId, int margin, bool lookInLTM) const
{
UScopeMutex s(&_threadMutex);
if(_memory)
{
return _memory->getNeighborsId(nodeId, margin, lookInLTM?-1:0);
@@ -522,9 +469,8 @@ std::map<int, int> Rtabmap::getNeighbors(int nodeId, int margin, bool lookInLTM)
return std::map<int, int>();
}
bool Rtabmap::isInSTM(int locationId)
bool Rtabmap::isInSTM(int locationId) const
{
UScopeMutex s(&_threadMutex);
if(_memory)
{
return _memory->isInSTM(locationId);
@@ -532,125 +478,48 @@ bool Rtabmap::isInSTM(int locationId)
return false;
}
Statistics Rtabmap::getStatistics()
const Statistics & Rtabmap::getStatistics() const
{
UScopeMutex s(&_threadMutex);
return statistics_;
}
void Rtabmap::clearBufferedSensors()
void Rtabmap::generateGraph(const std::string & path, int id, int margin)
{
_imageMutex.lock();
{
_imageBuffer.clear();
}
_imageMutex.unlock();
}
void Rtabmap::mainLoopBegin()
{
if(!_memory || !_bayesFilter)
{
ULOGGER_DEBUG("Rtabmap thread started without all strategies defined...");
//this->killSafely();
}
}
void Rtabmap::mainLoopKill()
{
this->clearBufferedSensors();
// this will post the newData semaphore
_imageAdded.release();
}
void Rtabmap::mainLoop()
{
UScopeMutex s(&_threadMutex);
State state = kStateDetecting;
ParametersMap parameters;
_stateMutex.lock();
{
if(!_state.empty() && !_stateParam.empty())
{
state = _state.top();
_state.pop();
parameters = _stateParam.top();
_stateParam.pop();
}
}
_stateMutex.unlock();
switch(state)
{
case kStateDetecting:
this->process();
break;
case kStateChangingParameters:
this->parseParameters(parameters);
break;
case kStateReseting:
this->resetMemory();
break;
case kStateDumpingMemory:
if(_memory)
{
_memory->dumpMemory(this->getWorkingDir());
}
break;
case kStateDumpingPrediction:
if(_memory && _bayesFilter)
{
this->dumpPrediction();
}
break;
case kStateGeneratingGraph:
if(_memory)
{
_memory->joinTrashThread(); // make sure the trash is flushed
_memory->generateGraph(parameters.at("path"));
}
break;
case kStateGeneratingLocalGraph:
if(_memory)
{
_memory->joinTrashThread(); // make sure the trash is flushed
this->generateLocalGraph(parameters.at("path"), atoi(parameters.at("id").c_str()), atoi(parameters.at("margin").c_str()));
}
break;
case kStateDeletingMemory:
this->resetMemory(true);
break;
case kStateCleanSensorsBuffer:
this->clearBufferedSensors();
break;
default:
UFATAL("Invalid state !?!?");
break;
}
}
void Rtabmap::generateGraph(const std::string & path)
{
UScopeMutex s(&_threadMutex);
if(_memory)
{
_memory->joinTrashThread(); // make sure the trash is flushed
_memory->generateGraph(path);
if(id > 0)
{
std::map<int, int> ids = _memory->getNeighborsId(id, margin, -1, false);
if(ids.size() > 0)
{
ids.insert(std::pair<int,int>(id, 0));
std::set<int> idsSet;
for(std::map<int, int>::iterator iter = ids.begin(); iter!=ids.end(); ++iter)
{
idsSet.insert(idsSet.end(), iter->first);
}
_memory->generateGraph(path, idsSet);
}
else
{
UERROR("No neighbors found for signature %d.", id);
}
}
else
{
_memory->generateGraph(path);
}
}
}
void Rtabmap::resetMemory(bool dbOverwritten)
{
UScopeMutex s(&_threadMutex);
if(_memory)
{
UEventsManager::post(new RtabmapEventInit(RtabmapEventInit::kInitializing));
_memory->init(_wDir + kDefaultDatabaseName, dbOverwritten);
UEventsManager::post(new RtabmapEventInit(RtabmapEventInit::kInitialized));
if(_bayesFilter)
{
_bayesFilter->reset();
@@ -662,7 +531,6 @@ void Rtabmap::resetMemory(bool dbOverwritten)
// May be memory should be already created here, and use init above...
UFile::erase(_wDir + kDefaultDatabaseName);
}
this->clearBufferedSensors();
_retrievedId = 0;
_lcHypothesisValue = 0;
_lcHypothesisId = 0;
@@ -670,89 +538,10 @@ void Rtabmap::resetMemory(bool dbOverwritten)
this->setupLogFiles(dbOverwritten);
}
void Rtabmap::handleEvent(UEvent* event)
{
if(this->isRunning() && event->getClassName().compare("CameraEvent") == 0)
{
CameraEvent * e = (CameraEvent*)event;
if(e->getCode() == CameraEvent::kCodeImage || e->getCode() == CameraEvent::kCodeFeatures)
{
this->addImage(e->image());
}
}
else if(event->getClassName().compare("RtabmapEventCmd") == 0)
{
RtabmapEventCmd * rtabmapEvent = (RtabmapEventCmd*)event;
RtabmapEventCmd::Cmd cmd = rtabmapEvent->getCmd();
if(cmd == RtabmapEventCmd::kCmdResetMemory)
{
if(_memory)
{
ULOGGER_DEBUG("CMD_RESET_MEMORY");
pushNewState(kStateReseting);
}
}
else if(cmd == RtabmapEventCmd::kCmdDumpMemory)
{
if(_memory)
{
ULOGGER_DEBUG("CMD_DUMP_MEMORY");
pushNewState(kStateDumpingMemory);
}
}
else if(cmd == RtabmapEventCmd::kCmdDumpPrediction)
{
if(_memory && _bayesFilter)
{
ULOGGER_DEBUG("CMD_DUMP_PREDICTION");
pushNewState(kStateDumpingPrediction);
}
}
else if(cmd == RtabmapEventCmd::kCmdGenerateGraph)
{
if(_memory && !rtabmapEvent->getStr().empty())
{
ULOGGER_DEBUG("CMD_GENERATE_GRAPH");
ParametersMap param;
param.insert(ParametersPair("path", rtabmapEvent->getStr()));
pushNewState(kStateGeneratingGraph, param);
}
}
else if(cmd == RtabmapEventCmd::kCmdGenerateLocalGraph)
{
std::list<std::string> values = uSplit(rtabmapEvent->getStr(), ';');
if(_memory && values.size() == 3)
{
ULOGGER_DEBUG("CMD_GENERATE_LOCAL_GRAPH");
ParametersMap param;
param.insert(ParametersPair("path", *values.begin()));
param.insert(ParametersPair("id", *(++values.begin())));
param.insert(ParametersPair("margin", *values.rbegin()));
pushNewState(kStateGeneratingLocalGraph, param);
}
}
else if(cmd == RtabmapEventCmd::kCmdDeleteMemory)
{
ULOGGER_DEBUG("CMD_DELETE_MEMORY");
pushNewState(kStateDeletingMemory);
}
else if(cmd == RtabmapEventCmd::kCmdCleanSensorsBuffer)
{
ULOGGER_DEBUG("CMD_CLEAN_SENSORS_BUFFER");
pushNewState(kStateCleanSensorsBuffer);
}
}
else if(event->getClassName().compare("ParamEvent") == 0)
{
ULOGGER_DEBUG("changing parameters");
pushNewState(kStateChangingParameters, ((ParamEvent*)event)->getParameters());
}
}
//============================================================
// MAIN LOOP
//============================================================
void Rtabmap::process()
void Rtabmap::process(const Image & image)
{
UDEBUG("");
@@ -794,7 +583,6 @@ void Rtabmap::process()
const Signature * signature = 0;
const Signature * sLoop = 0;
Image image;
_lcHypothesisId = 0;
@@ -804,7 +592,6 @@ void Rtabmap::process()
// Wait for an image...
//============================================================
ULOGGER_INFO("getting data...");
this->getImage(image);
if(image.empty())
{
ULOGGER_INFO("image is null...");
@@ -859,11 +646,8 @@ void Rtabmap::process()
rawLikelihood = _memory->computeLikelihood(signature, signaturesToCompare);
// Adjust the likelihood (with mean and std dev)
adjustedLikelihood = rawLikelihood;
this->adjustLikelihood(adjustedLikelihood);
// Apply sensory attention
likelihood = adjustedLikelihood;
likelihood = rawLikelihood;
this->adjustLikelihood(likelihood);
timeLikelihoodCalculation = timer.ticks();
ULOGGER_INFO("timeLikelihoodCalculation=%fs",timeLikelihoodCalculation);
@@ -879,10 +663,7 @@ void Rtabmap::process()
timePosteriorCalculation = timer.ticks();
ULOGGER_INFO("timePosteriorCalculation=%fs",timePosteriorCalculation);
//for(std::map<int,float>::iterator iter=posterior.begin(); iter!=posterior.end(); ++iter)
//{
// UDEBUG("posterior (%d) = %f", iter->first, iter->second);
//}
timer.start();
//============================================================
// Select the highest hypothesis
@@ -1179,7 +960,6 @@ void Rtabmap::process()
statistics_.addStatistic(Statistics::kMemoryWorking_memory_size(), _memory->getWorkingMem().size());
statistics_.addStatistic(Statistics::kMemoryShort_time_memory_size(), _memory->getStMem().size());
statistics_.addStatistic(Statistics::kMemorySignatures_retrieved(), (float)signaturesRetrieved.size());
statistics_.addStatistic(Statistics::kMemoryImages_buffered(), (float)_imageBuffer.size());
// timing...
statistics_.addStatistic(Statistics::kTimingMemory_update(), timeMemoryUpdate*1000);
@@ -1296,36 +1076,6 @@ void Rtabmap::process()
statistics_.addStatistic(Statistics::kTimingMemory_cleanup(), timeMemoryCleanup*1000);
statistics_.addStatistic(Statistics::kMemorySignatures_removed(), signaturesRemoved);
}
ULOGGER_DEBUG("posting statistics_ event...");
this->post(new RtabmapEvent(statistics_));
std::list<float> nonNulls;
float sumLikelihoods = 0.0f;
float maxLikelihood = 0.0f;
float mean = 0.0f;
float stddev = 0.0f;
std::map<int, float>::iterator iter=likelihood.begin();
if(iter->first == -1)
{
++iter;
}
for(; iter!=likelihood.end(); ++iter)
{
if(iter->second > 0)
{
sumLikelihoods += iter->second;
nonNulls.push_back(iter->second);
}
if(iter->second > maxLikelihood)
{
maxLikelihood = iter->second;
}
}
if(nonNulls.size())
{
mean = sumLikelihoods / float(nonNulls.size());
stddev = uStdDev(nonNulls, mean);
}
// Log info...
// TODO : use a specific class which will handle the RtabmapEvent
@@ -1341,10 +1091,10 @@ void Rtabmap::process()
timeStatsCreation,
_lcHypothesisValue,
0.0f,
maxLikelihood,
sumLikelihoods,
mean,
stddev,
0.0f,
0.0f,
0.0f,
0.0f,
vpHypothesis,
timeJoiningTrash,
rehearsalValue,
@@ -1366,7 +1116,7 @@ void Rtabmap::process()
lcHypothesisReactivated,
refUniqueWordsCount,
_retrievedId,
int(nonNulls.size()),
0.0f,
rehearsalMaxId,
rehearsalMaxId>0?1:0);
if(_statisticLogsBufferedInRAM)
@@ -1390,52 +1140,6 @@ void Rtabmap::process()
}
void Rtabmap::addImage(const Image & image)
{
if(image.empty())
{
ULOGGER_ERROR("image empty !?");
return;
}
bool notify = true;
_imageMutex.lock();
{
_imageBuffer.push_back(image);
while(_imageBufferMaxSize > 0 && _imageBuffer.size() > (unsigned int)_imageBufferMaxSize)
{
ULOGGER_WARN("Data buffer is full, the oldest data is removed to add the new one.");
_imageBuffer.pop_front();
notify = false;
}
}
_imageMutex.unlock();
if(notify)
{
_imageAdded.release();
}
}
void Rtabmap::getImage(Image & image)
{
ULOGGER_DEBUG("");
ULOGGER_INFO("waiting for data");
_imageAdded.acquire();
ULOGGER_INFO("wake-up");
_imageMutex.lock();
{
if(!_imageBuffer.empty())
{
image = _imageBuffer.front();
_imageBuffer.pop_front();
}
}
_imageMutex.unlock();
}
// SETTERS
void Rtabmap::setTimeThreshold(float maxTimeAllowed)
{
@@ -1452,22 +1156,8 @@ void Rtabmap::setTimeThreshold(float maxTimeAllowed)
}
}
void Rtabmap::setDataBufferSize(int size)
{
if(size < 0)
{
ULOGGER_WARN("size < 0, then setting it to 0 (inf).");
_imageBufferMaxSize = 0;
}
else
{
_imageBufferMaxSize = size;
}
}
void Rtabmap::setWorkingDirectory(std::string path)
{
UScopeMutex scopeMutex(&_threadMutex);
if(path.size() && (path.at(path.size()-1) != '\\' || path.at(path.size()-1) != '/' ))
{
path += UDirectory::separator();
@@ -1481,12 +1171,8 @@ void Rtabmap::setWorkingDirectory(std::string path)
_wDir = path;
if(_memory)
{
UEventsManager::post(new RtabmapEventInit(RtabmapEventInit::kInitializing));
_memory->init(_wDir + kDefaultDatabaseName);
UEventsManager::post(new RtabmapEventInit(RtabmapEventInit::kInitialized));
setupLogFiles();
//clear all buffered images
this->clearBufferedSensors();
}
else
{
@@ -1502,7 +1188,7 @@ void Rtabmap::setWorkingDirectory(std::string path)
void Rtabmap::deleteLastLocation()
{
UScopeMutex s(&_threadMutex);
if(_memory)
{
_memory->deleteLastLocation();
@@ -1511,7 +1197,6 @@ void Rtabmap::deleteLastLocation()
void Rtabmap::rejectLastLoopClosure()
{
UScopeMutex s(&_threadMutex);
UDEBUG("_lcHypothesisId=%d", _lcHypothesisId);
if(_lcHypothesisId)
{
@@ -1530,40 +1215,18 @@ void Rtabmap::rejectLastLoopClosure()
void Rtabmap::process(const cv::Mat & image, int id)
{
UScopeMutex s(&_threadMutex);
this->process(Image(image, id));
}
void Rtabmap::process(const Image & image)
void Rtabmap::dumpData() const
{
UScopeMutex s(&_threadMutex);
if(!this->isRunning())
{
this->addImage(image);
this->process();
}
else
{
UERROR("The rtabmap thread is running! Don't start rtabmap thread if you want to use this method directly.");
}
}
void Rtabmap::dumpData()
{
UScopeMutex s(&_threadMutex);
if(_memory)
{
_memory->dumpMemory(this->getWorkingDir());
}
}
void Rtabmap::updateParameters(const ParametersMap & parameters)
{
UScopeMutex s(&_threadMutex);
this->parseParameters(parameters);
}
void Rtabmap::adjustLikelihood(std::map<int, float> & likelihood) const
{
ULOGGER_DEBUG("likelihood.size()=%d", likelihood.size());
@@ -1634,9 +1297,8 @@ void Rtabmap::adjustLikelihood(std::map<int, float> & likelihood) const
UDEBUG("mean=%f, stdDev=%f, max=%f, maxId=%d, time=%fs", mean, stdDev, max, maxId, time);
}
void Rtabmap::dumpPrediction()
void Rtabmap::dumpPrediction() const
{
UScopeMutex s(&_threadMutex);
if(_memory && _bayesFilter)
{
const std::set<int> & wm = _memory->getWorkingMem();
@@ -1669,31 +1331,6 @@ void Rtabmap::dumpPrediction()
}
}
void Rtabmap::generateLocalGraph(const std::string & path, int id, int margin)
{
if(_memory)
{
std::map<int, int> ids = _memory->getNeighborsId(id, margin, -1, false);
if(ids.size() > 0)
{
ids.insert(std::pair<int,int>(id, 0));
std::set<int> idsSet;
for(std::map<int, int>::iterator iter = ids.begin(); iter!=ids.end(); ++iter)
{
idsSet.insert(idsSet.end(), iter->first);
UINFO("Node %d", iter->first);
}
UINFO("idsSet=%d", idsSet.size());
_memory->generateGraph(path, idsSet);
}
else
{
UERROR("No neighbors found for signature %d.", id);
}
}
}
void Rtabmap::readParameters(const std::string & configFile, ParametersMap & parameters)
{
CSimpleIniA ini;

291
corelib/src/RtabmapThread.cpp Normal file
View File

@@ -0,0 +1,291 @@
/*
* 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/Rtabmap.h"
#include "rtabmap/core/RtabmapThread.h"
#include "rtabmap/core/RtabmapEvent.h"
#include "rtabmap/core/Camera.h"
#include <utilite/ULogger.h>
#include <utilite/UEventsManager.h>
#include <utilite/UStl.h>
namespace rtabmap {
RtabmapThread::RtabmapThread() :
_imageBufferMaxSize(Parameters::defaultRtabmapImageBufferSize()),
_rtabmap(new Rtabmap())
{
}
RtabmapThread::~RtabmapThread()
{
UEventsManager::removeHandler(this);
// Stop the thread first
join(true);
delete _rtabmap;
}
void RtabmapThread::pushNewState(State newState, const ParametersMap & parameters)
{
ULOGGER_DEBUG("to %d", newState);
_stateMutex.lock();
{
_state.push(newState);
_stateParam.push(parameters);
}
_stateMutex.unlock();
_imageAdded.release();
}
void RtabmapThread::setWorkingDirectory(const std::string & path)
{
_rtabmap->setWorkingDirectory(path);
}
void RtabmapThread::clearBufferedSensors()
{
_imageMutex.lock();
{
_imageBuffer.clear();
}
_imageMutex.unlock();
}
void RtabmapThread::mainLoopKill()
{
this->clearBufferedSensors();
// this will post the newData semaphore
_imageAdded.release();
}
void RtabmapThread::mainLoop()
{
State state = kStateDetecting;
ParametersMap parameters;
_stateMutex.lock();
{
if(!_state.empty() && !_stateParam.empty())
{
state = _state.top();
_state.pop();
parameters = _stateParam.top();
_stateParam.pop();
}
}
_stateMutex.unlock();
ParametersMap::iterator iter;
switch(state)
{
case kStateDetecting:
this->process();
break;
case kStateChangingParameters:
if((iter=parameters.find(Parameters::kRtabmapImageBufferSize())) != parameters.end())
{
_imageBufferMaxSize = std::atoi(iter->second.c_str());
}
_rtabmap->parseParameters(parameters);
break;
case kStateReseting:
_rtabmap->resetMemory();
this->clearBufferedSensors();
break;
case kStateDumpingMemory:
_rtabmap->dumpData();
break;
case kStateDumpingPrediction:
_rtabmap->dumpPrediction();
break;
case kStateGeneratingGraph:
_rtabmap->generateGraph(parameters.at("path"));
break;
case kStateGeneratingLocalGraph:
_rtabmap->generateGraph(parameters.at("path"), atoi(parameters.at("id").c_str()), atoi(parameters.at("margin").c_str()));
break;
case kStateDeletingMemory:
_rtabmap->resetMemory(true);
this->clearBufferedSensors();
break;
case kStateCleanSensorsBuffer:
this->clearBufferedSensors();
break;
default:
UFATAL("Invalid state !?!?");
break;
}
}
void RtabmapThread::handleEvent(UEvent* event)
{
if(this->isRunning() && event->getClassName().compare("CameraEvent") == 0)
{
CameraEvent * e = (CameraEvent*)event;
if(e->getCode() == CameraEvent::kCodeImage || e->getCode() == CameraEvent::kCodeFeatures)
{
this->addImage(e->image());
}
}
else if(event->getClassName().compare("RtabmapEventCmd") == 0)
{
RtabmapEventCmd * rtabmapEvent = (RtabmapEventCmd*)event;
RtabmapEventCmd::Cmd cmd = rtabmapEvent->getCmd();
if(cmd == RtabmapEventCmd::kCmdResetMemory)
{
ULOGGER_DEBUG("CMD_RESET_MEMORY");
pushNewState(kStateReseting);
}
else if(cmd == RtabmapEventCmd::kCmdDumpMemory)
{
ULOGGER_DEBUG("CMD_DUMP_MEMORY");
pushNewState(kStateDumpingMemory);
}
else if(cmd == RtabmapEventCmd::kCmdDumpPrediction)
{
ULOGGER_DEBUG("CMD_DUMP_PREDICTION");
pushNewState(kStateDumpingPrediction);
}
else if(cmd == RtabmapEventCmd::kCmdGenerateGraph)
{
if(!rtabmapEvent->getStr().empty())
{
ULOGGER_DEBUG("CMD_GENERATE_GRAPH");
ParametersMap param;
param.insert(ParametersPair("path", rtabmapEvent->getStr()));
pushNewState(kStateGeneratingGraph, param);
}
}
else if(cmd == RtabmapEventCmd::kCmdGenerateLocalGraph)
{
std::list<std::string> values = uSplit(rtabmapEvent->getStr(), ';');
if(values.size() == 3)
{
ULOGGER_DEBUG("CMD_GENERATE_LOCAL_GRAPH");
ParametersMap param;
param.insert(ParametersPair("path", *values.begin()));
param.insert(ParametersPair("id", *(++values.begin())));
param.insert(ParametersPair("margin", *values.rbegin()));
pushNewState(kStateGeneratingLocalGraph, param);
}
}
else if(cmd == RtabmapEventCmd::kCmdDeleteMemory)
{
ULOGGER_DEBUG("CMD_DELETE_MEMORY");
pushNewState(kStateDeletingMemory);
}
else if(cmd == RtabmapEventCmd::kCmdCleanSensorsBuffer)
{
ULOGGER_DEBUG("CMD_CLEAN_SENSORS_BUFFER");
pushNewState(kStateCleanSensorsBuffer);
}
}
else if(event->getClassName().compare("ParamEvent") == 0)
{
ULOGGER_DEBUG("changing parameters");
pushNewState(kStateChangingParameters, ((ParamEvent*)event)->getParameters());
}
}
//============================================================
// MAIN LOOP
//============================================================
void RtabmapThread::process()
{
Image image;
getImage(image);
if(!image.empty())
{
_rtabmap->process(image);
Statistics stats = _rtabmap->getStatistics();
stats.addStatistic(Statistics::kMemoryImages_buffered(), (float)_imageBuffer.size());
ULOGGER_DEBUG("posting statistics_ event...");
this->post(new RtabmapEvent(stats));
}
}
void RtabmapThread::addImage(const Image & image)
{
if(image.empty())
{
ULOGGER_ERROR("image empty !?");
return;
}
bool notify = true;
_imageMutex.lock();
{
_imageBuffer.push_back(image);
while(_imageBufferMaxSize > 0 && _imageBuffer.size() > (unsigned int)_imageBufferMaxSize)
{
ULOGGER_WARN("Data buffer is full, the oldest data is removed to add the new one.");
_imageBuffer.pop_front();
notify = false;
}
}
_imageMutex.unlock();
if(notify)
{
_imageAdded.release();
}
}
void RtabmapThread::getImage(Image & image)
{
ULOGGER_DEBUG("");
ULOGGER_INFO("waiting for data");
_imageAdded.acquire();
ULOGGER_INFO("wake-up");
_imageMutex.lock();
{
if(!_imageBuffer.empty())
{
image = _imageBuffer.front();
_imageBuffer.pop_front();
}
}
_imageMutex.unlock();
}
void RtabmapThread::setDataBufferSize(int size)
{
if(size < 0)
{
ULOGGER_WARN("size < 0, then setting it to 0 (inf).");
_imageBufferMaxSize = 0;
}
else
{
_imageBufferMaxSize = size;
}
}
} /* namespace rtabmap */

2
corelib/src/RtabmapEvent.cpp → corelib/src/Statistics.cpp
View File

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