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

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