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

306
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,24 +1663,18 @@ 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())
{
// 2D
@@ -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>