i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-03 01:50:24 +08:00
Code Issues Packages Projects Releases Wiki Activity

Memory efficiency: changed std::vector<unsigned char> bytes arrays to cv::Mat to avoid multiple copies when Signature objects are copied.

Browse Source 
git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1937 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2014-10-28 19:57:21 +00:00
parent eace13b7a8
commit 0f7cc38688
17 changed files with 258 additions and 199 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -96,8 +96,8 @@ public:
// Specific queries... // Specific queries...
void loadNodeData(std::list<Signature *> & signatures, bool loadMetricData) const; void loadNodeData(std::list<Signature *> & signatures, bool loadMetricData) const;
void getNodeData(int signatureId, std::vector<unsigned char> & image, std::vector<unsigned char> & depth, std::vector<unsigned char> & depth2d, float & fx, float & fy, float & cx, float & cy, Transform & localTransform) const; void getNodeData(int signatureId, cv::Mat & imageCompressed, cv::Mat & depthCompressed, cv::Mat & depth2dCompressed, float & fx, float & fy, float & cx, float & cy, Transform & localTransform) const;
void getNodeData(int signatureId, std::vector<unsigned char> & image) const; void getNodeData(int signatureId, cv::Mat & imageCompressed) const;
void getPose(int signatureId, Transform & pose, int & mapId) const; void getPose(int signatureId, Transform & pose, int & mapId) const;
void loadNeighbors(int signatureId, std::map<int, Transform> & neighbors) const; void loadNeighbors(int signatureId, std::map<int, Transform> & neighbors) const;
void loadLoopClosures(int signatureId, std::map<int, Transform> & loopIds, std::map<int, Transform> & childIds) const; void loadLoopClosures(int signatureId, std::map<int, Transform> & loopIds, std::map<int, Transform> & childIds) const;
@@ -135,8 +135,8 @@ private:
virtual void loadLoopClosuresQuery(int signatureId, std::map<int, Transform> & loopIds, std::map<int, Transform> & childIds) const = 0; virtual void loadLoopClosuresQuery(int signatureId, std::map<int, Transform> & loopIds, std::map<int, Transform> & childIds) const = 0;
virtual void loadNodeDataQuery(std::list<Signature *> & signatures, bool loadMetricData) const = 0; virtual void loadNodeDataQuery(std::list<Signature *> & signatures, bool loadMetricData) const = 0;
virtual void getNodeDataQuery(int signatureId, std::vector<unsigned char> & image, std::vector<unsigned char> & depth, std::vector<unsigned char> & depth2d, float & fx, float & fy, float & cx, float & cy, Transform & localTransform) const = 0; virtual void getNodeDataQuery(int signatureId, cv::Mat & imageCompressed, cv::Mat & depthCompressed, cv::Mat & depth2dCompressed, float & fx, float & fy, float & cx, float & cy, Transform & localTransform) const = 0;
virtual void getNodeDataQuery(int signatureId, std::vector<unsigned char> & image) const = 0; virtual void getNodeDataQuery(int signatureId, cv::Mat & imageCompressed) const = 0;
virtual void getPoseQuery(int signatureId, Transform & pose, int & mapId) const = 0; virtual void getPoseQuery(int signatureId, Transform & pose, int & mapId) const = 0;
virtual void getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren) 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 getLastIdQuery(const std::string & tableName, int & id) const = 0;

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

@@ -113,7 +113,7 @@ public:
int getDatabaseMemoryUsed() const; // in bytes int getDatabaseMemoryUsed() const; // in bytes
double getDbSavingTime() const; double getDbSavingTime() const;
int getMapId(int signatureId) const; int getMapId(int signatureId) const;
std::vector<unsigned char> getImage(int signatureId) const; cv::Mat getImageCompressed(int signatureId) const;
Signature getSignatureData(int locationId, bool uncompressedData = false); Signature getSignatureData(int locationId, bool uncompressedData = false);
std::set<int> getAllSignatureIds() const; std::set<int> getAllSignatureIds() const;
bool memoryChanged() const {return _memoryChanged;} bool memoryChanged() const {return _memoryChanged;}

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

@@ -56,9 +56,9 @@ public:
const std::multimap<int, cv::KeyPoint> & words, const std::multimap<int, cv::KeyPoint> & words,
const std::multimap<int, pcl::PointXYZ> & words3, const std::multimap<int, pcl::PointXYZ> & words3,
const Transform & pose = Transform(), const Transform & pose = Transform(),
const std::vector<unsigned char> & depth2D = std::vector<unsigned char>(), const cv::Mat & depth2D = cv::Mat(),
const std::vector<unsigned char> & image = std::vector<unsigned char>(), const cv::Mat & image = cv::Mat(),
const std::vector<unsigned char> & depth = std::vector<unsigned char>(), const cv::Mat & depth = cv::Mat(),
float fx = 0.0f, float fx = 0.0f,
float fy = 0.0f, float fy = 0.0f,
float cx = 0.0f, float cx = 0.0f,
@@ -69,7 +69,7 @@ public:
/** /**
* Must return a value between >=0 and <=1 (1 means 100% similarity). * Must return a value between >=0 and <=1 (1 means 100% similarity).
*/ */
float compareTo(const Signature * signature) const; float compareTo(const Signature & signature) const;
bool isBadSignature() const; bool isBadSignature() const;
int id() const {return _id;} int id() const {return _id;}
@@ -113,20 +113,20 @@ public:
void setEnabled(bool enabled) {_enabled = enabled;} void setEnabled(bool enabled) {_enabled = enabled;}
const std::multimap<int, cv::KeyPoint> & getWords() const {return _words;} const std::multimap<int, cv::KeyPoint> & getWords() const {return _words;}
const std::map<int, int> & getWordsChanged() const {return _wordsChanged;} const std::map<int, int> & getWordsChanged() const {return _wordsChanged;}
void setImage(const std::vector<unsigned char> & image) {_image = image;} void setImageCompressed(const cv::Mat & bytes) {_imageCompressed = bytes;}
const std::vector<unsigned char> & getImage() const {return _image;} const cv::Mat & getImageCompressed() const {return _imageCompressed;}
void setImageRaw(const cv::Mat & image) {_imageRaw = image;} void setImageRaw(const cv::Mat & image) {_imageRaw = image;}
const cv::Mat & getImageRaw() const {return _imageRaw;} const cv::Mat & getImageRaw() const {return _imageRaw;}
//metric stuff //metric stuff
void setWords3(const std::multimap<int, pcl::PointXYZ> & words3) {_words3 = words3;} void setWords3(const std::multimap<int, pcl::PointXYZ> & words3) {_words3 = words3;}
void setDepth(const std::vector<unsigned char> & depth, float fx, float fy, float cx, float cy); void setDepthCompressed(const cv::Mat & bytes, float fx, float fy, float cx, float cy);
void setDepth2D(const std::vector<unsigned char> & depth2D) {_depth2D = depth2D;} void setDepth2DCompressed(const cv::Mat & bytes) {_depth2DCompressed = bytes;}
void setLocalTransform(const Transform & t) {_localTransform = t;} void setLocalTransform(const Transform & t) {_localTransform = t;}
void setPose(const Transform & pose) {_pose = pose;} void setPose(const Transform & pose) {_pose = pose;}
const std::multimap<int, pcl::PointXYZ> & getWords3() const {return _words3;} const std::multimap<int, pcl::PointXYZ> & getWords3() const {return _words3;}
const std::vector<unsigned char> & getDepth() const {return _depth;} const cv::Mat & getDepthCompressed() const {return _depthCompressed;}
const std::vector<unsigned char> & getDepth2D() const {return _depth2D;} const cv::Mat & getDepth2DCompressed() const {return _depth2DCompressed;}
float getDepthFx() const {return _fx;} float getDepthFx() const {return _fx;}
float getDepthFy() const {return _fy;} float getDepthFy() const {return _fy;}
float getDepthCx() const {return _cx;} float getDepthCx() const {return _cx;}
@@ -140,7 +140,7 @@ public:
SensorData toSensorData(); SensorData toSensorData();
void uncompressData(); void uncompressData();
void uncompressData(cv::Mat * image, cv::Mat * depth, cv::Mat * depth2D) const; void uncompressData(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::Mat * depth2DRaw) const;
private: private:
int _id; int _id;
@@ -159,10 +159,10 @@ private:
std::multimap<int, cv::KeyPoint> _words; // word <id, keypoint> std::multimap<int, cv::KeyPoint> _words; // word <id, keypoint>
std::map<int, int> _wordsChanged; // <oldId, newId> std::map<int, int> _wordsChanged; // <oldId, newId>
bool _enabled; bool _enabled;
std::vector<unsigned char> _image; //compressed image CV_8UC1 or CV_8UC3 cv::Mat _imageCompressed; // compressed image
std::vector<unsigned char> _depth; // compressed image CV_16UC1 cv::Mat _depthCompressed; // compressed image
std::vector<unsigned char> _depth2D; // compressed data CV_32FC2 cv::Mat _depth2DCompressed; // compressed data
float _fx; float _fx;
float _fy; float _fy;
float _cx; float _cx;
@@ -171,9 +171,9 @@ private:
Transform _localTransform; // camera_link -> base_link Transform _localTransform; // camera_link -> base_link
std::multimap<int, pcl::PointXYZ> _words3; // word <id, keypoint> std::multimap<int, pcl::PointXYZ> _words3; // word <id, keypoint>
cv::Mat _imageRaw; cv::Mat _imageRaw; // CV_8UC1 or CV_8UC3
cv::Mat _depthRaw; cv::Mat _depthRaw; // depth CV_16UC1 or CV_32FC1, right image CV_8UC1
cv::Mat _depth2DRaw; cv::Mat _depth2DRaw; // CV_32FC2
}; };
} // namespace rtabmap } // namespace rtabmap

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

@@ -71,14 +71,13 @@ class RTABMAP_EXP CompressionThread : public UThread
public: public:
// format : ".png" ".jpg" "" (empty is general) // format : ".png" ".jpg" "" (empty is general)
CompressionThread(const cv::Mat & mat, const std::string & format = ""); CompressionThread(const cv::Mat & mat, const std::string & format = "");
CompressionThread(const std::vector<unsigned char> * bytes, bool isImage); CompressionThread(const cv::Mat & bytes, bool isImage);
const std::vector<unsigned char> & getCompressedData() const {return compressedData_;} const cv::Mat & getCompressedData() const {return compressedData_;}
cv::Mat & getUncompressedData() {return uncompressedData_;} cv::Mat & getUncompressedData() {return uncompressedData_;}
protected: protected:
virtual void mainLoop(); virtual void mainLoop();
private: private:
const std::vector<unsigned char> * constCompressedData_; cv::Mat compressedData_;
std::vector<unsigned char> compressedData_;
cv::Mat uncompressedData_; cv::Mat uncompressedData_;
std::string format_; std::string format_;
bool image_; bool image_;
@@ -235,10 +234,17 @@ cv::Mat RTABMAP_EXP depth2DFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> &
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP depth2DToPointCloud(const cv::Mat & depth2D); pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP depth2DToPointCloud(const cv::Mat & depth2D);
std::vector<unsigned char> RTABMAP_EXP compressImage(const cv::Mat & image, const std::string & format = ".png"); std::vector<unsigned char> RTABMAP_EXP compressImage(const cv::Mat & image, const std::string & format = ".png");
cv::Mat RTABMAP_EXP compressImage2(const cv::Mat & image, const std::string & format = ".png");
cv::Mat RTABMAP_EXP uncompressImage(const cv::Mat & bytes);
cv::Mat RTABMAP_EXP uncompressImage(const std::vector<unsigned char> & bytes); cv::Mat RTABMAP_EXP uncompressImage(const std::vector<unsigned char> & bytes);
std::vector<unsigned char> RTABMAP_EXP compressData(const cv::Mat & data); std::vector<unsigned char> RTABMAP_EXP compressData(const cv::Mat & data);
cv::Mat RTABMAP_EXP compressData2(const cv::Mat & data);
cv::Mat RTABMAP_EXP uncompressData(const cv::Mat & bytes);
cv::Mat RTABMAP_EXP uncompressData(const std::vector<unsigned char> & bytes); cv::Mat RTABMAP_EXP uncompressData(const std::vector<unsigned char> & bytes);
cv::Mat RTABMAP_EXP uncompressData(const unsigned char * bytes, unsigned long size);
// remove depth by z axis // remove depth by z axis
void RTABMAP_EXP extractXYZCorrespondences(const std::multimap<int, pcl::PointXYZ> & words1, void RTABMAP_EXP extractXYZCorrespondences(const std::multimap<int, pcl::PointXYZ> & words1,

12
corelib/src/DBDriver.cpp
View File

@@ -404,9 +404,9 @@ void DBDriver::loadNodeData(std::list<Signature *> & signatures, bool loadMetric
//TODO Check also in the trash ? //TODO Check also in the trash ?
void DBDriver::getNodeData( void DBDriver::getNodeData(
int signatureId, int signatureId,
std::vector<unsigned char> & image, cv::Mat & imageCompressed,
std::vector<unsigned char> & depth, cv::Mat & depthCompressed,
std::vector<unsigned char> & depth2d, cv::Mat & depth2dCompressed,
float & fx, float & fx,
float & fy, float & fy,
float & cx, float & cx,
@@ -414,15 +414,15 @@ void DBDriver::getNodeData(
Transform & localTransform) const Transform & localTransform) const
{ {
_dbSafeAccessMutex.lock(); _dbSafeAccessMutex.lock();
this->getNodeDataQuery(signatureId, image, depth, depth2d, fx, fy, cx, cy, localTransform); this->getNodeDataQuery(signatureId, imageCompressed, depthCompressed, depth2dCompressed, fx, fy, cx, cy, localTransform);
_dbSafeAccessMutex.unlock(); _dbSafeAccessMutex.unlock();
} }
//TODO Check also in the trash ? //TODO Check also in the trash ?
void DBDriver::getNodeData(int signatureId, std::vector<unsigned char> & image) const void DBDriver::getNodeData(int signatureId, cv::Mat & imageCompressed) const
{ {
_dbSafeAccessMutex.lock(); _dbSafeAccessMutex.lock();
this->getNodeDataQuery(signatureId, image); this->getNodeDataQuery(signatureId, imageCompressed);
_dbSafeAccessMutex.unlock(); _dbSafeAccessMutex.unlock();
} }

72
corelib/src/DBDriverSqlite3.cpp
View File

@@ -515,9 +515,7 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
//Create the image //Create the image
if(dataSize>4 && data) if(dataSize>4 && data)
{ {
std::vector<unsigned char> image(dataSize); (*iter)->setImageCompressed(cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone());
memcpy(image.data(), data, dataSize);
(*iter)->setImage(image);
} }
if(loadMetricData) if(loadMetricData)
@@ -526,17 +524,16 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
dataSize = sqlite3_column_bytes(ppStmt, index++); dataSize = sqlite3_column_bytes(ppStmt, index++);
//Create the depth image //Create the depth image
std::vector<unsigned char> depth; cv::Mat depthCompressed;
if(dataSize>4 && data) if(dataSize>4 && data)
{ {
depth.resize(dataSize); depthCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone();
memcpy(depth.data(), data, dataSize);
} }
if(uStrNumCmp(_version, "0.7.0") < 0) if(uStrNumCmp(_version, "0.7.0") < 0)
{ {
float depthConstant = sqlite3_column_double(ppStmt, index++); float depthConstant = sqlite3_column_double(ppStmt, index++);
(*iter)->setDepth(depth, 1.0f/depthConstant, 1.0f/depthConstant, 0, 0); (*iter)->setDepthCompressed(depthCompressed, 1.0f/depthConstant, 1.0f/depthConstant, 0, 0);
} }
else else
{ {
@@ -544,7 +541,7 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
float fy = sqlite3_column_double(ppStmt, index++); float fy = sqlite3_column_double(ppStmt, index++);
float cx = sqlite3_column_double(ppStmt, index++); float cx = sqlite3_column_double(ppStmt, index++);
float cy = sqlite3_column_double(ppStmt, index++); float cy = sqlite3_column_double(ppStmt, index++);
(*iter)->setDepth(depth, fx, fy, cx, cy); (*iter)->setDepthCompressed(depthCompressed, fx, fy, cx, cy);
} }
data = sqlite3_column_blob(ppStmt, index); // local transform data = sqlite3_column_blob(ppStmt, index); // local transform
@@ -559,13 +556,12 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
data = sqlite3_column_blob(ppStmt, index); data = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++); dataSize = sqlite3_column_bytes(ppStmt, index++);
//Create the depth2d //Create the depth2d
std::vector<unsigned char> depth2d; cv::Mat depth2dCompressed;
if(dataSize>4 && data) if(dataSize>4 && data)
{ {
depth2d.resize(dataSize); (*iter)->setDepth2DCompressed(cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone()); // depth2d
memcpy(depth2d.data(), data, dataSize);
} }
(*iter)->setDepth2D(depth2d); // depth2d
} }
rc = sqlite3_step(ppStmt); // next result... rc = sqlite3_step(ppStmt); // next result...
@@ -586,9 +582,9 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
void DBDriverSqlite3::getNodeDataQuery( void DBDriverSqlite3::getNodeDataQuery(
int signatureId, int signatureId,
std::vector<unsigned char> & image, cv::Mat & imageCompressed,
std::vector<unsigned char> & depth, cv::Mat & depthCompressed,
std::vector<unsigned char> & depth2d, cv::Mat & depth2dCompressed,
float & fx, float & fx,
float & fy, float & fy,
float & cx, float & cx,
@@ -645,8 +641,7 @@ void DBDriverSqlite3::getNodeDataQuery(
//Create the image //Create the image
if(dataSize>4 && data) if(dataSize>4 && data)
{ {
image.resize(dataSize); imageCompressed = cv::Mat(1, dataSize, CV_8UC1).clone();
memcpy(image.data(), data, dataSize);
} }
data = sqlite3_column_blob(ppStmt, index); data = sqlite3_column_blob(ppStmt, index);
@@ -655,8 +650,7 @@ void DBDriverSqlite3::getNodeDataQuery(
//Create the depth image //Create the depth image
if(dataSize>4 && data) if(dataSize>4 && data)
{ {
depth.resize(dataSize); depthCompressed = cv::Mat(1, dataSize, CV_8UC1).clone();
memcpy(depth.data(), data, dataSize);
} }
if(uStrNumCmp(_version, "0.7.0") < 0) if(uStrNumCmp(_version, "0.7.0") < 0)
@@ -687,11 +681,10 @@ void DBDriverSqlite3::getNodeDataQuery(
//Create the depth2d //Create the depth2d
if(dataSize>4 && data) if(dataSize>4 && data)
{ {
depth2d.resize(dataSize); depth2dCompressed = cv::Mat(1, dataSize, CV_8UC1).clone();
memcpy(depth2d.data(), data, dataSize);
} }
if(depth.empty() || fx <= 0 || fy <= 0 || cx < 0 || cy < 0) if(depthCompressed.empty() || fx <= 0 || fy <= 0 || cx < 0 || cy < 0)
{ {
UWARN("No metric data loaded!? Consider using getNodeDataQuery() with image only."); UWARN("No metric data loaded!? Consider using getNodeDataQuery() with image only.");
} }
@@ -708,7 +701,7 @@ void DBDriverSqlite3::getNodeDataQuery(
} }
} }
void DBDriverSqlite3::getNodeDataQuery(int signatureId, std::vector<unsigned char> & image) const void DBDriverSqlite3::getNodeDataQuery(int signatureId, cv::Mat & imageCompressed) const
{ {
if(_ppDb) if(_ppDb)
{ {
@@ -744,8 +737,7 @@ void DBDriverSqlite3::getNodeDataQuery(int signatureId, std::vector<unsigned cha
//Create the image //Create the image
if(dataSize>4 && data) if(dataSize>4 && data)
{ {
image.resize(dataSize); imageCompressed = cv::Mat(1, dataSize, CV_8UC1).clone();
memcpy(image.data(), data, dataSize);
} }
rc = sqlite3_step(ppStmt); // next result... rc = sqlite3_step(ppStmt); // next result...
@@ -1823,9 +1815,9 @@ void DBDriverSqlite3::saveQuery(const std::list<Signature *> & signatures) const
for(std::list<Signature *>::const_iterator i=signatures.begin(); i!=signatures.end(); ++i) for(std::list<Signature *>::const_iterator i=signatures.begin(); i!=signatures.end(); ++i)
{ {
if((*i)->getImage().size()) if(!(*i)->getImageCompressed().empty())
{ {
stepImage(ppStmt, (*i)->id(), (*i)->getImage()); stepImage(ppStmt, (*i)->id(), (*i)->getImageCompressed());
} }
} }
@@ -1841,9 +1833,9 @@ void DBDriverSqlite3::saveQuery(const std::list<Signature *> & signatures) const
for(std::list<Signature *>::const_iterator i=signatures.begin(); i!=signatures.end(); ++i) for(std::list<Signature *>::const_iterator i=signatures.begin(); i!=signatures.end(); ++i)
{ {
//metric //metric
if((*i)->getDepth().size() || (*i)->getDepth2D().size()) if(!(*i)->getDepthCompressed().empty() || !(*i)->getDepth2DCompressed().empty())
{ {
stepDepth(ppStmt, (*i)->id(), (*i)->getDepth(), (*i)->getDepth2D(), (*i)->getDepthFx(), (*i)->getDepthFy(), (*i)->getDepthCx(), (*i)->getDepthCy(), (*i)->getLocalTransform()); stepDepth(ppStmt, (*i)->id(), (*i)->getDepthCompressed(), (*i)->getDepth2DCompressed(), (*i)->getDepthFx(), (*i)->getDepthFy(), (*i)->getDepthCx(), (*i)->getDepthCy(), (*i)->getLocalTransform());
} }
} }
// Finalize (delete) the statement // Finalize (delete) the statement
@@ -1948,9 +1940,9 @@ std::string DBDriverSqlite3::queryStepImage() const
} }
void DBDriverSqlite3::stepImage(sqlite3_stmt * ppStmt, void DBDriverSqlite3::stepImage(sqlite3_stmt * ppStmt,
int id, int id,
const std::vector<unsigned char> & image) const const cv::Mat & imageBytes) const
{ {
UDEBUG("Save image %d (size=%d)", id, (int)image.size()); UDEBUG("Save image %d (size=%d)", id, (int)imageBytes.cols);
if(!ppStmt) if(!ppStmt)
{ {
UFATAL(""); UFATAL("");
@@ -1962,9 +1954,9 @@ void DBDriverSqlite3::stepImage(sqlite3_stmt * ppStmt,
rc = sqlite3_bind_int(ppStmt, index++, id); rc = sqlite3_bind_int(ppStmt, index++, id);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str()); UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
if(image.size()) if(!imageBytes.empty())
{ {
rc = sqlite3_bind_blob(ppStmt, index++, image.data(), (int)image.size(), SQLITE_STATIC); rc = sqlite3_bind_blob(ppStmt, index++, imageBytes.data, (int)imageBytes.cols, SQLITE_STATIC);
} }
else else
{ {
@@ -1993,15 +1985,15 @@ std::string DBDriverSqlite3::queryStepDepth() const
} }
void DBDriverSqlite3::stepDepth(sqlite3_stmt * ppStmt, void DBDriverSqlite3::stepDepth(sqlite3_stmt * ppStmt,
int id, int id,
const std::vector<unsigned char> & depth, const cv::Mat & depthBytes,
const std::vector<unsigned char> & depth2d, const cv::Mat & depth2dBytes,
float fx, float fx,
float fy, float fy,
float cx, float cx,
float cy, float cy,
const Transform & localTransform) const const Transform & localTransform) const
{ {
UDEBUG("Save depth %d (size=%d) depth2d = %d", id, (int)depth.size(), (int)depth2d.size()); UDEBUG("Save depth %d (size=%d) depth2d = %d", id, (int)depthBytes.cols, (int)depth2dBytes.cols);
if(!ppStmt) if(!ppStmt)
{ {
UFATAL(""); UFATAL("");
@@ -2013,9 +2005,9 @@ void DBDriverSqlite3::stepDepth(sqlite3_stmt * ppStmt,
rc = sqlite3_bind_int(ppStmt, index++, id); rc = sqlite3_bind_int(ppStmt, index++, id);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str()); UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
if(depth.size()) if(!depthBytes.empty())
{ {
rc = sqlite3_bind_blob(ppStmt, index++, depth.data(), (int)depth.size(), SQLITE_STATIC); rc = sqlite3_bind_blob(ppStmt, index++, depthBytes.data, (int)depthBytes.cols, SQLITE_STATIC);
} }
else else
{ {
@@ -2043,9 +2035,9 @@ void DBDriverSqlite3::stepDepth(sqlite3_stmt * ppStmt,
rc = sqlite3_bind_blob(ppStmt, index++, localTransform.data(), localTransform.size()*sizeof(float), SQLITE_STATIC); 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()); UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
if(depth2d.size()) if(!depth2dBytes.empty())
{ {
rc = sqlite3_bind_blob(ppStmt, index++, depth2d.data(), (int)depth2d.size(), SQLITE_STATIC); rc = sqlite3_bind_blob(ppStmt, index++, depth2dBytes.data, (int)depth2dBytes.cols, SQLITE_STATIC);
} }
else else
{ {

14
corelib/src/DBDriverSqlite3.h
View File

@@ -77,15 +77,15 @@ private:
virtual void loadNodeDataQuery(std::list<Signature *> & signatures, bool loadMetricData) const; virtual void loadNodeDataQuery(std::list<Signature *> & signatures, bool loadMetricData) const;
virtual void getNodeDataQuery( virtual void getNodeDataQuery(
int signatureId, int signatureId,
std::vector<unsigned char> & image, cv::Mat & imageCompressed,
std::vector<unsigned char> & depth, cv::Mat & depthCompressed,
std::vector<unsigned char> & depth2d, cv::Mat & depth2dCompressed,
float & fx, float & fx,
float & fy, float & fy,
float & cx, float & cx,
float & cy, float & cy,
Transform & localTransform) const; Transform & localTransform) const;
virtual void getNodeDataQuery(int signatureId, std::vector<unsigned char> & image) const; virtual void getNodeDataQuery(int signatureId, cv::Mat & imageCompressed) const;
virtual void getPoseQuery(int signatureId, Transform & pose, int & mapId) const; virtual void getPoseQuery(int signatureId, Transform & pose, int & mapId) const;
virtual void getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren) const; virtual void getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren) const;
virtual void getLastIdQuery(const std::string & tableName, int & id) const; virtual void getLastIdQuery(const std::string & tableName, int & id) const;
@@ -102,12 +102,12 @@ private:
void stepImage( void stepImage(
sqlite3_stmt * ppStmt, sqlite3_stmt * ppStmt,
int id, int id,
const std::vector<unsigned char> & image) const; const cv::Mat & imageBytes) const;
void stepDepth( void stepDepth(
sqlite3_stmt * ppStmt, sqlite3_stmt * ppStmt,
int id, int id,
const std::vector<unsigned char> & depth, const cv::Mat & depthBytes,
const std::vector<unsigned char> & depth2d, const cv::Mat & depth2dBytes,
float fx, float fx,
float fy, float fy,
float cx, float cx,

12
corelib/src/DBReader.cpp
View File

@@ -207,9 +207,9 @@ void DBReader::getNextImage(
if(!this->isKilled() && _currentId != _ids.end()) if(!this->isKilled() && _currentId != _ids.end())
{ {
std::vector<unsigned char> imageBytes; cv::Mat imageBytes;
std::vector<unsigned char> depthBytes; cv::Mat depthBytes;
std::vector<unsigned char> depth2dBytes; cv::Mat depth2dBytes;
int mapId; int mapId;
_dbDriver->getNodeData(*_currentId, imageBytes, depthBytes, depth2dBytes, fx, fy, cx, cy, localTransform); _dbDriver->getNodeData(*_currentId, imageBytes, depthBytes, depth2dBytes, fx, fy, cx, cy, localTransform);
_dbDriver->getPose(*_currentId, pose, mapId); _dbDriver->getPose(*_currentId, pose, mapId);
@@ -220,9 +220,9 @@ void DBReader::getNextImage(
UWARN("No image loaded from the database for id=%d!", *_currentId); UWARN("No image loaded from the database for id=%d!", *_currentId);
} }
util3d::CompressionThread ctImage(&imageBytes, true); util3d::CompressionThread ctImage(imageBytes, true);
util3d::CompressionThread ctDepth(&depthBytes, true); util3d::CompressionThread ctDepth(depthBytes, true);
util3d::CompressionThread ctDepth2D(&depth2dBytes, false); util3d::CompressionThread ctDepth2D(depth2dBytes, false);
ctImage.start(); ctImage.start();
ctDepth.start(); ctDepth.start();
ctDepth2D.start(); ctDepth2D.start();

60
corelib/src/Memory.cpp
View File

@@ -1129,7 +1129,7 @@ std::map<int, float> Memory::computeLikelihood(const Signature * signature, cons
{ {
UFATAL("Signature %d not found in WM ?!?", *iter); UFATAL("Signature %d not found in WM ?!?", *iter);
} }
sim = signature->compareTo(sB); sim = signature->compareTo(*sB);
} }
likelihood.insert(likelihood.end(), std::pair<int, float>(*iter, sim)); likelihood.insert(likelihood.end(), std::pair<int, float>(*iter, sim));
@@ -1809,12 +1809,12 @@ Transform Memory::computeIcpTransform(int oldId, int newId, Transform guess, boo
if(icp3D) if(icp3D)
{ {
//Depth required, if not in RAM, load it from LTM //Depth required, if not in RAM, load it from LTM
if(oldS->getDepth().empty()) if(oldS->getDepthCompressed().empty())
{ {
depthToLoad.push_back(oldS); depthToLoad.push_back(oldS);
added.insert(oldS->id()); added.insert(oldS->id());
} }
if(newS->getDepth().empty()) if(newS->getDepthCompressed().empty())
{ {
depthToLoad.push_back(newS); depthToLoad.push_back(newS);
added.insert(newS->id()); added.insert(newS->id());
@@ -1823,11 +1823,11 @@ Transform Memory::computeIcpTransform(int oldId, int newId, Transform guess, boo
else else
{ {
//Depth required, if not in RAM, load it from LTM //Depth required, if not in RAM, load it from LTM
if(oldS->getDepth2D().size() == 0 && added.find(oldS->id()) == added.end()) if(oldS->getDepth2DCompressed().empty() && added.find(oldS->id()) == added.end())
{ {
depthToLoad.push_back(oldS); depthToLoad.push_back(oldS);
} }
if(newS->getDepth2D().size() == 0 && added.find(newS->id()) == added.end()) if(newS->getDepth2DCompressed().empty() && added.find(newS->id()) == added.end())
{ {
depthToLoad.push_back(newS); depthToLoad.push_back(newS);
} }
@@ -1846,22 +1846,22 @@ Transform Memory::computeIcpTransform(int oldId, int newId, Transform guess, boo
{ {
if(oldS->getDepthRaw().empty()) if(oldS->getDepthRaw().empty())
{ {
oldS->setDepthRaw(util3d::uncompressImage(oldS->getDepth())); oldS->setDepthRaw(util3d::uncompressImage(oldS->getDepthCompressed()));
} }
if(newS->getDepthRaw().empty()) if(newS->getDepthRaw().empty())
{ {
newS->setDepthRaw(util3d::uncompressImage(newS->getDepth())); newS->setDepthRaw(util3d::uncompressImage(newS->getDepthCompressed()));
} }
} }
else else
{ {
if(oldS->getDepth2DRaw().empty()) if(oldS->getDepth2DRaw().empty())
{ {
oldS->setDepth2DRaw(util3d::uncompressData(oldS->getDepth2D())); oldS->setDepth2DRaw(util3d::uncompressData(oldS->getDepth2DCompressed()));
} }
if(newS->getDepth2DRaw().empty()) if(newS->getDepth2DRaw().empty())
{ {
newS->setDepth2DRaw(util3d::uncompressData(newS->getDepth2D())); newS->setDepth2DRaw(util3d::uncompressData(newS->getDepth2DCompressed()));
} }
} }
@@ -2089,7 +2089,7 @@ Transform Memory::computeScanMatchingTransform(
{ {
Signature * s = _getSignature(iter->first); Signature * s = _getSignature(iter->first);
UASSERT(s != 0); UASSERT(s != 0);
if(s->getDepth2D().size() == 0) if(s->getDepth2DCompressed().empty())
{ {
depthToLoad.push_back(s); depthToLoad.push_back(s);
} }
@@ -2106,9 +2106,9 @@ Transform Memory::computeScanMatchingTransform(
if(iter->first != newId) if(iter->first != newId)
{ {
const Signature * s = this->getSignature(iter->first); const Signature * s = this->getSignature(iter->first);
if(s->getDepth2D().size()) if(!s->getDepth2DCompressed().empty())
{ {
*assembledOldClouds += *util3d::cvMat2Cloud(util3d::uncompressData(s->getDepth2D()), iter->second); *assembledOldClouds += *util3d::cvMat2Cloud(util3d::uncompressData(s->getDepth2DCompressed()), iter->second);
} }
else else
{ {
@@ -2127,7 +2127,7 @@ Transform Memory::computeScanMatchingTransform(
const Signature * newS = getSignature(newId); const Signature * newS = getSignature(newId);
pcl::PointCloud<pcl::PointXYZ>::Ptr newCloud; pcl::PointCloud<pcl::PointXYZ>::Ptr newCloud;
UASSERT(uContains(poses, newId)); UASSERT(uContains(poses, newId));
newCloud = util3d::cvMat2Cloud(util3d::uncompressData(newS->getDepth2D()), poses.at(newId)); newCloud = util3d::cvMat2Cloud(util3d::uncompressData(newS->getDepth2DCompressed()), poses.at(newId));
//voxelize //voxelize
if(newCloud->size() && _icp2VoxelSize > 0.0f) if(newCloud->size() && _icp2VoxelSize > 0.0f)
@@ -2394,7 +2394,7 @@ void Memory::rehearsal(Signature * signature, Statistics * stats)
{ {
UFATAL("Signature %d null?!?", id); UFATAL("Signature %d null?!?", id);
} }
float sim = signature->compareTo(sB); float sim = signature->compareTo(*sB);
int merged = 0; int merged = 0;
if(sim >= _similarityThreshold) if(sim >= _similarityThreshold)
@@ -2556,13 +2556,13 @@ int Memory::getMapId(int signatureId) const
return mapId; return mapId;
} }
std::vector<unsigned char> Memory::getImage(int signatureId) const cv::Mat Memory::getImageCompressed(int signatureId) const
{ {
std::vector<unsigned char> image; cv::Mat image;
const Signature * s = this->getSignature(signatureId); const Signature * s = this->getSignature(signatureId);
if(s) if(s)
{ {
image = s->getImage(); image = s->getImageCompressed();
} }
if(image.empty() && this->isRawDataKept() && _dbDriver) if(image.empty() && this->isRawDataKept() && _dbDriver)
{ {
@@ -2576,7 +2576,7 @@ Signature Memory::getSignatureData(int locationId, bool uncompressedData)
UDEBUG(""); UDEBUG("");
Signature r; Signature r;
Signature * s = this->_getSignature(locationId); Signature * s = this->_getSignature(locationId);
if(s && s->getImage().size()) if(s && !s->getImageCompressed().empty())
{ {
r = *s; r = *s;
} }
@@ -2600,7 +2600,7 @@ Signature Memory::getSignatureData(int locationId, bool uncompressedData)
if(signatures.size()) if(signatures.size())
{ {
Signature * sTmp = signatures.front(); Signature * sTmp = signatures.front();
if(sTmp->getImage().size() == 0) if(sTmp->getImageCompressed().empty())
{ {
_dbDriver->loadNodeData(signatures, !sTmp->getPose().isNull()); _dbDriver->loadNodeData(signatures, !sTmp->getPose().isNull());
} }
@@ -2619,7 +2619,7 @@ Signature Memory::getSignatureData(int locationId, bool uncompressedData)
} }
UDEBUG(""); UDEBUG("");
if(uncompressedData && r.getImageRaw().empty() && r.getImage().size()) if(uncompressedData && r.getImageRaw().empty() && !r.getImageCompressed().empty())
{ {
//uncompress data //uncompress data
if(s) if(s)
@@ -2961,25 +2961,25 @@ void Memory::copyData(const Signature * from, Signature * to)
if(from->isSaved() && _dbDriver) if(from->isSaved() && _dbDriver)
{ {
std::vector<unsigned char> image; cv::Mat image;
std::vector<unsigned char> depth; cv::Mat depth;
std::vector<unsigned char> depth2d; cv::Mat depth2d;
float fx, fy, cx, cy; float fx, fy, cx, cy;
Transform localTransform; Transform localTransform;
_dbDriver->getNodeData(from->id(), image, depth, depth2d, fx, fy, cx, cy, localTransform); _dbDriver->getNodeData(from->id(), image, depth, depth2d, fx, fy, cx, cy, localTransform);
to->setImage(image); to->setImageCompressed(image);
to->setDepth(depth, fx, fy, cx, cy); to->setDepthCompressed(depth, fx, fy, cx, cy);
to->setDepth2D(depth2d); to->setDepth2DCompressed(depth2d);
to->setLocalTransform(localTransform); to->setLocalTransform(localTransform);
UDEBUG("Loaded image data from database"); UDEBUG("Loaded image data from database");
} }
else else
{ {
to->setImage(from->getImage()); to->setImageCompressed(from->getImageCompressed());
to->setDepth(from->getDepth(), from->getDepthFx(), from->getDepthFy(), from->getDepthCx(), from->getDepthCy()); to->setDepthCompressed(from->getDepthCompressed(), from->getDepthFx(), from->getDepthFy(), from->getDepthCx(), from->getDepthCy());
to->setDepth2D(from->getDepth2D()); to->setDepth2DCompressed(from->getDepth2DCompressed());
to->setLocalTransform(from->getLocalTransform()); to->setLocalTransform(from->getLocalTransform());
} }
@@ -3400,7 +3400,7 @@ Signature * Memory::createSignature(const SensorData & data, bool keepRawData, S
words, words,
words3D, words3D,
data.pose(), data.pose(),
util3d::compressData(data.depth2d())); util3d::compressData2(data.depth2d()));
} }

4
corelib/src/Rtabmap.cpp
View File

@@ -858,7 +858,7 @@ bool Rtabmap::process(const SensorData & data)
//============================================================ //============================================================
if(_scanMatchingSize>0 && if(_scanMatchingSize>0 &&
signature->getNeighbors().size() == 1 && signature->getNeighbors().size() == 1 &&
signature->getDepth2D().size() && !signature->getDepth2DCompressed().empty() &&
rehearsedId == 0) // don't do it if rehearsal happened rehearsedId == 0) // don't do it if rehearsal happened
{ {
UINFO("Odometry correction by scan matching (size=%d)...", _scanMatchingSize); UINFO("Odometry correction by scan matching (size=%d)...", _scanMatchingSize);
@@ -1382,7 +1382,7 @@ bool Rtabmap::process(const SensorData & data)
int localSpaceNearestId = 0; int localSpaceNearestId = 0;
if(_lcHypothesisId == 0 && if(_lcHypothesisId == 0 &&
_localLoopClosureDetectionSpace && _localLoopClosureDetectionSpace &&
signature->getDepth2D().size()) !signature->getDepth2DCompressed().empty())
{ {
//============================================================ //============================================================
// Scan matching LOCAL LOOP CLOSURE SPACE // Scan matching LOCAL LOOP CLOSURE SPACE

68
corelib/src/Signature.cpp
View File

@@ -57,9 +57,9 @@ Signature::Signature(
const std::multimap<int, cv::KeyPoint> & words, const std::multimap<int, cv::KeyPoint> & words,
const std::multimap<int, pcl::PointXYZ> & words3, // in base_link frame (localTransform applied) const std::multimap<int, pcl::PointXYZ> & words3, // in base_link frame (localTransform applied)
const Transform & pose, const Transform & pose,
const std::vector<unsigned char> & depth2D, // in base_link frame const cv::Mat & depth2DCompressed, // in base_link frame
const std::vector<unsigned char> & image, // in camera_link frame const cv::Mat & imageCompressed, // in camera_link frame
const std::vector<unsigned char> & depth, // in camera_link frame const cv::Mat & depthCompressed, // in camera_link frame
float fx, float fx,
float fy, float fy,
float cx, float cx,
@@ -73,9 +73,9 @@ Signature::Signature(
_neighborsModified(true), _neighborsModified(true),
_words(words), _words(words),
_enabled(false), _enabled(false),
_image(image), _imageCompressed(imageCompressed),
_depth(depth), _depthCompressed(depthCompressed),
_depth2D(depth2D), _depth2DCompressed(depth2DCompressed),
_fx(fx), _fx(fx),
_fy(fy), _fy(fy),
_cx(cx), _cx(cx),
@@ -88,7 +88,7 @@ Signature::Signature(
Signature::~Signature() Signature::~Signature()
{ {
//ULOGGER_DEBUG("id=%d", _id); //UDEBUG("id=%d", _id);
} }
void Signature::addNeighbors(const std::map<int, Transform> & neighbors) void Signature::addNeighbors(const std::map<int, Transform> & neighbors)
@@ -165,10 +165,10 @@ void Signature::changeLoopClosureId(int idFrom, int idTo)
} }
float Signature::compareTo(const Signature * s) const float Signature::compareTo(const Signature & s) const
{ {
float similarity = 0.0f; float similarity = 0.0f;
const std::multimap<int, cv::KeyPoint> & words = s->getWords(); const std::multimap<int, cv::KeyPoint> & words = s.getWords();
if(words.size() != 0 && _words.size() != 0) if(words.size() != 0 && _words.size() != 0)
{ {
std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > pairs; std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > pairs;
@@ -217,10 +217,10 @@ void Signature::removeWord(int wordId)
_words3.erase(wordId); _words3.erase(wordId);
} }
void Signature::setDepth(const std::vector<unsigned char> & depth, float fx, float fy, float cx, float cy) void Signature::setDepthCompressed(const cv::Mat & bytes, float fx, float fy, float cx, float cy)
{ {
UASSERT_MSG(depth.empty() || (!depth.empty() && fx > 0.0f && fy > 0.0f && cx >= 0.0f && cy >= 0.0f), uFormat("fx=%f fy=%f cx=%f cy=%f",fx,fy,cx,cy).c_str()); UASSERT_MSG(bytes.empty() || (!bytes.empty() && fx > 0.0f && fy > 0.0f && cx >= 0.0f && cy >= 0.0f), uFormat("fx=%f fy=%f cx=%f cy=%f",fx,fy,cx,cy).c_str());
_depth = depth; _depthCompressed = bytes;
_fx=fx; _fx=fx;
_fy=fy; _fy=fy;
_cx=cx; _cx=cx;
@@ -247,53 +247,53 @@ void Signature::uncompressData()
uncompressData(&_imageRaw, &_depthRaw, &_depth2DRaw); uncompressData(&_imageRaw, &_depthRaw, &_depth2DRaw);
} }
void Signature::uncompressData(cv::Mat * image, cv::Mat * depth, cv::Mat * depth2D) const void Signature::uncompressData(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::Mat * depth2DRaw) const
{ {
if(image) if(imageRaw)
{ {
*image = _imageRaw; *imageRaw = _imageRaw;
} }
if(depth) if(depthRaw)
{ {
*depth = _depthRaw; *depthRaw = _depthRaw;
} }
if(depth2D) if(depth2DRaw)
{ {
*depth2D = _depth2DRaw; *depth2DRaw = _depth2DRaw;
} }
if( (image && image->empty()) || if( (imageRaw && imageRaw->empty()) ||
(depth && depth->empty()) || (depthRaw && depthRaw->empty()) ||
(depth2D && depth2D->empty())) (depth2DRaw && depth2DRaw->empty()))
{ {
util3d::CompressionThread ctImage(&_image, true); util3d::CompressionThread ctImage(_imageCompressed, true);
util3d::CompressionThread ctDepth(&_depth, true); util3d::CompressionThread ctDepth(_depthCompressed, true);
util3d::CompressionThread ctDepth2D(&_depth2D, false); util3d::CompressionThread ctDepth2D(_depth2DCompressed, false);
if(image && image->empty()) if(imageRaw && imageRaw->empty())
{ {
ctImage.start(); ctImage.start();
} }
if(depth && depth->empty()) if(depthRaw && depthRaw->empty())
{ {
ctDepth.start(); ctDepth.start();
} }
if(depth2D && depth2D->empty()) if(depth2DRaw && depth2DRaw->empty())
{ {
ctDepth2D.start(); ctDepth2D.start();
} }
ctImage.join(); ctImage.join();
ctDepth.join(); ctDepth.join();
ctDepth2D.join(); ctDepth2D.join();
if(image && image->empty()) if(imageRaw && imageRaw->empty())
{ {
*image = ctImage.getUncompressedData(); *imageRaw = ctImage.getUncompressedData();
} }
if(depth && depth->empty()) if(depthRaw && depthRaw->empty())
{ {
*depth = ctDepth.getUncompressedData(); *depthRaw = ctDepth.getUncompressedData();
} }
if(depth2D && depth2D->empty()) if(depth2DRaw && depth2DRaw->empty())
{ {
*depth2D = ctDepth2D.getUncompressedData(); *depth2DRaw = ctDepth2D.getUncompressedData();
} }
} }
} }

95
corelib/src/util3d.cpp
View File

@@ -66,7 +66,6 @@ namespace util3d
// format : ".png" ".jpg" "" (empty is general) // format : ".png" ".jpg" "" (empty is general)
CompressionThread::CompressionThread(const cv::Mat & mat, const std::string & format) : CompressionThread::CompressionThread(const cv::Mat & mat, const std::string & format) :
constCompressedData_(0),
uncompressedData_(mat), uncompressedData_(mat),
format_(format), format_(format),
image_(!format.empty()), image_(!format.empty()),
@@ -75,8 +74,8 @@ CompressionThread::CompressionThread(const cv::Mat & mat, const std::string & fo
UASSERT(format.empty() || format.compare(".png") == 0 || format.compare(".jpg") == 0); UASSERT(format.empty() || format.compare(".png") == 0 || format.compare(".jpg") == 0);
} }
// assume image // assume image
CompressionThread::CompressionThread(const std::vector<unsigned char> * bytes, bool isImage) : CompressionThread::CompressionThread(const cv::Mat & bytes, bool isImage) :
constCompressedData_(bytes), compressedData_(bytes),
image_(isImage), image_(isImage),
compressMode_(false) compressMode_(false)
{} {}
@@ -88,25 +87,25 @@ void CompressionThread::mainLoop()
{ {
if(image_) if(image_)
{ {
compressedData_ = compressImage(uncompressedData_, format_); compressedData_ = compressImage2(uncompressedData_, format_);
} }
else else
{ {
compressedData_ = compressData(uncompressedData_); compressedData_ = compressData2(uncompressedData_);
} }
} }
} }
else // uncompress else // uncompress
{ {
if(constCompressedData_ && constCompressedData_->size()) if(!compressedData_.empty())
{ {
if(image_) if(image_)
{ {
uncompressedData_ = uncompressImage(*constCompressedData_); uncompressedData_ = uncompressImage(compressedData_);
} }
else else
{ {
uncompressedData_ = uncompressData(*constCompressedData_); uncompressedData_ = uncompressData(compressedData_);
} }
} }
} }
@@ -1105,6 +1104,31 @@ std::vector<unsigned char> compressImage(const cv::Mat & image, const std::strin
return bytes; return bytes;
} }
// ".png" or ".jpg"
cv::Mat compressImage2(const cv::Mat & image, const std::string & format)
{
std::vector<unsigned char> bytes = compressImage(image, format);
if(bytes.size())
{
return cv::Mat(1, bytes.size(), CV_8UC1, bytes.data()).clone();
}
return cv::Mat();
}
cv::Mat uncompressImage(const cv::Mat & bytes)
{
cv::Mat image;
if(!bytes.empty())
{
#if CV_MAJOR_VERSION >=2 && CV_MINOR_VERSION >=4
image = cv::imdecode(bytes, cv::IMREAD_UNCHANGED);
#else
image = cv::imdecode(bytes, -1);
#endif
}
return image;
}
cv::Mat uncompressImage(const std::vector<unsigned char> & bytes) cv::Mat uncompressImage(const std::vector<unsigned char> & bytes)
{ {
cv::Mat image; cv::Mat image;
@@ -1150,20 +1174,61 @@ std::vector<unsigned char> compressData(const cv::Mat & data)
return bytes; return bytes;
} }
cv::Mat compressData2(const cv::Mat & data)
{
cv::Mat bytes;
if(!data.empty())
{
uLong sourceLen = uLong(data.total())*uLong(data.elemSize());
uLong destLen = compressBound(sourceLen);
bytes = cv::Mat(1, destLen+3*sizeof(int), CV_8UC1);
int errCode = compress(
(Bytef *)bytes.data,
&destLen,
(const Bytef *)data.data,
sourceLen);
bytes = cv::Mat(bytes, cv::Rect(0,0, destLen+3*sizeof(int), 1));
*((int*)&bytes.data[destLen]) = data.rows;
*((int*)&bytes.data[destLen+sizeof(int)]) = data.cols;
*((int*)&bytes.data[destLen+2*sizeof(int)]) = data.type();
if(errCode == Z_MEM_ERROR)
{
UERROR("Z_MEM_ERROR : Insufficient memory.");
}
else if(errCode == Z_BUF_ERROR)
{
UERROR("Z_BUF_ERROR : The buffer dest was not large enough to hold the uncompressed data.");
}
}
return bytes;
}
cv::Mat uncompressData(const cv::Mat & bytes)
{
UASSERT(bytes.empty() || bytes.type() == CV_8UC1);
return uncompressData(bytes.data, bytes.cols*bytes.rows);
}
cv::Mat uncompressData(const std::vector<unsigned char> & bytes) cv::Mat uncompressData(const std::vector<unsigned char> & bytes)
{
return uncompressData(bytes.data(), bytes.size());
}
cv::Mat uncompressData(const unsigned char * bytes, unsigned long size)
{ {
cv::Mat data; cv::Mat data;
if(bytes.size()>=3*sizeof(int)) if(bytes && size>=3*sizeof(int))
{ {
//last 3 int elements are matrix size and type //last 3 int elements are matrix size and type
int height = *((int*)&bytes[bytes.size()-3*sizeof(int)]); int height = *((int*)&bytes[size-3*sizeof(int)]);
int width = *((int*)&bytes[bytes.size()-2*sizeof(int)]); int width = *((int*)&bytes[size-2*sizeof(int)]);
int type = *((int*)&bytes[bytes.size()-1*sizeof(int)]); int type = *((int*)&bytes[size-1*sizeof(int)]);
// If the size is higher, it may be a wrong data format. // If the size is higher, it may be a wrong data format.
UASSERT_MSG(height>=0 && height<10000 && UASSERT_MSG(height>=0 && height<10000 &&
width>=0 && width<10000, width>=0 && width<10000,
uFormat("size=%d, height=%d width=%d type=%d", bytes.size(), height, width, type).c_str()); uFormat("size=%d, height=%d width=%d type=%d", size, height, width, type).c_str());
data = cv::Mat(height, width, type); data = cv::Mat(height, width, type);
uLongf totalUncompressed = uLongf(data.total())*uLongf(data.elemSize()); uLongf totalUncompressed = uLongf(data.total())*uLongf(data.elemSize());
@@ -1171,8 +1236,8 @@ cv::Mat uncompressData(const std::vector<unsigned char> & bytes)
int errCode = uncompress( int errCode = uncompress(
(Bytef*)data.data, (Bytef*)data.data,
&totalUncompressed, &totalUncompressed,
(const Bytef*)bytes.data(), (const Bytef*)bytes,
uLong(bytes.size())); uLong(size));
if(errCode == Z_MEM_ERROR) if(errCode == Z_MEM_ERROR)
{ {

1
guilib/include/rtabmap/gui/DatabaseViewer.h
View File

@@ -63,6 +63,7 @@ public:
bool openDatabase(const QString & path); bool openDatabase(const QString & path);
protected: protected:
virtual void showEvent(QShowEvent* anEvent);
virtual void resizeEvent(QResizeEvent* anEvent); virtual void resizeEvent(QResizeEvent* anEvent);
virtual void closeEvent(QCloseEvent* event); virtual void closeEvent(QCloseEvent* event);

1
guilib/include/rtabmap/gui/MainWindow.h
View File

@@ -260,6 +260,7 @@ private:
QMap<int, Signature> _cachedSignatures; QMap<int, Signature> _cachedSignatures;
QMap<int, int> _mapIds; QMap<int, int> _mapIds;
std::map<int, Transform> _currentPosesMap; std::map<int, Transform> _currentPosesMap;
std::multimap<int, Link> _currentLinksMap;
std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr > _createdClouds; std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr > _createdClouds;
std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > _createdScans; std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > _createdScans;
std::map<int, std::pair<cv::Mat, cv::Mat> > _occupancyLocalMaps; // <ground, obstacles> std::map<int, std::pair<cv::Mat, cv::Mat> > _occupancyLocalMaps; // <ground, obstacles>

47
guilib/src/DatabaseViewer.cpp
View File

@@ -279,6 +279,14 @@ void DatabaseViewer::closeEvent(QCloseEvent* event)
} }
} }
void DatabaseViewer::showEvent(QShowEvent* anEvent)
{
ui_->graphicsView_A->fitInView(ui_->graphicsView_A->sceneRect(), Qt::KeepAspectRatio);
ui_->graphicsView_B->fitInView(ui_->graphicsView_B->sceneRect(), Qt::KeepAspectRatio);
ui_->graphicsView_A->resetZoom();
ui_->graphicsView_B->resetZoom();
}
void DatabaseViewer::resizeEvent(QResizeEvent* anEvent) void DatabaseViewer::resizeEvent(QResizeEvent* anEvent)
{ {
ui_->graphicsView_A->fitInView(ui_->graphicsView_A->sceneRect(), Qt::KeepAspectRatio); ui_->graphicsView_A->fitInView(ui_->graphicsView_A->sceneRect(), Qt::KeepAspectRatio);
@@ -350,8 +358,8 @@ void DatabaseViewer::extractImages()
for(int i=0; i<ids_.size(); i+=1) for(int i=0; i<ids_.size(); i+=1)
{ {
int id = ids_.at(i); int id = ids_.at(i);
std::vector<unsigned char> compressedRgb = memory_->getImage(id); cv::Mat compressedRgb = memory_->getImageCompressed(id);
if(compressedRgb.size()) if(!compressedRgb.empty())
{ {
cv::Mat imageMat = rtabmap::util3d::uncompressImage(compressedRgb); cv::Mat imageMat = rtabmap::util3d::uncompressImage(compressedRgb);
cv::imwrite(QString("%1/%2.png").arg(path).arg(id).toStdString(), imageMat); cv::imwrite(QString("%1/%2.png").arg(path).arg(id).toStdString(), imageMat);
@@ -1300,31 +1308,16 @@ void DatabaseViewer::updateConstraintView(const rtabmap::Link & link,
bool updateB = false; bool updateB = false;
ui_->horizontalSlider_A->blockSignals(true); ui_->horizontalSlider_A->blockSignals(true);
ui_->horizontalSlider_B->blockSignals(true); ui_->horizontalSlider_B->blockSignals(true);
if(ui_->horizontalSlider_A->value() == idToIndex_.value(link.from())) // set from on left and to on right
{ if(ui_->horizontalSlider_A->value() != idToIndex_.value(link.from()))
ui_->horizontalSlider_B->setValue(idToIndex_.value(link.to()));
updateB=true;
}
else if(ui_->horizontalSlider_A->value() == idToIndex_.value(link.to()))
{
ui_->horizontalSlider_B->setValue(idToIndex_.value(link.from()));
updateB=true;
}
else if(ui_->horizontalSlider_B->value() == idToIndex_.value(link.from()))
{
ui_->horizontalSlider_A->setValue(idToIndex_.value(link.to()));
updateA = true;
}
else if(ui_->horizontalSlider_B->value() == idToIndex_.value(link.to()))
{ {
ui_->horizontalSlider_A->setValue(idToIndex_.value(link.from())); ui_->horizontalSlider_A->setValue(idToIndex_.value(link.from()));
updateA=true; updateA=true;
} }
else if(ui_->horizontalSlider_B->value() != idToIndex_.value(link.to()))
{ {
ui_->horizontalSlider_A->setValue(idToIndex_.value(link.from()));
ui_->horizontalSlider_B->setValue(idToIndex_.value(link.to())); ui_->horizontalSlider_B->setValue(idToIndex_.value(link.to()));
updateA=updateB=true; updateB=true;
} }
ui_->horizontalSlider_A->blockSignals(false); ui_->horizontalSlider_A->blockSignals(false);
ui_->horizontalSlider_B->blockSignals(false); ui_->horizontalSlider_B->blockSignals(false);
@@ -1505,10 +1498,10 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
for(int i=0; i<ids_.size(); ++i) for(int i=0; i<ids_.size(); ++i)
{ {
Signature data = memory_->getSignatureData(ids_.at(i), false); Signature data = memory_->getSignatureData(ids_.at(i), false);
if(data.getDepth2D().size()) if(!data.getDepth2DCompressed().empty())
{ {
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud; pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
cv::Mat depth2d = rtabmap::util3d::uncompressData(data.getDepth2D()); cv::Mat depth2d = rtabmap::util3d::uncompressData(data.getDepth2DCompressed());
cloud = rtabmap::util3d::depth2DToPointCloud(depth2d); cloud = rtabmap::util3d::depth2DToPointCloud(depth2d);
scans_.insert(std::make_pair(ids_.at(i), cloud)); scans_.insert(std::make_pair(ids_.at(i), cloud));
} }
@@ -1681,8 +1674,8 @@ void DatabaseViewer::refineConstraint(int from, int to)
if(ui_->checkBox_icp_2d->isChecked()) if(ui_->checkBox_icp_2d->isChecked())
{ {
//2D //2D
cv::Mat oldDepth2D = util3d::uncompressData(dataFrom.getDepth2D()); cv::Mat oldDepth2D = util3d::uncompressData(dataFrom.getDepth2DCompressed());
cv::Mat newDepth2D = util3d::uncompressData(dataTo.getDepth2D()); cv::Mat newDepth2D = util3d::uncompressData(dataTo.getDepth2DCompressed());
if(!oldDepth2D.empty() && !newDepth2D.empty()) if(!oldDepth2D.empty() && !newDepth2D.empty())
{ {
@@ -1711,8 +1704,8 @@ void DatabaseViewer::refineConstraint(int from, int to)
else else
{ {
//3D //3D
cv::Mat depthA = rtabmap::util3d::uncompressImage(dataFrom.getDepth()); cv::Mat depthA = rtabmap::util3d::uncompressImage(dataFrom.getDepthCompressed());
cv::Mat depthB = rtabmap::util3d::uncompressImage(dataTo.getDepth()); cv::Mat depthB = rtabmap::util3d::uncompressImage(dataTo.getDepthCompressed());
if(depthA.type() == CV_8UC1 || depthB.type() == CV_8UC1) if(depthA.type() == CV_8UC1 || depthB.type() == CV_8UC1)
{ {

11
guilib/src/MainWindow.cpp
View File

@@ -928,7 +928,7 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
// update clouds // update clouds
if(stat.poses().size()) if(stat.poses().size())
{ {
// update pose only if a odometry is not received // update pose only if odometry is not received
updateMapCloud(stat.poses(), updateMapCloud(stat.poses(),
_odometryReceived||stat.poses().size()==0?Transform():stat.poses().rbegin()->second, _odometryReceived||stat.poses().size()==0?Transform():stat.poses().rbegin()->second,
stat.constraints()); stat.constraints());
@@ -986,11 +986,12 @@ void MainWindow::updateMapCloud(
if(posesIn.size()) if(posesIn.size())
{ {
_currentPosesMap = posesIn; _currentPosesMap = posesIn;
_currentLinksMap = constraints;
if(_currentPosesMap.size()) if(_currentPosesMap.size())
{ {
if(!_ui->actionSave_point_cloud->isEnabled() && if(!_ui->actionSave_point_cloud->isEnabled() &&
_cachedSignatures.size() && _cachedSignatures.size() &&
(--_cachedSignatures.end())->getDepth().size()) !(--_cachedSignatures.end())->getDepthCompressed().empty())
{ {
//enable save cloud action //enable save cloud action
_ui->actionSave_point_cloud->setEnabled(true); _ui->actionSave_point_cloud->setEnabled(true);
@@ -999,7 +1000,7 @@ void MainWindow::updateMapCloud(
if(!_ui->actionView_scans->isEnabled() && if(!_ui->actionView_scans->isEnabled() &&
_cachedSignatures.size() && _cachedSignatures.size() &&
(--_cachedSignatures.end())->getDepth2D().size()) !(--_cachedSignatures.end())->getDepth2DCompressed().empty())
{ {
_ui->actionExport_2D_scans_ply_pcd->setEnabled(true); _ui->actionExport_2D_scans_ply_pcd->setEnabled(true);
_ui->actionExport_2D_Grid_map_bmp_png->setEnabled(true); _ui->actionExport_2D_Grid_map_bmp_png->setEnabled(true);
@@ -1065,7 +1066,7 @@ void MainWindow::updateMapCloud(
else if(_cachedSignatures.contains(iter->first)) else if(_cachedSignatures.contains(iter->first))
{ {
QMap<int, Signature>::iterator jter = _cachedSignatures.find(iter->first); QMap<int, Signature>::iterator jter = _cachedSignatures.find(iter->first);
if(jter->getImage().size() && jter->getDepth().size()) if(!jter->getImageCompressed().empty() && !jter->getDepthCompressed().empty())
{ {
this->createAndAddCloudToMap(iter->first, iter->second); this->createAndAddCloudToMap(iter->first, iter->second);
} }
@@ -1100,7 +1101,7 @@ void MainWindow::updateMapCloud(
else if(_cachedSignatures.contains(iter->first)) else if(_cachedSignatures.contains(iter->first))
{ {
QMap<int, Signature>::iterator jter = _cachedSignatures.find(iter->first); QMap<int, Signature>::iterator jter = _cachedSignatures.find(iter->first);
if(jter->getDepth2D().size()) if(!jter->getDepth2DCompressed().empty())
{ {
this->createAndAddScanToMap(iter->first, iter->second); this->createAndAddScanToMap(iter->first, iter->second);
} }

2
guilib/src/PdfPlot.cpp
View File

@@ -70,7 +70,7 @@ void PdfPlotItem::showDescription(bool shown)
{ {
QImage img; QImage img;
QMap<int, Signature>::const_iterator iter = _signaturesRef->find(int(this->data().x())); QMap<int, Signature>::const_iterator iter = _signaturesRef->find(int(this->data().x()));
if(iter != _signaturesRef->constEnd() && !iter.value().getImage().empty()) if(iter != _signaturesRef->constEnd() && !iter.value().getImageCompressed().empty())
{ {
cv::Mat image; cv::Mat image;
iter.value().uncompressData(&image, 0, 0); iter.value().uncompressData(&image, 0, 0);