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

0.16.2: Added wm_state field to database's Statistics table. New parameter "Rtabmap/SaveWMState" (default false). DbViewer: added option in GraphView to show only poses of the root id that were in WM at that time

Browse Source
This commit is contained in:
matlabbe
2018-02-21 18:19:20 -05:00
parent 93e1d732c9
commit 297cf3f51e
16 changed files with 208 additions and 159 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -146,7 +146,7 @@ public:
int getTotalNodesSize() const;
int getTotalDictionarySize() const;
ParametersMap getLastParameters() const;
std::map<std::string, float> getStatistics(int nodeId, double & stamp) const;
std::map<std::string, float> getStatistics(int nodeId, double & stamp, std::vector<int> * wmState=0) const;
std::map<int, std::pair<std::map<std::string, float>, double> > getAllStatistics() const;
void executeNoResult(const std::string & sql) const;
@@ -198,7 +198,7 @@ private:
virtual int getTotalNodesSizeQuery() const = 0;
virtual int getTotalDictionarySizeQuery() const = 0;
virtual ParametersMap getLastParametersQuery() const = 0;
virtual std::map<std::string, float> getStatisticsQuery(int nodeId, double & stamp) const = 0;
virtual std::map<std::string, float> getStatisticsQuery(int nodeId, double & stamp, std::vector<int> * wmState) const = 0;
virtual std::map<int, std::pair<std::map<std::string, float>, double> > getAllStatisticsQuery() const = 0;
virtual void executeNoResultQuery(const std::string & sql) const = 0;

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

@@ -174,6 +174,7 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(Rtabmap, PublishLikelihood, bool, true, "Publishing likelihood.");
RTABMAP_PARAM(Rtabmap, PublishRAMUsage, bool, false, "Publishing RAM usage in statistics (may add a small overhead to get info from the system).");
RTABMAP_PARAM(Rtabmap, ComputeRMSE, bool, true, "Compute root mean square error (RMSE) and publish it in statistics, if ground truth is provided.");
RTABMAP_PARAM(Rtabmap, SaveWMState, bool, false, "Save working memory state after each update in statistics.");
RTABMAP_PARAM(Rtabmap, TimeThr, float, 0, "Maximum time allowed for the detector (ms) (0 means infinity).");
RTABMAP_PARAM(Rtabmap, MemoryThr, int, 0, "Maximum signatures in the Working Memory (ms) (0 means infinity).");
RTABMAP_PARAM(Rtabmap, DetectionRate, float, 1, "Detection rate (Hz). RTAB-Map will filter input images to satisfy this rate.");

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

@@ -214,6 +214,7 @@ private:
bool _publishLikelihood;
bool _publishRAMUsage;
bool _computeRMSE;
bool _saveWMState;
float _maxTimeAllowed; // in ms
unsigned int _maxMemoryAllowed; // signatures count in WM
float _loopThr;

4
corelib/include/rtabmap/core/Statistics.h
View File

@@ -189,6 +189,7 @@ public:
void setLocalPath(const std::vector<int> & localPath) {_localPath=localPath;}
void setCurrentGoalId(int goal) {_currentGoalId=goal;}
void setReducedIds(const std::map<int, int> & reducedIds) {_reducedIds = reducedIds;}
void setWmState(const std::vector<int> & state) {_wmState = state;}
// getters
bool extended() const {return _extended;}
@@ -210,6 +211,7 @@ public:
const std::vector<int> & localPath() const {return _localPath;}
int currentGoalId() const {return _currentGoalId;}
const std::map<int, int> & reducedIds() const {return _reducedIds;}
const std::vector<int> & wmState() const {return _wmState;}
const std::map<std::string, float> & data() const {return _data;}
@@ -238,6 +240,8 @@ private:
std::map<int, int> _reducedIds;
std::vector<int> _wmState;
// Format for statistics (Plottable statistics must go in that map) :
// {"Group/Name/Unit", value}
// Example : {"Timing/Total time/ms", 500.0f}

4
corelib/src/DBDriver.cpp
View File

@@ -245,11 +245,11 @@ ParametersMap DBDriver::getLastParameters() const
return parameters;
}
std::map<std::string, float> DBDriver::getStatistics(int nodeId, double & stamp) const
std::map<std::string, float> DBDriver::getStatistics(int nodeId, double & stamp, std::vector<int> * wmState) const
{
std::map<std::string, float> statistics;
_dbSafeAccessMutex.lock();
statistics = getStatisticsQuery(nodeId, stamp);
statistics = getStatisticsQuery(nodeId, stamp, wmState);
_dbSafeAccessMutex.unlock();
return statistics;
}

65
corelib/src/DBDriverSqlite3.cpp
View File

@@ -875,7 +875,11 @@ long DBDriverSqlite3::getStatisticsMemoryUsedQuery() const
if(_ppDb)
{
std::string query;
if(uStrNumCmp(_version, "0.11.11") >= 0)
if(uStrNumCmp(_version, "0.16.2") >= 0)
{
query = "SELECT sum(length(id) + length(stamp) + length(data) + length(wm_state)) FROM Statistics";
}
else if(uStrNumCmp(_version, "0.11.11") >= 0)
{
query = "SELECT sum(length(id) + length(stamp) + length(data)) FROM Statistics";
}
@@ -1059,7 +1063,7 @@ ParametersMap DBDriverSqlite3::getLastParametersQuery() const
return parameters;
}
std::map<std::string, float> DBDriverSqlite3::getStatisticsQuery(int nodeId, double & stamp) const
std::map<std::string, float> DBDriverSqlite3::getStatisticsQuery(int nodeId, double & stamp, std::vector<int> * wmState) const
{
UDEBUG("nodeId=%d", nodeId);
std::map<std::string, float> data;
@@ -1069,9 +1073,18 @@ std::map<std::string, float> DBDriverSqlite3::getStatisticsQuery(int nodeId, dou
{
std::stringstream query;
query << "SELECT stamp, data "
<< "FROM Statistics "
<< "WHERE id=" << nodeId << ";";
if(uStrNumCmp(_version, "0.16.2") >= 0 && wmState)
{
query << "SELECT stamp, data, wm_state "
<< "FROM Statistics "
<< "WHERE id=" << nodeId << ";";
}
else
{
query << "SELECT stamp, data "
<< "FROM Statistics "
<< "WHERE id=" << nodeId << ";";
}
int rc = SQLITE_OK;
sqlite3_stmt * ppStmt = 0;
@@ -1086,10 +1099,8 @@ std::map<std::string, float> DBDriverSqlite3::getStatisticsQuery(int nodeId, dou
std::string text;
if(uStrNumCmp(this->getDatabaseVersion(), "0.15.0") >= 0)
{
const void * dataPtr = 0;
int dataSize = 0;
dataPtr = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++);
const void * dataPtr = sqlite3_column_blob(ppStmt, index);
int dataSize = sqlite3_column_bytes(ppStmt, index++);
if(dataSize>0 && dataPtr)
{
text = uncompressString(cv::Mat(1, dataSize, CV_8UC1, (void *)dataPtr));
@@ -1105,6 +1116,19 @@ std::map<std::string, float> DBDriverSqlite3::getStatisticsQuery(int nodeId, dou
data = Statistics::deserializeData(text);
}
if(uStrNumCmp(_version, "0.16.2") >= 0 && wmState)
{
const void * dataPtr = sqlite3_column_blob(ppStmt, index);
int dataSize = sqlite3_column_bytes(ppStmt, index++);
if(dataSize>0 && dataPtr)
{
cv::Mat wmStateMat = uncompressData(cv::Mat(1, dataSize, CV_8UC1, (void *)dataPtr));
UASSERT(wmStateMat.type() == CV_32SC1 && wmStateMat.rows == 1);
wmState->resize(wmStateMat.cols);
memcpy(wmState->data(), wmStateMat.data, wmState->size()*sizeof(int));
}
}
rc = sqlite3_step(ppStmt);
}
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
@@ -4041,7 +4065,15 @@ void DBDriverSqlite3::addStatisticsQuery(const Statistics & statistics) const
std::string param = Statistics::serializeData(statistics.data());
if(param.size() && statistics.refImageId()>0)
{
std::string query = "INSERT INTO Statistics(id, stamp, data) values(?,?,?);";
std::string query;
if(uStrNumCmp(this->getDatabaseVersion(), "0.16.2") >= 0)
{
query = "INSERT INTO Statistics(id, stamp, data, wm_state) values(?,?,?,?);";
}
else
{
query = "INSERT INTO Statistics(id, stamp, data) values(?,?,?);";
}
rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
@@ -4064,6 +4096,19 @@ void DBDriverSqlite3::addStatisticsQuery(const Statistics & statistics) const
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
cv::Mat compressedWmState;
if(uStrNumCmp(this->getDatabaseVersion(), "0.16.2") >= 0 && !statistics.wmState().empty())
{
compressedWmState = compressData2(cv::Mat(1, statistics.wmState().size(), CV_32SC1, (void *)statistics.wmState().data()));
rc = sqlite3_bind_blob(ppStmt, index++, compressedWmState.data, compressedWmState.cols, SQLITE_STATIC);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
else
{
rc = sqlite3_bind_null(ppStmt, index++);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
//step
rc=sqlite3_step(ppStmt);
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());

2
corelib/src/DBDriverSqlite3.h
View File

@@ -70,7 +70,7 @@ private:
virtual int getTotalNodesSizeQuery() const;
virtual int getTotalDictionarySizeQuery() const;
virtual ParametersMap getLastParametersQuery() const;
virtual std::map<std::string, float> getStatisticsQuery(int nodeId, double & stamp) const;
virtual std::map<std::string, float> getStatisticsQuery(int nodeId, double & stamp, std::vector<int> * wmState) const;
virtual std::map<int, std::pair<std::map<std::string, float>, double> > getAllStatisticsQuery() const;
virtual void executeNoResultQuery(const std::string & sql) const;

16
corelib/src/Rtabmap.cpp
View File

@@ -82,6 +82,7 @@ Rtabmap::Rtabmap() :
_publishLikelihood(Parameters::defaultRtabmapPublishLikelihood()),
_publishRAMUsage(Parameters::defaultRtabmapPublishRAMUsage()),
_computeRMSE(Parameters::defaultRtabmapComputeRMSE()),
_saveWMState(Parameters::defaultRtabmapSaveWMState()),
_maxTimeAllowed(Parameters::defaultRtabmapTimeThr()), // 700 ms
_maxMemoryAllowed(Parameters::defaultRtabmapMemoryThr()), // 0=inf
_loopThr(Parameters::defaultRtabmapLoopThr()),
@@ -405,6 +406,7 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kRtabmapPublishLikelihood(), _publishLikelihood);
Parameters::parse(parameters, Parameters::kRtabmapPublishRAMUsage(), _publishRAMUsage);
Parameters::parse(parameters, Parameters::kRtabmapComputeRMSE(), _computeRMSE);
Parameters::parse(parameters, Parameters::kRtabmapSaveWMState(), _saveWMState);
Parameters::parse(parameters, Parameters::kRtabmapTimeThr(), _maxTimeAllowed);
Parameters::parse(parameters, Parameters::kRtabmapMemoryThr(), _maxMemoryAllowed);
Parameters::parse(parameters, Parameters::kRtabmapLoopThr(), _loopThr);
@@ -2776,6 +2778,20 @@ bool Rtabmap::process(
statistics_.addStatistic(Statistics::kGtRotational_max(), rotational_max);
}
if(_saveWMState && _memory->isIncremental())
{
std::vector<int> ids = uKeys(_memory->getWorkingMem());
if(_memory->getStMem().size())
{
ids.resize(ids.size() + _memory->getStMem().size());
for(std::set<int>::const_iterator iter=_memory->getStMem().begin(); iter!=_memory->getStMem().end(); ++iter)
{
ids.push_back(*iter);
}
}
statistics_.setWmState(ids);
}
UDEBUG("");
}

3
corelib/src/resources/DatabaseSchema.sql.in
View File

@@ -100,7 +100,8 @@ CREATE TABLE Info (
CREATE TABLE Statistics (
id INTEGER NOT NULL,
stamp FLOAT,
data BLOB,
data BLOB, -- compressed string
wm_state BLOB, -- compressed data
FOREIGN KEY (id) REFERENCES Node(id)
);