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

Refactored how visualization data are transfered between core and gui: using only Signature object instead of separated image,deph,fx,fy...

Browse Source 
git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1928 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2014-10-26 21:47:29 +00:00
parent 60b0fd2e98
commit 457c068e0f
23 changed files with 514 additions and 1027 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -114,25 +114,7 @@ public:
double getDbSavingTime() const;
int getMapId(int signatureId) const;
std::vector<unsigned char> getImage(int signatureId) const;
void getImageDepth(
int locationId,
std::vector<unsigned char> & rgb,
std::vector<unsigned char> & depth,
std::vector<unsigned char> & depth2d,
float & fx,
float & fy,
float & cx,
float & cy,
Transform & localTransform);
void getImageDepthRaw(
int locationId,
cv::Mat & rgb,
cv::Mat & depth,
float & fx,
float & fy,
float & cx,
float & cy,
Transform & localTransform);
Signature getSignatureData(int locationId, bool uncompressedData = false);
std::set<int> getAllSignatureIds() const;
bool memoryChanged() const {return _memoryChanged;}
bool isIncremental() const {return _incrementalMemory;}
@@ -159,7 +141,6 @@ public:
//keypoint stuff
const VWDictionary * getVWDictionary() const;
std::multimap<int, cv::KeyPoint> getWords(int signatureId) const;
Feature2D::Type getFeatureType() const {return _featureType;}
// RGB-D stuff

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

@@ -131,7 +131,7 @@ class RTABMAP_EXP Parameters
// Rtabmap parameters
RTABMAP_PARAM(Rtabmap, VhStrategy, int, 0, "None 0, Similarity 1, Epipolar 2.");
RTABMAP_PARAM(Rtabmap, PublishStats, bool, true, "Publishing statistics.");
RTABMAP_PARAM(Rtabmap, PublishImage, bool, true, "Publishing image.");
RTABMAP_PARAM(Rtabmap, PublishLastSignature, bool, true, "Publishing last signature.");
RTABMAP_PARAM(Rtabmap, PublishPdf, bool, true, "Publishing pdf.");
RTABMAP_PARAM(Rtabmap, PublishLikelihood, bool, true, "Publishing likelihood.");
RTABMAP_PARAM(Rtabmap, TimeThr, float, 0.0, "Maximum time allowed for the detector (ms) (0 means infinity).");
@@ -163,7 +163,6 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(Mem, InitWMWithAllNodes, bool, false, "Initialize the Working Memory with all nodes in Long-Term Memory. When false, it is initialized with nodes of the previous session.")
// KeypointMemory (Keypoint-based)
RTABMAP_PARAM(Kp, PublishKeypoints, bool, true, "Publishing keypoints.");
RTABMAP_PARAM(Kp, NNStrategy, int, 1, "kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4");
RTABMAP_PARAM(Kp, IncrementalDictionary, bool, true, "");
RTABMAP_PARAM(Kp, MaxDepth, float, 0.0, "Filter extracted keypoints by depth (0=inf)");

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

@@ -106,14 +106,7 @@ public:
void setDatabasePath(const std::string & path);
void deleteLocation(int locationId); // Only nodes in STM can be deleted
void rejectLoopClosure(int oldId, int newId);
void get3DMap(std::map<int, std::vector<unsigned char> > & images,
std::map<int, std::vector<unsigned char> > & depths,
std::map<int, std::vector<unsigned char> > & depths2d,
std::map<int, float> & depthFxs,
std::map<int, float> & depthFys,
std::map<int, float> & depthCxs,
std::map<int, float> & depthCys,
std::map<int, Transform> & localTransforms,
void get3DMap(std::map<int, Signature> & signatures,
std::map<int, Transform> & poses,
std::multimap<int, Link> & constraints,
std::map<int, int> & mapIds,
@@ -142,10 +135,9 @@ private:
private:
// Modifiable parameters
bool _publishStats;
bool _publishImage;
bool _publishLastSignature;
bool _publishPdf;
bool _publishLikelihood;
bool _publishKeypoints;
float _maxTimeAllowed; // in ms
unsigned int _maxMemoryAllowed; // signatures count in WM
float _loopThr;

36
corelib/include/rtabmap/core/RtabmapEvent.h
View File

@@ -152,26 +152,12 @@ public:
RtabmapEvent3DMap(int codeError = 0):
UEvent(codeError){}
RtabmapEvent3DMap(
const std::map<int, std::vector<unsigned char> > & images,
const std::map<int, std::vector<unsigned char> > & depths,
const std::map<int, std::vector<unsigned char> > & depths2d,
const std::map<int, float> & depthFxs,
const std::map<int, float> & depthFys,
const std::map<int, float> & depthCxs,
const std::map<int, float> & depthCys,
const std::map<int, Transform> & localTransforms,
const std::map<int, Signature> & signatures,
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & constraints,
const std::map<int, int> & mapIds) :
UEvent(0),
_images(images),
_depths(depths),
_depths2d(depths2d),
_depthFxs(depthFxs),
_depthFys(depthFys),
_depthCxs(depthCxs),
_depthCys(depthCys),
_localTransforms(localTransforms),
_signatures(signatures),
_poses(poses),
_constraints(constraints),
_mapIds(mapIds)
@@ -179,14 +165,7 @@ public:
virtual ~RtabmapEvent3DMap() {}
const std::map<int, std::vector<unsigned char> > & getImages() const {return _images;}
const std::map<int, std::vector<unsigned char> > & getDepths() const {return _depths;}
const std::map<int, std::vector<unsigned char> > & getDepths2d() const {return _depths2d;}
const std::map<int, float> & getDepthFxs() const {return _depthFxs;}
const std::map<int, float> & getDepthFys() const {return _depthFys;}
const std::map<int, float> & getDepthCxs() const {return _depthCxs;}
const std::map<int, float> & getDepthCys() const {return _depthCys;}
const std::map<int, Transform> & getLocalTransforms() const {return _localTransforms;}
const std::map<int, Signature> & getSignatures() const {return _signatures;}
const std::map<int, Transform> & getPoses() const {return _poses;}
const std::multimap<int, Link> & getConstraints() const {return _constraints;}
const std::map<int, int> & getMapIds() const {return _mapIds;}
@@ -194,14 +173,7 @@ public:
virtual std::string getClassName() const {return std::string("RtabmapEvent3DMap");}
private:
std::map<int, std::vector<unsigned char> > _images;
std::map<int, std::vector<unsigned char> > _depths;
std::map<int, std::vector<unsigned char> > _depths2d;
std::map<int, float> _depthFxs;
std::map<int, float> _depthFys;
std::map<int, float> _depthCxs;
std::map<int, float> _depthCys;
std::map<int, Transform> _localTransforms;
std::map<int, Signature> _signatures;
std::map<int, Transform> _poses;
std::multimap<int, Link> _constraints;
std::map<int, int> _mapIds;

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

@@ -76,7 +76,8 @@ public:
RTABMAP_DEPRECATED(bool empty() const, "Use !isValid() instead.");
const cv::Mat & image() const {return _image;}
int id() const {return _id;};
int id() const {return _id;}
void setId(int id) {_id = id;}
bool isMetric() const {return !_depthOrRightImage.empty() || _fx != 0.0f || _fyOrBaseline != 0.0f || !_pose.isNull();}
void setPose(const Transform & pose) {_pose = pose;}

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

@@ -29,7 +29,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
@@ -40,6 +39,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <set>
#include <rtabmap/core/Transform.h>
#include <rtabmap/core/SensorData.h>
namespace rtabmap
{
@@ -50,6 +50,7 @@ class RTABMAP_EXP Signature
{
public:
Signature();
Signature(int id,
int mapId,
const std::multimap<int, cv::KeyPoint> & words,
@@ -134,6 +135,11 @@ public:
const Transform & getLocalTransform() const {return _localTransform;}
void setDepthRaw(const cv::Mat & depth) {_depthRaw = depth;}
const cv::Mat & getDepthRaw() const {return _depthRaw;}
void setDepth2DRaw(const cv::Mat & depth2D) {_depth2DRaw = depth2D;}
const cv::Mat & getDepth2DRaw() const {return _depth2DRaw;}
SensorData toSensorData();
void uncompressData();
private:
int _id;
@@ -166,6 +172,7 @@ private:
cv::Mat _imageRaw;
cv::Mat _depthRaw;
cv::Mat _depth2DRaw;
};
} // namespace rtabmap

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

@@ -35,6 +35,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <opencv2/imgproc/imgproc.hpp>
#include <list>
#include <vector>
#include <rtabmap/core/Signature.h>
#include <rtabmap/core/Link.h>
namespace rtabmap {
@@ -129,26 +130,16 @@ public:
void setLocalLoopClosureId(int localLoopClosureId) {_localLoopClosureId = localLoopClosureId;}
void setMapIds(const std::map<int, int> & mapIds) {_mapIds = mapIds;}
void setImages(const std::map<int, std::vector<unsigned char> > & images) {_images = images;}
void setDepths(const std::map<int, std::vector<unsigned char> > & depths) {_depths = depths;}
void setDepth2ds(const std::map<int, std::vector<unsigned char> > & depth2ds) {_depth2ds = depth2ds;}
void setDepthFxs(const std::map<int, float> & fxs) {_depthFxs = fxs;}
void setDepthFys(const std::map<int, float> & fys) {_depthFys = fys;}
void setDepthCxs(const std::map<int, float> & cxs) {_depthCxs = cxs;}
void setDepthCys(const std::map<int, float> & cys) {_depthCys = cys;}
void setLocalTransforms(const std::map<int, Transform> & localTransforms) {_localTransforms = localTransforms;}
void setSignature(const Signature & s) {_signature = s;}
void setPoses(const std::map<int, Transform> & poses) {_poses = poses;}
void setConstraints(const std::multimap<int, Link> & constraints) {_constraints = constraints;}
void setCurrentPose(const Transform & pose) {_currentPose = pose;}
void setMapCorrection(const Transform & mapCorrection) {_mapCorrection = mapCorrection;}
void setLoopClosureTransform(const Transform & loopClosureTransform) {_loopClosureTransform = loopClosureTransform;}
void setWeights(const std::map<int, int> & weights) {_weights = weights;}
void setPosterior(const std::map<int, float> & posterior) {_posterior = posterior;}
void setLikelihood(const std::map<int, float> & likelihood) {_likelihood = likelihood;}
void setRawLikelihood(const std::map<int, float> & rawLikelihood) {_rawLikelihood = rawLikelihood;}
void setRefWords(const std::multimap<int, cv::KeyPoint> & refWords) {_refWords = refWords;}
void setLoopWords(const std::multimap<int, cv::KeyPoint> & loopWords) {_loopWords = loopWords;}
// getters
bool extended() const {return _extended;}
@@ -157,26 +148,16 @@ public:
int localLoopClosureId() const {return _localLoopClosureId;}
const std::map<int, int> & getMapIds() const {return _mapIds;}
const std::map<int, std::vector<unsigned char> > & getImages() const {return _images;}
const std::map<int, std::vector<unsigned char> > & getDepths() const {return _depths;}
const std::map<int, std::vector<unsigned char> > & getDepth2ds() const {return _depth2ds;}
const std::map<int, float> & getDepthFxs() const {return _depthFxs;}
const std::map<int, float> & getDepthFys() const {return _depthFys;}
const std::map<int, float> & getDepthCxs() const {return _depthCxs;}
const std::map<int, float> & getDepthCys() const {return _depthCys;}
const std::map<int, Transform> & getLocalTransforms() const {return _localTransforms;}
const Signature & getSignature() const {return _signature;}
const std::map<int, Transform> & poses() const {return _poses;}
const std::multimap<int, Link> & constraints() const {return _constraints;}
const Transform & currentPose() const {return _currentPose;}
const Transform & mapCorrection() const {return _mapCorrection;}
const Transform & loopClosureTransform() const {return _loopClosureTransform;}
const std::map<int, int> & weights() const {return _weights;}
const std::map<int, float> & posterior() const {return _posterior;}
const std::map<int, float> & likelihood() const {return _likelihood;}
const std::map<int, float> & rawLikelihood() const {return _rawLikelihood;}
const std::multimap<int, cv::KeyPoint> & refWords() const {return _refWords;}
const std::multimap<int, cv::KeyPoint> & loopWords() const {return _loopWords;}
const std::map<std::string, float> & data() const {return _data;}
@@ -189,20 +170,12 @@ private:
// extended data start here...
std::map<int, int> _mapIds;
std::map<int, std::vector<unsigned char> > _images;
// Metric data
std::map<int, std::vector<unsigned char> > _depths;
std::map<int, std::vector<unsigned char> > _depth2ds;
std::map<int, float> _depthFxs;
std::map<int, float> _depthFys;
std::map<int, float> _depthCxs;
std::map<int, float> _depthCys;
std::map<int, Transform> _localTransforms;
// Signature data
Signature _signature;
std::map<int, Transform> _poses;
std::multimap<int, Link> _constraints;
Transform _currentPose;
Transform _mapCorrection;
Transform _loopClosureTransform;
@@ -211,10 +184,6 @@ private:
std::map<int, float> _likelihood;
std::map<int, float> _rawLikelihood;
//keypoint memory
std::multimap<int, cv::KeyPoint> _refWords;
std::multimap<int, cv::KeyPoint> _loopWords;
// Format for statistics (Plottable statistics must go in that map) :
// {"Group/Name/Unit", value}
// Example : {"Timing/Total time/ms", 500.0f}

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

@@ -46,8 +46,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
namespace rtabmap
{
class Signature;
namespace util3d
{

240
corelib/src/Memory.cpp
View File

@@ -1821,9 +1821,35 @@ Transform Memory::computeIcpTransform(int oldId, int newId, Transform guess, boo
_dbDriver->loadNodeData(depthToLoad, true);
}
}
Transform t;
if(oldS && newS)
{
//make sure data are uncompressed
if(icp3D)
{
if(oldS->getDepthRaw().empty())
{
oldS->setDepthRaw(util3d::uncompressImage(oldS->getDepth()));
}
if(newS->getDepthRaw().empty())
{
newS->setDepthRaw(util3d::uncompressImage(newS->getDepth()));
}
}
else
{
if(oldS->getDepth2DRaw().empty())
{
oldS->setDepth2DRaw(util3d::uncompressData(oldS->getDepth2D()));
}
if(newS->getDepth2DRaw().empty())
{
newS->setDepth2DRaw(util3d::uncompressData(newS->getDepth2D()));
}
}
t = computeIcpTransform(*oldS, *newS, guess, icp3D, rejectedMsg);
}
else
@@ -1860,24 +1886,16 @@ Transform Memory::computeIcpTransform(const Signature & oldS, const Signature &
if(icp3D)
{
UDEBUG("3D ICP");
util3d::CompressionThread ctOld(oldS.getDepth(), true);
util3d::CompressionThread ctNew(newS.getDepth(), true);
ctOld.start();
ctNew.start();
ctOld.join();
ctNew.join();
cv::Mat oldDepth = ctOld.getUncompressedData();
cv::Mat newDepth = ctNew.getUncompressedData();
if(!oldDepth.empty() && !newDepth.empty())
if(!oldS.getDepthRaw().empty() && !newS.getDepthRaw().empty())
{
if(oldDepth.type() == CV_8UC1 || newDepth.type() == CV_8UC1)
if(oldS.getDepthRaw().type() == CV_8UC1 || newS.getDepthRaw().type() == CV_8UC1)
{
UERROR("ICP 3D cannot be done on stereo images!");
}
else
{
pcl::PointCloud<pcl::PointXYZ>::Ptr oldCloudXYZ = util3d::getICPReadyCloud(
oldDepth,
oldS.getDepthRaw(),
oldS.getDepthFx(),
oldS.getDepthFy(),
oldS.getDepthCx(),
@@ -1888,7 +1906,7 @@ Transform Memory::computeIcpTransform(const Signature & oldS, const Signature &
_icpSamples,
oldS.getLocalTransform());
pcl::PointCloud<pcl::PointXYZ>::Ptr newCloudXYZ = util3d::getICPReadyCloud(
newDepth,
newS.getDepthRaw(),
newS.getDepthFx(),
newS.getDepthFy(),
newS.getDepthCx(),
@@ -1964,19 +1982,11 @@ Transform Memory::computeIcpTransform(const Signature & oldS, const Signature &
UINFO("2D ICP: Dropping z (%f), roll (%f) and pitch (%f) rotation!", z, r, p);
}
util3d::CompressionThread ctOld(oldS.getDepth2D(), false);
util3d::CompressionThread ctNew(newS.getDepth2D(), false);
ctOld.start();
ctNew.start();
ctOld.join();
ctNew.join();
cv::Mat oldDepth2D = ctOld.getUncompressedData();
cv::Mat newDepth2D = ctNew.getUncompressedData();
if(!oldDepth2D.empty() && !newDepth2D.empty())
if(!oldS.getDepth2DRaw().empty() && !newS.getDepth2DRaw().empty())
{
// 2D
pcl::PointCloud<pcl::PointXYZ>::Ptr oldCloud = util3d::cvMat2Cloud(oldDepth2D);
pcl::PointCloud<pcl::PointXYZ>::Ptr newCloud = util3d::cvMat2Cloud(newDepth2D, guess);
pcl::PointCloud<pcl::PointXYZ>::Ptr oldCloud = util3d::cvMat2Cloud(oldS.getDepth2DRaw());
pcl::PointCloud<pcl::PointXYZ>::Ptr newCloud = util3d::cvMat2Cloud(newS.getDepth2DRaw(), guess);
//voxelize
if(_icp2VoxelSize > 0.0f)
@@ -2545,106 +2555,71 @@ std::vector<unsigned char> Memory::getImage(int signatureId) const
return image;
}
void Memory::getImageDepth(
int locationId,
std::vector<unsigned char> & rgb,
std::vector<unsigned char> & depth,
std::vector<unsigned char> & depth2d,
float & fx,
float & fy,
float & cx,
float & cy,
Transform & localTransform)
Signature Memory::getSignatureData(int locationId, bool uncompressedData)
{
Signature r;
Signature * s = this->_getSignature(locationId);
if(s)
if(s && s->getImage().size())
{
rgb = s->getImage();
depth = s->getDepth();
depth2d = s->getDepth2D();
fx = s->getDepthFx();
fy = s->getDepthFy();
cx = s->getDepthCx();
cy = s->getDepthCy();
localTransform = s->getLocalTransform();
r = *s;
}
if(rgb.empty() && this->isRawDataKept() && _dbDriver)
else if(_dbDriver)
{
_dbDriver->getNodeData(locationId, rgb, depth, depth2d, fx, fy, cx, cy, localTransform);
// load from database
if(s)
{
// keep in cache
if(!rgb.empty())
std::list<Signature*> signatures;
signatures.push_back(s);
_dbDriver->loadNodeData(signatures, !s->getPose().isNull());
r = *s;
}
else
{
std::list<int> ids;
ids.push_back(locationId);
std::list<Signature*> signatures;
_dbDriver->loadSignatures(ids, signatures);
if(signatures.size())
{
s->setImage(rgb);
}
if(!depth.empty())
{
s->setDepth(depth, fx, fy, cx, cy);
}
if(!depth2d.empty())
{
s->setDepth2D(depth2d);
}
if(!localTransform.isNull())
{
s->setLocalTransform(localTransform);
Signature * sTmp = signatures.front();
if(sTmp->getImage().size() == 0)
{
_dbDriver->loadNodeData(signatures, !sTmp->getPose().isNull());
}
r = *sTmp;
this->moveToTrash(s);
}
}
}
}
void Memory::getImageDepthRaw(
int locationId,
cv::Mat & rgb,
cv::Mat & depth,
float & fx,
float & fy,
float & cx,
float & cy,
Transform & localTransform)
{
Signature * s = this->_getSignature(locationId);
if(s)
if(uncompressedData && r.getImageRaw().empty() && r.getImage().size())
{
rgb = s->getImageRaw();
depth = s->getDepthRaw();
fx = s->getDepthFx();
fy = s->getDepthFy();
cx = s->getDepthCx();
cy = s->getDepthCy();
localTransform = s->getLocalTransform();
}
if(rgb.empty())
{
std::vector<unsigned char> compressedRgb;
std::vector<unsigned char> compressedDepth;
std::vector<unsigned char> comressedDepth2d;
getImageDepth(locationId, compressedRgb, compressedDepth, comressedDepth2d, fx, fy, cx, cy, localTransform);
//uncomressed data
util3d::CompressionThread ctImage(compressedRgb, true);
util3d::CompressionThread ctDepth(compressedDepth, true);
ctImage.start();
ctDepth.start();
ctImage.join();
ctDepth.join();
rgb = ctImage.getUncompressedData();
depth = ctDepth.getUncompressedData();
//uncompress data
if(s)
{
//save it uncompressed in the signature
if(!rgb.empty())
{
s->setImageRaw(rgb);
}
if(!depth.empty())
{
s->setDepthRaw(depth);
}
s->uncompressData();
r.setImageRaw(s->getImageRaw());
r.setDepthRaw(s->getDepthRaw());
r.setDepth2DRaw(s->getDepth2DRaw());
}
else
{
util3d::CompressionThread ctImage(r.getImage(), true);
util3d::CompressionThread ctDepth(r.getDepth(), true);
util3d::CompressionThread ctDepth2D(r.getDepth2D(), false);
ctImage.start();
ctDepth.start();
ctDepth2D.start();
ctImage.join();
ctDepth.join();
ctDepth2D.join();
r.setImageRaw(ctImage.getUncompressedData());
r.setDepthRaw(ctDepth.getUncompressedData());
r.setDepth2DRaw(ctDepth2D.getUncompressedData());
}
}
return r;
}
void Memory::generateGraph(const std::string & fileName, std::set<int> ids)
@@ -2938,49 +2913,11 @@ void Memory::createGraph(GraphNode * parent, unsigned int maxDepth, const std::s
}
}
// Keypoint stuff
std::multimap<int, cv::KeyPoint> Memory::getWords(int signatureId) const
{
std::multimap<int, cv::KeyPoint> words;
if(signatureId>0)
{
const Signature * s = this->getSignature(signatureId);
if(s)
{
const Signature * ks = dynamic_cast<const Signature*>(s);
if(ks)
{
words = ks->getWords();
}
}
else if(_dbDriver)
{
std::list<int> ids;
ids.push_back(signatureId);
std::list<Signature *> signatures;
_dbDriver->loadSignatures(ids, signatures);
if(signatures.size())
{
const Signature * ks = dynamic_cast<const Signature*>(signatures.front());
if(ks)
{
words = ks->getWords();
}
}
for(std::list<Signature *>::iterator iter = signatures.begin(); iter!=signatures.end(); ++iter)
{
delete *iter;
}
}
}
return words;
}
int Memory::getNi(int signatureId) const
{
int ni = 0;
const Signature * s = this->getSignature(signatureId);
if(s) // Must be a SurfSignature
if(s)
{
ni = ((Signature *)s)->getWords().size();
}
@@ -2994,7 +2931,6 @@ int Memory::getNi(int signatureId) const
void Memory::copyData(const Signature * from, Signature * to)
{
// The signatures must be KeypointSignature
UTimer timer;
timer.start();
if(from && to)
@@ -3415,21 +3351,22 @@ Signature * Memory::createSignature(const SensorData & data, bool keepRawData, S
}
util3d::CompressionThread ctImage(data.image(), std::string(".jpg"));
util3d::CompressionThread ctDepth(depthOrRightImage, std::string(".png"));
util3d::CompressionThread ctDepth2d(data.depth2d());
ctImage.start();
ctDepth.start();
ctDepth2d.start();
ctImage.join();
ctDepth.join();
imageBytes = ctImage.getCompressedData();
depthBytes = ctDepth.getCompressedData();
ctDepth2d.join();
s = new Signature(id,
_idMapCount,
words,
words3D,
data.pose(),
util3d::compressData(data.depth2d()),
imageBytes,
depthBytes,
ctDepth2d.getCompressedData(),
ctImage.getCompressedData(),
ctDepth.getCompressedData(),
data.fx(),
data.fy()>0.0f?data.fy():data.baseline(),
data.cx(),
@@ -3437,6 +3374,7 @@ Signature * Memory::createSignature(const SensorData & data, bool keepRawData, S
data.localTransform());
s->setImageRaw(data.image());
s->setDepthRaw(depthOrRightImage);
s->setDepth2DRaw(data.depth2d());
}
else
{

164
corelib/src/Rtabmap.cpp
View File

@@ -74,10 +74,9 @@ namespace rtabmap
Rtabmap::Rtabmap() :
_publishStats(Parameters::defaultRtabmapPublishStats()),
_publishImage(Parameters::defaultRtabmapPublishImage()),
_publishLastSignature(Parameters::defaultRtabmapPublishLastSignature()),
_publishPdf(Parameters::defaultRtabmapPublishPdf()),
_publishLikelihood(Parameters::defaultRtabmapPublishLikelihood()),
_publishKeypoints(Parameters::defaultKpPublishKeypoints()),
_maxTimeAllowed(Parameters::defaultRtabmapTimeThr()), // 700 ms
_maxMemoryAllowed(Parameters::defaultRtabmapMemoryThr()), // 0=inf
_loopThr(Parameters::defaultRtabmapLoopThr()),
@@ -330,10 +329,9 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
}
Parameters::parse(parameters, Parameters::kRtabmapPublishStats(), _publishStats);
Parameters::parse(parameters, Parameters::kRtabmapPublishImage(), _publishImage);
Parameters::parse(parameters, Parameters::kRtabmapPublishLastSignature(), _publishLastSignature);
Parameters::parse(parameters, Parameters::kRtabmapPublishPdf(), _publishPdf);
Parameters::parse(parameters, Parameters::kRtabmapPublishLikelihood(), _publishLikelihood);
Parameters::parse(parameters, Parameters::kKpPublishKeypoints(), _publishKeypoints);
Parameters::parse(parameters, Parameters::kRtabmapTimeThr(), _maxTimeAllowed);
Parameters::parse(parameters, Parameters::kRtabmapMemoryThr(), _maxMemoryAllowed);
Parameters::parse(parameters, Parameters::kRtabmapLoopThr(), _loopThr);
@@ -1291,6 +1289,7 @@ bool Rtabmap::process(const SensorData & data)
uInsert(customParameters, ParametersPair(Parameters::kKpNndrRatio(), uNumber2Str(_reextractNNDR)));
uInsert(customParameters, ParametersPair(Parameters::kKpDetectorStrategy(), uNumber2Str(_reextractFeatureType))); // FAST/BRIEF
uInsert(customParameters, ParametersPair(Parameters::kKpWordsPerImage(), uNumber2Str(_reextractMaxWords)));
uInsert(customParameters, ParametersPair(Parameters::kMemGenerateIds(), "false"));
//for(ParametersMap::iterator iter = customParameters.begin(); iter!=customParameters.end(); ++iter)
//{
@@ -1302,30 +1301,25 @@ bool Rtabmap::process(const SensorData & data)
UTimer timeT;
// Add signatures
float fxA, fyA, cxA, cyA;
float fxB, fyB, cxB, cyB;
rtabmap::Transform localTransformA, localTransformB;
SensorData dataFrom = data;
dataFrom.setId(signature->id());
Signature tmpTo = _memory->getSignatureData(_lcHypothesisId, true);
SensorData dataTo = tmpTo.toSensorData();
UDEBUG("timeTo = %fs", timeT.ticks());
cv::Mat imageA, depthA;
_memory->getImageDepthRaw(signature->id(), imageA, depthA, fxA, fyA, cxA, cyA, localTransformA);
SensorData dataFrom(imageA, depthA, fxA, fyA, cxA, cyA, Transform::getIdentity(), localTransformA, 1);
UDEBUG("timeA = %fs", timeT.ticks());
cv::Mat imageB, depthB;
_memory->getImageDepthRaw(_lcHypothesisId, imageB, depthB, fxB, fyB, cxB, cyB, localTransformB);
SensorData dataTo(imageB, depthB, fxB, fyB, cxB, cyB, Transform::getIdentity(), localTransformB, 2);
UDEBUG("timeB = %fs", timeT.ticks());
if(dataFrom.isValid() && dataFrom.isMetric() && dataTo.isValid() && dataTo.isMetric())
if(dataFrom.isValid() &&
dataFrom.isMetric() &&
dataTo.isValid() &&
dataTo.isMetric() &&
dataFrom.id() != Memory::kIdInvalid &&
tmpTo.id() != Memory::kIdInvalid)
{
memory.update(dataFrom);
UDEBUG("timeUpA = %fs", timeT.ticks());
memory.update(dataTo);
UDEBUG("timeUpB = %fs", timeT.ticks());
UDEBUG("timeUpTo = %fs", timeT.ticks());
memory.update(dataFrom);
UDEBUG("timeUpFrom = %fs", timeT.ticks());
transform = memory.computeVisualTransform(2, 1, &rejectedMsg, &loopClosureVisualInliers);
transform = memory.computeVisualTransform(dataTo.id(), dataFrom.id(), &rejectedMsg, &loopClosureVisualInliers);
UDEBUG("timeTransform = %fs", timeT.ticks());
}
else
@@ -1580,74 +1574,9 @@ bool Rtabmap::process(const SensorData & data)
//Epipolar geometry constraint
statistics_.addStatistic(Statistics::kLoopRejectedHypothesis(), rejectedHypothesis?1.0f:0);
if(_publishImage)
if(_publishLastSignature)
{
std::map<int, std::vector<unsigned char> > images;
std::map<int, std::vector<unsigned char> > depths;
std::map<int, std::vector<unsigned char> > depth2ds;
std::map<int, float> depthFxs;
std::map<int, float> depthFys;
std::map<int, float> depthCxs;
std::map<int, float> depthCys;
std::map<int, Transform> localTransforms;
std::vector<int> ids(signaturesRetrieved.begin(), signaturesRetrieved.end());
ids.push_back(signature->id());
if(sLoop)
{
ids.push_back(sLoop->id());
}
UTimer tmpTimer;
for(unsigned int i=0; i<ids.size(); ++i)
{
// Add data
std::vector<unsigned char> im;
if(_rgbdSlamMode && _memory->isIncremental())
{
std::vector<unsigned char> depth, depth2d;
float fx, fy, cx, cy;
Transform localTransform;
_memory->getImageDepth(ids[i], im, depth, depth2d, fx, fy, cx, cy, localTransform);
if(!depth.empty())
{
depths.insert(std::make_pair(ids[i], depth));
depthFxs.insert(std::make_pair(ids[i], fx));
depthFys.insert(std::make_pair(ids[i], fy));
depthCxs.insert(std::make_pair(ids[i], cx));
depthCys.insert(std::make_pair(ids[i], cy));
localTransforms.insert(std::make_pair(ids[i], localTransform));
}
if(!depth2d.empty())
{
depth2ds.insert(std::make_pair(ids[i], depth2d));
}
}
else
{
im = _memory->getImage(ids[i]);
}
UASSERT(_memory->getSignature(ids[i]) != 0);
if(!im.empty())
{
images.insert(std::make_pair(ids[i], im));
}
}
if(tmpTimer.elapsed() > 0.03)
{
UWARN("getting data[%d] time = %fs", (int)ids.size(), tmpTimer.ticks());
}
statistics_.setImages(images);
statistics_.setDepths(depths);
statistics_.setDepth2ds(depth2ds);
statistics_.setDepthFxs(depthFxs);
statistics_.setDepthFys(depthFys);
statistics_.setDepthCxs(depthCxs);
statistics_.setDepthCys(depthCys);
statistics_.setLocalTransforms(localTransforms);
statistics_.setSignature(*signature);
}
if(_publishLikelihood || _publishPdf)
@@ -1664,17 +1593,6 @@ bool Rtabmap::process(const SensorData & data)
statistics_.setRawLikelihood(rawLikelihood);
}
}
if(_publishKeypoints)
{
//Copy keypoints
statistics_.setRefWords(signature->getWords());
if(sLoop)
{
//Copy keypoints
statistics_.setLoopWords(sLoop->getWords());
}
}
}
timeStatsCreation = timer.ticks();
@@ -1764,7 +1682,6 @@ bool Rtabmap::process(const SensorData & data)
statistics_.setPoses(_optimizedPoses);
statistics_.setConstraints(_constraints);
statistics_.setMapCorrection(_mapCorrection);
statistics_.setCurrentPose(_mapCorrection * currentRawOdomPose);
UINFO("Set map correction = %s", _mapCorrection.prettyPrint().c_str());
}
}
@@ -2292,14 +2209,7 @@ void Rtabmap::dumpPrediction() const
}
}
void Rtabmap::get3DMap(std::map<int, std::vector<unsigned char> > & images,
std::map<int, std::vector<unsigned char> > & depths,
std::map<int, std::vector<unsigned char> > & depths2d,
std::map<int, float> & depthFxs,
std::map<int, float> & depthFys,
std::map<int, float> & depthCxs,
std::map<int, float> & depthCys,
std::map<int, Transform> & localTransforms,
void Rtabmap::get3DMap(std::map<int, Signature> & signatures,
std::map<int, Transform> & poses,
std::multimap<int, Link> & constraints,
std::map<int, int> & mapIds,
@@ -2328,36 +2238,12 @@ void Rtabmap::get3DMap(std::map<int, std::vector<unsigned char> > & images,
for(std::set<int>::iterator iter = ids.begin(); iter!=ids.end(); ++iter)
{
std::vector<unsigned char> image, depth, depth2d;
float fx, fy, cx, cy;
Transform localTransform;
_memory->getImageDepth(*iter, image, depth, depth2d, fx, fy, cx, cy, localTransform);
if(image.size())
Signature data = _memory->getSignatureData(*iter);
if(data.id() != Memory::kIdInvalid)
{
images.insert(std::make_pair(*iter, image));
signatures.insert(std::make_pair(*iter, data));
mapIds.insert(std::make_pair(*iter, _memory->getMapId(*iter)));
}
if(depth.size())
{
depths.insert(std::make_pair(*iter, depth));
}
if(depth2d.size())
{
depths2d.insert(std::make_pair(*iter, depth2d));
}
if(fx > 0 && fy > 0)
{
depthFxs.insert(std::make_pair(*iter, fx));
depthFys.insert(std::make_pair(*iter, fy));
depthCxs.insert(std::make_pair(*iter, cx));
depthCys.insert(std::make_pair(*iter, cy));
}
if(!localTransform.isNull())
{
localTransforms.insert(std::make_pair(*iter, localTransform));
}
mapIds.insert(std::make_pair(*iter, _memory->getMapId(*iter)));
}
}
else if(_memory && (_memory->getStMem().size() || _memory->getWorkingMem().size()))

45
corelib/src/RtabmapThread.cpp
View File

@@ -100,40 +100,19 @@ void RtabmapThread::setBufferSize(int bufferSize)
void RtabmapThread::publishMap(bool optimized, bool full) const
{
std::map<int, std::vector<unsigned char> > images;
std::map<int, std::vector<unsigned char> > depths;
std::map<int, std::vector<unsigned char> > depths2d;
std::map<int, float> depthFxs;
std::map<int, float> depthFys;
std::map<int, float> depthCxs;
std::map<int, float> depthCys;
std::map<int, Transform> localTransforms;
std::map<int, Signature> signatures;
std::map<int, Transform> poses;
std::multimap<int, Link> constraints;
std::map<int, int> mapIds;
_rtabmap->get3DMap(images,
depths,
depths2d,
depthFxs,
depthFys,
depthCxs,
depthCys,
localTransforms,
_rtabmap->get3DMap(signatures,
poses,
constraints,
mapIds,
optimized,
full);
this->post(new RtabmapEvent3DMap(images,
depths,
depths2d,
depthFxs,
depthFys,
depthCxs,
depthCys,
localTransforms,
this->post(new RtabmapEvent3DMap(signatures,
poses,
constraints,
mapIds));
@@ -141,14 +120,7 @@ void RtabmapThread::publishMap(bool optimized, bool full) const
void RtabmapThread::publishTOROGraph(bool optimized, bool full) const
{
std::map<int, std::vector<unsigned char> > images;
std::map<int, std::vector<unsigned char> > depths;
std::map<int, std::vector<unsigned char> > depths2d;
std::map<int, float> depthFxs;
std::map<int, float> depthFys;
std::map<int, float> depthCxs;
std::map<int, float> depthCys;
std::map<int, Transform> localTransforms;
std::map<int, Signature> signatures;
std::map<int, Transform> poses;
std::multimap<int, Link> constraints;
std::map<int, int> mapIds;
@@ -159,14 +131,7 @@ void RtabmapThread::publishTOROGraph(bool optimized, bool full) const
optimized,
full);
this->post(new RtabmapEvent3DMap(images,
depths,
depths2d,
depthFxs,
depthFys,
depthCxs,
depthCys,
localTransforms,
this->post(new RtabmapEvent3DMap(signatures,
poses,
constraints,
mapIds));

54
corelib/src/Signature.cpp
View File

@@ -36,9 +36,19 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
namespace rtabmap
{
Signature::~Signature()
Signature::Signature() :
_id(0), // invalid id
_mapId(-1),
_weight(-1),
_saved(false),
_modified(true),
_neighborsModified(true),
_enabled(false),
_fx(0.0f),
_fy(0.0f),
_cx(0.0f),
_cy(0.0f)
{
ULOGGER_DEBUG("id=%d", _id);
}
Signature::Signature(
@@ -76,6 +86,11 @@ Signature::Signature(
{
}
Signature::~Signature()
{
ULOGGER_DEBUG("id=%d", _id);
}
void Signature::addNeighbors(const std::map<int, Transform> & neighbors)
{
for(std::map<int, Transform>::const_iterator i=neighbors.begin(); i!=neighbors.end(); ++i)
@@ -212,4 +227,39 @@ void Signature::setDepth(const std::vector<unsigned char> & depth, float fx, flo
_cy=cy;
}
SensorData Signature::toSensorData()
{
this->uncompressData();
return SensorData(_imageRaw,
_depthRaw,
_depth2DRaw,
_fx,
_fy,
_cx,
_cy,
_pose,
_localTransform,
_id);
}
void Signature::uncompressData()
{
if(_imageRaw.empty() && _image.size())
{
//uncompress data
util3d::CompressionThread ctImage(_image, true);
util3d::CompressionThread ctDepth(_depth, true);
util3d::CompressionThread ctDepth2D(_depth2D, false);
ctImage.start();
ctDepth.start();
ctDepth2D.start();
ctImage.join();
ctDepth.join();
ctDepth2D.join();
_imageRaw = ctImage.getUncompressedData();
_depthRaw = ctDepth.getUncompressedData();
_depth2DRaw = ctDepth2D.getUncompressedData();
}
}
} //namespace rtabmap