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

-Added the grid map in the 3D map view (pcl::TextureMesh, available only when pcl>=1.7.2). Refactored how 2d maps are created.

Browse Source 
-Fixed export grid map on Mac OS X.


git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1623 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2014-07-29 21:48:37 +00:00
parent a81f5785a2
commit 69ad085f88
16 changed files with 517 additions and 249 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -20,6 +20,8 @@
#include <QtCore/QSet>
#include <QtCore/qnamespace.h>
#include <opencv2/opencv.hpp>
#include <pcl/visualization/mouse_event.h>
#include <pcl/PCLPointCloud2.h>
@@ -92,6 +94,14 @@ public:
const pcl::PolygonMesh::Ptr & mesh,
const Transform & pose = Transform::getIdentity());
bool addOccupancyGridMap(
const cv::Mat & map8U,
float resolution, // cell size
float xMin,
float yMin,
float opacity);
void removeOccupancyGridMap();
void updateCameraPosition(
const Transform & pose);
@@ -111,6 +121,7 @@ public:
void setCameraFree();
void setCameraLockZ(bool enabled = true);
void setGridShown(bool shown);
void setWorkingDirectory(const QString & path) {_workingDirectory = path;}
public slots:
void render();
@@ -150,6 +161,7 @@ private:
Transform _lastPose;
std::list<std::string> _gridLines;
QSet<Qt::Key> _keysPressed;
QString _workingDirectory;
};
} /* namespace rtabmap */

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

@@ -30,6 +30,8 @@
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <set>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <rtabmap/core/Link.h>
@@ -92,7 +94,7 @@ private:
std::list<std::map<int, rtabmap::Transform> > graphes_;
std::map<int, rtabmap::Transform> poses_;
std::multimap<int, rtabmap::Link> links_;
QMap<int, std::vector<unsigned char> > scans_;
std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > scans_;
};
#endif /* DATABASEVIEWER_H_ */

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

@@ -176,7 +176,7 @@ signals:
private:
void update3DMapVisibility(bool cloudsShown, bool scansShown);
void updateMapCloud(const std::map<int, Transform> & poses, const Transform & pose);
void updateMapCloud(const std::map<int, Transform> & poses, const Transform & pose, const std::multimap<int, Link> & constraints);
void createAndAddCloudToMap(int nodeId, const Transform & pose);
void createAndAddScanToMap(int nodeId, const Transform & pose);
void drawKeypoints(const std::multimap<int, cv::KeyPoint> & refWords, const std::multimap<int, cv::KeyPoint> & loopWords);
@@ -248,6 +248,7 @@ private:
QMap<int, Transform> _localTransformsMap;
std::map<int, Transform> _currentPosesMap;
QMap<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr > _createdClouds;
std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > _createdScans;
Transform _odometryCorrection;
Transform _lastOdomPose;
bool _lastOdometryProcessed;

5
guilib/include/rtabmap/gui/PreferencesDialog.h
View File

@@ -134,6 +134,11 @@ public:
double getCloudFilteringRadius() const;
double getCloudFilteringAngle() const;
bool getGridMapShown() const;
double getGridMapResolution() const;
bool getGridMapFillEmptySpace() const;
double getGridMapOpacity() const;
QString getWorkingDirectory() const;
QString getDatabasePath() const;

3
guilib/include/rtabmap/gui/UCv2Qt.h
View File

@@ -65,6 +65,9 @@ inline QImage uCvMat2QImage(const cv::Mat & image, bool isBgr = true)
{
// mono grayscale
qtemp = QImage(image.data, image.cols, image.rows, image.cols, QImage::Format_Indexed8).copy();
QVector<QRgb> my_table;
for(int i = 0; i < 256; i++) my_table.push_back(qRgb(i,i,i));
qtemp.setColorTable(my_table);
}
else
{

82
guilib/src/CloudViewer.cpp
View File

@@ -54,7 +54,8 @@ CloudViewer::CloudViewer(QWidget *parent) :
_menu(0),
_trajectory(new pcl::PointCloud<pcl::PointXYZ>),
_maxTrajectorySize(100),
_lastPose(Transform::getIdentity())
_lastPose(Transform::getIdentity()),
_workingDirectory(".")
{
this->setMinimumSize(200, 200);
@@ -322,6 +323,80 @@ bool CloudViewer::addCloudMesh(
return false;
}
bool CloudViewer::addOccupancyGridMap(
const cv::Mat & map8U,
float resolution, // cell size
float xMin,
float yMin,
float opacity)
{
#if PCL_VERSION_COMPARE(>=, 1, 7, 2)
UASSERT(map8U.channels() == 1 && map8U.type() == CV_8U);
float xSize = float(map8U.cols) * resolution;
float ySize = float(map8U.rows) * resolution;
UDEBUG("resolution=%f, xSize=%f, ySize=%f, xMin=%f, yMin=%f", resolution, xSize, ySize, xMin, yMin);
if(_visualizer->getShapeActorMap()->find("map") == _visualizer->getShapeActorMap()->end())
{
_visualizer->removeShape("map");
}
if(xSize > 0.0f && ySize > 0.0f)
{
pcl::TextureMeshPtr mesh(new pcl::TextureMesh());
pcl::PointCloud<pcl::PointXYZ> cloud;
cloud.push_back(pcl::PointXYZ(xMin, yMin, 0));
cloud.push_back(pcl::PointXYZ(xSize+xMin, yMin, 0));
cloud.push_back(pcl::PointXYZ(xSize+xMin, ySize+yMin, 0));
cloud.push_back(pcl::PointXYZ(xMin, ySize+yMin, 0));
pcl::toPCLPointCloud2(cloud, mesh->cloud);
std::vector<pcl::Vertices> polygons(1);
polygons[0].vertices.push_back(0);
polygons[0].vertices.push_back(1);
polygons[0].vertices.push_back(2);
polygons[0].vertices.push_back(3);
polygons[0].vertices.push_back(0);
mesh->tex_polygons.push_back(polygons);
// default texture materials parameters
pcl::TexMaterial material;
// hack, can we read from memory?
std::string tmpPath = (_workingDirectory+"/.tmp_map.png").toStdString();
cv::imwrite(tmpPath, map8U);
material.tex_file = tmpPath;
mesh->tex_materials.push_back(material);
std::vector<Eigen::Vector2f> coordinates;
coordinates.push_back(Eigen::Vector2f(0,1));
coordinates.push_back(Eigen::Vector2f(1,1));
coordinates.push_back(Eigen::Vector2f(1,0));
coordinates.push_back(Eigen::Vector2f(0,0));
mesh->tex_coordinates.push_back(coordinates);
_visualizer->addTextureMesh(*mesh, "map");
_visualizer->getCloudActorMap()->find("map")->second.actor->GetProperty()->LightingOff();
setCloudOpacity("map", 0.7);
//removed tmp texture file
QFile::remove(tmpPath.c_str());
}
return true;
#else
// not implemented on lower version of PCL
return false;
#endif
}
void CloudViewer::removeOccupancyGridMap()
{
if(_visualizer->getShapeActorMap()->find("map") == _visualizer->getShapeActorMap()->end())
{
_visualizer->removeShape("map");
}
}
void CloudViewer::setTrajectoryShown(bool shown)
{
_aShowTrajectory->setChecked(shown);
@@ -448,8 +523,13 @@ void CloudViewer::updateCameraPosition(const Transform & pose)
cameras.front().view[2] = Fp[10];
}
#if PCL_VERSION_COMPARE(>=, 1, 7, 2)
_visualizer->removeCoordinateSystem("reference", 0);
_visualizer->addCoordinateSystem(0.2, m, "reference", 0);
#else
_visualizer->removeCoordinateSystem(0);
_visualizer->addCoordinateSystem(0.2, m, 0);
#endif
_visualizer->setCameraPosition(
cameras.front().pos[0], cameras.front().pos[1], cameras.front().pos[2],
cameras.front().focal[0], cameras.front().focal[1], cameras.front().focal[2],

14
guilib/src/DatabaseViewer.cpp
View File

@@ -914,13 +914,23 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
memory_->getImageDepth(ids_.at(i), imageBytes, depthBytes, depth2dBytes, fx, fy, cx, cy, localTransform);
if(depth2dBytes.size())
{
scans_.insert(ids_.at(i), depth2dBytes);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
cv::Mat depth2d = rtabmap::util3d::uncompressData(depth2dBytes);
cloud = rtabmap::util3d::depth2DToPointCloud(depth2d);
scans_.insert(std::make_pair(ids_.at(i), cloud));
}
}
UINFO("Update scans list... done");
}
std::map<int, rtabmap::Transform> & graph = uValueAt(graphes_, value);
ui_->graphViewer->updateGraph(graph, links_, scans_);
ui_->graphViewer->updateGraph(graph, links_);
if(graph.size() && scans_.size())
{
float xMin, yMin;
float cell = 0.05;
cv::Mat map = rtabmap::util3d::convertMap2Image8U(rtabmap::util3d::create2DMap(graph, scans_, cell, true, xMin, yMin));
ui_->graphViewer->updateMap(map, cell, xMin, yMin);
}
ui_->label_iterations->setNum(value);
//compute total length (neighbor links)

130
guilib/src/GraphViewer.cpp
View File

@@ -27,6 +27,7 @@
#include <rtabmap/core/util3d.h>
#include <rtabmap/gui/UCv2Qt.h>
#include <rtabmap/utilite/UStl.h>
#include <rtabmap/utilite/ULogger.h>
namespace rtabmap {
@@ -34,11 +35,10 @@ class NodeItem: public QGraphicsEllipseItem
{
public:
// in meter
NodeItem(int id, const Transform & pose, float radius, const cv::Mat & scan) :
NodeItem(int id, const Transform & pose, float radius) :
QGraphicsEllipseItem(QRectF(-radius,-radius,radius*2.0f,radius*2.0f)),
_id(id),
_pose(pose),
_scan(scan)
_pose(pose)
{
this->setPos(-pose.y(),-pose.x());
this->setBrush(pen().color());
@@ -56,8 +56,6 @@ public:
}
void setPose(const Transform & pose) {this->setPos(-pose.y(),-pose.x()); _pose=pose;}
void setScan(const cv::Mat & scan) {_scan = scan;}
const cv::Mat & getScan() const {return _scan;}
protected:
virtual void hoverEnterEvent ( QGraphicsSceneHoverEvent * event )
@@ -77,7 +75,6 @@ protected:
private:
int _id;
Transform _pose;
cv::Mat _scan;
};
class LinkItem: public QGraphicsLineItem
@@ -136,11 +133,8 @@ GraphViewer::GraphViewer(QWidget * parent) :
_linkWidth(0),
_gridMap(0),
_lastReferential(0),
_gridCellSize(0.05f),
_gridUnknownSpaceFilled(true)
_gridCellSize(0.0f)
{
Q_ASSERT(_gridCellSize > 0);
this->setScene(new QGraphicsScene(this));
this->setDragMode(QGraphicsView::ScrollHandDrag);
_workingDirectory = QDir::homePath();
@@ -170,8 +164,6 @@ GraphViewer::GraphViewer(QWidget * parent) :
_gridMap = this->scene()->addPixmap(QPixmap());
_gridMap->scale(_gridCellSize, -_gridCellSize);
_gridMap->setRotation(90);
_gridMap->setZValue(0);
_gridMap->setParentItem(_root);
}
@@ -181,8 +173,7 @@ GraphViewer::~GraphViewer()
}
void GraphViewer::updateGraph(const std::map<int, Transform> & poses,
const std::multimap<int, Link> & constraints,
const QMap<int, std::vector<unsigned char> > & scans)
const std::multimap<int, Link> & constraints)
{
//Hide nodes and links
for(QMap<int, NodeItem*>::iterator iter = _nodeItems.begin(); iter!=_nodeItems.end(); ++iter)
@@ -199,40 +190,19 @@ void GraphViewer::updateGraph(const std::map<int, Transform> & poses,
iter.value()->hide();
}
std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > scanClouds;
for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{
QMap<int, NodeItem*>::iterator itemIter = _nodeItems.find(iter->first);
if(itemIter != _nodeItems.end())
{
itemIter.value()->setPose(iter->second);
if(itemIter.value()->getScan().empty() && scans.contains(iter->first))
{
cv::Mat depth2d = util3d::uncompressData(scans.value(iter->first));
itemIter.value()->setScan(depth2d);
}
if(!itemIter.value()->getScan().empty() && _gridMap->isVisible())
{
scanClouds.insert(std::make_pair(iter->first, util3d::depth2DToPointCloud(itemIter.value()->getScan())));
}
itemIter.value()->show();
}
else
{
// create node item
QMap<int, std::vector<unsigned char> >::const_iterator jter = scans.find(iter->first);
cv::Mat depth2d;
if(jter != scans.end())
{
depth2d = util3d::uncompressData(jter.value());
if(_gridMap->isVisible())
{
scanClouds.insert(std::make_pair(iter->first, util3d::depth2DToPointCloud(depth2d)));
}
}
const Transform & pose = iter->second;
NodeItem * item = new NodeItem(iter->first, pose, _nodeRadius, depth2d);
NodeItem * item = new NodeItem(iter->first, pose, _nodeRadius);
this->scene()->addItem(item);
item->setZValue(2);
item->setColor(_nodeColor);
@@ -324,41 +294,6 @@ void GraphViewer::updateGraph(const std::map<int, Transform> & poses,
}
}
if(scanClouds.size())
{
float xMin=0.0f, yMin=0.0f;
cv::Mat map8S = util3d::create2DMap(poses, scanClouds, _gridCellSize, _gridUnknownSpaceFilled, xMin, yMin);
cv::Mat map8U(map8S.rows, map8S.cols, CV_8U);
//convert to gray scaled map
for (int i = 0; i < map8S.rows; ++i)
{
for (int j = 0; j < map8S.cols; ++j)
{
char v = map8S.at<char>(i, j);
unsigned char gray;
if(v == 0)
{
gray = 178;
}
else if(v == 100)
{
gray = 0;
}
else // -1
{
gray = 89;
}
map8U.at<unsigned char>(i, j) = gray;
}
}
QImage image = uCvMat2QImage(map8U, false);
_gridMap->resetTransform();
_gridMap->scale(_gridCellSize, -_gridCellSize);
_gridMap->setRotation(90);
_gridMap->setPixmap(QPixmap::fromImage(image));
_gridMap->setPos(-yMin, -xMin);
}
if(_nodeItems.size())
{
(--_nodeItems.end()).value()->setColor(Qt::green);
@@ -374,6 +309,25 @@ void GraphViewer::updateGraph(const std::map<int, Transform> & poses,
this->fitInView(this->scene()->itemsBoundingRect(), Qt::KeepAspectRatio);
}
void GraphViewer::updateMap(const cv::Mat & map8U, float resolution, float xMin, float yMin)
{
UASSERT(map8U.empty() || (!map8U.empty() && resolution > 0.0f));
if(!map8U.empty())
{
_gridCellSize = resolution;
QImage image = uCvMat2QImage(map8U, false);
_gridMap->resetTransform();
_gridMap->scale(resolution, -resolution);
_gridMap->setRotation(90);
_gridMap->setPixmap(QPixmap::fromImage(image));
_gridMap->setPos(-yMin, -xMin);
}
else
{
this->clearMap();
}
}
void GraphViewer::clearGraph()
{
qDeleteAll(_nodeItems);
@@ -382,11 +336,21 @@ void GraphViewer::clearGraph()
_neighborLinkItems.clear();
qDeleteAll(_loopLinkItems);
_loopLinkItems.clear();
_gridMap->setPixmap(QPixmap());
_lastReferential->resetTransform();
this->scene()->setSceneRect(this->scene()->itemsBoundingRect()); // Re-shrink the scene to it's bounding contents
}
void GraphViewer::clearMap()
{
_gridMap->setPixmap(QPixmap());
}
void GraphViewer::clearAll()
{
clearMap();
clearGraph();
}
void GraphViewer::wheelEvent ( QWheelEvent * event )
{
if(event->delta() > 0)
@@ -412,10 +376,6 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
QAction * aSetNodeSize = menu.addAction(tr("Set node radius..."));
QAction * aSetLinkSize = menu.addAction(tr("Set link width..."));
menu.addSeparator();
QAction * aSetGridCellSize = menu.addAction(tr("Set grid cell size..."));
QAction * aSetGridUnknownSpaceFilled = menu.addAction(tr("Unknown grid space filled"));
aSetGridUnknownSpaceFilled->setCheckable(true);
aSetGridUnknownSpaceFilled->setChecked(_gridUnknownSpaceFilled);
QAction * aShowHideGridMap;
if(_gridMap->isVisible())
{
@@ -448,7 +408,10 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
QString name = (QDateTime::currentDateTime().toString("yyMMddhhmmsszzz") + (isPNG?".png":".svg"));
//_root->setScale(this->transform().m11()); // current view
_root->setScale(1.0f/_gridCellSize); // grid map precision (for 5cm grid cell, x20 to have 1pix/5cm)
if(_gridCellSize)
{
_root->setScale(1.0f/_gridCellSize); // grid map precision (for 5cm grid cell, x20 to have 1pix/5cm)
}
this->scene()->clearSelection(); // Selections would also render to the file
this->scene()->setSceneRect(this->scene()->itemsBoundingRect()); // Re-shrink the scene to it's bounding contents
@@ -575,19 +538,6 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
}
}
}
else if(r == aSetGridCellSize)
{
bool ok;
double value = QInputDialog::getDouble(this, tr("Grid cell size"), tr("Width (m)"), _gridCellSize, 0.01, 10, 2, &ok);
if(ok)
{
_gridCellSize = value;
}
}
else if(r == aSetGridUnknownSpaceFilled)
{
_gridUnknownSpaceFilled = aSetGridUnknownSpaceFilled->isChecked();
}
else if(r == aShowHideGridMap)
{
_gridMap->setVisible(!_gridMap->isVisible());

9
guilib/src/GraphViewer.h
View File

@@ -11,6 +11,7 @@
#include <QtGui/QGraphicsView>
#include <QtCore/QMap>
#include <rtabmap/core/Link.h>
#include <opencv2/opencv.hpp>
#include <map>
class QGraphicsItem;
@@ -28,12 +29,13 @@ public:
virtual ~GraphViewer();
void updateGraph(const std::map<int, Transform> & poses,
const std::multimap<int, Link> & constraints,
const QMap<int, std::vector<unsigned char> > & scans);
const std::multimap<int, Link> & constraints);
void updateMap(const cv::Mat & map8U, float resolution, float xMin, float yMin);
void clearGraph();
void clearMap();
void clearAll();
void setWorkingDirectory(const QString & path) {_workingDirectory = path;}
void setGridCellSize(float size) {Q_ASSERT(_gridCellSize>0.0f); _gridCellSize= size;}
protected:
virtual void wheelEvent ( QWheelEvent * event );
@@ -53,7 +55,6 @@ private:
QGraphicsPixmapItem * _gridMap;
QGraphicsItemGroup * _lastReferential;
float _gridCellSize;
bool _gridUnknownSpaceFilled;
};
} /* namespace rtabmap */

117
guilib/src/MainWindow.cpp
View File

@@ -342,6 +342,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
_ui->statsToolBox->setWorkingDirectory(_preferencesDialog->getWorkingDirectory());
_ui->graphicsView_graphView->setWorkingDirectory(_preferencesDialog->getWorkingDirectory());
_ui->widget_cloudViewer->setWorkingDirectory(_preferencesDialog->getWorkingDirectory());
splash.close();
@@ -953,27 +954,7 @@ 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());
// update some widgets
if(_ui->graphicsView_graphView->isVisible())
{
std::map<int, Transform> poses;
if(_preferencesDialog->isCloudFiltering() && stat.poses().size())
{
float radius = _preferencesDialog->getCloudFilteringRadius();
float angle = _preferencesDialog->getCloudFilteringAngle()*CV_PI/180.0; // convert to rad
poses = util3d::radiusPosesFiltering(stat.poses(), radius, angle);
// make sure the last is here
poses.insert(*stat.poses().rbegin());
}
else
{
poses = stat.poses();
}
_ui->graphicsView_graphView->updateGraph(poses, stat.constraints(), _depths2DMap);
}
updateMapCloud(stat.poses(), _odometryReceived?Transform():stat.currentPose(), stat.constraints());
_odometryReceived = false;
@@ -1055,7 +1036,10 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
_processingStatistics = false;
}
void MainWindow::updateMapCloud(const std::map<int, Transform> & posesIn, const Transform & currentPose)
void MainWindow::updateMapCloud(
const std::map<int, Transform> & posesIn,
const Transform & currentPose,
const std::multimap<int, Link> & constraints)
{
if(posesIn.size())
{
@@ -1140,7 +1124,7 @@ void MainWindow::updateMapCloud(const std::map<int, Transform> & posesIn, const
// 2d point cloud
std::string scanName = uFormat("scan%d", iter->first);
if(_preferencesDialog->isScansShown(0))
if(_preferencesDialog->isScansShown(0) || _ui->graphicsView_graphView->isVisible() || _preferencesDialog->getGridMapShown())
{
if(viewerClouds.contains(scanName))
{
@@ -1162,6 +1146,11 @@ void MainWindow::updateMapCloud(const std::map<int, Transform> & posesIn, const
{
this->createAndAddScanToMap(iter->first, iter->second);
}
if(!_preferencesDialog->isScansShown(0))
{
UDEBUG("Hide scan %s", scanName.c_str());
_ui->widget_cloudViewer->setCloudVisibility(scanName.c_str(), false);
}
}
else if(viewerClouds.contains(scanName))
{
@@ -1192,6 +1181,39 @@ void MainWindow::updateMapCloud(const std::map<int, Transform> & posesIn, const
}
}
// Update occupancy grid map in 3D map view and graph view
if(_ui->graphicsView_graphView->isVisible() && constraints.size())
{
_ui->graphicsView_graphView->updateGraph(poses, constraints);
}
cv::Mat map8U;
if((_ui->graphicsView_graphView->isVisible() || _preferencesDialog->getGridMapShown()) && _depths2DMap.size())
{
float xMin, yMin;
float resolution = _preferencesDialog->getGridMapResolution();
bool fillEmptySpace = _preferencesDialog->getGridMapFillEmptySpace();
cv::Mat map8S = util3d::create2DMap(poses, _createdScans, resolution, fillEmptySpace, xMin, yMin);
if(!map8S.empty())
{
//convert to gray scaled map
map8U = util3d::convertMap2Image8U(map8S);
if(_preferencesDialog->getGridMapShown())
{
float opacity = _preferencesDialog->getGridMapOpacity();
_ui->widget_cloudViewer->addOccupancyGridMap(map8U, resolution, xMin, yMin, opacity);
}
if(_ui->graphicsView_graphView->isVisible())
{
_ui->graphicsView_graphView->updateMap(map8U, resolution, xMin, yMin);
}
}
}
if(!_preferencesDialog->getGridMapShown())
{
_ui->widget_cloudViewer->removeOccupancyGridMap();
}
if(viewerClouds.contains("cloudOdom"))
{
if(!_preferencesDialog->isCloudsShown(1))
@@ -1318,6 +1340,10 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose)
{
UERROR("Adding cloud %d to viewer failed!", nodeId);
}
else
{
_createdScans.insert(std::make_pair(nodeId, cloud));
}
_ui->widget_cloudViewer->setCloudOpacity(scanName, _preferencesDialog->getScanOpacity(0));
_ui->widget_cloudViewer->setCloudPointSize(scanName, _preferencesDialog->getScanPointSize(0));
}
@@ -1502,19 +1528,8 @@ void MainWindow::processRtabmapEvent3DMap(const rtabmap::RtabmapEvent3DMap & eve
_initProgressDialog->appendText("Updating the 3D map cloud...");
_initProgressDialog->incrementStep();
QApplication::processEvents();
this->updateMapCloud(event.getPoses(), Transform());
this->updateMapCloud(event.getPoses(), Transform(), event.getConstraints());
_initProgressDialog->appendText("Updating the 3D map cloud... done.");
if(_ui->graphicsView_graphView->isVisible())
{
_initProgressDialog->appendText("Updating the graph view...");
_initProgressDialog->incrementStep();
_ui->graphicsView_graphView->updateGraph(
event.getPoses(),
event.getConstraints(),
_depths2DMap);
_initProgressDialog->appendText("Updating the graph view... done.");
}
}
else
{
@@ -1590,7 +1605,7 @@ void MainWindow::applyPrefSettings(PreferencesDialog::PANEL_FLAGS flags)
UDEBUG("Cloud rendering settings changed...");
if(_currentPosesMap.size())
{
this->updateMapCloud(_currentPosesMap, Transform());
this->updateMapCloud(std::map<int, Transform>(_currentPosesMap), Transform(), std::multimap<int, Link>());
}
}
@@ -2682,6 +2697,7 @@ void MainWindow::clearTheCache()
_depthCysMap.clear();
_localTransformsMap.clear();
_createdClouds.clear();
_createdScans.clear();
_ui->widget_cloudViewer->removeAllClouds();
_ui->widget_cloudViewer->render();
_currentPosesMap.clear();
@@ -2703,7 +2719,7 @@ void MainWindow::clearTheCache()
_ui->label_stats_loopClosuresRejected->setText("0");
_refIds.clear();
_loopClosureIds.clear();
_ui->graphicsView_graphView->clearGraph();
_ui->graphicsView_graphView->clearAll();
}
void MainWindow::updateElapsedTime()
@@ -2942,12 +2958,22 @@ void MainWindow::exportGridMap()
map8U.at<unsigned char>(i, j) = gray;
}
}
QImage image = uCvMat2QImage(map8U, false);
QString path = QFileDialog::getSaveFileName(this, tr("Save to ..."), "grid.png", tr("Image (*.bmp *.png)"));
QString path = QFileDialog::getSaveFileName(this, tr("Save to ..."), "grid.png", tr("Image (*.png *.bmp)"));
if(!path.isEmpty())
{
QPixmap::fromImage(image.mirrored(false, true).transformed(QTransform().rotate(-90))).save(path);
if(QFileInfo(path).suffix() != "png" && QFileInfo(path).suffix() != "bmp")
{
//use png by default
path += ".png";
}
QImage img = image.mirrored(false, true).transformed(QTransform().rotate(-90));
QPixmap::fromImage(img).save(path);
QDesktopServices::openUrl(QUrl::fromLocalFile(path));
}
}
}
@@ -3032,17 +3058,16 @@ bool MainWindow::getExportedScans(std::map<int, pcl::PointCloud<pcl::PointXYZ>::
_initProgressDialog->setAutoClose(true, 1);
_initProgressDialog->resetProgress();
_initProgressDialog->show();
_initProgressDialog->setMaximumSteps(int(poses.size())*assemble?1:2+1);
_initProgressDialog->setMaximumSteps(int(poses.size())*(assemble?1:2)+1);
int count = 1;
int i = 0;
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
bool inserted = false;
if(_depths2DMap.contains(iter->first))
if(_createdScans.find(iter->first) != _createdScans.end())
{
cv::Mat depth2d = util3d::uncompressData(_depths2DMap.value(iter->first));
pcl::PointCloud<pcl::PointXYZ>::Ptr scan = util3d::depth2DToPointCloud(depth2d);
pcl::PointCloud<pcl::PointXYZ>::Ptr scan = _createdScans.at(iter->first);
if(scan->size())
{
if(assemble)
@@ -3316,7 +3341,7 @@ void MainWindow::saveClouds(const std::map<int, pcl::PointCloud<pcl::PointXYZRGB
{
if(clouds.size() == 1)
{
QString path = QFileDialog::getSaveFileName(this, tr("Save to ..."), "cloud.ply", tr("Point cloud data (*.ply *.pcd)"));
QString path = QFileDialog::getSaveFileName(this, tr("Save to ..."), _preferencesDialog->getWorkingDirectory()+"/cloud.ply", tr("Point cloud data (*.ply *.pcd)"));
if(!path.isEmpty())
{
if(clouds.begin()->second->size())
@@ -3424,7 +3449,7 @@ void MainWindow::saveMeshes(const std::map<int, pcl::PolygonMesh::Ptr> & meshes)
{
if(meshes.size() == 1)
{
QString path = QFileDialog::getSaveFileName(this, tr("Save to ..."), "mesh.ply", tr("Mesh (*.ply)"));
QString path = QFileDialog::getSaveFileName(this, tr("Save to ..."), _preferencesDialog->getWorkingDirectory()+"/mesh.ply", tr("Mesh (*.ply)"));
if(!path.isEmpty())
{
if(meshes.begin()->second->polygons.size())
@@ -3523,7 +3548,7 @@ void MainWindow::saveScans(const std::map<int, pcl::PointCloud<pcl::PointXYZ>::P
{
if(scans.size() == 1)
{
QString path = QFileDialog::getSaveFileName(this, tr("Save to ..."), "scan.ply", tr("Point cloud data (*.ply *.pcd)"));
QString path = QFileDialog::getSaveFileName(this, tr("Save to ..."), _preferencesDialog->getWorkingDirectory()+"/scan.ply", tr("Point cloud data (*.ply *.pcd)"));
if(!path.isEmpty())
{
if(scans.begin()->second->size())

42
guilib/src/PreferencesDialog.cpp
View File

@@ -96,6 +96,12 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->odom_bin_nn->removeItem(4);
}
#if PCL_VERSION_COMPARE(<, 1, 7, 2)
_ui->checkBox_map_shown->setChecked(false);
_ui->checkBox_map_shown->setEnabled(false);
_ui->label_map_shown->setText(_ui->label_map_shown->text() + " (Disabled, PCL >=1.7.2 required)");
#endif
_ui->predictionPlot->showLegend(false);
QButtonGroup * buttonGroup = new QButtonGroup(this);
@@ -180,6 +186,11 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
connect(_ui->doubleSpinBox_cloudFilterRadius, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
connect(_ui->doubleSpinBox_cloudFilterAngle, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
connect(_ui->checkBox_map_shown, SIGNAL(clicked(bool)), this, SLOT(makeObsoleteCloudRenderingPanel()));
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()));
//Logging panel
connect(_ui->comboBox_loggerLevel, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteLoggingPanel()));
connect(_ui->comboBox_loggerEventLevel, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteLoggingPanel()));
@@ -742,6 +753,11 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
_ui->groupBox_poseFiltering->setChecked(false);
_ui->doubleSpinBox_cloudFilterRadius->setValue(0.1);
_ui->doubleSpinBox_cloudFilterAngle->setValue(30);
_ui->checkBox_map_shown->setChecked(false);
_ui->doubleSpinBox_map_resolution->setValue(0.05);
_ui->checkBox_map_fillEmptySpace->setChecked(true);
_ui->doubleSpinBox_map_opacity->setValue(0.75);
}
else if(groupBox->objectName() == _ui->groupBox_logging1->objectName())
{
@@ -972,6 +988,11 @@ void PreferencesDialog::readGuiSettings(const QString & filePath)
_ui->doubleSpinBox_cloudFilterRadius->setValue(settings.value("cloudFilteringRadius", _ui->doubleSpinBox_cloudFilterRadius->value()).toDouble());
_ui->doubleSpinBox_cloudFilterAngle->setValue(settings.value("cloudFilteringAngle", _ui->doubleSpinBox_cloudFilterAngle->value()).toDouble());
_ui->checkBox_map_shown->setChecked(settings.value("gridMapShown", _ui->checkBox_map_shown->isChecked()).toBool());
_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->doubleSpinBox_map_opacity->setValue(settings.value("gridMapOpacity", _ui->doubleSpinBox_map_opacity->value()).toDouble());
settings.endGroup(); // General
settings.endGroup(); // rtabmap
@@ -1178,6 +1199,10 @@ void PreferencesDialog::writeGuiSettings(const QString & filePath)
settings.setValue("cloudFilteringRadius", _ui->doubleSpinBox_cloudFilterRadius->value());
settings.setValue("cloudFilteringAngle", _ui->doubleSpinBox_cloudFilterAngle->value());
settings.setValue("gridMapShown", _ui->checkBox_map_shown->isChecked());
settings.setValue("gridMapResolution", _ui->doubleSpinBox_map_resolution->value());
settings.setValue("gridMapFillEmptySpace", _ui->checkBox_map_fillEmptySpace->isChecked());
settings.setValue("gridMapOpacity", _ui->doubleSpinBox_map_opacity->value());
settings.endGroup(); // General
settings.endGroup(); // rtabmap
@@ -2467,6 +2492,23 @@ double PreferencesDialog::getCloudFilteringAngle() const
{
return _ui->doubleSpinBox_cloudFilterAngle->value();
}
bool PreferencesDialog::getGridMapShown() const
{
return _ui->checkBox_map_shown->isChecked();
}
double PreferencesDialog::getGridMapResolution() const
{
return _ui->doubleSpinBox_map_resolution->value();
}
bool PreferencesDialog::getGridMapFillEmptySpace() const
{
return _ui->checkBox_map_fillEmptySpace->isChecked();
}
double PreferencesDialog::getGridMapOpacity() const
{
return _ui->doubleSpinBox_map_opacity->value();
}
// Source
double PreferencesDialog::getGeneralInputRate() const

179
guilib/src/ui/exportCloudsDialog.ui
View File

@@ -7,7 +7,7 @@
<x>0</x>
<y>0</y>
<width>678</width>
<height>550</height>
<height>579</height>
</rect>
</property>
<property name="windowTitle">
@@ -22,91 +22,87 @@
<property name="checkable">
<bool>true</bool>
</property>
<layout class="QVBoxLayout" name="verticalLayout_3">
<item>
<layout class="QFormLayout" name="formLayout_2">
<item row="0" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_voxelSize">
<property name="suffix">
<string> m</string>
</property>
<property name="decimals">
<number>3</number>
</property>
<property name="maximum">
<double>1.000000000000000</double>
</property>
<property name="singleStep">
<double>0.010000000000000</double>
</property>
<property name="value">
<double>0.005000000000000</double>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLabel" name="label_107">
<property name="text">
<string>3D cloud voxel size.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QSpinBox" name="spinBox_decimation">
<property name="minimum">
<number>1</number>
</property>
<property name="maximum">
<number>32</number>
</property>
<property name="value">
<number>1</number>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="label_108">
<property name="text">
<string>3D cloud decimation (1-2-4-8-...).</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_maxDepth">
<property name="suffix">
<string> m</string>
</property>
<property name="decimals">
<number>1</number>
</property>
<property name="maximum">
<double>100.000000000000000</double>
</property>
<property name="singleStep">
<double>0.100000000000000</double>
</property>
<property name="value">
<double>4.000000000000000</double>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="label_132">
<property name="text">
<string>3D cloud maximum depth (0 means no limit).</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
</layout>
<layout class="QGridLayout" name="gridLayout" columnstretch="0,1">
<item row="0" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_voxelSize">
<property name="suffix">
<string> m</string>
</property>
<property name="decimals">
<number>3</number>
</property>
<property name="maximum">
<double>1.000000000000000</double>
</property>
<property name="singleStep">
<double>0.010000000000000</double>
</property>
<property name="value">
<double>0.005000000000000</double>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLabel" name="label_107">
<property name="text">
<string>3D cloud voxel size.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QSpinBox" name="spinBox_decimation">
<property name="minimum">
<number>1</number>
</property>
<property name="maximum">
<number>32</number>
</property>
<property name="value">
<number>1</number>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="label_108">
<property name="text">
<string>3D cloud decimation (1-2-4-8-...).</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_maxDepth">
<property name="suffix">
<string> m</string>
</property>
<property name="decimals">
<number>1</number>
</property>
<property name="maximum">
<double>100.000000000000000</double>
</property>
<property name="singleStep">
<double>0.100000000000000</double>
</property>
<property name="value">
<double>4.000000000000000</double>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="label_132">
<property name="text">
<string>3D cloud maximum depth (0 means no limit).</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
</layout>
</widget>
@@ -119,7 +115,7 @@
<property name="checkable">
<bool>true</bool>
</property>
<layout class="QFormLayout" name="formLayout_3">
<layout class="QGridLayout" name="gridLayout_2" columnstretch="0,1">
<item row="0" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_voxelSize_assembled">
<property name="suffix">
@@ -163,7 +159,7 @@
<property name="checked">
<bool>false</bool>
</property>
<layout class="QVBoxLayout" name="verticalLayout_4">
<layout class="QVBoxLayout" name="verticalLayout">
<item>
<widget class="QLabel" name="label">
<property name="text">
@@ -175,7 +171,7 @@
</widget>
</item>
<item>
<layout class="QFormLayout" name="formLayout_4">
<layout class="QGridLayout" name="gridLayout_3" columnstretch="0,1">
<item row="0" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_mlsRadius">
<property name="suffix">
@@ -222,10 +218,7 @@ Guidelines: 4 times the voxel size, 0.025 for voxel=0.</string>
<property name="checked">
<bool>false</bool>
</property>
<layout class="QFormLayout" name="formLayout">
<property name="fieldGrowthPolicy">
<enum>QFormLayout::AllNonFixedFieldsGrow</enum>
</property>
<layout class="QGridLayout" name="gridLayout_4" columnstretch="0,1">
<item row="0" column="0">
<widget class="QSpinBox" name="spinBox_normalKSearch">
<property name="minimum">

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

@@ -63,9 +63,9 @@
<property name="geometry">
<rect>
<x>0</x>
<y>-185</y>
<width>737</width>
<height>893</height>
<y>0</y>
<width>732</width>
<height>1198</height>
</rect>
</property>
<layout class="QVBoxLayout" name="verticalLayout_16">
@@ -361,7 +361,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
</widget>
</item>
<item>
<layout class="QFormLayout" name="formLayout_22">
<layout class="QGridLayout" name="gridLayout_17" rowstretch="0,0" columnstretch="0,1">
<item row="0" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_cloudFilterRadius">
<property name="suffix">
@@ -382,13 +382,6 @@ Show a yellow background when the number of odometry inliers goes under this thr
</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">
@@ -405,6 +398,122 @@ Show a yellow background when the number of odometry inliers goes under this thr
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="label_48">
<property name="text">
<string>Angle</string>
</property>
</widget>
</item>
</layout>
</item>
</layout>
</widget>
</item>
<item>
<widget class="QGroupBox" name="groupBox_gridMap">
<property name="title">
<string>Occupancy grid map</string>
</property>
<property name="checkable">
<bool>false</bool>
</property>
<property name="checked">
<bool>false</bool>
</property>
<layout class="QVBoxLayout" name="verticalLayout_45">
<item>
<widget class="QLabel" name="label_167">
<property name="text">
<string>When laser scans are used, a 2D grid map can be created. When the Graph view is visible or if the &quot;Show in 3D map view&quot; below is checked, the grid map is generated.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item>
<layout class="QGridLayout" name="gridLayout_20" columnstretch="0,1">
<item row="1" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_map_resolution">
<property name="suffix">
<string> m</string>
</property>
<property name="minimum">
<double>0.010000000000000</double>
</property>
<property name="maximum">
<double>1.000000000000000</double>
</property>
<property name="value">
<double>0.050000000000000</double>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="label_159">
<property name="text">
<string>Resolution (cell size)</string>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="label_164">
<property name="text">
<string>Fill empty space</string>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QCheckBox" name="checkBox_map_fillEmptySpace">
<property name="text">
<string/>
</property>
<property name="checked">
<bool>true</bool>
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_map_opacity">
<property name="suffix">
<string/>
</property>
<property name="minimum">
<double>0.750000000000000</double>
</property>
<property name="maximum">
<double>1.000000000000000</double>
</property>
<property name="value">
<double>0.750000000000000</double>
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QLabel" name="label_170">
<property name="text">
<string>Opacity</string>
</property>
</widget>
</item>
<item row="0" column="0">
<widget class="QCheckBox" name="checkBox_map_shown">
<property name="text">
<string/>
</property>
<property name="checked">
<bool>false</bool>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLabel" name="label_map_shown">
<property name="text">
<string>Show in 3D map view</string>
</property>
</widget>
</item>
</layout>
</item>
</layout>