i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-01 17:10:26 +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

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

@@ -97,7 +97,7 @@ public:
void loadNodeData(std::list<Signature *> & signatures, bool loadMetricData) const;
void getNodeData(int signatureId, cv::Mat & imageCompressed, cv::Mat & depthCompressed, cv::Mat & laserScanCompressed, float & fx, float & fy, float & cx, float & cy, Transform & localTransform) const;
void getNodeData(int signatureId, cv::Mat & imageCompressed) const;
bool getNodeInfo(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp) const;
bool getNodeInfo(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, std::vector<unsigned char> & userData) const;
void loadLinks(int signatureId, std::map<int, Link> & links, Link::Type type = Link::kUndef) const;
void getWeight(int signatureId, int & weight) const;
void getAllNodeIds(std::set<int> & ids, bool ignoreChildren = false) const;
@@ -136,7 +136,7 @@ private:
virtual void loadNodeDataQuery(std::list<Signature *> & signatures, bool loadMetricData) const = 0;
virtual void getNodeDataQuery(int signatureId, cv::Mat & imageCompressed, cv::Mat & depthCompressed, cv::Mat & laserScanCompressed, float & fx, float & fy, float & cx, float & cy, Transform & localTransform) const = 0;
virtual void getNodeDataQuery(int signatureId, cv::Mat & imageCompressed) const = 0;
virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp) const = 0;
virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, std::vector<unsigned char> & userData) const = 0;
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;

2
corelib/include/rtabmap/core/Memory.h
View File

@@ -110,6 +110,7 @@ public:
int getSignatureIdByLabel(const std::string & label, bool lookInDatabase = true) const;
bool labelSignature(int id, const std::string & label);
std::map<int, std::string> getAllLabels() const;
bool setUserData(int id, const std::vector<unsigned char> & data);
int getDatabaseMemoryUsed() const; // in bytes
double getDbSavingTime() const;
Transform getOdomPose(int signatureId, bool lookInDatabase = false) const;
@@ -119,6 +120,7 @@ public:
int & weight,
std::string & label,
double & stamp,
std::vector<unsigned char> & userData,
bool lookInDatabase = false) const;
cv::Mat getImageCompressed(int signatureId) const;
Signature getSignatureData(int locationId, bool uncompressedData = false);

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

@@ -103,6 +103,7 @@ public:
int triggerNewMap();
bool labelLocation(int id, const std::string & label);
bool setUserData(int id, const std::vector<unsigned char> & data);
void generateDOTGraph(const std::string & path, int id=0, int margin=5);
void generateTOROGraph(const std::string & path, bool optimized, bool global);
void resetMemory();

6
corelib/include/rtabmap/core/RtabmapThread.h
View File

@@ -69,7 +69,8 @@ public:
kStatePublishingMapGlobal,
kStatePublishingTOROGraphLocal,
kStatePublishingTOROGraphGlobal,
kStateTriggeringMap
kStateTriggeringMap,
kStateAddingUserData
};
public:
@@ -112,6 +113,9 @@ private:
Transform lastPose_;
float _rotVariance;
float _transVariance;
std::vector<unsigned char> _userData;
UMutex _userDataMutex;
};
} /* namespace rtabmap */

14
corelib/include/rtabmap/core/SensorData.h
View File

@@ -43,7 +43,7 @@ class RTABMAP_EXP SensorData
{
public:
SensorData(); // empty constructor
SensorData(const cv::Mat & image, int id = 0, double stamp = 0.0);
SensorData(const cv::Mat & image, int id = 0, double stamp = 0.0, const std::vector<unsigned char> & userData = std::vector<unsigned char>());
// Metric constructor
SensorData(const cv::Mat & image,
@@ -57,7 +57,8 @@ public:
float poseRotVariance,
float poseTransVariance,
int id,
double stamp);
double stamp,
const std::vector<unsigned char> & userData = std::vector<unsigned char>());
// Metric constructor + 2d laser scan
SensorData(const cv::Mat & laserScan,
@@ -72,7 +73,8 @@ public:
float poseRotVariance,
float poseTransVariance,
int id,
double stamp);
double stamp,
const std::vector<unsigned char> & userData = std::vector<unsigned char>());
virtual ~SensorData() {}
@@ -111,6 +113,9 @@ public:
const std::vector<cv::KeyPoint> & keypoints() const {return _keypoints;}
const cv::Mat & descriptors() const {return _descriptors;}
void setUserData(const std::vector<unsigned char> & data) {_userData = data;}
const std::vector<unsigned char> userData() const {return _userData;}
private:
cv::Mat _image;
int _id;
@@ -131,6 +136,9 @@ private:
// features
std::vector<cv::KeyPoint> _keypoints;
cv::Mat _descriptors;
// user data
std::vector<unsigned char> _userData;
};
}

5
corelib/include/rtabmap/core/Signature.h
View File

@@ -60,6 +60,7 @@ public:
const std::multimap<int, cv::KeyPoint> & words,
const std::multimap<int, pcl::PointXYZ> & words3,
const Transform & pose = Transform(),
const std::vector<unsigned char> & userData = std::vector<unsigned char>(),
const cv::Mat & laserScan = cv::Mat(),
const cv::Mat & image = cv::Mat(),
const cv::Mat & depth = cv::Mat(),
@@ -85,6 +86,9 @@ public:
void setLabel(const std::string & label) {_modified=_label.compare(label)!=0;_label = label;}
const std::string & getLabel() const {return _label;}
void setUserData(const std::vector<unsigned char> & data);
const std::vector<unsigned char> & getUserData() const {return _userData;}
double getStamp() const {return _stamp;}
void addLinks(const std::list<Link> & links);
@@ -153,6 +157,7 @@ private:
std::map<int, Link> _links; // id, transform
int _weight;
std::string _label;
std::vector<unsigned char> _userData;
bool _saved; // If it's saved to bd
bool _modified;
bool _linksModified; // Optimization when updating signatures in database

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

@@ -129,6 +129,8 @@ public:
void setMapIds(const std::map<int, int> & mapIds) {_mapIds = mapIds;}
void setLabels(const std::map<int, std::string> & labels) {_labels = labels;}
void setStamps(const std::map<int, double> & stamps) {_stamps = stamps;}
void setUserDatas(const std::map<int, std::vector<unsigned char> > & userDatas) {_userDatas = userDatas;}
void setSignature(const Signature & s) {_signature = s;}
void setPoses(const std::map<int, Transform> & poses) {_poses = poses;}
@@ -149,6 +151,8 @@ public:
const std::map<int, int> & getMapIds() const {return _mapIds;}
const std::map<int, std::string> & getLabels() const {return _labels;}
const std::map<int, double> & getStamps() const {return _stamps;}
const std::map<int, std::vector<unsigned char> > & getUserDatas() const {return _userDatas;}
const Signature & getSignature() const {return _signature;}
const std::map<int, Transform> & poses() const {return _poses;}
@@ -173,6 +177,8 @@ private:
// extended data start here...
std::map<int, int> _mapIds;
std::map<int, std::string> _labels;
std::map<int, double> _stamps;
std::map<int, std::vector<unsigned char> > _userDatas;
// Signature data
Signature _signature;

59
corelib/include/rtabmap/core/UserDataEvent.h Normal file
View File

@@ -0,0 +1,59 @@
/*
Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef USERDATAEVENT_H_
#define USERDATAEVENT_H_
#include <opencv2/opencv.hpp>
#include <rtabmap/utilite/UEvent.h>
namespace rtabmap
{
/**
* The user data event.
*/
class UserDataEvent : public UEvent
{
public:
UserDataEvent(const std::vector<unsigned char> & data) :
UEvent(0),
data_(data)
{}
~UserDataEvent() {}
virtual std::string getClassName() const {return "UserDataEvent";}
const std::vector<unsigned char> & data() const {return data_;}
private:
std::vector<unsigned char> data_;
};
}
#endif /* USERDATAEVENT_H_ */

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)
);