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

2
examples/BOWMapping/CMakeLists.txt
View File

@@ -1,10 +1,12 @@
SET(INCLUDE_DIRS
${PROJECT_SOURCE_DIR}/corelib/include
${OpenCV_INCLUDE_DIRS}
${PCL_INCLUDE_DIRS}
)
SET(LIBRARIES
${OpenCV_LIBRARIES}
${PCL_LIBRARIES}
)
INCLUDE_DIRECTORIES(${INCLUDE_DIRS})

23
examples/RGBDMapping/MapBuilder.h
View File

@@ -163,23 +163,16 @@ private slots:
cloudViewer_->setCloudVisibility(cloudName, true);
}
else if(iter->first == stats.refImageId() &&
uContains(stats.getImages(), iter->first) &&
uContains(stats.getDepths(), iter->first) &&
uContains(stats.getDepthFxs(), iter->first) &&
uContains(stats.getDepthFys(), iter->first) &&
uContains(stats.getLocalTransforms(), iter->first))
stats.getSignature().id() == iter->first)
{
// Add the new cloud
cv::Mat rgb = util3d::uncompressImage(stats.getImages().at(iter->first));
cv::Mat depth = util3d::uncompressImage(stats.getDepths().at(iter->first));
Transform localTransform = stats.getLocalTransforms().at(iter->first);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::cloudFromDepthRGB(
rgb,
depth,
stats.getDepthCxs().at(iter->first),
stats.getDepthCys().at(iter->first),
stats.getDepthFxs().at(iter->first),
stats.getDepthFys().at(iter->first),
stats.getSignature().getImageRaw(),
stats.getSignature().getDepthRaw(),
stats.getSignature().getDepthCx(),
stats.getSignature().getDepthCy(),
stats.getSignature().getDepthFx(),
stats.getSignature().getDepthFy(),
8); // decimation
if(cloud->size())
@@ -187,7 +180,7 @@ private slots:
cloud = util3d::passThrough<pcl::PointXYZRGB>(cloud, "z", 0, 4.0f);
if(cloud->size())
{
cloud = util3d::transformPointCloud<pcl::PointXYZRGB>(cloud, localTransform);
cloud = util3d::transformPointCloud<pcl::PointXYZRGB>(cloud, stats.getSignature().getLocalTransform());
}
}
if(!cloudViewer_->addOrUpdateCloud(cloudName, cloud, iter->second))

14
guilib/include/rtabmap/gui/LoopClosureViewer.h
View File

@@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/Parameters.h>
#include <rtabmap/core/Transform.h>
#include <rtabmap/core/Signature.h>
#include <opencv2/opencv.hpp>
#include <QtGui/QWidget>
@@ -39,8 +40,6 @@ class Ui_loopClosureViewer;
namespace rtabmap {
class Signature;
class RTABMAPGUI_EXP LoopClosureViewer : public QWidget {
Q_OBJECT
@@ -49,11 +48,10 @@ public:
LoopClosureViewer(QWidget * parent);
virtual ~LoopClosureViewer();
// take ownership
void setData(Signature * sA, Signature * sB); // sB contains loop transform as pose() from sA
void setData(const Signature & sA, const Signature & sB); // sB contains loop transform as pose() from sA
const Signature * sA() const {return sA_;}
const Signature * sB() const {return sB_;}
const Signature & sA() const {return sA_;}
const Signature & sB() const {return sB_;}
public slots:
void setDecimation(int decimation) {decimation_ = decimation;}
@@ -67,8 +65,8 @@ protected:
private:
Ui_loopClosureViewer * ui_;
Signature * sA_;
Signature * sB_;
Signature sA_;
Signature sB_;
Transform transform_;
int decimation_;

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

@@ -248,15 +248,8 @@ private:
bool _processingStatistics;
bool _odometryReceived;
QMap<int, std::vector<unsigned char> > _imagesMap;
QMap<int, std::vector<unsigned char> > _depthsMap;
QMap<int, std::vector<unsigned char> > _depths2DMap;
QMap<int, float> _depthFxsMap;
QMap<int, float> _depthFysMap;
QMap<int, float> _depthCxsMap;
QMap<int, float> _depthCysMap;
QMap<int, Signature> _cachedSignatures;
QMap<int, int> _mapIds;
QMap<int, Transform> _localTransformsMap;
std::map<int, Transform> _currentPosesMap;
std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr > _createdClouds;
std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > _createdScans;

304
guilib/src/DatabaseViewer.cpp
View File

@@ -304,40 +304,10 @@ void DatabaseViewer::exportDatabase()
for(int i=0; i<ids_.size(); i+=1+framesIgnored)
{
int id = ids_.at(i);
std::vector<unsigned char> compressedRgb, compressedDepth, compressedDepth2d;
float tmpFx, tmpFy, tmpCx, tmpCy;
rtabmap::Transform tmpLocalTransform, pose;
memory_->getImageDepth(id, compressedRgb, compressedDepth, compressedDepth2d, tmpFx, tmpFy, tmpCx, tmpCy, tmpLocalTransform);
if(dialog.isOdomExported())
{
memory_->getPose(id, pose, true);
}
cv::Mat rgb, depth, depth2d;
float fx = 0, fy = 0, cx = 0, cy = 0;
rtabmap::Transform localTransform;
if(dialog.isRgbExported())
{
rgb = rtabmap::util3d::uncompressImage(compressedRgb);
}
if(dialog.isDepthExported())
{
depth = rtabmap::util3d::uncompressImage(compressedDepth);
fx = tmpFx;
fy = tmpFy;
cx = tmpCx;
cy = tmpCy;
localTransform = tmpLocalTransform;
}
if(dialog.isDepth2dExported())
{
depth2d = rtabmap::util3d::uncompressData(compressedDepth2d);
}
rtabmap::SensorData data(rgb, depth, depth2d, fx, fy, cx, cy, pose, localTransform, id);
recorder.addData(data);
Signature data = memory_->getSignatureData(id, true);
rtabmap::SensorData sensorData = data.toSensorData();
recorder.addData(sensorData);
progressDialog.appendText(tr("Exported node %1").arg(id));
progressDialog.incrementStep();
@@ -764,40 +734,35 @@ void DatabaseViewer::view3DMap()
rtabmap::Transform pose = iter->second;
if(!pose.isNull())
{
std::vector<unsigned char> image, depth, depth2d;
float fx, fy, cx, cy;
rtabmap::Transform localTransform;
memory_->getImageDepth(iter->first, image, depth, depth2d, fx, fy, cx, cy, localTransform);
Signature data = memory_->getSignatureData(iter->first, true);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
cv::Mat imageMat = rtabmap::util3d::uncompressImage(image);
cv::Mat depthMat = rtabmap::util3d::uncompressImage(depth);
UASSERT(imageMat.empty() || imageMat.type()==CV_8UC3 || imageMat.type() == CV_8UC1);
UASSERT(depthMat.empty() || depthMat.type()==CV_8UC1 || depthMat.type() == CV_16UC1 || depthMat.type() == CV_32FC1);
if(depthMat.type() == CV_8UC1)
UASSERT(data.getImageRaw().empty() || data.getImageRaw().type()==CV_8UC3 || data.getImageRaw().type() == CV_8UC1);
UASSERT(data.getDepthRaw().empty() || data.getDepthRaw().type()==CV_8UC1 || data.getDepthRaw().type() == CV_16UC1 || data.getDepthRaw().type() == CV_32FC1);
if(data.getDepthRaw().type() == CV_8UC1)
{
cv::Mat leftImg;
if(imageMat.channels() == 3)
if(data.getImageRaw().channels() == 3)
{
cv::cvtColor(imageMat, leftImg, CV_BGR2GRAY);
cv::cvtColor(data.getImageRaw(), leftImg, CV_BGR2GRAY);
}
else
{
leftImg = imageMat;
leftImg = data.getImageRaw();
}
cloud = rtabmap::util3d::cloudFromDisparityRGB(
imageMat,
util3d::disparityFromStereoImages(leftImg, depthMat),
cx, cy,
fx, fy,
data.getImageRaw(),
util3d::disparityFromStereoImages(leftImg, data.getDepthRaw()),
data.getDepthCx(), data.getDepthCy(),
data.getDepthFx(), data.getDepthFy(),
decimation);
}
else
{
cloud = rtabmap::util3d::cloudFromDepthRGB(
imageMat,
depthMat,
cx, cy,
fx, fy,
data.getImageRaw(),
data.getDepthRaw(),
data.getDepthCx(), data.getDepthCy(),
data.getDepthFx(), data.getDepthFy(),
decimation);
}
@@ -806,7 +771,7 @@ void DatabaseViewer::view3DMap()
cloud = rtabmap::util3d::passThrough<pcl::PointXYZRGB>(cloud, "z", 0, maxDepth);
}
cloud = rtabmap::util3d::transformPointCloud<pcl::PointXYZRGB>(cloud, localTransform);
cloud = rtabmap::util3d::transformPointCloud<pcl::PointXYZRGB>(cloud, data.getLocalTransform());
QColor color = Qt::red;
int mapId = memory_->getMapId(iter->first);
@@ -890,40 +855,35 @@ void DatabaseViewer::generate3DMap()
rtabmap::Transform pose = uValue(optimizedPoses, iter->first, rtabmap::Transform());
if(!pose.isNull())
{
std::vector<unsigned char> image, depth, depth2d;
float fx, fy, cx, cy;
rtabmap::Transform localTransform;
memory_->getImageDepth(iter->first, image, depth, depth2d, fx, fy, cx, cy, localTransform);
Signature data = memory_->getSignatureData(iter->first, true);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
cv::Mat imageMat = rtabmap::util3d::uncompressImage(image);
cv::Mat depthMat = rtabmap::util3d::uncompressImage(depth);
UASSERT(imageMat.empty() || imageMat.type()==CV_8UC3 || imageMat.type() == CV_8UC1);
UASSERT(depthMat.empty() || depthMat.type()==CV_8UC1 || depthMat.type() == CV_16UC1 || depthMat.type() == CV_32FC1);
if(depthMat.type() == CV_8UC1)
UASSERT(data.getImageRaw().empty() || data.getImageRaw().type()==CV_8UC3 || data.getImageRaw().type() == CV_8UC1);
UASSERT(data.getDepthRaw().empty() || data.getDepthRaw().type()==CV_8UC1 || data.getDepthRaw().type() == CV_16UC1 || data.getDepthRaw().type() == CV_32FC1);
if(data.getDepthRaw().type() == CV_8UC1)
{
cv::Mat leftImg;
if(imageMat.channels() == 3)
if(data.getImageRaw().channels() == 3)
{
cv::cvtColor(imageMat, leftImg, CV_BGR2GRAY);
cv::cvtColor(data.getImageRaw(), leftImg, CV_BGR2GRAY);
}
else
{
leftImg = imageMat;
leftImg = data.getImageRaw();
}
cloud = rtabmap::util3d::cloudFromDisparityRGB(
imageMat,
util3d::disparityFromStereoImages(leftImg, depthMat),
cx, cy,
fx, fy,
data.getImageRaw(),
util3d::disparityFromStereoImages(leftImg, data.getDepthRaw()),
data.getDepthCx(), data.getDepthCy(),
data.getDepthFx(), data.getDepthFy(),
decimation);
}
else
{
cloud = rtabmap::util3d::cloudFromDepthRGB(
imageMat,
depthMat,
cx, cy,
fx, fy,
data.getImageRaw(),
data.getDepthRaw(),
data.getDepthCx(), data.getDepthCy(),
data.getDepthFx(), data.getDepthFy(),
decimation);
}
@@ -932,7 +892,7 @@ void DatabaseViewer::generate3DMap()
cloud = rtabmap::util3d::passThrough<pcl::PointXYZRGB>(cloud, "z", 0, maxDepth);
}
cloud = rtabmap::util3d::transformPointCloud<pcl::PointXYZRGB>(cloud, pose*localTransform);
cloud = rtabmap::util3d::transformPointCloud<pcl::PointXYZRGB>(cloud, pose*data.getLocalTransform());
std::string name = uFormat("%s/node%d.pcd", path.toStdString().c_str(), iter->first);
pcl::io::savePCDFile(name, *cloud);
UINFO("Saved %s (%d points)", name.c_str(), cloud->size());
@@ -1077,25 +1037,19 @@ void DatabaseViewer::update(int value,
QImage imgDepth;
if(memory_)
{
std::vector<unsigned char> image, depth, depth2d;
float fx, fy, cx, cy;
rtabmap::Transform localTransform;
memory_->getImageDepth(id, image, depth, depth2d, fx, fy, cx, cy, localTransform);
cv::Mat imageMat = rtabmap::util3d::uncompressImage(image);
cv::Mat depthMat = rtabmap::util3d::uncompressImage(depth);
if(!image.empty())
Signature data = memory_->getSignatureData(id, true);
if(!data.getImageRaw().empty())
{
img = uCvMat2QImage(imageMat);
img = uCvMat2QImage(data.getImageRaw());
}
if(!depth.empty())
if(!data.getDepthRaw().empty())
{
imgDepth = uCvMat2QImage(depthMat);
imgDepth = uCvMat2QImage(data.getDepthRaw());
}
std::multimap<int, cv::KeyPoint> words = memory_->getWords(id);
if(words.size())
if(data.getWords().size())
{
view->setFeatures(words);
view->setFeatures(data.getWords());
}
mapId = memory_->getMapId(id);
@@ -1388,84 +1342,77 @@ void DatabaseViewer::updateConstraintView(const rtabmap::Link & link,
if(cloudFrom->size() == 0 && cloudTo->size() == 0)
{
float fxA, fyA, cxA, cyA;
float fxB, fyB, cxB, cyB;
rtabmap::Transform localTransformA, localTransformB;
Signature dataFrom, dataTo;
std::vector<unsigned char> imageBytesA, depthBytesA, depth2dBytesA;
memory_->getImageDepth(link.from(), imageBytesA, depthBytesA, depth2dBytesA, fxA, fyA, cxA, cyA, localTransformA);
cv::Mat imageA = rtabmap::util3d::uncompressImage(imageBytesA);
cv::Mat depthA = rtabmap::util3d::uncompressImage(depthBytesA);
cv::Mat depth2dA = rtabmap::util3d::uncompressData(depth2dBytesA);
UASSERT(imageA.empty() || imageA.type()==CV_8UC3 || imageA.type() == CV_8UC1);
UASSERT(depthA.empty() || depthA.type()==CV_8UC1 || depthA.type() == CV_16UC1 || depthA.type() == CV_32FC1);
dataFrom = memory_->getSignatureData(link.from(), true);
UASSERT(dataFrom.getImageRaw().empty() || dataFrom.getImageRaw().type()==CV_8UC3 || dataFrom.getImageRaw().type() == CV_8UC1);
UASSERT(dataFrom.getDepthRaw().empty() || dataFrom.getDepthRaw().type()==CV_8UC1 || dataFrom.getDepthRaw().type() == CV_16UC1 || dataFrom.getDepthRaw().type() == CV_32FC1);
dataTo = memory_->getSignatureData(link.to(), true);
UASSERT(dataTo.getImageRaw().empty() || dataTo.getImageRaw().type()==CV_8UC3 || dataTo.getImageRaw().type() == CV_8UC1);
UASSERT(dataTo.getDepthRaw().empty() || dataTo.getDepthRaw().type()==CV_8UC1 || dataTo.getDepthRaw().type() == CV_16UC1 || dataTo.getDepthRaw().type() == CV_32FC1);
std::vector<unsigned char> imageBytesB, depthBytesB, depth2dBytesB;
memory_->getImageDepth(link.to(), imageBytesB, depthBytesB, depth2dBytesB, fxB, fyB, cxB, cyB, localTransformB);
cv::Mat imageB = rtabmap::util3d::uncompressImage(imageBytesB);
cv::Mat depthB = rtabmap::util3d::uncompressImage(depthBytesB);
cv::Mat depth2dB = rtabmap::util3d::uncompressData(depth2dBytesB);
//cloud 3d
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudA;
if(depthA.type() == CV_8UC1)
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFrom;
if(dataFrom.getDepthRaw().type() == CV_8UC1)
{
cloudA = rtabmap::util3d::cloudFromStereoImages(
imageA,
depthA,
cxA, cyA,
fxA, fyA,
cloudFrom = rtabmap::util3d::cloudFromStereoImages(
dataFrom.getImageRaw(),
dataFrom.getDepthRaw(),
dataFrom.getDepthCx(), dataFrom.getDepthCy(),
dataFrom.getDepthFx(), dataFrom.getDepthFy(),
1);
}
else
{
cloudA = rtabmap::util3d::cloudFromDepthRGB(
imageA,
depthA,
cxA, cyA,
fxA, fyA,
cloudFrom = rtabmap::util3d::cloudFromDepthRGB(
dataFrom.getImageRaw(),
dataFrom.getDepthRaw(),
dataFrom.getDepthCx(), dataFrom.getDepthCy(),
dataFrom.getDepthFx(), dataFrom.getDepthFy(),
1);
}
cloudA = rtabmap::util3d::removeNaNFromPointCloud<pcl::PointXYZRGB>(cloudA);
cloudA = rtabmap::util3d::transformPointCloud<pcl::PointXYZRGB>(cloudA, localTransformA);
cloudFrom = rtabmap::util3d::removeNaNFromPointCloud<pcl::PointXYZRGB>(cloudFrom);
cloudFrom = rtabmap::util3d::transformPointCloud<pcl::PointXYZRGB>(cloudFrom, dataFrom.getLocalTransform());
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudB;
if(depthB.type() == CV_8UC1)
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudTo;
if(dataTo.getDepthRaw().type() == CV_8UC1)
{
cloudB = rtabmap::util3d::cloudFromStereoImages(
imageB,
depthB,
cxB, cyB,
fxB, fyB,
cloudTo = rtabmap::util3d::cloudFromStereoImages(
dataTo.getImageRaw(),
dataTo.getDepthRaw(),
dataTo.getDepthCx(), dataTo.getDepthCy(),
dataTo.getDepthFx(), dataTo.getDepthFy(),
1);
}
else
{
cloudB = rtabmap::util3d::cloudFromDepthRGB(
imageB,
depthB,
cxB, cyB,
fxB, fyB,
cloudTo = rtabmap::util3d::cloudFromDepthRGB(
dataTo.getImageRaw(),
dataTo.getDepthRaw(),
dataTo.getDepthCx(), dataTo.getDepthCy(),
dataTo.getDepthFx(), dataTo.getDepthFy(),
1);
}
cloudB = rtabmap::util3d::removeNaNFromPointCloud<pcl::PointXYZRGB>(cloudB);
cloudB = rtabmap::util3d::transformPointCloud<pcl::PointXYZRGB>(cloudB, t*localTransformB);
cloudTo = rtabmap::util3d::removeNaNFromPointCloud<pcl::PointXYZRGB>(cloudTo);
cloudTo = rtabmap::util3d::transformPointCloud<pcl::PointXYZRGB>(cloudTo, t*dataTo.getLocalTransform());
//cloud 2d
pcl::PointCloud<pcl::PointXYZ>::Ptr scanA, scanB;
scanA = rtabmap::util3d::depth2DToPointCloud(depth2dA);
scanB = rtabmap::util3d::depth2DToPointCloud(depth2dB);
scanA = rtabmap::util3d::depth2DToPointCloud(dataFrom.getDepth2DRaw());
scanB = rtabmap::util3d::depth2DToPointCloud(dataTo.getDepth2DRaw());
scanB = rtabmap::util3d::transformPointCloud<pcl::PointXYZ>(scanB, t);
if(cloudA->size())
if(cloudFrom->size())
{
ui_->constraintsViewer->addOrUpdateCloud("cloud0", cloudA);
ui_->constraintsViewer->addOrUpdateCloud("cloud0", cloudFrom);
}
if(cloudB->size())
if(cloudTo->size())
{
ui_->constraintsViewer->addOrUpdateCloud("cloud1", cloudB);
ui_->constraintsViewer->addOrUpdateCloud("cloud1", cloudTo);
}
if(scanA->size())
{
@@ -1548,14 +1495,11 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
UINFO("Update scans list...");
for(int i=0; i<ids_.size(); ++i)
{
std::vector<unsigned char> imageBytes, depthBytes, depth2dBytes;
float fx, fy, cx, cy;
rtabmap::Transform localTransform;
memory_->getImageDepth(ids_.at(i), imageBytes, depthBytes, depth2dBytes, fx, fy, cx, cy, localTransform);
if(depth2dBytes.size())
Signature data = memory_->getSignatureData(ids_.at(i), false);
if(data.getDepth2D().size())
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
cv::Mat depth2d = rtabmap::util3d::uncompressData(depth2dBytes);
cv::Mat depth2d = rtabmap::util3d::uncompressData(data.getDepth2D());
cloud = rtabmap::util3d::depth2DToPointCloud(depth2d);
scans_.insert(std::make_pair(ids_.at(i), cloud));
}
@@ -1719,23 +1663,17 @@ void DatabaseViewer::refineConstraint(int from, int to)
double fitness = 0.0f;
Transform transform;
float fxA, fyA, cxA, cyA;
float fxB, fyB, cxB, cyB;
rtabmap::Transform localTransformA, localTransformB;
std::vector<unsigned char> imageBytesA, depthBytesA, depth2dBytesA;
memory_->getImageDepth(currentLink.from(), imageBytesA, depthBytesA, depth2dBytesA, fxA, fyA, cxA, cyA, localTransformA);
std::vector<unsigned char> imageBytesB, depthBytesB, depth2dBytesB;
memory_->getImageDepth(currentLink.to(), imageBytesB, depthBytesB, depth2dBytesB, fxB, fyB, cxB, cyB, localTransformB);
Signature dataFrom, dataTo;
dataFrom = memory_->getSignatureData(currentLink.from(), false);
dataTo = memory_->getSignatureData(currentLink.to(), false);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudA(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudB(new pcl::PointCloud<pcl::PointXYZ>);
if(ui_->checkBox_icp_2d->isChecked())
{
//2D
cv::Mat oldDepth2D = util3d::uncompressData(depth2dBytesA);
cv::Mat newDepth2D = util3d::uncompressData(depth2dBytesB);
cv::Mat oldDepth2D = util3d::uncompressData(dataFrom.getDepth2D());
cv::Mat newDepth2D = util3d::uncompressData(dataTo.getDepth2D());
if(!oldDepth2D.empty() && !newDepth2D.empty())
{
@@ -1764,8 +1702,8 @@ void DatabaseViewer::refineConstraint(int from, int to)
else
{
//3D
cv::Mat depthA = rtabmap::util3d::uncompressImage(depthBytesA);
cv::Mat depthB = rtabmap::util3d::uncompressImage(depthBytesB);
cv::Mat depthA = rtabmap::util3d::uncompressImage(dataFrom.getDepth());
cv::Mat depthB = rtabmap::util3d::uncompressImage(dataTo.getDepth());
if(depthA.type() == CV_8UC1 || depthB.type() == CV_8UC1)
{
@@ -1775,19 +1713,19 @@ void DatabaseViewer::refineConstraint(int from, int to)
}
cloudA = util3d::getICPReadyCloud(depthA,
fxA, fyA, cxA, cyA,
ui_->spinBox_icp_decimation->value(),
ui_->doubleSpinBox_icp_maxDepth->value(),
ui_->doubleSpinBox_icp_voxel->value(),
0, // no sampling
localTransformA);
dataFrom.getDepthFx(), dataFrom.getDepthFy(), dataFrom.getDepthCx(), dataFrom.getDepthCy(),
ui_->spinBox_icp_decimation->value(),
ui_->doubleSpinBox_icp_maxDepth->value(),
ui_->doubleSpinBox_icp_voxel->value(),
0, // no sampling
dataFrom.getLocalTransform());
cloudB = util3d::getICPReadyCloud(depthB,
fxB, fyB, cxB, cyB,
ui_->spinBox_icp_decimation->value(),
ui_->doubleSpinBox_icp_maxDepth->value(),
ui_->doubleSpinBox_icp_voxel->value(),
0, // no sampling
currentLink.transform() * localTransformB);
dataTo.getDepthFx(), dataTo.getDepthFy(), dataTo.getDepthCx(), dataTo.getDepthCy(),
ui_->spinBox_icp_decimation->value(),
ui_->doubleSpinBox_icp_maxDepth->value(),
ui_->doubleSpinBox_icp_voxel->value(),
0, // no sampling
currentLink.transform() * dataTo.getLocalTransform());
if(ui_->checkBox_icp_p2plane->isChecked())
{
@@ -1905,26 +1843,22 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent)
Memory tmpMemory(parameters);
// Add signatures
float fxA, fyA, cxA, cyA;
float fxB, fyB, cxB, cyB;
rtabmap::Transform localTransformA, localTransformB;
SensorData dataFrom = memory_->getSignatureData(from, true).toSensorData();
SensorData dataTo = memory_->getSignatureData(to, true).toSensorData();
std::vector<unsigned char> imageBytesA, depthBytesA, depth2dBytesA;
memory_->getImageDepth(from, imageBytesA, depthBytesA, depth2dBytesA, fxA, fyA, cxA, cyA, localTransformA);
cv::Mat imageA = rtabmap::util3d::uncompressImage(imageBytesA);
cv::Mat depthA = rtabmap::util3d::uncompressImage(depthBytesA);
SensorData dataFrom(imageA, depthA, fxA, fyA, cxA, cyA, Transform::getIdentity(), localTransformA, 1);
if(from > to)
{
tmpMemory.update(dataTo);
tmpMemory.update(dataFrom);
}
else
{
tmpMemory.update(dataFrom);
tmpMemory.update(dataTo);
}
std::vector<unsigned char> imageBytesB, depthBytesB, depth2dBytesB;
memory_->getImageDepth(to, imageBytesB, depthBytesB, depth2dBytesB, fxB, fyB, cxB, cyB, localTransformB);
cv::Mat imageB = rtabmap::util3d::uncompressImage(imageBytesB);
cv::Mat depthB = rtabmap::util3d::uncompressImage(depthBytesB);
SensorData dataTo(imageB, depthB, fxB, fyB, cxB, cyB, Transform::getIdentity(), localTransformB, 2);
tmpMemory.update(dataFrom);
tmpMemory.update(dataTo);
t = tmpMemory.computeVisualTransform(2, 1, &rejectedMsg);
t = tmpMemory.computeVisualTransform(to, from, &rejectedMsg);
}
else
{

99
guilib/src/LoopClosureViewer.cpp
View File

@@ -40,8 +40,6 @@ namespace rtabmap {
LoopClosureViewer::LoopClosureViewer(QWidget * parent) :
QWidget(parent),
sA_(0),
sB_(0),
decimation_(1),
maxDepth_(0),
samples_(0)
@@ -55,37 +53,21 @@ LoopClosureViewer::LoopClosureViewer(QWidget * parent) :
LoopClosureViewer::~LoopClosureViewer() {
delete ui_;
if(sA_)
{
delete sA_;
}
if(sB_)
{
delete sB_;
}
}
void LoopClosureViewer::setData(Signature * sA, Signature * sB)
void LoopClosureViewer::setData(const Signature & sA, const Signature & sB)
{
if(sA_)
{
delete sA_;
}
if(sB_)
{
delete sB_;
}
sA_ = sA;
sB_ = sB;
if(sA_ && sB_)
if(sA_.id()>0 && sB_.id()>0)
{
ui_->label_idA->setText(QString("[%1-%2]").arg(sA->id()).arg(sB->id()));
ui_->label_idA->setText(QString("[%1-%2]").arg(sA.id()).arg(sB.id()));
}
}
void LoopClosureViewer::updateView(const Transform & transform)
{
if(sA_ && sB_)
if(sA_.id()>0 && sB_.id()>0)
{
int decimation = 1;
float maxDepth = 0;
@@ -113,56 +95,31 @@ void LoopClosureViewer::updateView(const Transform & transform)
t = transform_;
}
else
{
{
t = sB_.getPose();
}
UDEBUG("t= %s", t.prettyPrint().c_str());
ui_->label_transform->setText(QString("(%1)").arg(t.prettyPrint().c_str()));
if(!t.isNull())
{
util3d::CompressionThread ctiA(sA_->getImage(), true);
util3d::CompressionThread ctdA(sA_->getDepth(), true);
util3d::CompressionThread ctiB(sB_->getImage(), true);
util3d::CompressionThread ctdB(sB_->getDepth(), true);
util3d::CompressionThread ct2dA(sA_->getDepth2D(), false);
util3d::CompressionThread ct2dB(sB_->getDepth2D(), false);
ctiA.start();
ctdA.start();
ctiB.start();
ctdB.start();
ct2dA.start();
ct2dB.start();
ctiA.join();
ctdA.join();
ctiB.join();
ctdB.join();
ct2dA.join();
ct2dB.join();
cv::Mat imageA = ctiA.getUncompressedData();
cv::Mat depthA = ctdA.getUncompressedData();
cv::Mat imageB = ctiB.getUncompressedData();
cv::Mat depthB = ctdB.getUncompressedData();
cv::Mat depth2dA = ct2dA.getUncompressedData();
cv::Mat depth2dB = ct2dB.getUncompressedData();
{
//cloud 3d
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudA;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudA;
if(sA_.getDepthRaw().type() == CV_8UC1)
{
cloudA = util3d::cloudFromStereoImages(
imageA,
depthA,
sA_->getDepthCx(), sA_->getDepthCy(),
cloudA = util3d::cloudFromStereoImages(
sA_.getImageRaw(),
sA_.getDepthRaw(),
sA_.getDepthCx(), sA_.getDepthCy(),
sA_.getDepthFx(), sA_.getDepthFy(),
decimation);
}
else
{
cloudA = util3d::cloudFromDepthRGB(
imageA,
depthA,
sA_->getDepthCx(), sA_->getDepthCy(),
cloudA = util3d::cloudFromDepthRGB(
sA_.getImageRaw(),
sA_.getDepthRaw(),
sA_.getDepthCx(), sA_.getDepthCy(),
sA_.getDepthFx(), sA_.getDepthFy(),
decimation);
}
@@ -176,25 +133,25 @@ void LoopClosureViewer::updateView(const Transform & transform)
if(samples>0 && (int)cloudA->size() > samples)
{
cloudA = util3d::sampling<pcl::PointXYZRGB>(cloudA, samples);
}
}
cloudA = util3d::transformPointCloud<pcl::PointXYZRGB>(cloudA, sA_.getLocalTransform());
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudB;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudB;
if(sB_.getDepthRaw().type() == CV_8UC1)
{
cloudB = util3d::cloudFromStereoImages(
imageB,
depthB,
sB_->getDepthCx(), sB_->getDepthCy(),
cloudB = util3d::cloudFromStereoImages(
sB_.getImageRaw(),
sB_.getDepthRaw(),
sB_.getDepthCx(), sB_.getDepthCy(),
sB_.getDepthFx(), sB_.getDepthFy(),
decimation);
}
else
{
cloudB = util3d::cloudFromDepthRGB(
imageB,
depthB,
sB_->getDepthCx(), sB_->getDepthCy(),
cloudB = util3d::cloudFromDepthRGB(
sB_.getImageRaw(),
sB_.getDepthRaw(),
sB_.getDepthCx(), sB_.getDepthCy(),
sB_.getDepthFx(), sB_.getDepthFy(),
decimation);
}
@@ -208,15 +165,15 @@ void LoopClosureViewer::updateView(const Transform & transform)
if(samples>0 && (int)cloudB->size() > samples)
{
cloudB = util3d::sampling<pcl::PointXYZRGB>(cloudB, samples);
}
}
cloudB = util3d::transformPointCloud<pcl::PointXYZRGB>(cloudB, t*sB_.getLocalTransform());
//cloud 2d
pcl::PointCloud<pcl::PointXYZ>::Ptr scanA, scanB;
scanA = util3d::depth2DToPointCloud(depth2dA);
pcl::PointCloud<pcl::PointXYZ>::Ptr scanA, scanB;
scanA = util3d::depth2DToPointCloud(sA_.getDepth2DRaw());
scanB = util3d::depth2DToPointCloud(sB_.getDepth2DRaw());
scanB = util3d::transformPointCloud<pcl::PointXYZ>(scanB, t);
ui_->label_idA->setText(QString("[%1 (%2) -> %3 (%4)]").arg(sB_.id()).arg(cloudB->size()).arg(sA_.id()).arg(cloudA->size()));
if(cloudA->size())

395
guilib/src/MainWindow.cpp
View File

@@ -189,7 +189,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
_ui->imageView_source->setBackgroundBrush(QBrush(Qt::black));
_ui->imageView_loopClosure->setBackgroundBrush(QBrush(Qt::black));
_posteriorCurve = new PdfPlotCurve("Posterior", &_imagesMap, this);
_posteriorCurve = new PdfPlotCurve("Posterior", &_cachedSignatures, this);
_ui->posteriorPlot->addCurve(_posteriorCurve, false);
_ui->posteriorPlot->showLegend(false);
_ui->posteriorPlot->setFixedYAxis(0,1);
@@ -197,11 +197,11 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
tc = _ui->posteriorPlot->addThreshold("Loop closure thr", float(_preferencesDialog->getLoopThr()));
connect(this, SIGNAL(loopClosureThrChanged(float)), tc, SLOT(setThreshold(float)));
_likelihoodCurve = new PdfPlotCurve("Likelihood", &_imagesMap, this);
_likelihoodCurve = new PdfPlotCurve("Likelihood", &_cachedSignatures, this);
_ui->likelihoodPlot->addCurve(_likelihoodCurve, false);
_ui->likelihoodPlot->showLegend(false);
_rawLikelihoodCurve = new PdfPlotCurve("Likelihood", &_imagesMap, this);
_rawLikelihoodCurve = new PdfPlotCurve("Likelihood", &_cachedSignatures, this);
_ui->rawLikelihoodPlot->addCurve(_rawLikelihoodCurve, false);
_ui->rawLikelihoodPlot->showLegend(false);
@@ -698,63 +698,16 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
_ui->imageView_loopClosure->setBackgroundBrush(QBrush(Qt::black));
// update cache
if(_preferencesDialog->isImagesKept())
{
// images
for(std::map<int, std::vector<unsigned char> >::const_iterator iter = stat.getImages().begin();
iter != stat.getImages().end();
++iter)
{
if(!iter->second.empty() && !_imagesMap.contains(iter->first))
{
_imagesMap.insert(iter->first, iter->second);
}
}
// depths
for(std::map<int, std::vector<unsigned char> >::const_iterator iter = stat.getDepths().begin();
iter != stat.getDepths().end();
++iter)
{
if(!iter->second.empty() && !_depthsMap.contains(iter->first))
{
float fx = uValue(stat.getDepthFxs(), iter->first, 0.0f);
float fy = uValue(stat.getDepthFys(), iter->first, 0.0f);
float cx = uValue(stat.getDepthCxs(), iter->first, 0.0f);
float cy = uValue(stat.getDepthCys(), iter->first, 0.0f);
Transform transform = uValue(stat.getLocalTransforms(), iter->first, Transform());
if(fx > 0.0f && fy > 0.0f && !transform.isNull())
{
_depthsMap.insert(iter->first, iter->second);
_depthFxsMap.insert(iter->first, fx);
_depthFysMap.insert(iter->first, fy);
_depthCxsMap.insert(iter->first, cx);
_depthCysMap.insert(iter->first, cy);
_localTransformsMap.insert(iter->first, transform);
}
else
{
UERROR("Invalid depth data for id=%d", iter->first);
}
}
}
// depths2d
for(std::map<int, std::vector<unsigned char> >::const_iterator iter = stat.getDepth2ds().begin();
iter != stat.getDepth2ds().end();
++iter)
{
if(!iter->second.empty())
{
_depths2DMap.insert(iter->first, iter->second);
}
}
Signature & signature = *_cachedSignatures.insert(stat.getSignature().id(), stat.getSignature());
signature.uncompressData(); // make sure data are already uncompressed
UDEBUG("");
// map ids
for(std::map<int, int>::const_iterator iter = stat.getMapIds().begin();
iter != stat.getMapIds().end();
++iter)
{
_mapIds.insert(iter->first, iter->second);
}
// map ids
for(std::map<int, int>::const_iterator iter = stat.getMapIds().begin();
iter != stat.getMapIds().end();
++iter)
{
_mapIds.insert(iter->first, iter->second);
}
int rehearsed = (int)uValue(stat.data(), Statistics::kMemoryRehearsal_merged(), 0.0f);
@@ -778,16 +731,12 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
UDEBUG("time= %d ms", time.restart());
std::vector<unsigned char> refImage = uValue(stat.getImages(), stat.refImageId(), std::vector<unsigned char>());
std::vector<unsigned char> refDepth = uValue(stat.getDepths(), stat.refImageId(), std::vector<unsigned char>());
std::vector<unsigned char> refDepth2D = uValue(stat.getDepth2ds(), stat.refImageId(), std::vector<unsigned char>());
std::vector<unsigned char> loopImage = uValue(stat.getImages(), stat.loopClosureId()>0?stat.loopClosureId():stat.localLoopClosureId(), std::vector<unsigned char>());
std::vector<unsigned char> loopDepth = uValue(stat.getDepths(), stat.loopClosureId()>0?stat.loopClosureId():stat.localLoopClosureId(), std::vector<unsigned char>());
std::vector<unsigned char> loopDepth2D = uValue(stat.getDepth2ds(), stat.loopClosureId()>0?stat.loopClosureId():stat.localLoopClosureId(), std::vector<unsigned char>());
int rejectedHyp = bool(uValue(stat.data(), Statistics::kLoopRejectedHypothesis(), 0.0f));
float highestHypothesisValue = uValue(stat.data(), Statistics::kLoopHighest_hypothesis_value(), 0.0f);
int matchId = 0;
cv::Mat loopImage;
cv::Mat loopDepth;
int shownLoopId = 0;
if(highestHypothesisId > 0 || stat.localLoopClosureId()>0)
{
bool show = true;
@@ -829,23 +778,12 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
if(show)
{
if(loopImage.empty())
shownLoopId = stat.loopClosureId()>0?stat.loopClosureId():stat.localLoopClosureId()>0?stat.localLoopClosureId():highestHypothesisId;
QMap<int, Signature>::iterator iter = _cachedSignatures.find(shownLoopId);
if(iter != _cachedSignatures.end())
{
int id = stat.loopClosureId()>0?stat.loopClosureId():stat.localLoopClosureId()>0?stat.localLoopClosureId():highestHypothesisId;
QMap<int, std::vector<unsigned char> >::iterator iter = _imagesMap.find(id);
if(iter != _imagesMap.end())
{
loopImage = iter.value();
}
}
if(loopDepth.empty())
{
int id = stat.loopClosureId()>0?stat.loopClosureId():stat.localLoopClosureId()>0?stat.localLoopClosureId():highestHypothesisId;
QMap<int, std::vector<unsigned char> >::iterator iter = _depthsMap.find(id);
if(iter != _depthsMap.end())
{
loopDepth = iter.value();
}
loopImage = iter.value().getImageRaw();
loopDepth = iter.value().getDepthRaw();
}
}
}
@@ -854,24 +792,10 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
//update image views
{
util3d::CompressionThread imageThread(refImage, true);
util3d::CompressionThread imageLoopThread(loopImage, true);
util3d::CompressionThread depthThread(refDepth, true);
util3d::CompressionThread depthLoopThread(loopDepth, true);
imageThread.start();
depthThread.start();
imageLoopThread.start();
depthLoopThread.start();
imageThread.join();
depthThread.join();
imageLoopThread.join();
depthLoopThread.join();
UDEBUG("time= %d ms", time.restart());
UCvMat2QImageThread qimageThread(imageThread.getUncompressedData());
UCvMat2QImageThread qimageLoopThread(imageLoopThread.getUncompressedData());
UCvMat2QImageThread qdepthThread(depthThread.getUncompressedData());
UCvMat2QImageThread qdepthLoopThread(depthLoopThread.getUncompressedData());
UCvMat2QImageThread qimageThread(signature.getImageRaw());
UCvMat2QImageThread qimageLoopThread(loopImage);
UCvMat2QImageThread qdepthThread(signature.getDepthRaw());
UCvMat2QImageThread qdepthLoopThread(loopDepth);
qimageThread.start();
qdepthThread.start();
qimageLoopThread.start();
@@ -912,7 +836,7 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
// We use the reference image to resize the 2 views
_ui->imageView_source->resetZoom();
_ui->imageView_loopClosure->resetZoom();
if(refImage.empty())
if(signature.getImageRaw().empty())
{
_ui->imageView_source->setSceneRect(_ui->imageView_source->scene()->itemsBoundingRect());
_ui->imageView_loopClosure->setSceneRect(_ui->imageView_source->scene()->itemsBoundingRect());
@@ -921,14 +845,16 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
_ui->imageView_loopClosure->fitInView(_ui->imageView_source->sceneRect(), Qt::KeepAspectRatio);
// do it after scaling
if(_ui->imageView_loopClosure->items().size() || stat.loopClosureId()>0)
std::multimap<int, cv::KeyPoint> loopWords;
if(shownLoopId)
{
this->drawKeypoints(stat.refWords(), stat.loopWords());
}
else
{
this->drawKeypoints(stat.refWords(), std::multimap<int, cv::KeyPoint>()); //empty loop keypoints...
QMap<int, Signature>::iterator iter = _cachedSignatures.find(shownLoopId);
if(iter!=_cachedSignatures.end())
{
loopWords = iter->getWords();
}
}
this->drawKeypoints(signature.getWords(), loopWords);
if(_preferencesDialog->isImageFlipped())
{
@@ -938,9 +864,8 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
UDEBUG("time= %d ms", time.restart());
_ui->statsToolBox->updateStat("Keypoint/Keypoints count in the last signature/", stat.refImageId(), stat.refWords().size());
_ui->statsToolBox->updateStat("Keypoint/Keypoints count in the loop signature/", stat.refImageId(), stat.loopWords().size());
ULOGGER_DEBUG("");
_ui->statsToolBox->updateStat("Keypoint/Keypoints count in the last signature/", stat.refImageId(), signature.getWords().size());
_ui->statsToolBox->updateStat("Keypoint/Keypoints count in the loop signature/", stat.refImageId(), loopWords.size());
// PDF AND LIKELIHOOD
if(!stat.posterior().empty() && _ui->dockWidget_posterior->isVisible())
@@ -986,7 +911,9 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
if(stat.poses().size())
{
// update pose only if a odometry is not received
updateMapCloud(stat.poses(), _odometryReceived?Transform():stat.currentPose(), stat.constraints());
updateMapCloud(stat.poses(),
_odometryReceived||stat.poses().size()==0?Transform():stat.poses().rbegin()->second,
stat.constraints());
_odometryReceived = false;
@@ -1008,44 +935,19 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
}
int loopNewId = stat.refImageId();
// Add to loop closure viewer if all data is saved
Signature * loopOld = new Signature(
loopOldId,
loopMapId,
std::multimap<int, cv::KeyPoint>(),
std::multimap<int, pcl::PointXYZ>(),
Transform(),
_depths2DMap.value(loopOldId, std::vector<unsigned char>()),
_imagesMap.value(loopOldId, std::vector<unsigned char>()),
_depthsMap.value(loopOldId, std::vector<unsigned char>()),
_depthFxsMap.value(loopOldId, 0.0f),
_depthFysMap.value(loopOldId, 0.0f),
_depthCxsMap.value(loopOldId, 0.0f),
_depthCysMap.value(loopOldId, 0.0f),
_localTransformsMap.value(loopOldId, Transform()));
QMap<int, Signature>::iterator newIter = _cachedSignatures.find(loopNewId);
QMap<int, Signature>::iterator oldIter = _cachedSignatures.find(loopOldId);
Signature * loopNew = new Signature(
loopNewId,
refMapId,
std::multimap<int, cv::KeyPoint>(),
std::multimap<int, pcl::PointXYZ>(),
loopClosureTransform,
_depths2DMap.value(loopNewId, std::vector<unsigned char>()),
_imagesMap.value(loopNewId, std::vector<unsigned char>()),
_depthsMap.value(loopNewId, std::vector<unsigned char>()),
_depthFxsMap.value(loopNewId, 0.0f),
_depthFysMap.value(loopNewId, 0.0f),
_depthCxsMap.value(loopNewId, 0.0f),
_depthCysMap.value(loopNewId, 0.0f),
_localTransformsMap.value(loopNewId, Transform()));
_ui->widget_loopClosureViewer->setData(loopOld, loopNew);
if(_ui->dockWidget_loopClosureViewer->isVisible())
if(newIter!=_cachedSignatures.end() && oldIter!=_cachedSignatures.end())
{
UTimer loopTimer;
_ui->widget_loopClosureViewer->updateView();
UINFO("Updating loop closure cloud view time=%fs", loopTimer.elapsed());
_ui->statsToolBox->updateStat("/Gui RGB-D closure view/ms", stat.refImageId(), int(loopTimer.elapsed()*1000.0f));
_ui->widget_loopClosureViewer->setData(*oldIter, *newIter);
if(_ui->dockWidget_loopClosureViewer->isVisible())
{
UTimer loopTimer;
_ui->widget_loopClosureViewer->updateView();
UINFO("Updating loop closure cloud view time=%fs", loopTimer.elapsed());
_ui->statsToolBox->updateStat("/Gui RGB-D closure view/ms", stat.refImageId(), int(loopTimer.elapsed()*1000.0f));
}
}
UDEBUG("time= %d ms", time.restart());
@@ -1078,14 +980,18 @@ void MainWindow::updateMapCloud(
_currentPosesMap = posesIn;
if(_currentPosesMap.size())
{
if(_depthsMap.size())
if(!_ui->actionSave_point_cloud->isEnabled() &&
_cachedSignatures.size() &&
(--_cachedSignatures.end())->getDepth().size())
{
//enable save cloud action
_ui->actionSave_point_cloud->setEnabled(true);
_ui->actionView_high_res_point_cloud->setEnabled(true);
}
if(_depths2DMap.size())
if(!_ui->actionView_scans->isEnabled() &&
_cachedSignatures.size() &&
(--_cachedSignatures.end())->getDepth2D().size())
{
_ui->actionExport_2D_scans_ply_pcd->setEnabled(true);
_ui->actionExport_2D_Grid_map_bmp_png->setEnabled(true);
@@ -1147,9 +1053,13 @@ void MainWindow::updateMapCloud(
_ui->widget_cloudViewer->setCloudOpacity(cloudName, _preferencesDialog->getCloudOpacity(0));
_ui->widget_cloudViewer->setCloudPointSize(cloudName, _preferencesDialog->getCloudPointSize(0));
}
else if(_imagesMap.contains(iter->first) && _depthsMap.contains(iter->first))
else if(_cachedSignatures.contains(iter->first))
{
this->createAndAddCloudToMap(iter->first, iter->second);
QMap<int, Signature>::iterator jter = _cachedSignatures.find(iter->first);
if(!jter->getImageRaw().empty() && !jter->getDepthRaw().empty())
{
this->createAndAddCloudToMap(iter->first, iter->second);
}
}
}
else if(viewerClouds.contains(cloudName))
@@ -1178,9 +1088,13 @@ void MainWindow::updateMapCloud(
_ui->widget_cloudViewer->setCloudOpacity(scanName, _preferencesDialog->getScanOpacity(0));
_ui->widget_cloudViewer->setCloudPointSize(scanName, _preferencesDialog->getScanPointSize(0));
}
else if(_depths2DMap.contains(iter->first))
else if(_cachedSignatures.contains(iter->first))
{
this->createAndAddScanToMap(iter->first, iter->second);
QMap<int, Signature>::iterator jter = _cachedSignatures.find(iter->first);
if(!jter->getDepth2DRaw().empty())
{
this->createAndAddScanToMap(iter->first, iter->second);
}
}
if(!_preferencesDialog->isScansShown(0))
{
@@ -1302,15 +1216,23 @@ void MainWindow::createAndAddCloudToMap(int nodeId, const Transform & pose)
UERROR("Cloud %d already added to map.", nodeId);
return;
}
QMap<int, Signature>::iterator iter = _cachedSignatures.find(nodeId);
if(iter == _cachedSignatures.end())
{
UERROR("Node %d is not in the cache.", nodeId);
return;
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
cloud = createCloud(nodeId,
util3d::uncompressImage(_imagesMap.value(nodeId)),
util3d::uncompressImage(_depthsMap.value(nodeId)),
_depthFxsMap.value(nodeId),
_depthFysMap.value(nodeId),
_depthCxsMap.value(nodeId),
_depthCysMap.value(nodeId),
_localTransformsMap.value(nodeId),
iter->getImageRaw(),
iter->getDepthRaw(),
iter->getDepthFx(),
iter->getDepthFy(),
iter->getDepthCx(),
iter->getDepthCy(),
iter->getLocalTransform(),
Transform::getIdentity(),
_preferencesDialog->getCloudVoxelSize(0),
_preferencesDialog->getCloudDecimation(0),
@@ -1318,6 +1240,7 @@ void MainWindow::createAndAddCloudToMap(int nodeId, const Transform & pose)
if(cloud->size() && _preferencesDialog->isGridMapFrom3DCloud())
{
UTimer timer;
float cellSize = _preferencesDialog->getGridMapResolution();
float groundNormalMaxAngle = M_PI_4;
int minClusterSize = 20;
@@ -1326,6 +1249,7 @@ void MainWindow::createAndAddCloudToMap(int nodeId, const Transform & pose)
{
_occupancyLocalMaps.insert(std::make_pair(nodeId, std::make_pair(ground, obstacles)));
}
UDEBUG("time gridMapFrom2DCloud = %f s", timer.ticks());
}
if(_preferencesDialog->isCloudMeshing())
@@ -1396,9 +1320,16 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose)
UERROR("Scan %d already added to map.", nodeId);
return;
}
QMap<int, Signature>::iterator iter = _cachedSignatures.find(nodeId);
if(iter == _cachedSignatures.end())
{
UERROR("Node %d is not in the cache.", nodeId);
return;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
cv::Mat depth2d = util3d::uncompressData(_depths2DMap.value(nodeId));
cloud = util3d::depth2DToPointCloud(depth2d);
cloud = util3d::depth2DToPointCloud(iter->getDepth2DRaw());
QColor color = Qt::red;
int mapId = _mapIds.value(nodeId, -1);
if(mapId >= 0)
@@ -1422,7 +1353,7 @@ void MainWindow::updateNodeVisibility(int nodeId, bool visible)
if(_currentPosesMap.find(nodeId) != _currentPosesMap.end())
{
QMap<std::string, Transform> viewerClouds = _ui->widget_cloudViewer->getAddedClouds();
if(_preferencesDialog->isCloudsShown(0) && _depthsMap.contains(nodeId))
if(_preferencesDialog->isCloudsShown(0) && _cachedSignatures.contains(nodeId))
{
std::string cloudName = uFormat("cloud%d", nodeId);
if(visible && !viewerClouds.contains(cloudName))
@@ -1440,7 +1371,7 @@ void MainWindow::updateNodeVisibility(int nodeId, bool visible)
}
}
if(_preferencesDialog->isScansShown(0) && _depths2DMap.contains(nodeId))
if(_preferencesDialog->isScansShown(0) && _cachedSignatures.contains(nodeId))
{
std::string scanName = uFormat("scan%d", nodeId);
if(visible && !viewerClouds.contains(scanName))
@@ -1501,72 +1432,31 @@ void MainWindow::processRtabmapEvent3DMap(const rtabmap::RtabmapEvent3DMap & eve
{
UINFO("Received map!");
UINFO(" images = %d", event.getImages().size());
UINFO(" depths = %d", event.getDepths().size());
UINFO(" depths2d = %d", event.getDepths2d().size());
UINFO(" depthFxs = %d", event.getDepthFxs().size());
UINFO(" depthFys = %d", event.getDepthFys().size());
UINFO(" depthCxs = %d", event.getDepthCxs().size());
UINFO(" depthCys = %d", event.getDepthCys().size());
UINFO(" signatures = %d", event.getSignatures().size());
UINFO(" map ids = %d", event.getMapIds().size());
UINFO(" localTransforms = %d", event.getLocalTransforms().size());
UINFO(" poses = %d", event.getPoses().size());
UINFO(" constraints = %d", event.getConstraints().size());
_initProgressDialog->appendText("Inserting data in the cache...");
_initProgressDialog->setMaximumSteps(event.getSignatures().size());
_initProgressDialog->appendText(QString("Inserting data in the cache (%1 signatures downloaded)...").arg(event.getSignatures().size()));
for(std::map<int, std::vector<unsigned char> >::const_iterator iter = event.getImages().begin();
iter!=event.getImages().end();
int addedSignatures = 0;
for(std::map<int, Signature>::const_iterator iter = event.getSignatures().begin();
iter!=event.getSignatures().end();
++iter)
{
_imagesMap.insert(iter->first, iter->second);
if(!_cachedSignatures.contains(iter->first))
{
QMap<int, Signature>::iterator inserted = _cachedSignatures.insert(iter->first, iter->second);
//uncompress data if required
if(inserted->getImageRaw().empty() && inserted->getImage().size())
{
inserted->uncompressData();
++addedSignatures;
}
}
}
_initProgressDialog->appendText(tr("Inserted %1 images.").arg(_imagesMap.size()));
_initProgressDialog->incrementStep();
for(std::map<int, std::vector<unsigned char> >::const_iterator iter = event.getDepths().begin();
iter!=event.getDepths().end();
++iter)
{
_depthsMap.insert(iter->first, iter->second);
}
_initProgressDialog->appendText(tr("Inserted %1 depth images.").arg(_depthsMap.size()));
_initProgressDialog->incrementStep();
for(std::map<int, float>::const_iterator iter = event.getDepthFxs().begin();
iter!=event.getDepthFxs().end();
++iter)
{
_depthFxsMap.insert(iter->first, iter->second);
}
_initProgressDialog->appendText(tr("Inserted %1 depth fx parameters.").arg(_depthFxsMap.size()));
_initProgressDialog->incrementStep();
for(std::map<int, float>::const_iterator iter = event.getDepthFys().begin();
iter!=event.getDepthFys().end();
++iter)
{
_depthFysMap.insert(iter->first, iter->second);
}
_initProgressDialog->appendText(tr("Inserted %1 depth fy parameters.").arg(_depthFysMap.size()));
_initProgressDialog->incrementStep();
for(std::map<int, float>::const_iterator iter = event.getDepthCxs().begin();
iter!=event.getDepthCxs().end();
++iter)
{
_depthCxsMap.insert(iter->first, iter->second);
}
_initProgressDialog->appendText(tr("Inserted %1 depth cx parameters.").arg(_depthCxsMap.size()));
_initProgressDialog->incrementStep();
for(std::map<int, float>::const_iterator iter = event.getDepthCys().begin();
iter!=event.getDepthCys().end();
++iter)
{
_depthCysMap.insert(iter->first, iter->second);
}
_initProgressDialog->appendText(tr("Inserted %1 depth cy parameters.").arg(_depthCysMap.size()));
_initProgressDialog->appendText(tr("Inserted %1 new signatures.").arg(addedSignatures));
_initProgressDialog->incrementStep();
for(std::map<int, int>::const_iterator iter = event.getMapIds().begin();
@@ -1578,24 +1468,6 @@ void MainWindow::processRtabmapEvent3DMap(const rtabmap::RtabmapEvent3DMap & eve
_initProgressDialog->appendText(tr("Inserted %1 map ids").arg(_mapIds.size()));
_initProgressDialog->incrementStep();
for(std::map<int, std::vector<unsigned char> >::const_iterator iter = event.getDepths2d().begin();
iter!=event.getDepths2d().end();
++iter)
{
_depths2DMap.insert(iter->first, iter->second);
}
_initProgressDialog->appendText(tr("Inserted %1 laser scans.").arg(_depths2DMap.size()));
_initProgressDialog->incrementStep();
for(std::map<int, Transform>::const_iterator iter = event.getLocalTransforms().begin();
iter!=event.getLocalTransforms().end();
++iter)
{
_localTransformsMap.insert(iter->first, iter->second);
}
_initProgressDialog->appendText(tr("Inserted %1 local transforms.").arg(_localTransformsMap.size()));
_initProgressDialog->incrementStep();
_initProgressDialog->appendText("Inserting data in the cache... done.");
if(event.getPoses().size())
@@ -2779,15 +2651,8 @@ void MainWindow::downloadPoseGraph()
void MainWindow::clearTheCache()
{
_imagesMap.clear();
_depthsMap.clear();
_depths2DMap.clear();
_depthFxsMap.clear();
_depthFysMap.clear();
_depthCxsMap.clear();
_depthCysMap.clear();
_cachedSignatures.clear();
_mapIds.clear();
_localTransformsMap.clear();
_createdClouds.clear();
_createdScans.clear();
_occupancyLocalMaps.clear();
@@ -3854,19 +3719,20 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr MainWindow::getAssembledCloud(
bool inserted = false;
if(!iter->second.isNull())
{
if(_imagesMap.contains(iter->first) && _depthsMap.contains(iter->first))
if(_cachedSignatures.contains(iter->first))
{
QMap<int, Signature>::const_iterator jter = _cachedSignatures.find(iter->first);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
if(regenerateClouds)
{
cloud = createCloud(iter->first,
util3d::uncompressImage(_imagesMap.value(iter->first)),
util3d::uncompressImage(_depthsMap.value(iter->first)),
_depthFxsMap.value(iter->first),
_depthFysMap.value(iter->first),
_depthCxsMap.value(iter->first),
_depthCysMap.value(iter->first),
_localTransformsMap.value(iter->first),
jter->getImageRaw(),
jter->getDepthRaw(),
jter->getDepthFx(),
jter->getDepthFy(),
jter->getDepthCx(),
jter->getDepthCy(),
jter->getLocalTransform(),
iter->second,
regenerateVoxelSize,
regenerateDecimation,
@@ -3937,19 +3803,20 @@ std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr > MainWindow::getClouds(
bool inserted = false;
if(!iter->second.isNull())
{
if(_imagesMap.contains(iter->first) && _depthsMap.contains(iter->first))
if(_cachedSignatures.contains(iter->first))
{
QMap<int, Signature>::const_iterator jter = _cachedSignatures.find(iter->first);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
if(regenerateClouds)
{
cloud = createCloud(iter->first,
util3d::uncompressImage(_imagesMap.value(iter->first)),
util3d::uncompressImage(_depthsMap.value(iter->first)),
_depthFxsMap.value(iter->first),
_depthFysMap.value(iter->first),
_depthCxsMap.value(iter->first),
_depthCysMap.value(iter->first),
_localTransformsMap.value(iter->first),
jter->getImageRaw(),
jter->getDepthRaw(),
jter->getDepthFx(),
jter->getDepthFy(),
jter->getDepthCx(),
jter->getDepthCy(),
jter->getLocalTransform(),
Transform::getIdentity(),
regenerateVoxelSize,
regenerateDecimation,

16
guilib/src/PdfPlot.cpp
View File

@@ -35,7 +35,7 @@ namespace rtabmap {
PdfPlotItem::PdfPlotItem(float dataX, float dataY, float width, int childCount) :
UPlotItem(dataX, dataY, width),
_img(0),
_imagesRef(0),
_signaturesRef(0),
_text(0)
{
setLikelihood(dataX, dataY, childCount);
@@ -66,13 +66,13 @@ void PdfPlotItem::showDescription(bool shown)
}
if(shown)
{
if(!_img && _imagesRef)
if(!_img && _signaturesRef)
{
QImage img;
QMap<int, std::vector<unsigned char> >::const_iterator iter = _imagesRef->find(int(this->data().x()));
if(iter != _imagesRef->constEnd())
QMap<int, Signature>::const_iterator iter = _signaturesRef->find(int(this->data().x()));
if(iter != _signaturesRef->constEnd() && !iter.value().getImageRaw().empty())
{
img = uCvMat2QImage(util3d::uncompressImage(iter.value()));
img = uCvMat2QImage(iter.value().getImageRaw());
QPixmap scaled = QPixmap::fromImage(img).scaledToWidth(128);
_img = new QGraphicsPixmapItem(scaled, this);
_img->setVisible(false);
@@ -111,9 +111,9 @@ void PdfPlotItem::showDescription(bool shown)
PdfPlotCurve::PdfPlotCurve(const QString & name, const QMap<int, std::vector<unsigned char> > * imagesMapRef = 0, QObject * parent) :
PdfPlotCurve::PdfPlotCurve(const QString & name, const QMap<int, Signature> * signaturesMapRef = 0, QObject * parent) :
UPlotCurve(name, parent),
_imagesMapRef(imagesMapRef)
_signaturesMapRef(signaturesMapRef)
{
}
@@ -139,7 +139,7 @@ void PdfPlotCurve::setData(const QMap<int, float> & dataMap, const QMap<int, int
while(margin < 0)
{
PdfPlotItem * newItem = new PdfPlotItem(0, 0, 2, 0);
newItem->setImagesRef(_imagesMapRef);
newItem->setSignaturesRef(_signaturesMapRef);
this->_addValue(newItem);
++margin;
}

9
guilib/src/PdfPlot.h
View File

@@ -30,6 +30,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <utilite/UPlot.h>
#include "opencv2/opencv.hpp"
#include "rtabmap/core/Signature.h"
namespace rtabmap {
@@ -40,7 +41,7 @@ public:
virtual ~PdfPlotItem();
void setLikelihood(int id, float value, int childCount);
void setImagesRef(const QMap<int, std::vector<unsigned char> > * imagesRef) {_imagesRef = imagesRef;}
void setSignaturesRef(const QMap<int, Signature> * signaturesRef) {_signaturesRef = signaturesRef;}
float value() const {return this->data().y();}
int id() const {return this->data().x();}
@@ -51,7 +52,7 @@ protected:
private:
QGraphicsPixmapItem * _img;
int _childCount;
const QMap<int, std::vector<unsigned char> > * _imagesRef;
const QMap<int, Signature> * _signaturesRef;
QGraphicsTextItem * _text;
};
@@ -61,14 +62,14 @@ class PdfPlotCurve : public UPlotCurve
Q_OBJECT
public:
PdfPlotCurve(const QString & name, const QMap<int, std::vector<unsigned char> > * imagesMapRef, QObject * parent = 0);
PdfPlotCurve(const QString & name, const QMap<int, Signature> * signaturesMapRef, QObject * parent = 0);
virtual ~PdfPlotCurve();
virtual void clear();
void setData(const QMap<int, float> & dataMap, const QMap<int, int> & weightsMap);
private:
const QMap<int, std::vector<unsigned char> > * _imagesMapRef;
const QMap<int, Signature> * _signaturesMapRef;
};
}

13
guilib/src/PreferencesDialog.cpp
View File

@@ -203,6 +203,8 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
connect(_ui->doubleSpinBox_map_resolution, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
connect(_ui->checkBox_map_fillEmptySpace, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
connect(_ui->doubleSpinBox_map_opacity, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
connect(_ui->spinbox_map_fillEmptyRadius, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
connect(_ui->checkBox_map_occupancyFrom3DCloud, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
//Logging panel
connect(_ui->comboBox_loggerLevel, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteLoggingPanel()));
@@ -287,7 +289,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
// Map objects name with the corresponding parameter key, needed for the addParameter() slots
//Rtabmap
_ui->groupBox_publishing->setObjectName(Parameters::kRtabmapPublishStats().c_str());
_ui->general_checkBox_publishRawData->setObjectName(Parameters::kRtabmapPublishImage().c_str());
_ui->general_checkBox_publishRawData->setObjectName(Parameters::kRtabmapPublishLastSignature().c_str());
_ui->general_checkBox_publishPdf->setObjectName(Parameters::kRtabmapPublishPdf().c_str());
_ui->general_checkBox_publishLikelihood->setObjectName(Parameters::kRtabmapPublishLikelihood().c_str());
_ui->general_checkBox_statisticLogsBufferedInRAM->setObjectName(Parameters::kRtabmapStatisticLogsBufferedInRAM().c_str());
@@ -334,7 +336,6 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
connect(_ui->lineEdit_bayes_predictionLC, SIGNAL(textChanged(const QString &)), this, SLOT(updatePredictionPlot()));
//Keypoint-based
_ui->checkBox_kp_publishKeypoints->setObjectName(Parameters::kKpPublishKeypoints().c_str());
_ui->comboBox_dictionary_strategy->setObjectName(Parameters::kKpNNStrategy().c_str());
_ui->checkBox_dictionary_incremental->setObjectName(Parameters::kKpIncrementalDictionary().c_str());
_ui->comboBox_detector_strategy->setObjectName(Parameters::kKpDetectorStrategy().c_str());
@@ -819,7 +820,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
_ui->doubleSpinBox_map_resolution->setValue(0.05);
_ui->checkBox_map_fillEmptySpace->setChecked(true);
_ui->checkBox_map_occupancyFrom3DCloud->setChecked(false);
_ui->checkBox_map_fillEmptyRadius->setValue(0);
_ui->spinbox_map_fillEmptyRadius->setValue(0);
_ui->doubleSpinBox_map_opacity->setValue(0.75);
}
else if(groupBox->objectName() == _ui->groupBox_logging1->objectName())
@@ -1056,7 +1057,7 @@ void PreferencesDialog::readGuiSettings(const QString & filePath)
_ui->doubleSpinBox_map_resolution->setValue(settings.value("gridMapResolution", _ui->doubleSpinBox_map_resolution->value()).toDouble());
_ui->checkBox_map_fillEmptySpace->setChecked(settings.value("gridMapFillEmptySpace", _ui->checkBox_map_fillEmptySpace->isChecked()).toBool());
_ui->checkBox_map_occupancyFrom3DCloud->setChecked(settings.value("gridMapOccupancyFrom3DCloud", _ui->checkBox_map_occupancyFrom3DCloud->isChecked()).toBool());
_ui->checkBox_map_fillEmptyRadius->setValue(settings.value("gridMapFillEmptyRadius", _ui->checkBox_map_fillEmptyRadius->value()).toInt());
_ui->spinbox_map_fillEmptyRadius->setValue(settings.value("gridMapFillEmptyRadius", _ui->spinbox_map_fillEmptyRadius->value()).toInt());
_ui->doubleSpinBox_map_opacity->setValue(settings.value("gridMapOpacity", _ui->doubleSpinBox_map_opacity->value()).toDouble());
settings.endGroup(); // General
@@ -1298,7 +1299,7 @@ void PreferencesDialog::writeGuiSettings(const QString & filePath)
settings.setValue("gridMapResolution", _ui->doubleSpinBox_map_resolution->value());
settings.setValue("gridMapFillEmptySpace", _ui->checkBox_map_fillEmptySpace->isChecked());
settings.setValue("gridMapOccupancyFrom3DCloud", _ui->checkBox_map_occupancyFrom3DCloud->isChecked());
settings.setValue("gridMapFillEmptyRadius", _ui->checkBox_map_fillEmptyRadius->value());
settings.setValue("gridMapFillEmptyRadius", _ui->spinbox_map_fillEmptyRadius->value());
settings.setValue("gridMapOpacity", _ui->doubleSpinBox_map_opacity->value());
settings.endGroup(); // General
@@ -2665,7 +2666,7 @@ bool PreferencesDialog::isGridMapFrom3DCloud() const
}
int PreferencesDialog::getGridMapFillEmptyRadius() const
{
return _ui->checkBox_map_fillEmptyRadius->value();
return _ui->spinbox_map_fillEmptyRadius->value();
}
double PreferencesDialog::getGridMapOpacity() const
{

25
guilib/src/ui/preferencesDialog.ui
View File

@@ -65,7 +65,7 @@
<x>0</x>
<y>0</y>
<width>744</width>
<height>978</height>
<height>1047</height>
</rect>
</property>
<layout class="QVBoxLayout" name="verticalLayout_16">
@@ -86,7 +86,7 @@
<enum>QFrame::Raised</enum>
</property>
<property name="currentIndex">
<number>3</number>
<number>1</number>
</property>
<widget class="QWidget" name="page_22">
<layout class="QVBoxLayout" name="verticalLayout_29">
@@ -558,7 +558,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
</widget>
</item>
<item row="5" column="0">
<widget class="QSpinBox" name="checkBox_map_fillEmptyRadius">
<widget class="QSpinBox" name="spinbox_map_fillEmptyRadius">
<property name="suffix">
<string> cells</string>
</property>
@@ -2422,7 +2422,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
<item row="0" column="1">
<widget class="QLabel" name="label_91">
<property name="text">
<string>Publish raw sensor data.</string>
<string>Publish signature data.</string>
</property>
</widget>
</item>
@@ -2460,23 +2460,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QLabel" name="label_144">
<property name="text">
<string>Publish visual words.</string>
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QCheckBox" name="checkBox_kp_publishKeypoints">
<property name="text">
<string/>
</property>
<property name="checked">
<bool>true</bool>
</property>
</widget>
</item>
</layout>
</widget>
</item>