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

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...
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, std::vector<unsigned char> & image) 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, cv::Mat & imageCompressed) const;
void getPose(int signatureId, Transform & pose, int & mapId) 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;
@@ -135,8 +135,8 @@ private:
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 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, std::vector<unsigned char> & image) 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, cv::Mat & imageCompressed) 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 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
double getDbSavingTime() 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);
std::set<int> getAllSignatureIds() const;
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, pcl::PointXYZ> & words3,
const Transform & pose = Transform(),
const std::vector<unsigned char> & depth2D = std::vector<unsigned char>(),
const std::vector<unsigned char> & image = std::vector<unsigned char>(),
const std::vector<unsigned char> & depth = std::vector<unsigned char>(),
const cv::Mat & depth2D = cv::Mat(),
const cv::Mat & image = cv::Mat(),
const cv::Mat & depth = cv::Mat(),
float fx = 0.0f,
float fy = 0.0f,
float cx = 0.0f,
@@ -69,7 +69,7 @@ public:
/**
* 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;
int id() const {return _id;}
@@ -113,20 +113,20 @@ public:
void setEnabled(bool enabled) {_enabled = enabled;}
const std::multimap<int, cv::KeyPoint> & getWords() const {return _words;}
const std::map<int, int> & getWordsChanged() const {return _wordsChanged;}
void setImage(const std::vector<unsigned char> & image) {_image = image;}
const std::vector<unsigned char> & getImage() const {return _image;}
void setImageCompressed(const cv::Mat & bytes) {_imageCompressed = bytes;}
const cv::Mat & getImageCompressed() const {return _imageCompressed;}
void setImageRaw(const cv::Mat & image) {_imageRaw = image;}
const cv::Mat & getImageRaw() const {return _imageRaw;}
//metric stuff
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 setDepth2D(const std::vector<unsigned char> & depth2D) {_depth2D = depth2D;}
void setDepthCompressed(const cv::Mat & bytes, float fx, float fy, float cx, float cy);
void setDepth2DCompressed(const cv::Mat & bytes) {_depth2DCompressed = bytes;}
void setLocalTransform(const Transform & t) {_localTransform = t;}
void setPose(const Transform & pose) {_pose = pose;}
const std::multimap<int, pcl::PointXYZ> & getWords3() const {return _words3;}
const std::vector<unsigned char> & getDepth() const {return _depth;}
const std::vector<unsigned char> & getDepth2D() const {return _depth2D;}
const cv::Mat & getDepthCompressed() const {return _depthCompressed;}
const cv::Mat & getDepth2DCompressed() const {return _depth2DCompressed;}
float getDepthFx() const {return _fx;}
float getDepthFy() const {return _fy;}
float getDepthCx() const {return _cx;}
@@ -140,7 +140,7 @@ public:
SensorData toSensorData();
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:
int _id;
@@ -159,10 +159,10 @@ private:
std::multimap<int, cv::KeyPoint> _words; // word <id, keypoint>
std::map<int, int> _wordsChanged; // <oldId, newId>
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
std::vector<unsigned char> _depth2D; // compressed data CV_32FC2
cv::Mat _depthCompressed; // compressed image
cv::Mat _depth2DCompressed; // compressed data
float _fx;
float _fy;
float _cx;
@@ -171,9 +171,9 @@ private:
Transform _localTransform; // camera_link -> base_link
std::multimap<int, pcl::PointXYZ> _words3; // word <id, keypoint>
cv::Mat _imageRaw;
cv::Mat _depthRaw;
cv::Mat _depth2DRaw;
cv::Mat _imageRaw; // CV_8UC1 or CV_8UC3
cv::Mat _depthRaw; // depth CV_16UC1 or CV_32FC1, right image CV_8UC1
cv::Mat _depth2DRaw; // CV_32FC2
};
} // namespace rtabmap

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

@@ -71,14 +71,13 @@ class RTABMAP_EXP CompressionThread : public UThread
public:
// format : ".png" ".jpg" "" (empty is general)
CompressionThread(const cv::Mat & mat, const std::string & format = "");
CompressionThread(const std::vector<unsigned char> * bytes, bool isImage);
const std::vector<unsigned char> & getCompressedData() const {return compressedData_;}
CompressionThread(const cv::Mat & bytes, bool isImage);
const cv::Mat & getCompressedData() const {return compressedData_;}
cv::Mat & getUncompressedData() {return uncompressedData_;}
protected:
virtual void mainLoop();
private:
const std::vector<unsigned char> * constCompressedData_;
std::vector<unsigned char> compressedData_;
cv::Mat compressedData_;
cv::Mat uncompressedData_;
std::string format_;
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);
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);
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 unsigned char * bytes, unsigned long size);
// remove depth by z axis
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 ?
void DBDriver::getNodeData(
int signatureId,
std::vector<unsigned char> & image,
std::vector<unsigned char> & depth,
std::vector<unsigned char> & depth2d,
cv::Mat & imageCompressed,
cv::Mat & depthCompressed,
cv::Mat & depth2dCompressed,
float & fx,
float & fy,
float & cx,
@@ -414,15 +414,15 @@ void DBDriver::getNodeData(
Transform & localTransform) const
{
_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();
}
//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();
this->getNodeDataQuery(signatureId, image);
this->getNodeDataQuery(signatureId, imageCompressed);
_dbSafeAccessMutex.unlock();
}

72
corelib/src/DBDriverSqlite3.cpp
View File

@@ -515,9 +515,7 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
//Create the image
if(dataSize>4 && data)
{
std::vector<unsigned char> image(dataSize);
memcpy(image.data(), data, dataSize);
(*iter)->setImage(image);
(*iter)->setImageCompressed(cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone());
}
if(loadMetricData)
@@ -526,17 +524,16 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
dataSize = sqlite3_column_bytes(ppStmt, index++);
//Create the depth image
std::vector<unsigned char> depth;
cv::Mat depthCompressed;
if(dataSize>4 && data)
{
depth.resize(dataSize);
memcpy(depth.data(), data, dataSize);
depthCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone();
}
if(uStrNumCmp(_version, "0.7.0") < 0)
{
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
{
@@ -544,7 +541,7 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
float fy = sqlite3_column_double(ppStmt, index++);
float cx = 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
@@ -559,13 +556,12 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
data = sqlite3_column_blob(ppStmt, index);
dataSize = sqlite3_column_bytes(ppStmt, index++);
//Create the depth2d
std::vector<unsigned char> depth2d;
cv::Mat depth2dCompressed;
if(dataSize>4 && data)
{
depth2d.resize(dataSize);
memcpy(depth2d.data(), data, dataSize);
(*iter)->setDepth2DCompressed(cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone()); // depth2d
}
(*iter)->setDepth2D(depth2d); // depth2d
}
rc = sqlite3_step(ppStmt); // next result...
@@ -586,9 +582,9 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
void DBDriverSqlite3::getNodeDataQuery(
int signatureId,
std::vector<unsigned char> & image,
std::vector<unsigned char> & depth,
std::vector<unsigned char> & depth2d,
cv::Mat & imageCompressed,
cv::Mat & depthCompressed,
cv::Mat & depth2dCompressed,
float & fx,
float & fy,
float & cx,
@@ -645,8 +641,7 @@ void DBDriverSqlite3::getNodeDataQuery(
//Create the image
if(dataSize>4 && data)
{
image.resize(dataSize);
memcpy(image.data(), data, dataSize);
imageCompressed = cv::Mat(1, dataSize, CV_8UC1).clone();
}
data = sqlite3_column_blob(ppStmt, index);
@@ -655,8 +650,7 @@ void DBDriverSqlite3::getNodeDataQuery(
//Create the depth image
if(dataSize>4 && data)
{
depth.resize(dataSize);
memcpy(depth.data(), data, dataSize);
depthCompressed = cv::Mat(1, dataSize, CV_8UC1).clone();
}
if(uStrNumCmp(_version, "0.7.0") < 0)
@@ -687,11 +681,10 @@ void DBDriverSqlite3::getNodeDataQuery(
//Create the depth2d
if(dataSize>4 && data)
{
depth2d.resize(dataSize);
memcpy(depth2d.data(), data, dataSize);
depth2dCompressed = cv::Mat(1, dataSize, CV_8UC1).clone();
}
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.");
}
@@ -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)
{
@@ -744,8 +737,7 @@ void DBDriverSqlite3::getNodeDataQuery(int signatureId, std::vector<unsigned cha
//Create the image
if(dataSize>4 && data)
{
image.resize(dataSize);
memcpy(image.data(), data, dataSize);
imageCompressed = cv::Mat(1, dataSize, CV_8UC1).clone();
}
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)
{
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)
{
//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
@@ -1948,9 +1940,9 @@ std::string DBDriverSqlite3::queryStepImage() const
}
void DBDriverSqlite3::stepImage(sqlite3_stmt * ppStmt,
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)
{
UFATAL("");
@@ -1962,9 +1954,9 @@ void DBDriverSqlite3::stepImage(sqlite3_stmt * ppStmt,
rc = sqlite3_bind_int(ppStmt, index++, id);
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
{
@@ -1993,15 +1985,15 @@ std::string DBDriverSqlite3::queryStepDepth() const
}
void DBDriverSqlite3::stepDepth(sqlite3_stmt * ppStmt,
int id,
const std::vector<unsigned char> & depth,
const std::vector<unsigned char> & depth2d,
const cv::Mat & depthBytes,
const cv::Mat & depth2dBytes,
float fx,
float fy,
float cx,
float cy,
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)
{
UFATAL("");
@@ -2013,9 +2005,9 @@ void DBDriverSqlite3::stepDepth(sqlite3_stmt * ppStmt,
rc = sqlite3_bind_int(ppStmt, index++, id);
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
{
@@ -2043,9 +2035,9 @@ void DBDriverSqlite3::stepDepth(sqlite3_stmt * ppStmt,
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());
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
{

14
corelib/src/DBDriverSqlite3.h
View File

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

12
corelib/src/DBReader.cpp
View File

@@ -207,9 +207,9 @@ void DBReader::getNextImage(
if(!this->isKilled() && _currentId != _ids.end())
{
std::vector<unsigned char> imageBytes;
std::vector<unsigned char> depthBytes;
std::vector<unsigned char> depth2dBytes;
cv::Mat imageBytes;
cv::Mat depthBytes;
cv::Mat depth2dBytes;
int mapId;
_dbDriver->getNodeData(*_currentId, imageBytes, depthBytes, depth2dBytes, fx, fy, cx, cy, localTransform);
_dbDriver->getPose(*_currentId, pose, mapId);
@@ -220,9 +220,9 @@ void DBReader::getNextImage(
UWARN("No image loaded from the database for id=%d!", *_currentId);
}
util3d::CompressionThread ctImage(&imageBytes, true);
util3d::CompressionThread ctDepth(&depthBytes, true);
util3d::CompressionThread ctDepth2D(&depth2dBytes, false);
util3d::CompressionThread ctImage(imageBytes, true);
util3d::CompressionThread ctDepth(depthBytes, true);
util3d::CompressionThread ctDepth2D(depth2dBytes, false);
ctImage.start();
ctDepth.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);
}
sim = signature->compareTo(sB);
sim = signature->compareTo(*sB);
}
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)
{
//Depth required, if not in RAM, load it from LTM
if(oldS->getDepth().empty())
if(oldS->getDepthCompressed().empty())
{
depthToLoad.push_back(oldS);
added.insert(oldS->id());
}
if(newS->getDepth().empty())
if(newS->getDepthCompressed().empty())
{
depthToLoad.push_back(newS);
added.insert(newS->id());
@@ -1823,11 +1823,11 @@ Transform Memory::computeIcpTransform(int oldId, int newId, Transform guess, boo
else
{
//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);
}
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);
}
@@ -1846,22 +1846,22 @@ Transform Memory::computeIcpTransform(int oldId, int newId, Transform guess, boo
{
if(oldS->getDepthRaw().empty())
{
oldS->setDepthRaw(util3d::uncompressImage(oldS->getDepth()));
oldS->setDepthRaw(util3d::uncompressImage(oldS->getDepthCompressed()));
}
if(newS->getDepthRaw().empty())
{
newS->setDepthRaw(util3d::uncompressImage(newS->getDepth()));
newS->setDepthRaw(util3d::uncompressImage(newS->getDepthCompressed()));
}
}
else
{
if(oldS->getDepth2DRaw().empty())
{
oldS->setDepth2DRaw(util3d::uncompressData(oldS->getDepth2D()));
oldS->setDepth2DRaw(util3d::uncompressData(oldS->getDepth2DCompressed()));
}
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);
UASSERT(s != 0);
if(s->getDepth2D().size() == 0)
if(s->getDepth2DCompressed().empty())
{
depthToLoad.push_back(s);
}
@@ -2106,9 +2106,9 @@ Transform Memory::computeScanMatchingTransform(
if(iter->first != newId)
{
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
{
@@ -2127,7 +2127,7 @@ Transform Memory::computeScanMatchingTransform(
const Signature * newS = getSignature(newId);
pcl::PointCloud<pcl::PointXYZ>::Ptr newCloud;
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
if(newCloud->size() && _icp2VoxelSize > 0.0f)
@@ -2394,7 +2394,7 @@ void Memory::rehearsal(Signature * signature, Statistics * stats)
{
UFATAL("Signature %d null?!?", id);
}
float sim = signature->compareTo(sB);
float sim = signature->compareTo(*sB);
int merged = 0;
if(sim >= _similarityThreshold)
@@ -2556,13 +2556,13 @@ int Memory::getMapId(int signatureId) const
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);
if(s)
{
image = s->getImage();
image = s->getImageCompressed();
}
if(image.empty() && this->isRawDataKept() && _dbDriver)
{
@@ -2576,7 +2576,7 @@ Signature Memory::getSignatureData(int locationId, bool uncompressedData)
UDEBUG("");
Signature r;
Signature * s = this->_getSignature(locationId);
if(s && s->getImage().size())
if(s && !s->getImageCompressed().empty())
{
r = *s;
}
@@ -2600,7 +2600,7 @@ Signature Memory::getSignatureData(int locationId, bool uncompressedData)
if(signatures.size())
{
Signature * sTmp = signatures.front();
if(sTmp->getImage().size() == 0)
if(sTmp->getImageCompressed().empty())
{
_dbDriver->loadNodeData(signatures, !sTmp->getPose().isNull());
}
@@ -2619,7 +2619,7 @@ Signature Memory::getSignatureData(int locationId, bool uncompressedData)
}
UDEBUG("");
if(uncompressedData && r.getImageRaw().empty() && r.getImage().size())
if(uncompressedData && r.getImageRaw().empty() && !r.getImageCompressed().empty())
{
//uncompress data
if(s)
@@ -2961,25 +2961,25 @@ void Memory::copyData(const Signature * from, Signature * to)
if(from->isSaved() && _dbDriver)
{
std::vector<unsigned char> image;
std::vector<unsigned char> depth;
std::vector<unsigned char> depth2d;
cv::Mat image;
cv::Mat depth;
cv::Mat depth2d;
float fx, fy, cx, cy;
Transform localTransform;
_dbDriver->getNodeData(from->id(), image, depth, depth2d, fx, fy, cx, cy, localTransform);
to->setImage(image);
to->setDepth(depth, fx, fy, cx, cy);
to->setDepth2D(depth2d);
to->setImageCompressed(image);
to->setDepthCompressed(depth, fx, fy, cx, cy);
to->setDepth2DCompressed(depth2d);
to->setLocalTransform(localTransform);
UDEBUG("Loaded image data from database");
}
else
{
to->setImage(from->getImage());
to->setDepth(from->getDepth(), from->getDepthFx(), from->getDepthFy(), from->getDepthCx(), from->getDepthCy());
to->setDepth2D(from->getDepth2D());
to->setImageCompressed(from->getImageCompressed());
to->setDepthCompressed(from->getDepthCompressed(), from->getDepthFx(), from->getDepthFy(), from->getDepthCx(), from->getDepthCy());
to->setDepth2DCompressed(from->getDepth2DCompressed());
to->setLocalTransform(from->getLocalTransform());
}
@@ -3400,7 +3400,7 @@ Signature * Memory::createSignature(const SensorData & data, bool keepRawData, S
words,
words3D,
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 &&
signature->getNeighbors().size() == 1 &&
signature->getDepth2D().size() &&
!signature->getDepth2DCompressed().empty() &&
rehearsedId == 0) // don't do it if rehearsal happened
{
UINFO("Odometry correction by scan matching (size=%d)...", _scanMatchingSize);
@@ -1382,7 +1382,7 @@ bool Rtabmap::process(const SensorData & data)
int localSpaceNearestId = 0;
if(_lcHypothesisId == 0 &&
_localLoopClosureDetectionSpace &&
signature->getDepth2D().size())
!signature->getDepth2DCompressed().empty())
{
//============================================================
// 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, pcl::PointXYZ> & words3, // in base_link frame (localTransform applied)
const Transform & pose,
const std::vector<unsigned char> & depth2D, // in base_link frame
const std::vector<unsigned char> & image, // in camera_link frame
const std::vector<unsigned char> & depth, // in camera_link frame
const cv::Mat & depth2DCompressed, // in base_link frame
const cv::Mat & imageCompressed, // in camera_link frame
const cv::Mat & depthCompressed, // in camera_link frame
float fx,
float fy,
float cx,
@@ -73,9 +73,9 @@ Signature::Signature(
_neighborsModified(true),
_words(words),
_enabled(false),
_image(image),
_depth(depth),
_depth2D(depth2D),
_imageCompressed(imageCompressed),
_depthCompressed(depthCompressed),
_depth2DCompressed(depth2DCompressed),
_fx(fx),
_fy(fy),
_cx(cx),
@@ -88,7 +88,7 @@ Signature::Signature(
Signature::~Signature()
{
//ULOGGER_DEBUG("id=%d", _id);
//UDEBUG("id=%d", _id);
}
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;
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)
{
std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > pairs;
@@ -217,10 +217,10 @@ void Signature::removeWord(int 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());
_depth = depth;
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());
_depthCompressed = bytes;
_fx=fx;
_fy=fy;
_cx=cx;
@@ -247,53 +247,53 @@ void Signature::uncompressData()
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()) ||
(depth && depth->empty()) ||
(depth2D && depth2D->empty()))
if( (imageRaw && imageRaw->empty()) ||
(depthRaw && depthRaw->empty()) ||
(depth2DRaw && depth2DRaw->empty()))
{
util3d::CompressionThread ctImage(&_image, true);
util3d::CompressionThread ctDepth(&_depth, true);
util3d::CompressionThread ctDepth2D(&_depth2D, false);
if(image && image->empty())
util3d::CompressionThread ctImage(_imageCompressed, true);
util3d::CompressionThread ctDepth(_depthCompressed, true);
util3d::CompressionThread ctDepth2D(_depth2DCompressed, false);
if(imageRaw && imageRaw->empty())
{
ctImage.start();
}
if(depth && depth->empty())
if(depthRaw && depthRaw->empty())
{
ctDepth.start();
}
if(depth2D && depth2D->empty())
if(depth2DRaw && depth2DRaw->empty())
{
ctDepth2D.start();
}
ctImage.join();
ctDepth.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)
CompressionThread::CompressionThread(const cv::Mat & mat, const std::string & format) :
constCompressedData_(0),
uncompressedData_(mat),
format_(format),
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);
}
// assume image
CompressionThread::CompressionThread(const std::vector<unsigned char> * bytes, bool isImage) :
constCompressedData_(bytes),
CompressionThread::CompressionThread(const cv::Mat & bytes, bool isImage) :
compressedData_(bytes),
image_(isImage),
compressMode_(false)
{}
@@ -88,25 +87,25 @@ void CompressionThread::mainLoop()
{
if(image_)
{
compressedData_ = compressImage(uncompressedData_, format_);
compressedData_ = compressImage2(uncompressedData_, format_);
}
else
{
compressedData_ = compressData(uncompressedData_);
compressedData_ = compressData2(uncompressedData_);
}
}
}
else // uncompress
{
if(constCompressedData_ && constCompressedData_->size())
if(!compressedData_.empty())
{
if(image_)
{
uncompressedData_ = uncompressImage(*constCompressedData_);
uncompressedData_ = uncompressImage(compressedData_);
}
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;
}
// ".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 image;
@@ -1150,20 +1174,61 @@ std::vector<unsigned char> compressData(const cv::Mat & data)
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)
{
return uncompressData(bytes.data(), bytes.size());
}
cv::Mat uncompressData(const unsigned char * bytes, unsigned long size)
{
cv::Mat data;
if(bytes.size()>=3*sizeof(int))
if(bytes && size>=3*sizeof(int))
{
//last 3 int elements are matrix size and type
int height = *((int*)&bytes[bytes.size()-3*sizeof(int)]);
int width = *((int*)&bytes[bytes.size()-2*sizeof(int)]);
int type = *((int*)&bytes[bytes.size()-1*sizeof(int)]);
int height = *((int*)&bytes[size-3*sizeof(int)]);
int width = *((int*)&bytes[size-2*sizeof(int)]);
int type = *((int*)&bytes[size-1*sizeof(int)]);
// If the size is higher, it may be a wrong data format.
UASSERT_MSG(height>=0 && height<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);
uLongf totalUncompressed = uLongf(data.total())*uLongf(data.elemSize());
@@ -1171,8 +1236,8 @@ cv::Mat uncompressData(const std::vector<unsigned char> & bytes)
int errCode = uncompress(
(Bytef*)data.data,
&totalUncompressed,
(const Bytef*)bytes.data(),
uLong(bytes.size()));
(const Bytef*)bytes,
uLong(size));
if(errCode == Z_MEM_ERROR)
{