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

Save/view high-res point clouds depending on the visible clouds in Map view (only those checked in MapVisibility view)

Browse Source 
git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1437 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2014-06-23 20:43:08 +00:00
parent e9ae29f909
commit 7b0a34b150
13 changed files with 310 additions and 215 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
guilib/include/rtabmap/gui/CloudViewer.h
View File

@@ -106,8 +106,10 @@ public:
const QMap<std::string, Transform> & getAddedClouds() {return _addedClouds;} //including meshes
void setCameraTargetLocked();
void setCameraTargetFollow();
void setCameraTargetLocked(bool enabled = true);
void setCameraTargetFollow(bool enabled = true);
void setCameraFree();
void setCameraLockZ(bool enabled = true);
public slots:
void render();

2
guilib/include/rtabmap/gui/DatabaseViewer.h
View File

@@ -92,7 +92,7 @@ private:
std::list<std::map<int, rtabmap::Transform> > graphes_;
std::map<int, rtabmap::Transform> poses_;
std::multimap<int, rtabmap::Link> links_;
std::map<int, std::vector<unsigned char> > scans_;
QMap<int, std::vector<unsigned char> > scans_;
};
#endif /* DATABASEVIEWER_H_ */

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

@@ -176,6 +176,8 @@ signals:
private:
void update3DMapVisibility(bool cloudsShown, bool scansShown);
void updateMapCloud(const std::map<int, Transform> & poses, const Transform & pose);
void createAndAddCloudToMap(int nodeId, const Transform & pose);
void createAndAddScanToMap(int nodeId, const Transform & pose);
std::map<int, Transform> radiusPosesFiltering(const std::map<int, Transform> & poses) const;
void drawKeypoints(const std::multimap<int, cv::KeyPoint> & refWords, const std::multimap<int, cv::KeyPoint> & loopWords);
void setupMainLayout(bool vertical);
@@ -183,7 +185,7 @@ private:
void updateSelectSourceDatabase(bool used);
void updateSelectSourceRGBDMenu(bool used, PreferencesDialog::Src src);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr createAssembledCloud();
pcl::PointCloud<pcl::PointXYZRGB>::Ptr createAssembledCloud(const std::map<int, Transform> & poses) const;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr createCloud(
int id,
const cv::Mat & rgb,
@@ -193,9 +195,9 @@ private:
const Transform & pose,
float voxelSize,
int decimation,
float maxDepth);
std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr > createPointClouds();
std::map<int, pcl::PolygonMesh::Ptr> createMeshes();
float maxDepth) const;
std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr > createPointClouds(const std::map<int, Transform> & poses) const;
std::map<int, pcl::PolygonMesh::Ptr> createMeshes(const std::map<int, Transform> & poses) const;
void savePointClouds(const std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr> & clouds);
void saveMeshes(const std::map<int, pcl::PolygonMesh::Ptr> & meshes);
@@ -222,7 +224,7 @@ private:
QMap<int, std::vector<unsigned char> > _imagesMap;
QMap<int, std::vector<unsigned char> > _depthsMap;
std::map<int, std::vector<unsigned char> > _depths2DMap;
QMap<int, std::vector<unsigned char> > _depths2DMap;
QMap<int, float> _depthConstantsMap;
QMap<int, Transform> _localTransformsMap;
std::map<int, Transform> _currentPosesMap;

19
guilib/src/CloudViewer.cpp
View File

@@ -536,14 +536,25 @@ void CloudViewer::setCloudPointSize(const std::string & id, int size)
}
}
void CloudViewer::setCameraTargetLocked()
void CloudViewer::setCameraTargetLocked(bool enabled)
{
_aLockCamera->setChecked(true);
_aLockCamera->setChecked(enabled);
}
void CloudViewer::setCameraTargetFollow()
void CloudViewer::setCameraTargetFollow(bool enabled)
{
_aFollowCamera->setChecked(true);
_aFollowCamera->setChecked(enabled);
}
void CloudViewer::setCameraFree()
{
_aLockCamera->setChecked(false);
_aFollowCamera->setChecked(false);
}
void CloudViewer::setCameraLockZ(bool enabled)
{
_aLockViewZ->setChecked(enabled);
}
Eigen::Vector3f rotatePointAroundAxe(

2
guilib/src/DatabaseViewer.cpp
View File

@@ -904,7 +904,7 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
memory_->getImageDepth(ids_.at(i), imageBytesA, depthBytesA, depth2dBytesA, depthConstantA, localTransformA);
if(depth2dBytesA.size())
{
scans_.insert(std::make_pair(ids_.at(i), depth2dBytesA));
scans_.insert(ids_.at(i), depth2dBytesA);
}
}
UINFO("Update scans list... done");

27
guilib/src/GraphViewer.cpp
View File

@@ -132,7 +132,7 @@ GraphViewer::GraphViewer(QWidget * parent) :
_neighborColor(Qt::blue),
_loopClosureColor(Qt::red),
_root(0),
_nodeRadius(0.1),
_nodeRadius(0.01),
_linkWidth(0),
_gridMap(0),
_gridCellSize(0.05f)
@@ -167,7 +167,7 @@ GraphViewer::~GraphViewer()
void GraphViewer::updateGraph(const std::map<int, Transform> & poses,
const std::multimap<int, Link> & constraints,
const std::map<int, std::vector<unsigned char> > & scans)
const QMap<int, std::vector<unsigned char> > & scans)
{
//Hide nodes and links
for(QMap<int, NodeItem*>::iterator iter = _nodeItems.begin(); iter!=_nodeItems.end(); ++iter)
@@ -191,9 +191,9 @@ void GraphViewer::updateGraph(const std::map<int, Transform> & poses,
if(itemIter != _nodeItems.end())
{
itemIter.value()->setPose(iter->second);
if(itemIter.value()->getScan().empty() && uContains(scans, iter->first))
if(itemIter.value()->getScan().empty() && scans.contains(iter->first))
{
cv::Mat depth2d = util3d::uncompressData(scans.at(iter->first));
cv::Mat depth2d = util3d::uncompressData(scans.value(iter->first));
itemIter.value()->setScan(depth2d);
}
if(!itemIter.value()->getScan().empty() && _gridMap->isVisible())
@@ -205,11 +205,11 @@ void GraphViewer::updateGraph(const std::map<int, Transform> & poses,
else
{
// create node item
std::map<int, std::vector<unsigned char> >::const_iterator jter = scans.find(iter->first);
QMap<int, std::vector<unsigned char> >::const_iterator jter = scans.find(iter->first);
cv::Mat depth2d;
if(jter != scans.end())
{
depth2d = util3d::uncompressData(jter->second);
depth2d = util3d::uncompressData(jter.value());
if(_gridMap->isVisible())
{
scanClouds.insert(std::make_pair(iter->first, util3d::depth2DToPointCloud(depth2d)));
@@ -319,19 +319,20 @@ void GraphViewer::updateGraph(const std::map<int, Transform> & poses,
{
for (int j = 0; j < map8S.cols; ++j)
{
unsigned char gray = map8S.at<unsigned char>(i, j);
if(gray == -1)
{
gray = 89;
}
else if(gray == 0)
char v = map8S.at<char>(i, j);
unsigned char gray;
if(v == 0)
{
gray = 178;
}
else if(gray == 100)
else if(v == 100)
{
gray = 0;
}
else // -1
{
gray = 89;
}
map8U.at<unsigned char>(i, j) = gray;
}
}

2
guilib/src/GraphViewer.h
View File

@@ -28,7 +28,7 @@ public:
void updateGraph(const std::map<int, Transform> & poses,
const std::multimap<int, Link> & constraints,
const std::map<int, std::vector<unsigned char> > & scans);
const QMap<int, std::vector<unsigned char> > & scans);
void clearGraph();
void setWorkingDirectory(const QString & path) {_workingDirectory = path;}

254
guilib/src/MainWindow.cpp
View File

@@ -704,7 +704,7 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
{
if(!iter->second.empty())
{
_depths2DMap.insert(std::make_pair(iter->first, iter->second));
_depths2DMap.insert(iter->first, iter->second);
}
}
}
@@ -941,7 +941,6 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
updateMapCloud(stat.poses(), _odometryReceived?Transform():stat.currentPose());
// update some widgets
_ui->widget_mapVisibility->setMap(stat.poses());
if(_ui->graphicsView_graphView->isVisible())
{
_ui->graphicsView_graphView->updateGraph(stat.poses(), stat.constraints(), _depths2DMap);
@@ -974,7 +973,7 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
std::multimap<int, cv::KeyPoint>(),
std::multimap<int, pcl::PointXYZ>(),
Transform(),
uValue(_depths2DMap, loopOldId, std::vector<unsigned char>()),
_depths2DMap.value(loopOldId, std::vector<unsigned char>()),
_imagesMap.value(loopOldId, std::vector<unsigned char>()),
_depthsMap.value(loopOldId, std::vector<unsigned char>()),
_depthConstantsMap.value(loopOldId, 0.0f),
@@ -986,7 +985,7 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
std::multimap<int, cv::KeyPoint>(),
std::multimap<int, pcl::PointXYZ>(),
loopClosureTransform,
uValue(_depths2DMap, loopNewId, std::vector<unsigned char>()),
_depths2DMap.value(loopNewId, std::vector<unsigned char>()),
_imagesMap.value(loopNewId, std::vector<unsigned char>()),
_depthsMap.value(loopNewId, std::vector<unsigned char>()),
_depthConstantsMap.value(loopNewId, 0.0f),
@@ -1046,6 +1045,12 @@ void MainWindow::updateMapCloud(const std::map<int, Transform> & posesIn, const
{
poses = posesIn;
}
std::map<int, bool> posesMask;
for(std::map<int, Transform>::const_iterator iter = posesIn.begin(); iter!=posesIn.end(); ++iter)
{
posesMask.insert(posesMask.end(), std::make_pair(iter->first, poses.find(iter->first) != poses.end()));
}
_ui->widget_mapVisibility->setMap(posesIn, posesMask);
// Map updated! regenerate the assembled cloud, last pose is the new one
UDEBUG("Update map with %d locations (currentPose=%s)", poses.size(), currentPose.prettyPrint().c_str());
@@ -1077,62 +1082,7 @@ void MainWindow::updateMapCloud(const std::map<int, Transform> & posesIn, const
}
else if(_imagesMap.contains(iter->first) && _depthsMap.contains(iter->first))
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
cloud = createCloud(iter->first,
util3d::uncompressImage(_imagesMap.value(iter->first)),
util3d::uncompressImage(_depthsMap.value(iter->first)),
_depthConstantsMap.value(iter->first),
_localTransformsMap.value(iter->first),
Transform::getIdentity(),
_preferencesDialog->getCloudVoxelSize(0),
_preferencesDialog->getCloudDecimation(0),
_preferencesDialog->getCloudMaxDepth(0));
if(_preferencesDialog->isCloudMeshing(0))
{
pcl::PolygonMesh::Ptr mesh(new pcl::PolygonMesh);
if(cloud->size())
{
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudWithNormals;
if(_preferencesDialog->getMeshSmoothing(0))
{
cloudWithNormals = util3d::computeNormalsSmoothed(cloud, (float)_preferencesDialog->getMeshSmoothingRadius(0));
}
else
{
cloudWithNormals = util3d::computeNormals(cloud, _preferencesDialog->getMeshNormalKSearch(0));
}
mesh = util3d::createMesh(cloudWithNormals, _preferencesDialog->getMeshGP3Radius(0));
}
if(mesh->polygons.size())
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::fromPCLPointCloud2(mesh->cloud, *tmp);
if(!_ui->widget_cloudViewer->addCloudMesh(cloudName, tmp, mesh->polygons, iter->second))
{
UERROR("Adding mesh cloud %d to viewer failed!", iter->first);
}
}
}
else
{
if(_preferencesDialog->getMeshSmoothing(0))
{
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudWithNormals;
cloudWithNormals = util3d::computeNormalsSmoothed(cloud, (float)_preferencesDialog->getMeshSmoothingRadius(0));
cloud->clear();
pcl::copyPointCloud(*cloudWithNormals, *cloud);
}
if(!_ui->widget_cloudViewer->addOrUpdateCloud(cloudName, cloud, iter->second))
{
UERROR("Adding cloud %d to viewer failed!", iter->first);
}
}
_ui->widget_cloudViewer->setCloudOpacity(cloudName, _preferencesDialog->getCloudOpacity(0));
_ui->widget_cloudViewer->setCloudPointSize(cloudName, _preferencesDialog->getCloudPointSize(0));
this->createAndAddCloudToMap(iter->first, iter->second);
}
}
else if(viewerClouds.contains(cloudName))
@@ -1161,17 +1111,9 @@ void MainWindow::updateMapCloud(const std::map<int, Transform> & posesIn, const
_ui->widget_cloudViewer->setCloudOpacity(scanName, _preferencesDialog->getScanOpacity(0));
_ui->widget_cloudViewer->setCloudPointSize(scanName, _preferencesDialog->getScanPointSize(0));
}
else if(uContains(_depths2DMap, iter->first))
else if(_depths2DMap.contains(iter->first))
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
cv::Mat depth2d = util3d::uncompressData(_depths2DMap.at(iter->first));
cloud = util3d::depth2DToPointCloud(depth2d);
if(!_ui->widget_cloudViewer->addOrUpdateCloud(scanName, cloud, iter->second))
{
UERROR("Adding cloud %d to viewer failed!", iter->first);
}
_ui->widget_cloudViewer->setCloudOpacity(scanName, _preferencesDialog->getScanOpacity(0));
_ui->widget_cloudViewer->setCloudPointSize(scanName, _preferencesDialog->getScanPointSize(0));
this->createAndAddScanToMap(iter->first, iter->second);
}
}
else if(viewerClouds.contains(scanName))
@@ -1238,20 +1180,120 @@ void MainWindow::updateMapCloud(const std::map<int, Transform> & posesIn, const
_ui->widget_cloudViewer->render();
}
void MainWindow::createAndAddCloudToMap(int nodeId, const Transform & pose)
{
std::string cloudName = uFormat("cloud%d", nodeId);
if(_ui->widget_cloudViewer->getAddedClouds().contains(cloudName))
{
UERROR("Cloud %d already added to map.", nodeId);
return;
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
cloud = createCloud(nodeId,
util3d::uncompressImage(_imagesMap.value(nodeId)),
util3d::uncompressImage(_depthsMap.value(nodeId)),
_depthConstantsMap.value(nodeId),
_localTransformsMap.value(nodeId),
Transform::getIdentity(),
_preferencesDialog->getCloudVoxelSize(0),
_preferencesDialog->getCloudDecimation(0),
_preferencesDialog->getCloudMaxDepth(0));
if(_preferencesDialog->isCloudMeshing(0))
{
pcl::PolygonMesh::Ptr mesh(new pcl::PolygonMesh);
if(cloud->size())
{
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudWithNormals;
if(_preferencesDialog->getMeshSmoothing(0))
{
cloudWithNormals = util3d::computeNormalsSmoothed(cloud, (float)_preferencesDialog->getMeshSmoothingRadius(0));
}
else
{
cloudWithNormals = util3d::computeNormals(cloud, _preferencesDialog->getMeshNormalKSearch(0));
}
mesh = util3d::createMesh(cloudWithNormals, _preferencesDialog->getMeshGP3Radius(0));
}
if(mesh->polygons.size())
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::fromPCLPointCloud2(mesh->cloud, *tmp);
if(!_ui->widget_cloudViewer->addCloudMesh(cloudName, tmp, mesh->polygons, pose))
{
UERROR("Adding mesh cloud %d to viewer failed!", nodeId);
}
}
}
else
{
if(_preferencesDialog->getMeshSmoothing(0))
{
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudWithNormals;
cloudWithNormals = util3d::computeNormalsSmoothed(cloud, (float)_preferencesDialog->getMeshSmoothingRadius(0));
cloud->clear();
pcl::copyPointCloud(*cloudWithNormals, *cloud);
}
if(!_ui->widget_cloudViewer->addOrUpdateCloud(cloudName, cloud, pose))
{
UERROR("Adding cloud %d to viewer failed!", nodeId);
}
}
_ui->widget_cloudViewer->setCloudOpacity(cloudName, _preferencesDialog->getCloudOpacity(0));
_ui->widget_cloudViewer->setCloudPointSize(cloudName, _preferencesDialog->getCloudPointSize(0));
}
void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose)
{
std::string scanName = uFormat("scan%d", nodeId);
if(_ui->widget_cloudViewer->getAddedClouds().contains(scanName))
{
UERROR("Scan %d already added to map.", nodeId);
return;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
cv::Mat depth2d = util3d::uncompressData(_depths2DMap.value(nodeId));
cloud = util3d::depth2DToPointCloud(depth2d);
if(!_ui->widget_cloudViewer->addOrUpdateCloud(scanName, cloud, pose))
{
UERROR("Adding cloud %d to viewer failed!", nodeId);
}
_ui->widget_cloudViewer->setCloudOpacity(scanName, _preferencesDialog->getScanOpacity(0));
_ui->widget_cloudViewer->setCloudPointSize(scanName, _preferencesDialog->getScanPointSize(0));
}
void MainWindow::updateNodeVisibility(int nodeId, bool visible)
{
if(_currentPosesMap.find(nodeId) != _currentPosesMap.end())
{
if(_preferencesDialog->isCloudsShown(0))
QMap<std::string, Transform> viewerClouds = _ui->widget_cloudViewer->getAddedClouds();
if(_preferencesDialog->isCloudsShown(0) && _depthsMap.contains(nodeId))
{
std::string cloudName = uFormat("cloud%d", nodeId);
_ui->widget_cloudViewer->setCloudVisibility(cloudName, visible);
if(visible && !viewerClouds.contains(cloudName))
{
createAndAddCloudToMap(nodeId, _currentPosesMap.find(nodeId)->second);
}
else if(viewerClouds.contains(cloudName))
{
_ui->widget_cloudViewer->setCloudVisibility(cloudName, visible);
}
}
if(_preferencesDialog->isScansShown(0))
if(_preferencesDialog->isScansShown(0) && _depths2DMap.contains(nodeId))
{
std::string scanName = uFormat("scan%d", nodeId);
_ui->widget_cloudViewer->setCloudVisibility(scanName, visible);
if(visible && !viewerClouds.contains(scanName))
{
createAndAddScanToMap(nodeId, _currentPosesMap.find(nodeId)->second);
}
else if(viewerClouds.contains(scanName))
{
_ui->widget_cloudViewer->setCloudVisibility(scanName, visible);
}
}
}
_ui->widget_cloudViewer->render();
@@ -1425,7 +1467,7 @@ void MainWindow::processRtabmapEvent3DMap(const rtabmap::RtabmapEvent3DMap & eve
iter!=event.getDepths2d().end();
++iter)
{
_depths2DMap.insert(std::make_pair(iter->first, iter->second));
_depths2DMap.insert(iter->first, iter->second);
}
_initProgressDialog->appendText(tr("Inserted %1 laser scans.").arg(_depths2DMap.size()));
_initProgressDialog->incrementStep();
@@ -1458,7 +1500,7 @@ void MainWindow::processRtabmapEvent3DMap(const rtabmap::RtabmapEvent3DMap & eve
_ui->graphicsView_graphView->updateGraph(
event.getPoses(),
event.getConstraints(),
event.getDepths2d().size()?event.getDepths2d():_depths2DMap);
_depths2DMap);
_initProgressDialog->appendText("Updating the graph view... done.");
}
}
@@ -2825,15 +2867,17 @@ void MainWindow::savePointClouds()
if(button == QMessageBox::Yes || button == QMessageBox::No)
{
std::map<int, Transform> poses = _ui->widget_mapVisibility->getVisiblePoses();
_initProgressDialog->setAutoClose(true, 1);
_initProgressDialog->resetProgress();
_initProgressDialog->show();
_initProgressDialog->setMaximumSteps(int(_currentPosesMap.size())*2+1);
_initProgressDialog->setMaximumSteps(int(poses.size())*2+1);
std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr> clouds;
if(button == QMessageBox::Yes)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = this->createAssembledCloud();
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = this->createAssembledCloud(poses);
if(_preferencesDialog->getMeshSmoothing(1))
{
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudWithNormals;
@@ -2849,7 +2893,7 @@ void MainWindow::savePointClouds()
}
else
{
clouds = this->createPointClouds();
clouds = this->createPointClouds(poses);
}
savePointClouds(clouds);
_initProgressDialog->setValue(_initProgressDialog->maximumSteps());
@@ -2867,15 +2911,17 @@ void MainWindow::saveMeshes()
if(button == QMessageBox::Yes || button == QMessageBox::No)
{
std::map<int, Transform> poses = _ui->widget_mapVisibility->getVisiblePoses();
_initProgressDialog->setAutoClose(true, 1);
_initProgressDialog->resetProgress();
_initProgressDialog->show();
_initProgressDialog->setMaximumSteps(int(_currentPosesMap.size())*2+1);
_initProgressDialog->setMaximumSteps(int(poses.size())*2+1);
std::map<int, pcl::PolygonMesh::Ptr> meshes;
if(button == QMessageBox::Yes)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = this->createAssembledCloud();
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = this->createAssembledCloud(poses);
_initProgressDialog->appendText(tr("Meshing the assembled cloud (%1 points)...").arg(cloud->size()));
_initProgressDialog->incrementStep();
QApplication::processEvents();
@@ -2907,7 +2953,7 @@ void MainWindow::saveMeshes()
}
else
{
meshes = this->createMeshes();
meshes = this->createMeshes(poses);
}
saveMeshes(meshes);
_initProgressDialog->setValue(_initProgressDialog->maximumSteps());
@@ -2925,15 +2971,17 @@ void MainWindow::viewPointClouds()
if(button == QMessageBox::Yes || button == QMessageBox::No)
{
std::map<int, Transform> poses = _ui->widget_mapVisibility->getVisiblePoses();
_initProgressDialog->setAutoClose(true, 1);
_initProgressDialog->resetProgress();
_initProgressDialog->show();
_initProgressDialog->setMaximumSteps(int(_currentPosesMap.size())+1);
_initProgressDialog->setMaximumSteps(int(poses.size())+1);
std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr> clouds;
if(button == QMessageBox::Yes)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = this->createAssembledCloud();
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = this->createAssembledCloud(poses);
if(_preferencesDialog->getMeshSmoothing(1))
{
@@ -2951,7 +2999,7 @@ void MainWindow::viewPointClouds()
}
else
{
clouds = this->createPointClouds();
clouds = this->createPointClouds(poses);
}
if(clouds.size())
@@ -2964,6 +3012,7 @@ void MainWindow::viewPointClouds()
window->setMinimumHeight(600);
CloudViewer * viewer = new CloudViewer(window);
viewer->setCameraLockZ(false);
QVBoxLayout *layout = new QVBoxLayout();
layout->addWidget(viewer);
@@ -3001,15 +3050,17 @@ void MainWindow::viewMeshes()
if(button == QMessageBox::Yes || button == QMessageBox::No)
{
std::map<int, Transform> poses = _ui->widget_mapVisibility->getVisiblePoses();
_initProgressDialog->setAutoClose(true, 1);
_initProgressDialog->resetProgress();
_initProgressDialog->show();
_initProgressDialog->setMaximumSteps(int(_currentPosesMap.size())+1);
_initProgressDialog->setMaximumSteps(int(poses.size())+1);
std::map<int, pcl::PolygonMesh::Ptr> meshes;
if(button == QMessageBox::Yes)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = this->createAssembledCloud();
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = this->createAssembledCloud(poses);
_initProgressDialog->appendText(tr("Meshing the assembled cloud (%1 points)...").arg(cloud->size()));
_initProgressDialog->incrementStep();
QApplication::processEvents();
@@ -3041,7 +3092,7 @@ void MainWindow::viewMeshes()
}
else
{
meshes = this->createMeshes();
meshes = this->createMeshes(poses);
}
if(meshes.size())
@@ -3053,6 +3104,7 @@ void MainWindow::viewMeshes()
window->setMinimumHeight(600);
CloudViewer * viewer = new CloudViewer(window);
viewer->setCameraLockZ(false);
QVBoxLayout *layout = new QVBoxLayout();
layout->addWidget(viewer);
@@ -3375,7 +3427,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr MainWindow::createCloud(
const Transform & pose,
float voxelSize,
int decimation,
float maxDepth)
float maxDepth) const
{
UTimer timer;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::cloudFromDepthRGB(
@@ -3416,12 +3468,12 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr MainWindow::createCloud(
return cloud;
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr MainWindow::createAssembledCloud()
pcl::PointCloud<pcl::PointXYZRGB>::Ptr MainWindow::createAssembledCloud(const std::map<int, Transform> & poses) const
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr assembledCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
int i=0;
int count = 0;
for(std::map<int, Transform>::const_iterator iter = _currentPosesMap.begin(); iter!=_currentPosesMap.end(); ++iter)
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
bool inserted = false;
if(!iter->second.isNull())
@@ -3465,7 +3517,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr MainWindow::createAssembledCloud()
if(inserted)
{
_initProgressDialog->appendText(tr("Generated cloud %1 (%2/%3).").arg(iter->first).arg(++i).arg(_currentPosesMap.size()));
_initProgressDialog->appendText(tr("Generated cloud %1 (%2/%3).").arg(iter->first).arg(++i).arg(poses.size()));
if(count % 100 == 0)
{
@@ -3477,7 +3529,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr MainWindow::createAssembledCloud()
}
else
{
_initProgressDialog->appendText(tr("Ignored cloud %1 (%2/%3).").arg(iter->first).arg(++i).arg(_currentPosesMap.size()));
_initProgressDialog->appendText(tr("Ignored cloud %1 (%2/%3).").arg(iter->first).arg(++i).arg(poses.size()));
}
_initProgressDialog->incrementStep();
QApplication::processEvents();
@@ -3491,11 +3543,11 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr MainWindow::createAssembledCloud()
return assembledCloud;
}
std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr > MainWindow::createPointClouds()
std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr > MainWindow::createPointClouds(const std::map<int, Transform> & poses) const
{
std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr> clouds;
int i=0;
for(std::map<int, Transform>::const_iterator iter = _currentPosesMap.begin(); iter!=_currentPosesMap.end(); ++iter)
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
bool inserted = false;
if(!iter->second.isNull())
@@ -3545,11 +3597,11 @@ std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr > MainWindow::createPointCl
if(inserted)
{
_initProgressDialog->appendText(tr("Generated cloud %1 (%2/%3).").arg(iter->first).arg(++i).arg(_currentPosesMap.size()));
_initProgressDialog->appendText(tr("Generated cloud %1 (%2/%3).").arg(iter->first).arg(++i).arg(poses.size()));
}
else
{
_initProgressDialog->appendText(tr("Ignored cloud %1 (%2/%3).").arg(iter->first).arg(++i).arg(_currentPosesMap.size()));
_initProgressDialog->appendText(tr("Ignored cloud %1 (%2/%3).").arg(iter->first).arg(++i).arg(poses.size()));
}
_initProgressDialog->incrementStep();
QApplication::processEvents();
@@ -3558,11 +3610,11 @@ std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr > MainWindow::createPointCl
return clouds;
}
std::map<int, pcl::PolygonMesh::Ptr> MainWindow::createMeshes()
std::map<int, pcl::PolygonMesh::Ptr> MainWindow::createMeshes(const std::map<int, Transform> & poses) const
{
std::map<int, pcl::PolygonMesh::Ptr> meshes;
int i=0;
for(std::map<int, Transform>::const_iterator iter = _currentPosesMap.begin(); iter!=_currentPosesMap.end(); ++iter)
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
bool inserted = false;
if(!iter->second.isNull())
@@ -3619,11 +3671,11 @@ std::map<int, pcl::PolygonMesh::Ptr> MainWindow::createMeshes()
if(inserted)
{
_initProgressDialog->appendText(tr("Generated mesh %1 (%2/%3).").arg(iter->first).arg(++i).arg(_currentPosesMap.size()));
_initProgressDialog->appendText(tr("Generated mesh %1 (%2/%3).").arg(iter->first).arg(++i).arg(poses.size()));
}
else
{
_initProgressDialog->appendText(tr("Ignored mesh %1 (%2/%3).").arg(iter->first).arg(++i).arg(_currentPosesMap.size()));
_initProgressDialog->appendText(tr("Ignored mesh %1 (%2/%3).").arg(iter->first).arg(++i).arg(poses.size()));
}
_initProgressDialog->incrementStep();
QApplication::processEvents();

20
guilib/src/MapVisibilityWidget.cpp
View File

@@ -57,7 +57,7 @@ void MapVisibilityWidget::updateCheckBoxes()
added = true;
}
checkboxes[i]->setText(QString("%1 (%2)").arg(iter->first).arg(iter->second.prettyPrint().c_str()));
checkboxes[i]->setChecked(true);
checkboxes[i]->setChecked(_mask.at(iter->first));
if(added)
{
connect(checkboxes[i], SIGNAL(stateChanged(int)), this, SLOT(signalVisibility()));
@@ -68,18 +68,34 @@ void MapVisibilityWidget::updateCheckBoxes()
}
}
void MapVisibilityWidget::setMap(const std::map<int, Transform> & poses)
void MapVisibilityWidget::setMap(const std::map<int, Transform> & poses, const std::map<int, bool> & mask)
{
UASSERT(poses.size() == mask.size());
_poses = poses;
_mask = mask;
if(this->isVisible())
{
updateCheckBoxes();
}
}
std::map<int, Transform> MapVisibilityWidget::getVisiblePoses() const
{
std::map<int, Transform> poses;
for(std::map<int, Transform>::const_iterator iter=_poses.begin(); iter!=_poses.end(); ++iter)
{
if(_mask.at(iter->first))
{
poses.insert(*iter);
}
}
return poses;
}
void MapVisibilityWidget::signalVisibility()
{
QCheckBox * check = qobject_cast<QCheckBox*>(sender());
_mask.at(check->text().split('(').first().toInt()) = check->isChecked();
emit visibilityChanged(check->text().split('(').first().toInt(), check->isChecked());
}

4
guilib/src/MapVisibilityWidget.h
View File

@@ -20,7 +20,8 @@ public:
MapVisibilityWidget(QWidget * parent = 0);
virtual ~MapVisibilityWidget();
void setMap(const std::map<int, Transform> & poses);
void setMap(const std::map<int, Transform> & poses, const std::map<int, bool> & mask);
std::map<int, Transform> getVisiblePoses() const;
protected:
virtual void showEvent(QShowEvent * event);
@@ -36,6 +37,7 @@ signals:
private:
std::map<int, Transform> _poses;
std::map<int, bool> _mask;
};
} /* namespace rtabmap */

21
guilib/src/PreferencesDialog.cpp
View File

@@ -2324,12 +2324,21 @@ void PreferencesDialog::updateKpROI()
void PreferencesDialog::changeDatabasePath()
{
QString path = QFileDialog::getSaveFileName(
this,
tr("Select database file..."),
_ui->lineEdit_databasePath->text(),
tr("RTAB-Map database files (*.db)"),
0);
QFileDialog fileDialog;
fileDialog.setFileMode(QFileDialog::AnyFile);
fileDialog.setNameFilter(tr("RTAB-Map database files (*.db)"));
fileDialog.setDefaultSuffix("db");
fileDialog.setDirectory(QFileInfo(_ui->lineEdit_databasePath->text()).dir());
fileDialog.selectFile (_ui->lineEdit_databasePath->text());
QStringList fileNames;
QString path;
if(fileDialog.exec())
{
if(!fileDialog.selectedFiles().empty())
{
path = fileDialog.selectedFiles().first();
}
}
if(!path.isEmpty())
{

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

@@ -6,8 +6,8 @@
<rect>
<x>0</x>
<y>0</y>
<width>1007</width>
<height>494</height>
<width>876</width>
<height>526</height>
</rect>
</property>
<property name="sizePolicy">
@@ -64,8 +64,8 @@
<rect>
<x>0</x>
<y>0</y>
<width>709</width>
<height>752</height>
<width>577</width>
<height>632</height>
</rect>
</property>
<layout class="QVBoxLayout" name="verticalLayout_16">
@@ -86,7 +86,7 @@
<enum>QFrame::Raised</enum>
</property>
<property name="currentIndex">
<number>9</number>
<number>0</number>
</property>
<widget class="QWidget" name="page_22">
<layout class="QVBoxLayout" name="verticalLayout_29">
@@ -338,6 +338,78 @@ Show a yellow background when the number of odometry inliers goes under this thr
<string>3D Rendering</string>
</property>
<layout class="QVBoxLayout" name="verticalLayout_31">
<item>
<widget class="QGroupBox" name="groupBox_poseFiltering">
<property name="title">
<string>Cloud filtering</string>
</property>
<property name="checkable">
<bool>true</bool>
</property>
<property name="checked">
<bool>false</bool>
</property>
<layout class="QVBoxLayout" name="verticalLayout_37">
<item>
<widget class="QLabel" name="label_33">
<property name="text">
<string>For visualization, superposed clouds are not shown. By comparing poses in the same area, only one cloud in a fixed radius and angle is kept.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item>
<layout class="QFormLayout" name="formLayout_22">
<item row="0" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_cloudFilterRadius">
<property name="suffix">
<string> m</string>
</property>
<property name="minimum">
<double>0.010000000000000</double>
</property>
<property name="value">
<double>0.500000000000000</double>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLabel" name="label_37">
<property name="text">
<string>Radius</string>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="label_48">
<property name="text">
<string>Angle</string>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_cloudFilterAngle">
<property name="suffix">
<string> degrees</string>
</property>
<property name="decimals">
<number>0</number>
</property>
<property name="maximum">
<double>180.000000000000000</double>
</property>
<property name="value">
<double>30.000000000000000</double>
</property>
</widget>
</item>
</layout>
</item>
</layout>
</widget>
</item>
<item>
<widget class="QFrame" name="frame">
<property name="frameShape">
@@ -979,78 +1051,6 @@ High-res view</string>
</layout>
</widget>
</item>
<item>
<widget class="QGroupBox" name="groupBox_poseFiltering">
<property name="title">
<string>Cloud filtering</string>
</property>
<property name="checkable">
<bool>true</bool>
</property>
<property name="checked">
<bool>false</bool>
</property>
<layout class="QVBoxLayout" name="verticalLayout_37">
<item>
<widget class="QLabel" name="label_33">
<property name="text">
<string>For visualization, superposed clouds are not shown. By comparing poses in the same area, only one cloud in a fixed radius and angle is kept.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item>
<layout class="QFormLayout" name="formLayout_22">
<item row="0" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_cloudFilterRadius">
<property name="suffix">
<string> m</string>
</property>
<property name="minimum">
<double>0.010000000000000</double>
</property>
<property name="value">
<double>0.500000000000000</double>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLabel" name="label_37">
<property name="text">
<string>Radius</string>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="label_48">
<property name="text">
<string>Angle</string>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_cloudFilterAngle">
<property name="suffix">
<string> degrees</string>
</property>
<property name="decimals">
<number>0</number>
</property>
<property name="maximum">
<double>180.000000000000000</double>
</property>
<property name="value">
<double>30.000000000000000</double>
</property>
</widget>
</item>
</layout>
</item>
</layout>
</widget>
</item>
</layout>
</widget>
</item>