Database update (version 0.8.5): added Node.stamp and Node.label fields

Added labeling mechanism of nodes (by default the first node of a map is tagged "map#")
This commit is contained in:
Mathieu Labbe
2015-02-27 15:24:15 -05:00
parent 1d39db2bcc
commit 721de2e76b
21 changed files with 411 additions and 44 deletions

View File

@@ -44,9 +44,9 @@ public:
};
public:
CameraEvent(const cv::Mat & image, int seq=0) :
CameraEvent(const cv::Mat & image, int seq=0, double stamp = 0.0) :
UEvent(kCodeImage),
data_(image, seq)
data_(image, seq, stamp)
{
}
@@ -55,9 +55,9 @@ public:
{
}
CameraEvent(const cv::Mat & rgb, const cv::Mat & depth, float fx, float fy, float cx, float cy, const Transform & localTransform, int id) :
CameraEvent(const cv::Mat & rgb, const cv::Mat & depth, float fx, float fy, float cx, float cy, const Transform & localTransform, int id, double stamp) :
UEvent(kCodeImageDepth),
data_(rgb, depth, fx, fy, cx, cy, localTransform, Transform(), 1.0f, 1.0f, id)
data_(rgb, depth, fx, fy, cx, cy, localTransform, Transform(), 1.0f, 1.0f, id, stamp)
{
}

View File

@@ -79,8 +79,6 @@ public:
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, bool overwritten = false);
void closeConnection();
@@ -106,6 +104,7 @@ public:
void getLastNodeId(int & id) const;
void getLastWordId(int & id) const;
void getInvertedIndexNi(int signatureId, int & ni) const;
void getNodeIdByLabel(const std::string & label, int & id) const;
protected:
DBDriver(const ParametersMap & parameters = ParametersMap());
@@ -140,6 +139,7 @@ private:
virtual void getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren) const = 0;
virtual void getLastIdQuery(const std::string & tableName, int & id) const = 0;
virtual void getInvertedIndexNiQuery(int signatureId, int & ni) const = 0;
virtual void getNodeIdByLabelQuery(const std::string & label, int & id) const = 0;
private:
//non-abstract methods

View File

@@ -109,6 +109,8 @@ public:
std::map<int, int> getWeights() const;
int getLastSignatureId() const;
const Signature * getLastWorkingSignature() const;
int getSignatureIdByLabel(const std::string & label, bool lookInDatabase = true) const;
bool labelSignature(int id, const std::string & label);
int getDatabaseMemoryUsed() const; // in bytes
double getDbSavingTime() const;
int getMapId(int signatureId) const;

View File

@@ -99,6 +99,7 @@ public:
void setTimeThreshold(float maxTimeAllowed); // in ms
int triggerNewMap();
bool labelLocation(int id, const std::string & label);
void generateDOTGraph(const std::string & path, int id=0, int margin=5);
void generateTOROGraph(const std::string & path, bool optimized, bool global);
void resetMemory();

View File

@@ -43,7 +43,7 @@ class RTABMAP_EXP SensorData
{
public:
SensorData(); // empty constructor
SensorData(const cv::Mat & image, int id = 0);
SensorData(const cv::Mat & image, int id = 0, double stamp = 0.0);
// Metric constructor
SensorData(const cv::Mat & image,
@@ -56,7 +56,8 @@ public:
const Transform & pose,
float poseRotVariance,
float poseTransVariance,
int id);
int id,
double stamp);
// Metric constructor + 2d laser scan
SensorData(const cv::Mat & laserScan,
@@ -70,7 +71,8 @@ public:
const Transform & pose,
float poseRotVariance,
float poseTransVariance,
int id);
int id,
double stamp);
virtual ~SensorData() {}
@@ -82,6 +84,7 @@ public:
const cv::Mat & image() const {return _image;}
int id() const {return _id;}
void setId(int id) {_id = id;}
double stamp() const {return _stamp;}
bool isMetric() const {return !_depthOrRightImage.empty() || _fx != 0.0f || _fyOrBaseline != 0.0f || !_pose.isNull();}
void setPose(const Transform & pose, float rotVariance, float transVariance) {_pose = pose; _poseRotVariance=rotVariance; _poseTransVariance = transVariance;}
@@ -111,6 +114,7 @@ public:
private:
cv::Mat _image;
int _id;
double _stamp;
// Metric stuff
cv::Mat _depthOrRightImage;

View File

@@ -54,6 +54,9 @@ public:
Signature();
Signature(int id,
int mapId,
int weight,
double stamp,
const std::string & label,
const std::multimap<int, cv::KeyPoint> & words,
const std::multimap<int, pcl::PointXYZ> & words3,
const Transform & pose = Transform(),
@@ -76,9 +79,14 @@ public:
int id() const {return _id;}
int mapId() const {return _mapId;}
void setWeight(int weight) {if(_weight!=weight)_modified=true;_weight = weight;}
void setWeight(int weight) {_modified=_weight!=weight;_weight = weight;}
int getWeight() const {return _weight;}
void setLabel(const std::string & label) {_modified=_label.compare(label)!=0;_label = label;}
const std::string & getLabel() const {return _label;}
double getStamp() const {return _stamp;}
void addLinks(const std::list<Link> & links);
void addLinks(const std::map<int, Link> & links);
void addLink(const Link & link);
@@ -141,8 +149,10 @@ public:
private:
int _id;
int _mapId;
double _stamp;
std::map<int, Link> _links; // id, transform
int _weight;
std::string _label;
bool _saved; // If it's saved to bd
bool _modified;
bool _linksModified; // Optimization when updating signatures in database

View File

@@ -125,11 +125,11 @@ void CameraThread::mainLoop()
{
if(_cameraRGBD)
{
this->post(new CameraEvent(rgb, depth, fx, fy, cx, cy, _cameraRGBD->getLocalTransform(), ++_seq));
this->post(new CameraEvent(rgb, depth, fx, fy, cx, cy, _cameraRGBD->getLocalTransform(), ++_seq, UTimer::now()));
}
else
{
this->post(new CameraEvent(rgb, ++_seq));
this->post(new CameraEvent(rgb, ++_seq, UTimer::now()));
}
}
else if(!this->isKilled())

View File

@@ -483,6 +483,41 @@ void DBDriver::getInvertedIndexNi(int signatureId, int & ni) const
_dbSafeAccessMutex.unlock();
}
void DBDriver::getNodeIdByLabel(const std::string & label, int & id) const
{
if(!label.empty())
{
int idFound = 0;
// look in the trash
_trashesMutex.lock();
for(std::map<int, Signature*>::const_iterator sIter = _trashSignatures.begin(); sIter!=_trashSignatures.end(); ++sIter)
{
if(sIter->second->getLabel().compare(label) == 0)
{
idFound = sIter->first;
break;
}
}
_trashesMutex.unlock();
// then look in the database
if(idFound == 0)
{
_dbSafeAccessMutex.lock();
this->getNodeIdByLabelQuery(label, id);
_dbSafeAccessMutex.unlock();
}
else
{
id = idFound;
}
}
else
{
UWARN("Can't search with an empty label!");
}
}
void DBDriver::addStatisticsAfterRun(int stMemSize, int lastSignAdded, int processMemUsed, int databaseMemUsed, int dictionarySize) const
{
ULOGGER_DEBUG("");

View File

@@ -927,6 +927,36 @@ void DBDriverSqlite3::getInvertedIndexNiQuery(int nodeId, int & ni) const
}
}
void DBDriverSqlite3::getNodeIdByLabelQuery(const std::string & label, int & id) const
{
if(_ppDb && !label.empty())
{
UTimer timer;
timer.start();
int rc = SQLITE_OK;
sqlite3_stmt * ppStmt = 0;
std::stringstream query;
query << "SELECT id FROM Node WHERE label='" << label <<"'";
rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
// Process the result if one
rc = sqlite3_step(ppStmt);
if(rc == SQLITE_ROW)
{
id = sqlite3_column_int(ppStmt, 0);
rc = sqlite3_step(ppStmt);
}
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
// Finalize (delete) the statement
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
ULOGGER_DEBUG("Time=%f", timer.ticks());
}
}
void DBDriverSqlite3::getWeightQuery(int nodeId, int & weight) const
{
weight = 0;
@@ -976,9 +1006,18 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
unsigned int loaded = 0;
// Load nodes information
query << "SELECT id, map_id, weight, pose "
<< "FROM Node "
<< "WHERE id=?;";
if(uStrNumCmp(_version, "0.8.5") >= 0)
{
query << "SELECT id, map_id, weight, pose, stamp, label "
<< "FROM Node "
<< "WHERE id=?;";
}
else
{
query << "SELECT id, map_id, weight, pose "
<< "FROM Node "
<< "WHERE id=?;";
}
rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
@@ -992,10 +1031,12 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
int id = 0;
int mapId = 0;
double stamp = 0.0;
int weight = 0;
Transform pose;
const void * data = 0;
int dataSize = 0;
std::string label;
// Process the result if one
rc = sqlite3_step(ppStmt);
@@ -1012,6 +1053,17 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
{
memcpy(pose.data(), data, dataSize);
}
if(uStrNumCmp(_version, "0.8.5") >= 0)
{
stamp = sqlite3_column_double(ppStmt, index++);
const unsigned char * p = sqlite3_column_text(ppStmt, index++);
if(p)
{
label = reinterpret_cast<const char*>(p);
}
}
rc = sqlite3_step(ppStmt);
}
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
@@ -1023,10 +1075,12 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
Signature * s = new Signature(
id,
mapId,
weight,
stamp,
label,
std::multimap<int, cv::KeyPoint>(),
std::multimap<int, pcl::PointXYZ>(),
pose);
s->setWeight(weight);
s->setSaved(true);
nodes.push_back(s);
++loaded;
@@ -1583,13 +1637,27 @@ void DBDriverSqlite3::updateQuery(const std::list<Signature *> & nodes, bool upd
Signature * s = 0;
std::string query;
if(updateTimestamp)
if(uStrNumCmp(_version, "0.8.5") >= 0)
{
query = "UPDATE Node SET weight=?, time_enter = DATETIME('NOW') WHERE id=?;";
if(updateTimestamp)
{
query = "UPDATE Node SET weight=?, label=?, time_enter = DATETIME('NOW') WHERE id=?;";
}
else
{
query = "UPDATE Node SET weight=?, label=? WHERE id=?;";
}
}
else
{
query = "UPDATE Node SET weight=? WHERE id=?;";
if(updateTimestamp)
{
query = "UPDATE Node SET weight=?, time_enter = DATETIME('NOW') WHERE id=?;";
}
else
{
query = "UPDATE Node SET weight=? WHERE id=?;";
}
}
rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
@@ -1603,6 +1671,20 @@ void DBDriverSqlite3::updateQuery(const std::list<Signature *> & nodes, bool upd
rc = sqlite3_bind_int(ppStmt, index++, s->getWeight());
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
if(uStrNumCmp(_version, "0.8.5") >= 0)
{
if(s->getLabel().empty())
{
rc = sqlite3_bind_null(ppStmt, index++);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
}
else
{
rc = sqlite3_bind_text(ppStmt, index++, s->getLabel().c_str(), -1, SQLITE_STATIC);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
}
}
rc = sqlite3_bind_int(ppStmt, index++, s->id());
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
@@ -1900,6 +1982,10 @@ void DBDriverSqlite3::saveQuery(const std::list<VisualWord *> & words) const
std::string DBDriverSqlite3::queryStepNode() const
{
if(uStrNumCmp(_version, "0.8.5") >= 0)
{
return "INSERT INTO Node(id, map_id, weight, pose, stamp, label) VALUES(?,?,?,?,?,?);";
}
return "INSERT INTO Node(id, map_id, weight, pose) VALUES(?,?,?,?);";
}
void DBDriverSqlite3::stepNode(sqlite3_stmt * ppStmt, const Signature * s) const
@@ -1921,6 +2007,23 @@ void DBDriverSqlite3::stepNode(sqlite3_stmt * ppStmt, const Signature * s) const
rc = sqlite3_bind_blob(ppStmt, index++, s->getPose().data(), s->getPose().size()*sizeof(float), SQLITE_STATIC);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
if(uStrNumCmp(_version, "0.8.5") >= 0)
{
rc = sqlite3_bind_double(ppStmt, index++, s->getStamp());
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
if(s->getLabel().empty())
{
rc = sqlite3_bind_null(ppStmt, index++);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
}
else
{
rc = sqlite3_bind_text(ppStmt, index++, s->getLabel().c_str(), -1, SQLITE_STATIC);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
}
}
//step
rc=sqlite3_step(ppStmt);
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

View File

@@ -86,6 +86,7 @@ private:
virtual void getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren) const;
virtual void getLastIdQuery(const std::string & tableName, int & id) const;
virtual void getInvertedIndexNiQuery(int signatureId, int & ni) const;
virtual void getNodeIdByLabelQuery(const std::string & label, int & id) const;
private:
std::string queryStepNode() const;

View File

@@ -233,7 +233,8 @@ SensorData DBReader::getNextData()
pose,
rotVariance,
transVariance,
seq);
seq,
UTimer::now());
UDEBUG("Laser=%d RGB/Left=%d Depth=%d Right=%d",
data.laserScan().empty()?0:1,
data.image().empty()?0:1,

View File

@@ -689,6 +689,24 @@ void Memory::addSignatureToStm(Signature * signature, float poseRotVariance, flo
UDEBUG("Ignoring neighbor link between %d and %d because they are not in the same map! (%d vs %d)",
*_stMem.rbegin(), signature->id(),
_signatures.at(*_stMem.rbegin())->mapId(), signature->mapId());
//Tag the first node of the map
std::string tag = uFormat("map%d", signature->mapId());
if(getSignatureIdByLabel(tag, false) == 0)
{
UINFO("Tagging node %d with label \"%s\"", tag.c_str());
signature->setLabel(tag);
}
}
}
else
{
//Tag the first node of the map
std::string tag = uFormat("map%d", signature->mapId());
if(getSignatureIdByLabel(tag, false) == 0)
{
UINFO("Tagging node %d with label \"%s\"", tag.c_str());
signature->setLabel(tag);
}
}
@@ -1580,6 +1598,64 @@ const Signature * Memory::getLastWorkingSignature() const
return _lastSignature;
}
int Memory::getSignatureIdByLabel(const std::string & label, bool lookInDatabase) const
{
int id = 0;
for(std::map<int, Signature*>::const_iterator iter=_signatures.begin(); iter!=_signatures.end(); ++iter)
{
if(iter->second->getLabel().compare(label) == 0)
{
id = iter->second->id();
break;
}
}
if(id == 0 && _dbDriver && lookInDatabase)
{
_dbDriver->getNodeIdByLabel(label, id);
}
return id;
}
bool Memory::labelSignature(int id, const std::string & label)
{
if(!label.empty())
{
// verify that this label is not used
int idFound=getSignatureIdByLabel(label);
if(idFound == 0 || idFound == id)
{
Signature * s = this->_getSignature(id);
if(s)
{
s->setLabel(label);
return true;
}
else if(_dbDriver)
{
std::list<int> ids;
ids.push_back(id);
std::list<Signature *> signatures;
_dbDriver->loadSignatures(ids,signatures);
if(signatures.size())
{
signatures.front()->setLabel(label);
_dbDriver->asyncSave(signatures.front()); // move it again to trash
return true;
}
}
else
{
UERROR("Node %d not found, failed to set label \"%s\"!", id, label.c_str());
}
}
else if(idFound)
{
UWARN("Node %d has already label \"%s\"", idFound, label.c_str());
}
}
return false;
}
void Memory::deleteLocation(int locationId, std::list<int> * deletedWords)
{
UDEBUG("Deleting location %d", locationId);
@@ -3509,6 +3585,9 @@ Signature * Memory::createSignature(const SensorData & data, Statistics * stats)
s = new Signature(id,
_idMapCount,
0,
data.stamp(),
"",
words,
words3D,
data.pose(),
@@ -3525,6 +3604,9 @@ Signature * Memory::createSignature(const SensorData & data, Statistics * stats)
{
s = new Signature(id,
_idMapCount,
0,
data.stamp(),
"",
words,
words3D,
data.pose(),

View File

@@ -599,6 +599,26 @@ int Rtabmap::triggerNewMap()
return mapId;
}
bool Rtabmap::labelLocation(int id, const std::string & label)
{
if(!label.empty() && _memory)
{
if(id > 0)
{
return _memory->labelSignature(id, label);
}
else if(_memory->getLastWorkingSignature())
{
return _memory->labelSignature(_memory->getLastWorkingSignature()->id(), label);
}
else
{
UERROR("Last signature is null! Cannot set label \"%s\"", label.c_str());
}
}
return false;
}
void Rtabmap::generateDOTGraph(const std::string & path, int id, int margin)
{
if(_memory)

View File

@@ -38,6 +38,7 @@ namespace rtabmap
SensorData::SensorData() :
_image(cv::Mat()),
_id(0),
_stamp(0.0),
_fx(0.0f),
_fyOrBaseline(0.0f),
_cx(0.0f),
@@ -49,9 +50,11 @@ SensorData::SensorData() :
}
SensorData::SensorData(const cv::Mat & image,
int id) :
int id,
double stamp) :
_image(image),
_id(id),
_stamp(stamp),
_fx(0.0f),
_fyOrBaseline(0.0f),
_cx(0.0f),
@@ -75,9 +78,11 @@ SensorData::SensorData(const cv::Mat & image,
const Transform & pose,
float poseRotVariance,
float poseTransVariance,
int id) :
int id,
double stamp) :
_image(image),
_id(id),
_stamp(stamp),
_depthOrRightImage(depthOrRightImage),
_fx(fx),
_fyOrBaseline(fyOrBaseline),
@@ -109,9 +114,11 @@ SensorData::SensorData(const cv::Mat & laserScan,
const Transform & pose,
float poseRotVariance,
float poseTransVariance,
int id) :
int id,
double stamp) :
_image(image),
_id(id),
_stamp(stamp),
_depthOrRightImage(depthOrRightImage),
_laserScan(laserScan),
_fx(fx),

View File

@@ -39,6 +39,7 @@ namespace rtabmap
Signature::Signature() :
_id(0), // invalid id
_mapId(-1),
_stamp(0.0),
_weight(-1),
_saved(false),
_modified(true),
@@ -54,6 +55,9 @@ Signature::Signature() :
Signature::Signature(
int id,
int mapId,
int weight,
double stamp,
const std::string & label,
const std::multimap<int, cv::KeyPoint> & words,
const std::multimap<int, pcl::PointXYZ> & words3, // in base_link frame (localTransform applied)
const Transform & pose,
@@ -67,7 +71,9 @@ Signature::Signature(
const Transform & localTransform) :
_id(id),
_mapId(mapId),
_weight(0),
_stamp(stamp),
_weight(weight),
_label(label),
_saved(false),
_modified(true),
_linksModified(true),
@@ -258,7 +264,8 @@ SensorData Signature::toSensorData()
_pose,
rotVariance,
transVariance,
_id);
_id,
_stamp);
}
void Signature::uncompressData()

View File

@@ -17,7 +17,9 @@ CREATE TABLE Node (
id INTEGER NOT NULL,
map_id INTEGER NOT NULL,
weight INTEGER,
stamp FLOAT,
pose BLOB,
label TEXT,
time_enter DATE,
PRIMARY KEY (id)
);
@@ -122,6 +124,7 @@ END;
-- *******************************************************************
CREATE INDEX IDX_Map_Node_Word_node_id on Map_Node_Word (node_id);
CREATE INDEX IDX_Link_from_id on Link (from_id);
CREATE UNIQUE INDEX IDX_node_label on Node (label);
-- *******************************************************************
-- VERSION