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

Version 0.8.8: added "user_data" field in database. Added WifiMapping example. Fixed FATAL error when initializing rtabmap with no database.

Browse Source
This commit is contained in:
Mathieu Labbe
2015-03-25 13:31:12 -04:00
parent 2be511813e
commit 186dc60a49
32 changed files with 1064 additions and 141 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
corelib/src/DBDriver.cpp
View File

@@ -474,7 +474,13 @@ void DBDriver::getNodeData(int signatureId, cv::Mat & imageCompressed) const
}
}
bool DBDriver::getNodeInfo(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp) const
bool DBDriver::getNodeInfo(int signatureId,
Transform & pose,
int & mapId,
int & weight,
std::string & label,
double & stamp,
std::vector<unsigned char> & userData) const
{
bool found = false;
// look in the trash
@@ -486,6 +492,7 @@ bool DBDriver::getNodeInfo(int signatureId, Transform & pose, int & mapId, int &
weight = _trashSignatures.at(signatureId)->getWeight();
label = _trashSignatures.at(signatureId)->getLabel();
stamp = _trashSignatures.at(signatureId)->getStamp();
userData = _trashSignatures.at(signatureId)->getUserData();
found = true;
}
_trashesMutex.unlock();
@@ -493,7 +500,7 @@ bool DBDriver::getNodeInfo(int signatureId, Transform & pose, int & mapId, int &
if(!found)
{
_dbSafeAccessMutex.lock();
found = this->getNodeInfoQuery(signatureId, pose, mapId, weight, label, stamp);
found = this->getNodeInfoQuery(signatureId, pose, mapId, weight, label, stamp, userData);
_dbSafeAccessMutex.unlock();
}
return found;

132
corelib/src/DBDriverSqlite3.cpp
View File

@@ -458,10 +458,10 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
if(loadMetricData)
{
if(uStrNumCmp(_version, "0.7.0") < 0)
if(uStrNumCmp(_version, "0.7.0") >= 0)
{
query << "SELECT Image.data, "
"Depth.data, Depth.constant, Depth.local_transform, Depth.data2d "
"Depth.data, Depth.fx, Depth.fy, Depth.cx, Depth.cy, Depth.local_transform, Depth.data2d "
<< "FROM Image "
<< "LEFT OUTER JOIN Depth " // returns all images even if there are no metric data
<< "ON Image.id = Depth.id "
@@ -471,7 +471,7 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
else
{
query << "SELECT Image.data, "
"Depth.data, Depth.fx, Depth.fy, Depth.cx, Depth.cy, Depth.local_transform, Depth.data2d "
"Depth.data, Depth.constant, Depth.local_transform, Depth.data2d "
<< "FROM Image "
<< "LEFT OUTER JOIN Depth " // returns all images even if there are no metric data
<< "ON Image.id = Depth.id "
@@ -598,10 +598,10 @@ void DBDriverSqlite3::getNodeDataQuery(
sqlite3_stmt * ppStmt = 0;
std::stringstream query;
if(uStrNumCmp(_version, "0.7.0") < 0)
if(uStrNumCmp(_version, "0.7.0") >= 0)
{
query << "SELECT Image.data, "
"Depth.data, Depth.constant, Depth.local_transform, Depth.data2d "
"Depth.data, Depth.fx, Depth.fy, Depth.cx, Depth.cy, Depth.local_transform, Depth.data2d "
<< "FROM Image "
<< "LEFT OUTER JOIN Depth " // returns all images even if there are no metric data
<< "ON Image.id = Depth.id "
@@ -611,7 +611,7 @@ void DBDriverSqlite3::getNodeDataQuery(
else
{
query << "SELECT Image.data, "
"Depth.data, Depth.fx, Depth.fy, Depth.cx, Depth.cy, Depth.local_transform, Depth.data2d "
"Depth.data, Depth.constant, Depth.local_transform, Depth.data2d "
<< "FROM Image "
<< "LEFT OUTER JOIN Depth " // returns all images even if there are no metric data
<< "ON Image.id = Depth.id "
@@ -756,7 +756,8 @@ bool DBDriverSqlite3::getNodeInfoQuery(int signatureId,
int & mapId,
int & weight,
std::string & label,
double & stamp) const
double & stamp,
std::vector<unsigned char> & userData) const
{
bool found = false;
if(_ppDb && signatureId)
@@ -766,9 +767,16 @@ bool DBDriverSqlite3::getNodeInfoQuery(int signatureId,
std::stringstream query;
// Prepare the query... Get the map from signature and visual words
if(uStrNumCmp(_version, "0.8.5") >= 0)
if(uStrNumCmp(_version, "0.8.8") >= 0)
{
query << "SELECT pose, map_id, weight, label "
query << "SELECT pose, map_id, weight, label, stamp, user_data"
"FROM Node "
"WHERE id = " << signatureId <<
";";
}
else if(uStrNumCmp(_version, "0.8.5") >= 0)
{
query << "SELECT pose, map_id, weight, label, stamp "
"FROM Node "
"WHERE id = " << signatureId <<
";";
@@ -810,6 +818,19 @@ bool DBDriverSqlite3::getNodeInfoQuery(int signatureId,
{
label = reinterpret_cast<const char*>(p); // label
}
stamp = sqlite3_column_double(ppStmt, index++); // stamp
}
if(uStrNumCmp(_version, "0.8.8") >= 0)
{
data = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++); // user_data
if(dataSize && data)
{
userData.resize(dataSize);
memcpy(userData.data(), data, dataSize);
}
}
rc = sqlite3_step(ppStmt); // next result...
@@ -1073,7 +1094,13 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
unsigned int loaded = 0;
// Load nodes information
if(uStrNumCmp(_version, "0.8.5") >= 0)
if(uStrNumCmp(_version, "0.8.8") >= 0)
{
query << "SELECT id, map_id, weight, pose, stamp, label, user_data "
<< "FROM Node "
<< "WHERE id=?;";
}
else if(uStrNumCmp(_version, "0.8.5") >= 0)
{
query << "SELECT id, map_id, weight, pose, stamp, label "
<< "FROM Node "
@@ -1104,6 +1131,7 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
const void * data = 0;
int dataSize = 0;
std::string label;
std::vector<unsigned char> userData;
// Process the result if one
rc = sqlite3_step(ppStmt);
@@ -1123,14 +1151,26 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
if(uStrNumCmp(_version, "0.8.5") >= 0)
{
stamp = sqlite3_column_double(ppStmt, index++);
const unsigned char * p = sqlite3_column_text(ppStmt, index++);
stamp = sqlite3_column_double(ppStmt, index++); // stamp
const unsigned char * p = sqlite3_column_text(ppStmt, index++); // label
if(p)
{
label = reinterpret_cast<const char*>(p);
}
}
if(uStrNumCmp(_version, "0.8.8") >= 0)
{
data = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++); // user_data
if(dataSize && data)
{
userData.resize(dataSize);
memcpy(userData.data(), data, dataSize);
}
}
rc = sqlite3_step(ppStmt);
}
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
@@ -1147,7 +1187,8 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
label,
std::multimap<int, cv::KeyPoint>(),
std::multimap<int, pcl::PointXYZ>(),
pose);
pose,
userData);
s->setSaved(true);
nodes.push_back(s);
++loaded;
@@ -1704,7 +1745,18 @@ void DBDriverSqlite3::updateQuery(const std::list<Signature *> & nodes, bool upd
Signature * s = 0;
std::string query;
if(uStrNumCmp(_version, "0.8.5") >= 0)
if(uStrNumCmp(_version, "0.8.8") >= 0)
{
if(updateTimestamp)
{
query = "UPDATE Node SET weight=?, label=?, user_data=?, time_enter = DATETIME('NOW') WHERE id=?;";
}
else
{
query = "UPDATE Node SET weight=?, label=?, user_data=? WHERE id=?;";
}
}
else if(uStrNumCmp(_version, "0.8.5") >= 0)
{
if(updateTimestamp)
{
@@ -1752,6 +1804,20 @@ void DBDriverSqlite3::updateQuery(const std::list<Signature *> & nodes, bool upd
}
}
if(uStrNumCmp(_version, "0.8.8") >= 0)
{
if(s->getUserData().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_blob(ppStmt, index++, s->getUserData().data(), (int)s->getUserData().size(), 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());
@@ -2049,7 +2115,11 @@ void DBDriverSqlite3::saveQuery(const std::list<VisualWord *> & words) const
std::string DBDriverSqlite3::queryStepNode() const
{
if(uStrNumCmp(_version, "0.8.5") >= 0)
if(uStrNumCmp(_version, "0.8.8") >= 0)
{
return "INSERT INTO Node(id, map_id, weight, pose, stamp, label, user_data) VALUES(?,?,?,?,?,?,?);";
}
else if(uStrNumCmp(_version, "0.8.5") >= 0)
{
return "INSERT INTO Node(id, map_id, weight, pose, stamp, label) VALUES(?,?,?,?,?,?);";
}
@@ -2091,6 +2161,20 @@ void DBDriverSqlite3::stepNode(sqlite3_stmt * ppStmt, const Signature * s) const
}
}
if(uStrNumCmp(_version, "0.8.8") >= 0)
{
if(s->getUserData().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_blob(ppStmt, index++, s->getUserData().data(), (int)s->getUserData().size(), 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());
@@ -2139,13 +2223,13 @@ void DBDriverSqlite3::stepImage(sqlite3_stmt * ppStmt,
std::string DBDriverSqlite3::queryStepDepth() const
{
if(uStrNumCmp(_version, "0.7.0") < 0)
if(uStrNumCmp(_version, "0.7.0") >= 0)
{
return "INSERT INTO Depth(id, data, constant, local_transform, data2d) VALUES(?,?,?,?,?);";
return "INSERT INTO Depth(id, data, fx, fy, cx, cy, local_transform, data2d) VALUES(?,?,?,?,?,?,?,?);";
}
else
{
return "INSERT INTO Depth(id, data, fx, fy, cx, cy, local_transform, data2d) VALUES(?,?,?,?,?,?,?,?);";
return "INSERT INTO Depth(id, data, constant, local_transform, data2d) VALUES(?,?,?,?,?);";
}
}
void DBDriverSqlite3::stepDepth(sqlite3_stmt * ppStmt,
@@ -2180,12 +2264,7 @@ void DBDriverSqlite3::stepDepth(sqlite3_stmt * ppStmt,
}
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
if(uStrNumCmp(_version, "0.7.0") < 0)
{
rc = sqlite3_bind_double(ppStmt, index++, 1.0f/fx);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
}
else
if(uStrNumCmp(_version, "0.7.0") >= 0)
{
rc = sqlite3_bind_double(ppStmt, index++, fx);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
@@ -2196,6 +2275,11 @@ void DBDriverSqlite3::stepDepth(sqlite3_stmt * ppStmt,
rc = sqlite3_bind_double(ppStmt, index++, cy);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
}
else
{
rc = sqlite3_bind_double(ppStmt, index++, 1.0f/fx);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
}
rc = sqlite3_bind_blob(ppStmt, index++, localTransform.data(), localTransform.size()*sizeof(float), SQLITE_STATIC);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

2
corelib/src/DBDriverSqlite3.h
View File

@@ -82,7 +82,7 @@ private:
float & cy,
Transform & localTransform) const;
virtual void getNodeDataQuery(int signatureId, cv::Mat & imageCompressed) const;
virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp) const;
virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, std::vector<unsigned char> & userData) const;
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;

20
corelib/src/DBReader.cpp
View File

@@ -195,13 +195,17 @@ SensorData DBReader::getNextData()
Transform localTransform, pose;
float rotVariance = 1.0f;
float transVariance = 1.0f;
std::vector<unsigned char> userData;
_dbDriver->getNodeData(*_currentId, imageBytes, depthBytes, laserScanBytes, fx, fy, cx, cy, localTransform);
// info
int weight;
std::string label;
double stamp;
_dbDriver->getNodeInfo(*_currentId, pose, mapId, weight, label, stamp, userData);
if(!_odometryIgnored)
{
int weight;
std::string label;
double stamp;
_dbDriver->getNodeInfo(*_currentId, pose, mapId, weight, label, stamp);
std::map<int, Link> links;
_dbDriver->loadLinks(*_currentId, links, Link::kNeighbor);
if(links.size())
@@ -211,6 +215,11 @@ SensorData DBReader::getNextData()
transVariance = links.begin()->second.transVariance();
}
}
else
{
pose.setNull();
}
int seq = *_currentId;
++_currentId;
if(imageBytes.empty())
@@ -237,7 +246,8 @@ SensorData DBReader::getNextData()
rotVariance,
transVariance,
seq,
UTimer::now());
UTimer::now(),
userData);
UDEBUG("Laser=%d RGB/Left=%d Depth=%d Right=%d",
data.laserScan().empty()?0:1,
data.image().empty()?0:1,

86
corelib/src/Memory.cpp
View File

@@ -1648,41 +1648,38 @@ int Memory::getSignatureIdByLabel(const std::string & label, bool lookInDatabase
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)
{
// verify that this label is not used
int idFound=getSignatureIdByLabel(label);
if(idFound == 0 || idFound == id)
Signature * s = this->_getSignature(id);
if(s)
{
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())
{
s->setLabel(label);
signatures.front()->setLabel(label);
_dbDriver->asyncSave(signatures.front()); // move it again to trash
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)
else
{
UWARN("Node %d has already label \"%s\"", idFound, label.c_str());
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;
}
@@ -1703,6 +1700,34 @@ std::map<int, std::string> Memory::getAllLabels() const
return labels;
}
bool Memory::setUserData(int id, const std::vector<unsigned char> & data)
{
Signature * s = this->_getSignature(id);
if(s)
{
s->setUserData(data);
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()->setUserData(data);
_dbDriver->asyncSave(signatures.front()); // move it again to trash
return true;
}
}
else
{
UERROR("Node %d not found, failed to set user data (size=%d)!", id, data.size());
}
return false;
}
void Memory::deleteLocation(int locationId, std::list<int> * deletedWords)
{
UDEBUG("Deleting location %d", locationId);
@@ -2711,7 +2736,8 @@ Transform Memory::getOdomPose(int signatureId, bool lookInDatabase) const
int mapId, weight;
std::string label;
double stamp;
getNodeInfo(signatureId, pose, mapId, weight, label, stamp, lookInDatabase);
std::vector<unsigned char> userData;
getNodeInfo(signatureId, pose, mapId, weight, label, stamp, userData, lookInDatabase);
return pose;
}
@@ -2721,6 +2747,7 @@ bool Memory::getNodeInfo(int signatureId,
int & weight,
std::string & label,
double & stamp,
std::vector<unsigned char> & userData,
bool lookInDatabase) const
{
const Signature * s = this->getSignature(signatureId);
@@ -2731,11 +2758,12 @@ bool Memory::getNodeInfo(int signatureId,
weight = s->getWeight();
label = s->getLabel();
stamp = s->getStamp();
userData = s->getUserData();
return true;
}
else if(lookInDatabase && _dbDriver)
{
return _dbDriver->getNodeInfo(signatureId, odomPose, mapId, weight, label, stamp);
return _dbDriver->getNodeInfo(signatureId, odomPose, mapId, weight, label, stamp, userData);
}
return false;
}
@@ -3658,6 +3686,7 @@ Signature * Memory::createSignature(const SensorData & data, Statistics * stats)
words,
words3D,
data.pose(),
data.userData(),
ctDepth2d.getCompressedData(),
ctImage.getCompressedData(),
ctDepth.getCompressedData(),
@@ -3677,6 +3706,7 @@ Signature * Memory::createSignature(const SensorData & data, Statistics * stats)
words,
words3D,
data.pose(),
data.userData(),
rtabmap::compressData2(data.laserScan()));
}
if(this->isRawDataKept())

103
corelib/src/Rtabmap.cpp
View File

@@ -261,15 +261,32 @@ void Rtabmap::init(const ParametersMap & parameters, const std::string & databas
}
_databasePath = databasePath;
if(!_databasePath.empty())
{
UASSERT(UFile::getExtension(_databasePath).compare("db") == 0);
UINFO("Using database \"%s\".", _databasePath.c_str());
}
else
{
UWARN("Using empty database. Mapping session will not be saved.");
}
bool newDatabase = _databasePath.empty() || !UFile::exists(_databasePath);
// If not exist, create a memory
if(!_memory)
{
_memory = new Memory(parameters);
_memory->init(_databasePath, false, parameters, true);
}
// Parse all parameters
this->parseParameters(parameters);
if(_databasePath.empty())
{
_databasePath = _wDir + "/" + Parameters::getDefaultDatabaseName();
_statisticLogged = false;
}
UASSERT(UFile::getExtension(_databasePath).compare("db") == 0);
UINFO("Using database \"%s\".", _databasePath.c_str());
bool newDatabase = !UFile::exists(_databasePath);
this->parseParameters(parameters);
setupLogFiles(newDatabase);
}
@@ -417,16 +434,13 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
{
_graphOptimizer->parseParameters(parameters);
}
if(!_memory)
{
if(!_databasePath.empty())
{
_memory = new Memory(parameters);
_memory->init(_databasePath, false, parameters, true);
}
}
else
{
optimizerType = (graph::Optimizer::Type)Parameters::defaultRGBDOptimizeStrategy();
_graphOptimizer = graph::Optimizer::create(optimizerType, parameters);
}
if(_memory)
{
_memory->parseParameters(parameters);
}
@@ -621,7 +635,7 @@ int Rtabmap::triggerNewMap()
bool Rtabmap::labelLocation(int id, const std::string & label)
{
if(!label.empty() && _memory)
if(_memory)
{
if(id > 0)
{
@@ -639,6 +653,26 @@ bool Rtabmap::labelLocation(int id, const std::string & label)
return false;
}
bool Rtabmap::setUserData(int id, const std::vector<unsigned char> & data)
{
if(_memory)
{
if(id > 0)
{
return _memory->setUserData(id, data);
}
else if(_memory->getLastWorkingSignature())
{
return _memory->setUserData(_memory->getLastWorkingSignature()->id(), data);
}
else
{
UERROR("Last signature is null! Cannot set user data!");
}
}
return false;
}
void Rtabmap::generateDOTGraph(const std::string & path, int id, int margin)
{
if(_memory)
@@ -792,10 +826,9 @@ bool Rtabmap::process(const SensorData & data)
timer.start();
timerTotal.start();
if(!_memory || !_bayesFilter || !_graphOptimizer)
{
UFATAL("RTAB-Map is not initialized, data received is ignored.");
}
UASSERT_MSG(_memory, "RTAB-Map is not initialized!");
UASSERT_MSG(_bayesFilter, "RTAB-Map is not initialized!");
UASSERT_MSG(_graphOptimizer, "RTAB-Map is not initialized!");
//============================================================
// If RGBD SLAM is enabled, a pose must be set.
@@ -1684,6 +1717,8 @@ bool Rtabmap::process(const SensorData & data)
std::map<int, Transform> poses;
std::map<int, int> mapIds;
std::map<int, std::string> labels;
std::map<int, double> stamps;
std::map<int, std::vector<unsigned char> > userDatas;
std::multimap<int, Link> constraints;
_memory->getMetricConstraints(uKeys(ids), poses, constraints, false);
for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
@@ -1693,14 +1728,22 @@ bool Rtabmap::process(const SensorData & data)
int mapId = -1;
std::string label;
double stamp = 0;
_memory->getNodeInfo(iter->first, odomPose, mapId, weight, label, stamp, false);
std::vector<unsigned char> userData;
_memory->getNodeInfo(iter->first, odomPose, mapId, weight, label, stamp, userData, false);
mapIds.insert(std::make_pair(iter->first, mapId));
labels.insert(std::make_pair(iter->first, label));
stamps.insert(std::make_pair(iter->first, stamp));
if(userData.size())
{
userDatas.insert(std::make_pair(iter->first, userData));
}
}
statistics_.setPoses(poses);
statistics_.setConstraints(constraints);
statistics_.setMapIds(mapIds);
statistics_.setLabels(labels);
statistics_.setStamps(stamps);
statistics_.setUserDatas(userDatas);
}
else // RGBD-SLAM mode
{
@@ -1875,6 +1918,8 @@ bool Rtabmap::process(const SensorData & data)
{
std::map<int, int> mapIds;
std::map<int, std::string> labels;
std::map<int, double> stamps;
std::map<int, std::vector<unsigned char> > userDatas;
for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
{
Transform odomPose;
@@ -1882,14 +1927,22 @@ bool Rtabmap::process(const SensorData & data)
int mapId = -1;
std::string label;
double stamp = 0;
_memory->getNodeInfo(iter->first, odomPose, mapId, weight, label, stamp, true);
std::vector<unsigned char> userData;
_memory->getNodeInfo(iter->first, odomPose, mapId, weight, label, stamp, userData, true);
mapIds.insert(std::make_pair(iter->first, mapId));
labels.insert(std::make_pair(iter->first, label));
stamps.insert(std::make_pair(iter->first, stamp));
if(userData.size())
{
userDatas.insert(std::make_pair(iter->first, userData));
}
}
statistics_.setPoses(_optimizedPoses);
statistics_.setConstraints(_constraints);
statistics_.setMapIds(mapIds);
statistics_.setLabels(labels);
statistics_.setStamps(stamps);
statistics_.setUserDatas(userDatas);
}
}
@@ -2361,7 +2414,8 @@ void Rtabmap::get3DMap(std::map<int, Signature> & signatures,
int mapId = -1;
std::string label;
double stamp = 0;
_memory->getNodeInfo(iter->first, odomPose, mapId, weight, label, stamp, true);
std::vector<unsigned char> userData;
_memory->getNodeInfo(iter->first, odomPose, mapId, weight, label, stamp, userData, true);
mapIds.insert(std::make_pair(iter->first, mapId));
labels.insert(std::make_pair(iter->first, label));
}
@@ -2434,7 +2488,8 @@ void Rtabmap::getGraph(
int mapId = -1;
std::string label;
double stamp = 0;
_memory->getNodeInfo(iter->first, odomPose, mapId, weight, label, stamp, true);
std::vector<unsigned char> userData;
_memory->getNodeInfo(iter->first, odomPose, mapId, weight, label, stamp, userData, true);
mapIds.insert(std::make_pair(iter->first, mapId));
labels.insert(std::make_pair(iter->first, label));
}

43
corelib/src/RtabmapThread.cpp
View File

@@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/CameraEvent.h"
#include "rtabmap/core/ParamEvent.h"
#include "rtabmap/core/OdometryEvent.h"
#include "rtabmap/core/UserDataEvent.h"
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UEventsManager.h>
@@ -90,6 +91,12 @@ void RtabmapThread::clearBufferedData()
_transVariance = 0;
}
_dataMutex.unlock();
_userDataMutex.lock();
{
_userData = cv::Mat();
}
_userDataMutex.unlock();
}
void RtabmapThread::setDetectorRate(float rate)
@@ -175,6 +182,7 @@ void RtabmapThread::mainLoop()
}
_stateMutex.unlock();
std::vector<unsigned char> userData;
switch(state)
{
case kStateDetecting:
@@ -243,6 +251,15 @@ void RtabmapThread::mainLoop()
case kStateTriggeringMap:
_rtabmap->triggerNewMap();
break;
case kStateAddingUserData:
_userDataMutex.lock();
{
userData = _userData;
_userData.clear();
}
_userDataMutex.unlock();
_rtabmap->setUserData(0, userData);
break;
default:
UFATAL("Invalid state !?!?");
break;
@@ -274,6 +291,32 @@ void RtabmapThread::handleEvent(UEvent* event)
lastPose_.setNull();
}
}
else if(event->getClassName().compare("UserDataEvent") == 0)
{
if(!_paused)
{
UDEBUG("UserDataEvent");
bool updated = false;
UserDataEvent * e = (UserDataEvent*)event;
_userDataMutex.lock();
if(!e->data().empty())
{
updated = !_userData.empty();
_userData = e->data();
}
_userDataMutex.unlock();
if(updated)
{
UWARN("New user data received before the last one was processed... replacing "
"user data with this new one. Note that UserDataEvent should be used only "
"if the rate of UserDataEvent is lower than RTAB-Map's detection rate (%f Hz).", _rate);
}
else
{
pushNewState(kStateAddingUserData);
}
}
}
else if(event->getClassName().compare("RtabmapEventCmd") == 0)
{
RtabmapEventCmd * rtabmapEvent = (RtabmapEventCmd*)event;

19
corelib/src/SensorData.cpp
View File

@@ -36,7 +36,6 @@ namespace rtabmap
* An id is automatically generated if id=0.
*/
SensorData::SensorData() :
_image(cv::Mat()),
_id(0),
_stamp(0.0),
_fx(0.0f),
@@ -51,7 +50,8 @@ SensorData::SensorData() :
SensorData::SensorData(const cv::Mat & image,
int id,
double stamp) :
double stamp,
const std::vector<unsigned char> & userData) :
_image(image),
_id(id),
_stamp(stamp),
@@ -61,7 +61,8 @@ SensorData::SensorData(const cv::Mat & image,
_cy(0.0f),
_localTransform(Transform::getIdentity()),
_poseRotVariance(1.0f),
_poseTransVariance(1.0f)
_poseTransVariance(1.0f),
_userData(userData)
{
UASSERT(image.type() == CV_8UC1 || // Mono
image.type() == CV_8UC3); // RGB
@@ -79,7 +80,8 @@ SensorData::SensorData(const cv::Mat & image,
float poseRotVariance,
float poseTransVariance,
int id,
double stamp) :
double stamp,
const std::vector<unsigned char> & userData) :
_image(image),
_id(id),
_stamp(stamp),
@@ -91,7 +93,8 @@ SensorData::SensorData(const cv::Mat & image,
_pose(pose),
_localTransform(localTransform),
_poseRotVariance(poseRotVariance),
_poseTransVariance(poseTransVariance)
_poseTransVariance(poseTransVariance),
_userData(userData)
{
UASSERT(image.type() == CV_8UC1 || // Mono
image.type() == CV_8UC3); // RGB
@@ -115,7 +118,8 @@ SensorData::SensorData(const cv::Mat & laserScan,
float poseRotVariance,
float poseTransVariance,
int id,
double stamp) :
double stamp,
const std::vector<unsigned char> & userData) :
_image(image),
_id(id),
_stamp(stamp),
@@ -128,7 +132,8 @@ SensorData::SensorData(const cv::Mat & laserScan,
_pose(pose),
_localTransform(localTransform),
_poseRotVariance(poseRotVariance),
_poseTransVariance(poseTransVariance)
_poseTransVariance(poseTransVariance),
_userData(userData)
{
UASSERT(_laserScan.empty() || _laserScan.type() == CV_32FC2);
UASSERT(image.type() == CV_8UC1 || // Mono

17
corelib/src/Signature.cpp
View File

@@ -61,6 +61,7 @@ Signature::Signature(
const std::multimap<int, cv::KeyPoint> & words,
const std::multimap<int, pcl::PointXYZ> & words3, // in base_link frame (localTransform applied)
const Transform & pose,
const std::vector<unsigned char> & userData,
const cv::Mat & laserScanCompressed, // in base_link frame
const cv::Mat & imageCompressed, // in camera_link frame
const cv::Mat & depthCompressed, // in camera_link frame
@@ -74,6 +75,7 @@ Signature::Signature(
_stamp(stamp),
_weight(weight),
_label(label),
_userData(userData),
_saved(false),
_modified(true),
_linksModified(true),
@@ -97,6 +99,18 @@ Signature::~Signature()
//UDEBUG("id=%d", _id);
}
void Signature::setUserData(const std::vector<unsigned char> & data)
{
if(!_userData.empty() && !data.empty())
{
UWARN("Node %d: Current user data (%d bytes) overwritten by new data (%d bytes)",
_id, (int)_userData.size(), (int)data.size());
}
_modified = true;
_userData = data;
}
void Signature::addLinks(const std::list<Link> & links)
{
for(std::list<Link>::const_iterator iter = links.begin(); iter!=links.end(); ++iter)
@@ -265,7 +279,8 @@ SensorData Signature::toSensorData()
rotVariance,
transVariance,
_id,
_stamp);
_stamp,
_userData);
}
void Signature::uncompressData()

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

@@ -20,26 +20,28 @@ CREATE TABLE Node (
stamp FLOAT,
pose BLOB,
label TEXT,
user_data BLOB,
time_enter DATE,
PRIMARY KEY (id)
);
CREATE TABLE Image (
id INTEGER NOT NULL,
data BLOB,
data BLOB, -- compressed image (RGB)
time_enter DATE,
PRIMARY KEY (id)
);
-- TODO: Merge "Image" and "Depth" tables to "Data" table.
CREATE TABLE Depth (
id INTEGER NOT NULL,
data BLOB, -- CV_32FC1, width = Image/raw_width, height=Image/raw_height
data BLOB, -- compressed image (Depth or Right image)
fx FLOAT,
fy FLOAT,
fy FLOAT, -- baseline if stereo
cx FLOAT,
cy FLOAT,
local_transform BLOB,
data2d BLOB, -- CV_32FC2, Example: Laser scan
data2d BLOB, -- compressed data (Laser scan)
time_enter DATE,
PRIMARY KEY (id)
);