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.10.1: user_data is now a cv::Mat to avoid a deep copy when SensorData is copied

Browse Source
This commit is contained in:
matlabbe
2015-06-28 19:22:18 -04:00
parent 7d3a3317b7
commit 4f96fd3530
22 changed files with 516 additions and 640 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -95,9 +95,9 @@ public:
void loadWords(const std::set<int> & wordIds, std::list<VisualWord *> & vws);
// Specific queries...
void loadNodeData(std::list<Signature *> & signatures, bool loadMetricData) const;
void loadNodeData(std::list<Signature *> & signatures) const;
void getNodeData(int signatureId, SensorData & data) const;
bool getNodeInfo(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, std::vector<unsigned char> & userData) const;
bool getNodeInfo(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp) 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;
@@ -134,9 +134,8 @@ private:
virtual void loadWordsQuery(const std::set<int> & wordIds, std::list<VisualWord *> & vws) const = 0;
virtual void loadLinksQuery(int signatureId, std::map<int, Link> & links, Link::Type type = Link::kUndef) const = 0;
virtual void loadNodeDataQuery(std::list<Signature *> & signatures, bool loadMetricData) const = 0;
virtual void getNodeDataQuery(int signatureId, SensorData & data) 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 loadNodeDataQuery(std::list<Signature *> & signatures) const = 0;
virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp) const = 0;
virtual void getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren) const = 0;
virtual void getAllLinksQuery(std::multimap<int, Link> & links, bool ignoreNullLinks) const = 0;
virtual void getLastIdQuery(const std::string & tableName, int & id) const = 0;

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

@@ -127,7 +127,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);
bool setUserData(int id, const cv::Mat & data);
int getDatabaseMemoryUsed() const; // in bytes
double getDbSavingTime() const;
Transform getOdomPose(int signatureId, bool lookInDatabase = false) const;
@@ -137,7 +137,6 @@ public:
int & weight,
std::string & label,
double & stamp,
std::vector<unsigned char> & userData,
bool lookInDatabase = false) const;
cv::Mat getImageCompressed(int signatureId) const;
SensorData getNodeData(int nodeId, bool uncompressedData = false);

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

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

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

@@ -120,7 +120,7 @@ private:
double _rotVariance;
double _transVariance;
std::vector<unsigned char> _userData;
cv::Mat _userData;
UMutex _userDataMutex;
};

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

@@ -52,7 +52,7 @@ public:
const cv::Mat & image,
int id = 0,
double stamp = 0.0,
const std::vector<unsigned char> & userData = std::vector<unsigned char>());
const cv::Mat & userData = cv::Mat());
// Mono constructor
SensorData(
@@ -60,7 +60,7 @@ public:
const CameraModel & cameraModel,
int id = 0,
double stamp = 0.0,
const std::vector<unsigned char> & userData = std::vector<unsigned char>());
const cv::Mat & userData = cv::Mat());
// RGB-D constructor
SensorData(
@@ -69,7 +69,7 @@ public:
const CameraModel & cameraModel,
int id = 0,
double stamp = 0.0,
const std::vector<unsigned char> & userData = std::vector<unsigned char>());
const cv::Mat & userData = cv::Mat());
// RGB-D constructor + 2d laser scan
SensorData(
@@ -80,7 +80,7 @@ public:
const CameraModel & cameraModel,
int id = 0,
double stamp = 0.0,
const std::vector<unsigned char> & userData = std::vector<unsigned char>());
const cv::Mat & userData = cv::Mat());
// Multi-cameras RGB-D constructor
SensorData(
@@ -89,7 +89,7 @@ public:
const std::vector<CameraModel> & cameraModels,
int id = 0,
double stamp = 0.0,
const std::vector<unsigned char> & userData = std::vector<unsigned char>());
const cv::Mat & userData = cv::Mat());
// Multi-cameras RGB-D constructor + 2d laser scan
SensorData(
@@ -100,7 +100,7 @@ public:
const std::vector<CameraModel> & cameraModels,
int id = 0,
double stamp = 0.0,
const std::vector<unsigned char> & userData = std::vector<unsigned char>());
const cv::Mat & userData = cv::Mat());
// Stereo constructor
SensorData(
@@ -109,7 +109,7 @@ public:
const StereoCameraModel & cameraModel,
int id = 0,
double stamp = 0.0,
const std::vector<unsigned char> & userData = std::vector<unsigned char>());
const cv::Mat & userData = cv::Mat());
// Stereo constructor + 2d laser scan
SensorData(
@@ -120,7 +120,7 @@ public:
const StereoCameraModel & cameraModel,
int id = 0,
double stamp = 0.0,
const std::vector<unsigned char> & userData = std::vector<unsigned char>());
const cv::Mat & userData = cv::Mat());
virtual ~SensorData() {}
@@ -136,7 +136,8 @@ public:
_laserScanCompressed.empty() &&
_cameraModels.size() == 0 &&
!_stereoCameraModel.isValid() &&
_userData.size() == 0 &&
!_userDataRaw.empty() &&
!_userDataCompressed.empty() &&
_keypoints.size() == 0 &&
_descriptors.empty());
}
@@ -166,14 +167,16 @@ public:
cv::Mat rightRaw() const {return _depthOrRightRaw.type()==CV_8UC1?_depthOrRightRaw:cv::Mat();}
void uncompressData();
void uncompressData(cv::Mat * imageRaw, cv::Mat * depthOrRightRaw, cv::Mat * laserScanRaw);
void uncompressDataConst(cv::Mat * imageRaw, cv::Mat * depthOrRightRaw, cv::Mat * laserScanRaw) const;
void uncompressData(cv::Mat * imageRaw, cv::Mat * depthOrRightRaw, cv::Mat * laserScanRaw = 0, cv::Mat * userDataRaw = 0);
void uncompressDataConst(cv::Mat * imageRaw, cv::Mat * depthOrRightRaw, cv::Mat * laserScanRaw = 0, cv::Mat * userDataRaw = 0) const;
const std::vector<CameraModel> & cameraModels() const {return _cameraModels;}
const StereoCameraModel & stereoCameraModel() const {return _stereoCameraModel;}
void setUserData(const std::vector<unsigned char> & data) {_userData = data;}
const std::vector<unsigned char> & userData() const {return _userData;}
void setUserDataRaw(const cv::Mat & userDataRaw); // only set raw
void setUserData(const cv::Mat & userData); // detect automatically if raw or compressed. If raw, the data is compressed too.
const cv::Mat & userDataRaw() const {return _userDataRaw;}
const cv::Mat & userDataCompressed() const {return _userDataCompressed;}
void setFeatures(const std::vector<cv::KeyPoint> & keypoints, const cv::Mat & descriptors)
{
@@ -200,7 +203,8 @@ private:
StereoCameraModel _stereoCameraModel;
// user data
std::vector<unsigned char> _userData;
cv::Mat _userDataCompressed; // compressed data
cv::Mat _userDataRaw;
// features
std::vector<cv::KeyPoint> _keypoints;

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

@@ -58,7 +58,6 @@ public:
double stamp = 0.0,
const std::string & label = std::string(),
const Transform & pose = Transform(),
const std::vector<unsigned char> & userData = std::vector<unsigned char>(),
const SensorData & sensorData = SensorData());
virtual ~Signature();
@@ -77,9 +76,6 @@ 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);
@@ -130,7 +126,6 @@ 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/UserDataEvent.h
View File

@@ -40,17 +40,17 @@ namespace rtabmap
class UserDataEvent : public UEvent
{
public:
UserDataEvent(const std::vector<unsigned char> & data) :
UserDataEvent(const cv::Mat & data) :
UEvent(0),
data_(data)
{}
~UserDataEvent() {}
virtual std::string getClassName() const {return "UserDataEvent";}
const std::vector<unsigned char> & data() const {return data_;}
const cv::Mat & data() const {return data_;}
private:
std::vector<unsigned char> data_;
cv::Mat data_;
};
}

16
corelib/src/DBDriver.cpp
View File

@@ -390,7 +390,7 @@ void DBDriver::loadWords(const std::set<int> & wordIds, std::list<VisualWord *>
}
}
void DBDriver::loadNodeData(std::list<Signature *> & signatures, bool loadMetricData) const
void DBDriver::loadNodeData(std::list<Signature *> & signatures) const
{
// Don't look in the trash, we assume that if we want to load
// data of a signature, it is not in thrash! Print an error if so.
@@ -406,7 +406,7 @@ void DBDriver::loadNodeData(std::list<Signature *> & signatures, bool loadMetric
_trashesMutex.unlock();
_dbSafeAccessMutex.lock();
this->loadNodeDataQuery(signatures, loadMetricData);
this->loadNodeDataQuery(signatures);
_dbSafeAccessMutex.unlock();
}
@@ -431,7 +431,11 @@ void DBDriver::getNodeData(
if(!found)
{
_dbSafeAccessMutex.lock();
this->getNodeDataQuery(signatureId, data);
std::list<Signature *> signatures;
Signature tmp(signatureId);
signatures.push_back(&tmp);
loadNodeDataQuery(signatures);
data = signatures.front()->sensorData();
_dbSafeAccessMutex.unlock();
}
}
@@ -441,8 +445,7 @@ bool DBDriver::getNodeInfo(int signatureId,
int & mapId,
int & weight,
std::string & label,
double & stamp,
std::vector<unsigned char> & userData) const
double & stamp) const
{
bool found = false;
// look in the trash
@@ -454,7 +457,6 @@ bool DBDriver::getNodeInfo(int signatureId,
weight = _trashSignatures.at(signatureId)->getWeight();
label = _trashSignatures.at(signatureId)->getLabel();
stamp = _trashSignatures.at(signatureId)->getStamp();
userData = _trashSignatures.at(signatureId)->getUserData();
found = true;
}
_trashesMutex.unlock();
@@ -462,7 +464,7 @@ bool DBDriver::getNodeInfo(int signatureId,
if(!found)
{
_dbSafeAccessMutex.lock();
found = this->getNodeInfoQuery(signatureId, pose, mapId, weight, label, stamp, userData);
found = this->getNodeInfoQuery(signatureId, pose, mapId, weight, label, stamp);
_dbSafeAccessMutex.unlock();
}
return found;

670
corelib/src/DBDriverSqlite3.cpp
View File

@@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "VisualWord.h"
#include "rtabmap/core/VWDictionary.h"
#include "rtabmap/core/util3d.h"
#include "rtabmap/core/Compression.h"
#include "DatabaseSchema_sql.h"
#include <set>
@@ -445,9 +446,9 @@ long DBDriverSqlite3::getMemoryUsedQuery() const
}
}
void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, bool loadMetricData) const
void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures) const
{
UDEBUG("load data (metric=%s) for %d signatures", loadMetricData?"true":"false", (int)signatures.size());
UDEBUG("load data for %d signatures", (int)signatures.size());
if(_ppDb)
{
UTimer timer;
@@ -456,62 +457,65 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
sqlite3_stmt * ppStmt = 0;
std::stringstream query;
if(loadMetricData)
if(uStrNumCmp(_version, "0.10.1") >= 0)
{
if(uStrNumCmp(_version, "0.10.0") >= 0)
{
query << "SELECT image, depth, calibration, scan_max_pts, scan "
<< "FROM Data "
<< "WHERE id = ?"
<<";";
}
else if(uStrNumCmp(_version, "0.8.11") >= 0)
{
query << "SELECT Image.data, "
"Depth.data, Depth.local_transform, Depth.fx, Depth.fy, Depth.cx, Depth.cy, Depth.data2d_max_pts, Depth.data2d "
<< "FROM Image "
<< "LEFT OUTER JOIN Depth " // returns all images even if there are no metric data
<< "ON Image.id = Depth.id "
<< "WHERE Image.id = ?"
<<";";
}
else if(uStrNumCmp(_version, "0.7.0") >= 0)
{
query << "SELECT Image.data, "
"Depth.data, Depth.local_transform, Depth.fx, Depth.fy, Depth.cx, Depth.cy, Depth.data2d "
<< "FROM Image "
<< "LEFT OUTER JOIN Depth " // returns all images even if there are no metric data
<< "ON Image.id = Depth.id "
<< "WHERE Image.id = ?"
<<";";
}
else
{
query << "SELECT Image.data, "
"Depth.data, Depth.local_transform, Depth.constant, Depth.data2d "
<< "FROM Image "
<< "LEFT OUTER JOIN Depth " // returns all images even if there are no metric data
<< "ON Image.id = Depth.id "
<< "WHERE Image.id = ?"
<<";";
}
query << "SELECT image, depth, calibration, scan_max_pts, scan, user_data "
<< "FROM Data "
<< "WHERE id = ?"
<<";";
}
else if(uStrNumCmp(_version, "0.10.0") >= 0)
{
query << "SELECT Data.image, Data.depth, Data.calibration, Data.scan_max_pts, Data.scan, Node.user_data "
<< "FROM Data "
<< "INNER JOIN Node "
<< "ON Data.id = Node.id "
<< "WHERE Data.id = ?"
<<";";
}
else if(uStrNumCmp(_version, "0.8.11") >= 0)
{
query << "SELECT Image.data, "
"Depth.data, Depth.local_transform, Depth.fx, Depth.fy, Depth.cx, Depth.cy, Depth.data2d_max_pts, Depth.data2d, Node.user_data "
<< "FROM Image "
<< "INNER JOIN Node "
<< "on Image.id = Node.id "
<< "LEFT OUTER JOIN Depth " // returns all images even if there are no metric data
<< "ON Image.id = Depth.id "
<< "WHERE Image.id = ?"
<<";";
}
else if(uStrNumCmp(_version, "0.8.8") >= 0)
{
query << "SELECT Image.data, "
"Depth.data, Depth.local_transform, Depth.fx, Depth.fy, Depth.cx, Depth.cy, Depth.data2d, Node.user_data "
<< "FROM Image "
<< "INNER JOIN Node "
<< "on Image.id = Node.id "
<< "LEFT OUTER JOIN Depth " // returns all images even if there are no metric data
<< "ON Image.id = Depth.id "
<< "WHERE Image.id = ?"
<<";";
}
else if(uStrNumCmp(_version, "0.7.0") >= 0)
{
query << "SELECT Image.data, "
"Depth.data, Depth.local_transform, Depth.fx, Depth.fy, Depth.cx, Depth.cy, Depth.data2d "
<< "FROM Image "
<< "LEFT OUTER JOIN Depth " // returns all images even if there are no metric data
<< "ON Image.id = Depth.id "
<< "WHERE Image.id = ?"
<<";";
}
else
{
if(uStrNumCmp(_version, "0.10.0") >= 0)
{
query << "SELECT image "
<< "FROM Data "
<< "WHERE id = ?"
<<";";
}
else
{
query << "SELECT data "
<< "FROM Image "
<< "WHERE id = ?"
<<";";
}
query << "SELECT Image.data, "
"Depth.data, Depth.local_transform, Depth.constant, Depth.data2d "
<< "FROM Image "
<< "LEFT OUTER JOIN Depth " // returns all images even if there are no metric data
<< "ON Image.id = Depth.id "
<< "WHERE Image.id = ?"
<<";";
}
rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
@@ -542,6 +546,7 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
StereoCameraModel stereoModel;
Transform localTransform = Transform::getIdentity();
cv::Mat scanCompressed;
cv::Mat userDataCompressed;
data = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++);
@@ -552,133 +557,152 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
imageCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone();
}
if(loadMetricData)
data = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++);
//Create the depth image
if(dataSize>4 && data)
{
depthOrRightCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone();
}
if(uStrNumCmp(_version, "0.10.0") < 0)
{
data = sqlite3_column_blob(ppStmt, index); // local transform
dataSize = sqlite3_column_bytes(ppStmt, index++);
if((unsigned int)dataSize == localTransform.size()*sizeof(float) && data)
{
memcpy(localTransform.data(), data, dataSize);
}
}
// calibration
if(uStrNumCmp(_version, "0.10.0") >= 0)
{
data = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++);
//Create the depth image
if(dataSize>4 && data)
// multi-cameras [fx,fy,cx,cy,local_transform, ... ,fx,fy,cx,cy,local_transform] (4+12)*float * numCameras
// stereo [fx, fy, cx, cy, baseline, local_transform] (5+12)*float
if(dataSize > 0 && data)
{
depthOrRightCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone();
}
if(uStrNumCmp(_version, "0.10.0") < 0)
{
data = sqlite3_column_blob(ppStmt, index); // local transform
dataSize = sqlite3_column_bytes(ppStmt, index++);
if((unsigned int)dataSize == localTransform.size()*sizeof(float) && data)
float * dataFloat = (float*)data;
if((unsigned int)dataSize % (4+localTransform.size())*sizeof(float) == 0)
{
memcpy(localTransform.data(), data, dataSize);
}
}
// calibration
if(uStrNumCmp(_version, "0.10.0") >= 0)
{
data = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++);
// multi-cameras [fx,fy,cx,cy,local_transform, ... ,fx,fy,cx,cy,local_transform] (4+12)*float * numCameras
// stereo [fx, fy, cx, cy, baseline, local_transform] (5+12)*float
if(dataSize > 0 && data)
{
float * dataFloat = (float*)data;
if((unsigned int)dataSize % (4+localTransform.size())*sizeof(float) == 0)
int cameraCount = dataSize / ((4+localTransform.size())*sizeof(float));
UDEBUG("Loading calibration for %d cameras (%d bytes)", cameraCount, dataSize);
int max = cameraCount*(4+localTransform.size());
for(int i=0; i<max; i+=4+localTransform.size())
{
int cameraCount = dataSize / ((4+localTransform.size())*sizeof(float));
UDEBUG("Loading calibration for %d cameras (%d bytes)", cameraCount, dataSize);
int max = cameraCount*(4+localTransform.size());
for(int i=0; i<max; i+=4+localTransform.size())
{
memcpy(localTransform.data(), dataFloat+i+4, localTransform.size()*sizeof(float));
models.push_back(CameraModel(
(double)dataFloat[i],
(double)dataFloat[i+1],
(double)dataFloat[i+2],
(double)dataFloat[i+3],
localTransform));
}
}
else if((unsigned int)dataSize == (5+localTransform.size())*sizeof(float))
{
UDEBUG("Loading calibration of a stereo camera");
memcpy(localTransform.data(), dataFloat+5, localTransform.size()*sizeof(float));
stereoModel = StereoCameraModel(
dataFloat[0], // fx
dataFloat[1], // fy
dataFloat[2], // cx
dataFloat[3], // cy
dataFloat[4], // baseline
localTransform);
}
else
{
UFATAL("Wrong format of the Data.calibration field (size=%d bytes)", dataSize);
memcpy(localTransform.data(), dataFloat+i+4, localTransform.size()*sizeof(float));
models.push_back(CameraModel(
(double)dataFloat[i],
(double)dataFloat[i+1],
(double)dataFloat[i+2],
(double)dataFloat[i+3],
localTransform));
}
}
}
else if(uStrNumCmp(_version, "0.7.0") >= 0)
{
double fx = sqlite3_column_double(ppStmt, index++);
double fyOrBaseline = sqlite3_column_double(ppStmt, index++);
double cx = sqlite3_column_double(ppStmt, index++);
double cy = sqlite3_column_double(ppStmt, index++);
if(fyOrBaseline < 1.0)
else if((unsigned int)dataSize == (5+localTransform.size())*sizeof(float))
{
//it is a baseline
stereoModel = StereoCameraModel(fx,fx,cx,cy,fyOrBaseline, localTransform);
UDEBUG("Loading calibration of a stereo camera");
memcpy(localTransform.data(), dataFloat+5, localTransform.size()*sizeof(float));
stereoModel = StereoCameraModel(
dataFloat[0], // fx
dataFloat[1], // fy
dataFloat[2], // cx
dataFloat[3], // cy
dataFloat[4], // baseline
localTransform);
}
else
{
models.push_back(CameraModel(fx, fyOrBaseline, cx, cy, localTransform));
UFATAL("Wrong format of the Data.calibration field (size=%d bytes)", dataSize);
}
}
}
else if(uStrNumCmp(_version, "0.7.0") >= 0)
{
double fx = sqlite3_column_double(ppStmt, index++);
double fyOrBaseline = sqlite3_column_double(ppStmt, index++);
double cx = sqlite3_column_double(ppStmt, index++);
double cy = sqlite3_column_double(ppStmt, index++);
if(fyOrBaseline < 1.0)
{
//it is a baseline
stereoModel = StereoCameraModel(fx,fx,cx,cy,fyOrBaseline, localTransform);
}
else
{
float depthConstant = sqlite3_column_double(ppStmt, index++);
float fx = 1.0f/depthConstant;
float fy = 1.0f/depthConstant;
float cx = 0.0f;
float cy = 0.0f;
models.push_back(CameraModel(fx, fy, cx, cy, localTransform));
models.push_back(CameraModel(fx, fyOrBaseline, cx, cy, localTransform));
}
}
else
{
float depthConstant = sqlite3_column_double(ppStmt, index++);
float fx = 1.0f/depthConstant;
float fy = 1.0f/depthConstant;
float cx = 0.0f;
float cy = 0.0f;
models.push_back(CameraModel(fx, fy, cx, cy, localTransform));
}
int laserScanMaxPts = 0;
if(uStrNumCmp(_version, "0.8.11") >= 0)
{
laserScanMaxPts = sqlite3_column_int(ppStmt, index++);
}
int laserScanMaxPts = 0;
if(uStrNumCmp(_version, "0.8.11") >= 0)
{
laserScanMaxPts = sqlite3_column_int(ppStmt, index++);
}
data = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++);
//Create the laserScan
if(dataSize>4 && data)
{
scanCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone(); // depth2d
}
if(uStrNumCmp(_version, "0.8.8") >= 0)
{
data = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++);
//Create the laserScan
//Create the userData
if(dataSize>4 && data)
{
scanCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone(); // depth2d
}
if(models.size())
{
(*iter)->sensorData() = SensorData(
scanCompressed,
laserScanMaxPts,
imageCompressed,
depthOrRightCompressed,
models,
(*iter)->id());
}
else
{
(*iter)->sensorData() = SensorData(
scanCompressed,
laserScanMaxPts,
imageCompressed,
depthOrRightCompressed,
stereoModel,
(*iter)->id());
if(uStrNumCmp(_version, "0.10.1") >= 0)
{
userDataCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone(); // userData
}
else
{
// compress data (set uncompressed data to signed to make difference with compressed type)
userDataCompressed = compressData2(cv::Mat(1, dataSize, CV_8SC1, (void *)data));
}
}
}
if(models.size())
{
(*iter)->sensorData() = SensorData(
scanCompressed,
laserScanMaxPts,
imageCompressed,
depthOrRightCompressed,
models,
(*iter)->id(),
0,
userDataCompressed);
}
else
{
(*iter)->sensorData() = SensorData(
scanCompressed,
laserScanMaxPts,
imageCompressed,
depthOrRightCompressed,
stereoModel,
(*iter)->id(),
0,
userDataCompressed);
}
rc = sqlite3_step(ppStmt); // next result...
@@ -697,232 +721,12 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
}
}
void DBDriverSqlite3::getNodeDataQuery(
int signatureId,
SensorData & sensorData) const
{
if(_ppDb)
{
UTimer timer;
timer.start();
int rc = SQLITE_OK;
sqlite3_stmt * ppStmt = 0;
std::stringstream query;
if(uStrNumCmp(_version, "0.10.0") >= 0)
{
query << "SELECT image, depth, calibration, scan_max_pts, scan "
<< "FROM Data "
<< "WHERE id = " << signatureId
<<";";
}
else if(uStrNumCmp(_version, "0.8.11") >= 0)
{
query << "SELECT Image.data, "
"Depth.data, Depth.local_transform, Depth.fx, Depth.fy, Depth.cx, Depth.cy, Depth.data2d_max_pts, Depth.data2d "
<< "FROM Image "
<< "LEFT OUTER JOIN Depth " // returns all images even if there are no metric data
<< "ON Image.id = Depth.id "
<< "WHERE Image.id = " << signatureId
<<";";
}
else if(uStrNumCmp(_version, "0.7.0") >= 0)
{
query << "SELECT Image.data, "
"Depth.data, Depth.local_transform, Depth.fx, Depth.fy, Depth.cx, Depth.cy, Depth.data2d "
<< "FROM Image "
<< "LEFT OUTER JOIN Depth " // returns all images even if there are no metric data
<< "ON Image.id = Depth.id "
<< "WHERE Image.id = " << signatureId
<<";";
}
else
{
query << "SELECT Image.data, "
"Depth.data, Depth.local_transform, Depth.constant, Depth.data2d "
<< "FROM Image "
<< "LEFT OUTER JOIN Depth " // returns all images even if there are no metric data
<< "ON Image.id = Depth.id "
<< "WHERE Image.id = " << signatureId
<<";";
}
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());
const void * data = 0;
int dataSize = 0;
int index = 0;
cv::Mat imageCompressed;
cv::Mat depthOrRightCompressed;
std::vector<CameraModel> models;
StereoCameraModel stereoModel;
Transform localTransform = Transform::getIdentity();
int laserScanMaxPts;
cv::Mat scanCompressed;
ULOGGER_DEBUG("Loading data for %d...", signatureId);
// Process the result if one
rc = sqlite3_step(ppStmt);
if(rc == SQLITE_ROW)
{
index = 0;
data = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++);
//Create the image
if(dataSize>4 && data)
{
imageCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone();
}
data = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++);
//Create the depth image
if(dataSize>4 && data)
{
depthOrRightCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone();
}
if(uStrNumCmp(_version, "0.10.0") < 0)
{
data = sqlite3_column_blob(ppStmt, index); // local transform
dataSize = sqlite3_column_bytes(ppStmt, index++);
if((unsigned int)dataSize == localTransform.size()*sizeof(float) && data)
{
memcpy(localTransform.data(), data, dataSize);
}
}
// calibration
if(uStrNumCmp(_version, "0.10.0") >= 0)
{
data = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++);
// multi-cameras [fx,fy,cx,cy,local_transform, ... ,fx,fy,cx,cy,local_transform] (4+12)*float * numCameras
// stereo [fx, fy, cx, cy, baseline, local_transform] (5+12)*float
if(dataSize > 0 && data)
{
float * dataFloat = (float*)data;
if((unsigned int)dataSize % (4+localTransform.size())*sizeof(float) == 0)
{
int cameraCount = dataSize / ((4+localTransform.size())*sizeof(float));
UDEBUG("Loading calibration for %d cameras", cameraCount);
int max = cameraCount*(4+localTransform.size());
for(int i=0; i<max; i+=4+localTransform.size())
{
memcpy(localTransform.data(), dataFloat+i+4, localTransform.size()*sizeof(float));
models.push_back(CameraModel(
dataFloat[i],
dataFloat[i+1],
dataFloat[i+2],
dataFloat[i+3],
localTransform));
}
}
else if((unsigned int)dataSize == (5+localTransform.size())*sizeof(float))
{
UDEBUG("Loading calibration for a stereo camera");
memcpy(localTransform.data(), dataFloat+5, localTransform.size()*sizeof(float));
stereoModel = StereoCameraModel(
dataFloat[0], // fx
dataFloat[1], // fy
dataFloat[2], // cx
dataFloat[3], // cy
dataFloat[4], // baseline
localTransform);
}
else
{
UFATAL("Wrong format of the Data.calibration field (size=%d bytes)", dataSize);
}
}
}
else if(uStrNumCmp(_version, "0.7.0") >= 0)
{
double fx = sqlite3_column_double(ppStmt, index++);
double fyOrBaseline = sqlite3_column_double(ppStmt, index++);
double cx = sqlite3_column_double(ppStmt, index++);
double cy = sqlite3_column_double(ppStmt, index++);
if(fyOrBaseline < 1.0)
{
//it is a baseline
stereoModel = StereoCameraModel(fx,fx,cx,cy,fyOrBaseline, localTransform);
}
else
{
models.push_back(CameraModel(fx, fyOrBaseline, cx, cy, localTransform));
}
}
else
{
float depthConstant = sqlite3_column_double(ppStmt, index++);
float fx = 1.0f/depthConstant;
float fy = 1.0f/depthConstant;
float cx = 0.0f;
float cy = 0.0f;
models.push_back(CameraModel(fx, fy, cx, cy, localTransform));
}
laserScanMaxPts = 0;
if(uStrNumCmp(_version, "0.8.11") >= 0)
{
laserScanMaxPts = sqlite3_column_int(ppStmt, index++);
}
data = sqlite3_column_blob(ppStmt, index); // depth2d
dataSize = sqlite3_column_bytes(ppStmt, index++);
//Create the depth2d
if(dataSize>4 && data)
{
scanCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone();
}
if(models.size())
{
sensorData = SensorData(
scanCompressed,
laserScanMaxPts,
imageCompressed,
depthOrRightCompressed,
models,
signatureId);
}
else
{
sensorData = SensorData(
scanCompressed,
laserScanMaxPts,
imageCompressed,
depthOrRightCompressed,
stereoModel,
signatureId);
}
rc = sqlite3_step(ppStmt); // next result...
}
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=%fs", timer.ticks());
}
}
bool DBDriverSqlite3::getNodeInfoQuery(int signatureId,
Transform & pose,
int & mapId,
int & weight,
std::string & label,
double & stamp,
std::vector<unsigned char> & userData) const
double & stamp) const
{
bool found = false;
if(_ppDb && signatureId)
@@ -931,15 +735,7 @@ bool DBDriverSqlite3::getNodeInfoQuery(int signatureId,
sqlite3_stmt * ppStmt = 0;
std::stringstream query;
// Prepare the query... Get the map from signature and visual words
if(uStrNumCmp(_version, "0.8.8") >= 0)
{
query << "SELECT pose, map_id, weight, label, stamp, user_data "
"FROM Node "
"WHERE id = " << signatureId <<
";";
}
else if(uStrNumCmp(_version, "0.8.5") >= 0)
if(uStrNumCmp(_version, "0.8.5") >= 0)
{
query << "SELECT pose, map_id, weight, label, stamp "
"FROM Node "
@@ -986,18 +782,6 @@ bool DBDriverSqlite3::getNodeInfoQuery(int signatureId,
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...
}
UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
@@ -1347,13 +1131,7 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
unsigned int loaded = 0;
// Load nodes information
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)
if(uStrNumCmp(_version, "0.8.5") >= 0)
{
query << "SELECT id, map_id, weight, pose, stamp, label "
<< "FROM Node "
@@ -1384,7 +1162,6 @@ 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);
@@ -1412,18 +1189,6 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
}
}
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());
@@ -1438,8 +1203,7 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
weight,
stamp,
label,
pose,
userData);
pose);
s->setSaved(true);
nodes.push_back(s);
++loaded;
@@ -1996,18 +1760,7 @@ void DBDriverSqlite3::updateQuery(const std::list<Signature *> & nodes, bool upd
Signature * s = 0;
std::string query;
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(uStrNumCmp(_version, "0.8.5") >= 0)
{
if(updateTimestamp)
{
@@ -2055,20 +1808,6 @@ 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());
@@ -2392,7 +2131,11 @@ void DBDriverSqlite3::saveQuery(const std::list<VisualWord *> & words) const
std::string DBDriverSqlite3::queryStepNode() const
{
if(uStrNumCmp(_version, "0.8.8") >= 0)
if(uStrNumCmp(_version, "0.10.1") >= 0)
{
return "INSERT INTO Node(id, map_id, weight, pose, stamp, label) VALUES(?,?,?,?,?,?);";
}
else if(uStrNumCmp(_version, "0.8.8") >= 0)
{
return "INSERT INTO Node(id, map_id, weight, pose, stamp, label, user_data) VALUES(?,?,?,?,?,?,?);";
}
@@ -2438,16 +2181,20 @@ void DBDriverSqlite3::stepNode(sqlite3_stmt * ppStmt, const Signature * s) const
}
}
if(uStrNumCmp(_version, "0.8.8") >= 0)
if(uStrNumCmp(_version, "0.10.1") >= 0)
{
if(s->getUserData().empty())
// ignore user_data
}
else if(uStrNumCmp(_version, "0.8.8") >= 0)
{
if(s->sensorData().userDataCompressed().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);
rc = sqlite3_bind_blob(ppStmt, index++, s->sensorData().userDataCompressed().data, (int)s->sensorData().userDataCompressed().cols, SQLITE_STATIC);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
}
}
@@ -2613,7 +2360,14 @@ void DBDriverSqlite3::stepDepth(sqlite3_stmt * ppStmt, const SensorData & sensor
std::string DBDriverSqlite3::queryStepSensorData() const
{
UASSERT(uStrNumCmp(_version, "0.10.0") >= 0);
return "INSERT INTO Data(id, image, depth, calibration, scan_max_pts, scan) VALUES(?,?,?,?,?,?);";
if(uStrNumCmp(_version, "0.10.1") >= 0)
{
return "INSERT INTO Data(id, image, depth, calibration, scan_max_pts, scan, user_data) VALUES(?,?,?,?,?,?,?);";
}
else
{
return "INSERT INTO Data(id, image, depth, calibration, scan_max_pts, scan) VALUES(?,?,?,?,?,?);";
}
}
void DBDriverSqlite3::stepSensorData(sqlite3_stmt * ppStmt,
const SensorData & sensorData) const
@@ -2712,6 +2466,20 @@ void DBDriverSqlite3::stepSensorData(sqlite3_stmt * ppStmt,
}
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
if(uStrNumCmp(_version, "0.10.1") >= 0)
{
// user_data
if(!sensorData.userDataCompressed().empty())
{
rc = sqlite3_bind_blob(ppStmt, index++, sensorData.userDataCompressed().data, (int)sensorData.userDataCompressed().cols, SQLITE_STATIC);
}
else
{
rc = sqlite3_bind_zeroblob(ppStmt, index++, 4);
}
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());

5
corelib/src/DBDriverSqlite3.h
View File

@@ -70,9 +70,8 @@ private:
virtual void loadWordsQuery(const std::set<int> & wordIds, std::list<VisualWord *> & vws) const;
virtual void loadLinksQuery(int signatureId, std::map<int, Link> & links, Link::Type type = Link::kUndef) const;
virtual void loadNodeDataQuery(std::list<Signature *> & signatures, bool loadMetricData) const;
virtual void getNodeDataQuery(int signatureId, SensorData & data) 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 loadNodeDataQuery(std::list<Signature *> & signatures) const;
virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp) const;
virtual void getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren) const;
virtual void getAllLinksQuery(std::multimap<int, Link> & links, bool ignoreNullLinks) const;
virtual void getLastIdQuery(const std::string & tableName, int & id) const;

21
corelib/src/DBReader.cpp
View File

@@ -156,17 +156,20 @@ void DBReader::mainLoop()
int goalId = 0;
double previousStamp = odom.data().stamp();
odom.data().setStamp(UTimer::now());
if(odom.data().userData().size() >= 6 && memcmp(odom.data().userData().data(), "GOAL:", 5) == 0)
if(odom.data().userDataRaw().type() == CV_8SC1 &&
odom.data().userDataRaw().cols >= 7 && // including null str ending
odom.data().userDataRaw().rows == 1 &&
memcmp(odom.data().userDataRaw().data, "GOAL:", 5) == 0)
{
//GOAL format detected, remove it from the user data and send it as goal event
std::string goalStr = uBytes2Str(odom.data().userData());
std::string goalStr = (const char *)odom.data().userDataRaw().data;
if(!goalStr.empty())
{
std::list<std::string> strs = uSplit(goalStr, ':');
if(strs.size() == 2)
{
goalId = atoi(strs.rbegin()->c_str());
odom.data().setUserData(std::vector<unsigned char>());
odom.data().setUserData(cv::Mat());
}
}
}
@@ -196,8 +199,7 @@ void DBReader::mainLoop()
double stamp;
int mapId;
Transform localTransform, pose;
std::vector<unsigned char> userData;
_dbDriver->getNodeInfo(*_currentId, pose, mapId, weight, label, stamp, userData);
_dbDriver->getNodeInfo(*_currentId, pose, mapId, weight, label, stamp);
if(previousStamp && stamp && stamp > previousStamp)
{
double delay = stamp - previousStamp;
@@ -252,7 +254,6 @@ OdometryEvent DBReader::getNextData()
if(!this->isKilled() && _currentId != _ids.end())
{
int mapId;
std::vector<unsigned char> userData;
SensorData data;
_dbDriver->getNodeData(*_currentId, data);
@@ -261,7 +262,7 @@ OdometryEvent DBReader::getNextData()
int weight;
std::string label;
double stamp;
_dbDriver->getNodeInfo(*_currentId, pose, mapId, weight, label, stamp, userData);
_dbDriver->getNodeInfo(*_currentId, pose, mapId, weight, label, stamp);
cv::Mat infMatrix = cv::Mat::eye(6,6,CV_64FC1);
if(!_odometryIgnored)
@@ -338,11 +339,11 @@ OdometryEvent DBReader::getNextData()
data.uncompressData();
data.setId(seq);
data.setStamp(stamp);
data.setUserData(userData);
UDEBUG("Laser=%d RGB/Left=%d Depth/Right=%d",
UDEBUG("Laser=%d RGB/Left=%d Depth/Right=%d, UserData=%d",
data.laserScanRaw().empty()?0:1,
data.imageRaw().empty()?0:1,
data.depthOrRightRaw().empty()?0:1);
data.depthOrRightRaw().empty()?0:1,
data.userDataRaw().empty()?0:1);
odom = OdometryEvent(data, pose, infMatrix.inv());
}

69
corelib/src/Memory.cpp
View File

@@ -1875,30 +1875,17 @@ std::map<int, std::string> Memory::getAllLabels() const
return labels;
}
bool Memory::setUserData(int id, const std::vector<unsigned char> & data)
bool Memory::setUserData(int id, const cv::Mat & data)
{
Signature * s = this->_getSignature(id);
if(s)
{
s->setUserData(data);
s->sensorData().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());
UERROR("Node %d not found in RAM, failed to set user data (size=%d)!", id, data.total());
}
return false;
}
@@ -2257,7 +2244,7 @@ Transform Memory::computeIcpTransform(
}
if(depthToLoad.size())
{
_dbDriver->loadNodeData(depthToLoad, true);
_dbDriver->loadNodeData(depthToLoad);
}
}
@@ -2614,7 +2601,7 @@ Transform Memory::computeScanMatchingTransform(
}
if(depthToLoad.size() && _dbDriver)
{
_dbDriver->loadNodeData(depthToLoad, true);
_dbDriver->loadNodeData(depthToLoad);
}
std::string msg;
@@ -3203,8 +3190,7 @@ Transform Memory::getOdomPose(int signatureId, bool lookInDatabase) const
int mapId, weight;
std::string label;
double stamp;
std::vector<unsigned char> userData;
getNodeInfo(signatureId, pose, mapId, weight, label, stamp, userData, lookInDatabase);
getNodeInfo(signatureId, pose, mapId, weight, label, stamp, lookInDatabase);
return pose;
}
@@ -3214,7 +3200,6 @@ 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);
@@ -3225,12 +3210,11 @@ 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, userData);
return _dbDriver->getNodeInfo(signatureId, odomPose, mapId, weight, label, stamp);
}
return false;
}
@@ -3272,7 +3256,7 @@ SensorData Memory::getNodeData(int nodeId, bool uncompressedData)
{
std::list<Signature*> signatures;
signatures.push_back(s);
_dbDriver->loadNodeData(signatures, true);
_dbDriver->loadNodeData(signatures);
if(uncompressedData)
{
s->sensorData().uncompressData();
@@ -3309,7 +3293,7 @@ SensorData Memory::getSignatureDataConst(int locationId) const
std::list<Signature*> signatures;
Signature tmp = *s;
signatures.push_back(&tmp);
_dbDriver->loadNodeData(signatures, true);
_dbDriver->loadNodeData(signatures);
r = tmp.sensorData();
}
else
@@ -3324,7 +3308,7 @@ SensorData Memory::getSignatureDataConst(int locationId) const
Signature * sTmp = signatures.front();
if(sTmp->sensorData().imageCompressed().empty())
{
_dbDriver->loadNodeData(signatures, !sTmp->getPose().isNull());
_dbDriver->loadNodeData(signatures);
}
r = sTmp->sensorData();
if(loadedFromTrash.size())
@@ -4156,12 +4140,15 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
rtabmap::CompressionThread ctImage(image, std::string(".jpg"));
rtabmap::CompressionThread ctDepth(depthOrRightImage, std::string(".png"));
rtabmap::CompressionThread ctDepth2d(laserScan);
rtabmap::CompressionThread ctUserData(data.userDataRaw());
ctImage.start();
ctDepth.start();
ctDepth2d.start();
ctUserData.start();
ctImage.join();
ctDepth.join();
ctDepth2d.join();
ctUserData.join();
s = new Signature(id,
_idMapCount,
@@ -4169,7 +4156,6 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
data.stamp(),
"",
pose,
data.userData(),
stereoCameraModel.isValid()?
SensorData(
ctDepth2d.getCompressedData(),
@@ -4177,39 +4163,53 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
ctImage.getCompressedData(),
ctDepth.getCompressedData(),
stereoCameraModel,
id):
id,
0,
ctUserData.getCompressedData()):
SensorData(
ctDepth2d.getCompressedData(),
data.laserScanMaxPts(),
ctImage.getCompressedData(),
ctDepth.getCompressedData(),
cameraModels,
id));
id,
0,
ctUserData.getCompressedData()));
}
else
{
rtabmap::CompressionThread ctDepth2d(laserScan);
rtabmap::CompressionThread ctUserData(data.userDataRaw());
ctDepth2d.start();
ctUserData.start();
ctDepth2d.join();
ctUserData.join();
s = new Signature(id,
_idMapCount,
(data.imageRaw().empty()&&data.laserScanRaw().empty()&&words.size()==0)?-1:0, // tag intermediate nodes as weight=-1
data.stamp(),
"",
pose,
data.userData(),
stereoCameraModel.isValid()?
SensorData(
rtabmap::compressData2(laserScan),
ctDepth2d.getCompressedData(),
data.laserScanMaxPts(),
cv::Mat(),
cv::Mat(),
stereoCameraModel,
id):
id,
0,
ctUserData.getCompressedData()):
SensorData(
rtabmap::compressData2(laserScan),
ctDepth2d.getCompressedData(),
data.laserScanMaxPts(),
cv::Mat(),
cv::Mat(),
cameraModels,
id));
id,
0,
ctUserData.getCompressedData()));
}
s->setWords(words);
s->setWords3(words3D);
@@ -4218,6 +4218,7 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
s->sensorData().setImageRaw(image);
s->sensorData().setDepthOrRightRaw(depthOrRightImage);
s->sensorData().setLaserScanRaw(laserScan, data.laserScanMaxPts());
s->sensorData().setUserDataRaw(data.userDataRaw());
}

19
corelib/src/Rtabmap.cpp
View File

@@ -666,7 +666,7 @@ bool Rtabmap::labelLocation(int id, const std::string & label)
return false;
}
bool Rtabmap::setUserData(int id, const std::vector<unsigned char> & data)
bool Rtabmap::setUserData(int id, const cv::Mat & data)
{
if(_memory)
{
@@ -2298,15 +2298,14 @@ bool Rtabmap::process(
std::string label;
double stamp = 0;
std::vector<unsigned char> userData;
_memory->getNodeInfo(iter->first, odomPose, mapId, weight, label, stamp, userData, false);
_memory->getNodeInfo(iter->first, odomPose, mapId, weight, label, stamp, false);
signatures.insert(std::make_pair(iter->first,
Signature(iter->first,
mapId,
weight,
stamp,
label,
odomPose,
userData)));
odomPose)));
}
statistics_.setPoses(poses);
statistics_.setConstraints(constraints);
@@ -2878,8 +2877,7 @@ void Rtabmap::get3DMap(
int mapId = -1;
std::string label;
double stamp = 0;
std::vector<unsigned char> userData;
_memory->getNodeInfo(*iter, odomPose, mapId, weight, label, stamp, userData, true);
_memory->getNodeInfo(*iter, odomPose, mapId, weight, label, stamp, true);
SensorData data = _memory->getNodeData(*iter);
data.setId(*iter);
signatures.insert(std::make_pair(*iter,
@@ -2889,7 +2887,6 @@ void Rtabmap::get3DMap(
stamp,
label,
odomPose,
userData,
data)));
}
}
@@ -2940,16 +2937,14 @@ void Rtabmap::getGraph(
int mapId = -1;
std::string label;
double stamp = 0;
std::vector<unsigned char> userData;
_memory->getNodeInfo(iter->first, odomPose, mapId, weight, label, stamp, userData, global);
_memory->getNodeInfo(iter->first, odomPose, mapId, weight, label, stamp, global);
signatures->insert(std::make_pair(iter->first,
Signature(iter->first,
mapId,
weight,
stamp,
label,
odomPose,
userData)));
odomPose)));
}
}
}
@@ -3058,7 +3053,7 @@ bool Rtabmap::computePath(int targetNode, bool global)
{
// set goal to latest signature
std::string goalStr = uFormat("GOAL:%d", targetNode);
setUserData(0, uStr2Bytes(goalStr));
setUserData(0, cv::Mat(1, goalStr.size()+1, CV_8SC1, (void *)goalStr.c_str()).clone());
}
updateGoalIndex();
}

6
corelib/src/RtabmapThread.cpp
View File

@@ -188,7 +188,7 @@ void RtabmapThread::mainLoop()
_stateMutex.unlock();
int id = 0;
std::vector<unsigned char> userData;
cv::Mat userData;
switch(state)
{
case kStateDetecting:
@@ -269,7 +269,7 @@ void RtabmapThread::mainLoop()
_userDataMutex.lock();
{
userData = _userData;
_userData.clear();
_userData = cv::Mat();
}
_userDataMutex.unlock();
_rtabmap->setUserData(0, userData);
@@ -596,7 +596,7 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent)
cv::Mat(),
odomEvent.data().id(),
odomEvent.data().stamp(),
odomEvent.data().userData());
odomEvent.data().userDataRaw());
_dataBuffer.push_back(OdometryEvent(tmp, odomEvent.pose(), _rotVariance, _transVariance));
}
else

179
corelib/src/SensorData.cpp
View File

@@ -47,11 +47,10 @@ SensorData::SensorData(
const cv::Mat & image,
int id,
double stamp,
const std::vector<unsigned char> & userData) :
const cv::Mat & userData) :
_id(id),
_stamp(stamp),
_laserScanMaxPts(0),
_userData(userData)
_laserScanMaxPts(0)
{
if(image.rows == 1)
{
@@ -64,6 +63,15 @@ SensorData::SensorData(
image.type() == CV_8UC3); // RGB
_imageRaw = image;
}
if(userData.type() == CV_8UC1) // Bytes
{
_userDataCompressed = userData; // assume compressed
}
else
{
_userDataRaw = userData;
}
}
// Mono constructor
@@ -72,12 +80,11 @@ SensorData::SensorData(
const CameraModel & cameraModel,
int id,
double stamp,
const std::vector<unsigned char> & userData) :
const cv::Mat & userData) :
_id(id),
_stamp(stamp),
_laserScanMaxPts(0),
_cameraModels(std::vector<CameraModel>(1, cameraModel)),
_userData(userData)
_cameraModels(std::vector<CameraModel>(1, cameraModel))
{
if(image.rows == 1)
{
@@ -90,6 +97,15 @@ SensorData::SensorData(
image.type() == CV_8UC3); // RGB
_imageRaw = image;
}
if(userData.type() == CV_8UC1) // Bytes
{
_userDataCompressed = userData; // assume compressed
}
else
{
_userDataRaw = userData;
}
}
// RGB-D constructor
@@ -99,12 +115,11 @@ SensorData::SensorData(
const CameraModel & cameraModel,
int id,
double stamp,
const std::vector<unsigned char> & userData) :
const cv::Mat & userData) :
_id(id),
_stamp(stamp),
_laserScanMaxPts(0),
_cameraModels(std::vector<CameraModel>(1, cameraModel)),
_userData(userData)
_cameraModels(std::vector<CameraModel>(1, cameraModel))
{
if(rgb.rows == 1)
{
@@ -129,6 +144,15 @@ SensorData::SensorData(
depth.type() == CV_16UC1); // Depth in millimetre
_depthOrRightRaw = depth;
}
if(userData.type() == CV_8UC1) // Bytes
{
_userDataCompressed = userData; // assume compressed
}
else
{
_userDataRaw = userData;
}
}
// RGB-D constructor + 2d laser scan
@@ -140,12 +164,11 @@ SensorData::SensorData(
const CameraModel & cameraModel,
int id,
double stamp,
const std::vector<unsigned char> & userData) :
const cv::Mat & userData) :
_id(id),
_stamp(stamp),
_laserScanMaxPts(laserScanMaxPts),
_cameraModels(std::vector<CameraModel>(1, cameraModel)),
_userData(userData)
_cameraModels(std::vector<CameraModel>(1, cameraModel))
{
if(rgb.rows == 1)
{
@@ -179,6 +202,15 @@ SensorData::SensorData(
UASSERT(laserScan.type() == CV_8UC1); // Bytes
_laserScanCompressed = laserScan;
}
if(userData.type() == CV_8UC1) // Bytes
{
_userDataCompressed = userData; // assume compressed
}
else
{
_userDataRaw = userData;
}
}
// Multi-cameras RGB-D constructor
@@ -188,12 +220,11 @@ SensorData::SensorData(
const std::vector<CameraModel> & cameraModels,
int id,
double stamp,
const std::vector<unsigned char> & userData) :
const cv::Mat & userData) :
_id(id),
_stamp(stamp),
_laserScanMaxPts(0),
_cameraModels(cameraModels),
_userData(userData)
_cameraModels(cameraModels)
{
if(rgb.rows == 1)
{
@@ -221,6 +252,15 @@ SensorData::SensorData(
{
UASSERT(cameraModels[i].isValid());
}
if(userData.type() == CV_8UC1) // Bytes
{
_userDataCompressed = userData; // assume compressed
}
else
{
_userDataRaw = userData;
}
}
// Multi-cameras RGB-D constructor + 2d laser scan
@@ -232,12 +272,11 @@ SensorData::SensorData(
const std::vector<CameraModel> & cameraModels,
int id,
double stamp,
const std::vector<unsigned char> & userData) :
const cv::Mat & userData) :
_id(id),
_stamp(stamp),
_laserScanMaxPts(laserScanMaxPts),
_cameraModels(cameraModels),
_userData(userData)
_cameraModels(cameraModels)
{
if(rgb.rows == 1)
{
@@ -276,6 +315,15 @@ SensorData::SensorData(
{
UASSERT(cameraModels[i].isValid());
}
if(userData.type() == CV_8UC1) // Bytes
{
_userDataCompressed = userData; // assume compressed
}
else
{
_userDataRaw = userData;
}
}
// Stereo constructor
@@ -285,12 +333,11 @@ SensorData::SensorData(
const StereoCameraModel & cameraModel,
int id,
double stamp,
const std::vector<unsigned char> & userData):
const cv::Mat & userData):
_id(id),
_stamp(stamp),
_laserScanMaxPts(0),
_stereoCameraModel(cameraModel),
_userData(userData)
_stereoCameraModel(cameraModel)
{
if(left.rows == 1)
{
@@ -314,6 +361,15 @@ SensorData::SensorData(
_depthOrRightRaw = right;
}
if(userData.type() == CV_8UC1) // Bytes
{
_userDataCompressed = userData; // assume compressed
}
else
{
_userDataRaw = userData;
}
}
// Stereo constructor + 2d laser scan
@@ -325,12 +381,11 @@ SensorData::SensorData(
const StereoCameraModel & cameraModel,
int id,
double stamp,
const std::vector<unsigned char> & userData) :
const cv::Mat & userData) :
_id(id),
_stamp(stamp),
_laserScanMaxPts(laserScanMaxPts),
_stereoCameraModel(cameraModel),
_userData(userData)
_stereoCameraModel(cameraModel)
{
if(left.rows == 1)
{
@@ -363,18 +418,60 @@ SensorData::SensorData(
UASSERT(laserScan.type() == CV_8UC1); // Bytes
_laserScanCompressed = laserScan;
}
if(userData.type() == CV_8UC1) // Bytes
{
_userDataCompressed = userData; // assume compressed
}
else
{
_userDataRaw = userData;
}
}
void SensorData::setUserDataRaw(const cv::Mat & userDataRaw)
{
if(!_userDataRaw.empty())
{
UWARN("Writing new user data over existing user data. This may result in data loss.");
}
_userDataRaw = userDataRaw;
}
void SensorData::setUserData(const cv::Mat & userData)
{
if(!userData.empty() && (!_userDataCompressed.empty() || !_userDataRaw.empty()))
{
UWARN("Writing new user data over existing user data. This may result in data loss.");
}
_userDataRaw = cv::Mat();
_userDataCompressed = cv::Mat();
if(!userData.empty())
{
if(userData.type() == CV_8UC1) // Bytes
{
_userDataCompressed = userData; // assume compressed
}
else
{
_userDataRaw = userData;
_userDataCompressed = compressData2(userData);
}
}
}
void SensorData::uncompressData()
{
uncompressData(_imageCompressed.empty()?0:&_imageRaw,
_depthOrRightCompressed.empty()?0:&_depthOrRightRaw,
_laserScanCompressed.empty()?0:&_laserScanRaw);
_laserScanCompressed.empty()?0:&_laserScanRaw,
_userDataCompressed.empty()?0:&_userDataRaw);
}
void SensorData::uncompressData(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::Mat * laserScanRaw)
void SensorData::uncompressData(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::Mat * laserScanRaw, cv::Mat * userDataRaw)
{
uncompressDataConst(imageRaw, depthRaw, laserScanRaw);
uncompressDataConst(imageRaw, depthRaw, laserScanRaw, userDataRaw);
if(imageRaw && !imageRaw->empty() && _imageRaw.empty())
{
_imageRaw = *imageRaw;
@@ -387,9 +484,13 @@ void SensorData::uncompressData(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::Mat
{
_laserScanRaw = *laserScanRaw;
}
if(userDataRaw && !userDataRaw->empty() && _userDataRaw.empty())
{
_userDataRaw = *userDataRaw;
}
}
void SensorData::uncompressDataConst(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::Mat * laserScanRaw) const
void SensorData::uncompressDataConst(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::Mat * laserScanRaw, cv::Mat * userDataRaw) const
{
if(imageRaw)
{
@@ -403,13 +504,19 @@ void SensorData::uncompressDataConst(cv::Mat * imageRaw, cv::Mat * depthRaw, cv:
{
*laserScanRaw = _laserScanRaw;
}
if(userDataRaw)
{
*userDataRaw = _userDataRaw;
}
if( (imageRaw && imageRaw->empty()) ||
(depthRaw && depthRaw->empty()) ||
(laserScanRaw && laserScanRaw->empty()))
(laserScanRaw && laserScanRaw->empty()) ||
(userDataRaw && userDataRaw->empty()))
{
rtabmap::CompressionThread ctImage(_imageCompressed, true);
rtabmap::CompressionThread ctDepth(_depthOrRightCompressed, true);
rtabmap::CompressionThread ctLaserScan(_laserScanCompressed, false);
rtabmap::CompressionThread ctUserData(_userDataCompressed, false);
if(imageRaw && imageRaw->empty())
{
ctImage.start();
@@ -422,9 +529,14 @@ void SensorData::uncompressDataConst(cv::Mat * imageRaw, cv::Mat * depthRaw, cv:
{
ctLaserScan.start();
}
if(userDataRaw && userDataRaw->empty())
{
ctUserData.start();
}
ctImage.join();
ctDepth.join();
ctLaserScan.join();
ctUserData.join();
if(imageRaw && imageRaw->empty())
{
*imageRaw = ctImage.getUncompressedData();
@@ -450,6 +562,15 @@ void SensorData::uncompressDataConst(cv::Mat * imageRaw, cv::Mat * depthRaw, cv:
UWARN("Requested laser scan data, but the sensor data (%d) doesn't have laser scan.", this->id());
}
}
if(userDataRaw && userDataRaw->empty())
{
*userDataRaw = ctUserData.getUncompressedData();
if(userDataRaw->empty())
{
UWARN("Requested user data, but the sensor data (%d) doesn't have user data.", this->id());
}
}
}
}

14
corelib/src/Signature.cpp
View File

@@ -54,14 +54,12 @@ Signature::Signature(
double stamp,
const std::string & label,
const Transform & pose,
const std::vector<unsigned char> & userData,
const SensorData & sensorData):
_id(id),
_mapId(mapId),
_stamp(stamp),
_weight(weight),
_label(label),
_userData(userData),
_saved(false),
_modified(true),
_linksModified(true),
@@ -81,18 +79,6 @@ 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)

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

@@ -20,7 +20,6 @@ CREATE TABLE Node (
stamp FLOAT,
pose BLOB,
label TEXT,
user_data BLOB,
time_enter DATE,
PRIMARY KEY (id)
);
@@ -32,6 +31,7 @@ CREATE TABLE Data (
calibration BLOB, -- fx, fy, cx, cy [,baseline] local_transform
scan BLOB, -- compressed data (Laser scan)
scan_max_pts INTEGER, -- Laser scan max points
user_data BLOB, -- compressed data (User data)
time_enter DATE,
PRIMARY KEY (id)
);