mirror of
https://github.com/introlab/rtabmap.git
synced 2026-09-02 01:20:25 +08:00
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:
@@ -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)
|
||||
{
|
||||
}
|
||||
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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;
|
||||
|
||||
@@ -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();
|
||||
|
||||
@@ -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;
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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())
|
||||
|
||||
@@ -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("");
|
||||
|
||||
@@ -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());
|
||||
|
||||
@@ -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;
|
||||
|
||||
@@ -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,
|
||||
|
||||
@@ -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(),
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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),
|
||||
|
||||
@@ -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()
|
||||
|
||||
@@ -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
|
||||
|
||||
Reference in New Issue
Block a user