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

0.20: added GlobalDescriptor table in database

Browse Source
This commit is contained in:
matlabbe
2020-05-03 21:46:25 -04:00
parent 50c3bb7ce9
commit c19da22ca4
11 changed files with 248 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
corelib/src/DBDriver.cpp
View File

@@ -1140,10 +1140,10 @@ void DBDriver::addInfoAfterRun(
}
}
void DBDriver::addStatistics(const Statistics & statistics) const
void DBDriver::addStatistics(const Statistics & statistics, bool saveWmState) const
{
_dbSafeAccessMutex.lock();
addStatisticsQuery(statistics);
addStatisticsQuery(statistics, saveWmState);
_dbSafeAccessMutex.unlock();
}

148
corelib/src/DBDriverSqlite3.cpp
View File

@@ -3384,6 +3384,74 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
ULOGGER_DEBUG("Time load %d calibrations=%fs", (int)nodes.size(), timer.ticks());
}
// load global descriptors
if(nodes.size() && uStrNumCmp(_version, "0.20.0") >= 0)
{
std::stringstream query3;
query3 << "SELECT type, info, data "
"FROM GlobalDescriptor "
"WHERE node_id = ? ";
rc = sqlite3_prepare_v2(_ppDb, query3.str().c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
for(std::list<Signature*>::const_iterator iter=nodes.begin(); iter!=nodes.end(); ++iter)
{
// bind id
rc = sqlite3_bind_int(ppStmt, 1, (*iter)->id());
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
std::vector<GlobalDescriptor> globalDescriptors;
rc = sqlite3_step(ppStmt);
while(rc == SQLITE_ROW)
{
int index=0;
const void * data = 0;
int dataSize = 0;
int type = -1;
cv::Mat info;
cv::Mat dataMat;
type = sqlite3_column_int(ppStmt, index++);
data = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++);
if(dataSize && data)
{
info = rtabmap::uncompressData(cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone());
}
data = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++);
if(dataSize && data)
{
dataMat = rtabmap::uncompressData(cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone());
}
UASSERT(!dataMat.empty());
globalDescriptors.push_back(GlobalDescriptor(type, dataMat, info));
rc = sqlite3_step(ppStmt);
}
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
if(!globalDescriptors.empty())
{
(*iter)->sensorData().setGlobalDescriptors(globalDescriptors);
ULOGGER_DEBUG("Add %d global descriptors to node %d", (int)globalDescriptors.size(), (*iter)->id());
}
//reset
rc = sqlite3_reset(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
// Finalize (delete) the statement
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
ULOGGER_DEBUG("Time load %d global descriptors=%fs", (int)nodes.size(), timer.ticks());
}
if(ids.size() != loaded)
{
UERROR("Some signatures not found in database");
@@ -4235,6 +4303,27 @@ void DBDriverSqlite3::saveQuery(const std::list<Signature *> & signatures)
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
UDEBUG("Time=%fs", timer.ticks());
if(uStrNumCmp(_version, "0.20.0") >= 0)
{
// Global descriptor table
std::string query = queryStepGlobalDescriptor();
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());
for(std::list<Signature *>::const_iterator i=signatures.begin(); i!=signatures.end(); ++i)
{
for(size_t d=0; d<(*i)->sensorData().globalDescriptors().size(); ++d)
{
stepGlobalDescriptor(ppStmt, (*i)->id(), (*i)->sensorData().globalDescriptors()[d]);
}
}
// Finalize (delete) the statement
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
UDEBUG("Time=%fs", timer.ticks());
}
if(uStrNumCmp(_version, "0.10.0") >= 0)
{
// Add SensorData
@@ -4519,7 +4608,7 @@ void DBDriverSqlite3::updateLaserScanQuery(
}
}
void DBDriverSqlite3::addStatisticsQuery(const Statistics & statistics) const
void DBDriverSqlite3::addStatisticsQuery(const Statistics & statistics, bool saveWmState) const
{
UDEBUG("Ref ID = %d", statistics.refImageId());
if(_ppDb)
@@ -4570,7 +4659,7 @@ void DBDriverSqlite3::addStatisticsQuery(const Statistics & statistics) const
cv::Mat compressedWmState;
if(uStrNumCmp(this->getDatabaseVersion(), "0.16.2") >= 0)
{
if(!statistics.wmState().empty())
if(saveWmState && !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);
@@ -6364,6 +6453,61 @@ void DBDriverSqlite3::stepKeypoint(sqlite3_stmt * ppStmt,
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
std::string DBDriverSqlite3::queryStepGlobalDescriptor() const
{
UASSERT(uStrNumCmp(_version, "0.20.0") >= 0);
return "INSERT INTO GlobalDescriptor(node_id, type, info, data) VALUES(?,?,?,?);";
}
void DBDriverSqlite3::stepGlobalDescriptor(sqlite3_stmt * ppStmt,
int nodeId,
const GlobalDescriptor & descriptor) const
{
if(!ppStmt)
{
UFATAL("");
}
int rc = SQLITE_OK;
int index = 1;
//node_if
rc = sqlite3_bind_int(ppStmt, index++, nodeId);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
//type
rc = sqlite3_bind_int(ppStmt, index++, nodeId);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
//info
std::vector<unsigned char> infoBytes = rtabmap::compressData(descriptor.info());
if(infoBytes.empty())
{
rc = sqlite3_bind_null(ppStmt, index++);
}
else
{
rc = sqlite3_bind_blob(ppStmt, index++, infoBytes.data(), infoBytes.size(), SQLITE_STATIC);
}
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
//data
std::vector<unsigned char> dataBytes = rtabmap::compressData(descriptor.data());
if(infoBytes.empty())
{
rc = sqlite3_bind_null(ppStmt, index++);
}
else
{
rc = sqlite3_bind_blob(ppStmt, index++, dataBytes.data(), dataBytes.size(), SQLITE_STATIC);
}
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc=sqlite3_step(ppStmt);
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
rc = sqlite3_reset(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
std::string DBDriverSqlite3::queryStepOccupancyGridUpdate() const
{
UASSERT(uStrNumCmp(_version, "0.11.10") >= 0);

5
corelib/src/Memory.cpp
View File

@@ -2003,11 +2003,11 @@ int Memory::cleanup()
return signatureRemoved;
}
void Memory::saveStatistics(const Statistics & statistics)
void Memory::saveStatistics(const Statistics & statistics, bool saveWmState)
{
if(_dbDriver)
{
_dbDriver->addStatistics(statistics);
_dbDriver->addStatistics(statistics, saveWmState);
}
}
@@ -5060,6 +5060,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
s->sensorData().setGroundTruth(data.groundTruth());
s->sensorData().setGPS(data.gps());
s->sensorData().setEnvSensors(data.envSensors());
s->sensorData().setGlobalDescriptors(data.globalDescriptors());
t = timer.ticks();
if(stats) stats->addStatistic(Statistics::kTimingMemCompressing_data(), t*1000.0f);

24
corelib/src/Rtabmap.cpp
View File

@@ -3535,26 +3535,24 @@ bool Rtabmap::process(
UDEBUG("Computing RMSE...done!");
}
if(_saveWMState && _memory->isIncremental())
std::vector<int> ids;
ids.reserve(_memory->getWorkingMem().size() + _memory->getStMem().size());
for(std::set<int>::const_iterator iter=_memory->getStMem().begin(); iter!=_memory->getStMem().end(); ++iter)
{
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);
ids.push_back(*iter);
}
UDEBUG("");
for(std::map<int, double>::const_iterator iter=_memory->getWorkingMem().lower_bound(0); iter!=_memory->getWorkingMem().end(); ++iter)
{
ids.push_back(iter->first);
}
statistics_.setWmState(ids);
UDEBUG("wmState=%d", (int)ids.size());
}
//Save statistics to database
if(_memory->isIncremental() || _memory->isLocalizationDataSaved())
{
_memory->saveStatistics(statistics_);
_memory->saveStatistics(statistics_, _saveWMState);
}
//Start trashing

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

@@ -87,6 +87,14 @@ CREATE TABLE Feature (
FOREIGN KEY (node_id) REFERENCES Node(id)
);
CREATE TABLE GlobalDescriptor (
node_id INTEGER NOT NULL,
type INTEGER NOT NULL,
info BLOB,
data BLOB NOT NULL,
FOREIGN KEY (node_id) REFERENCES Node(id)
);
--
CREATE TABLE Info (
@@ -162,6 +170,7 @@ END;
-- *******************************************************************
CREATE UNIQUE INDEX IDX_Node_id on Node (id);
CREATE INDEX IDX_Feature_node_id on Feature (node_id);
CREATE INDEX IDX_GlobalDescriptor_node_id on GlobalDescriptor (node_id);
CREATE INDEX IDX_Link_from_id on Link (from_id);
CREATE UNIQUE INDEX IDX_node_label on Node (label);
CREATE UNIQUE INDEX IDX_Statistics_id on Statistics (id);