i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-03 16:27:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

DatabaseViewer: Added ExportCloudsDialog, added Occupancy Grid View

Browse Source
This commit is contained in:
matlabbe
2017-03-31 18:56:52 -04:00
parent 622352b411
commit 2574f3a8ee
8 changed files with 892 additions and 522 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/OctoMap.cpp
+15 -17
View File
@@ -456,8 +456,8 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr OctoMap::createCloud(
(*cloud)[oi].g = g*255.0f;
(*cloud)[oi].b = b*255.0f;
}
(*cloud)[oi].x = pt.x();
(*cloud)[oi].y = pt.y();
(*cloud)[oi].x = pt.x()-octree_->getResolution()/2.0;
(*cloud)[oi].y = pt.y()-octree_->getResolution()/2.0;
(*cloud)[oi].z = pt.z();
if(obstacleIndices)
{
@@ -469,8 +469,8 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr OctoMap::createCloud(
{
octomap::point3d pt = octree_->keyToCoord(it.getKey());
(*cloud)[oi] = pcl::PointXYZRGB(it->getColor().r, it->getColor().g, it->getColor().b);
(*cloud)[oi].x = pt.x();
(*cloud)[oi].y = pt.y();
(*cloud)[oi].x = pt.x()-octree_->getResolution()/2.0f;
(*cloud)[oi].y = pt.y()-octree_->getResolution()/2.0f;
(*cloud)[oi].z = pt.z();
if(emptyIndices)
{
@@ -501,23 +501,21 @@ cv::Mat OctoMap::createProjectionMap(float & xMin, float & yMin, float & gridCel
pcl::PointCloud<pcl::PointXYZ>::Ptr ground(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr obstacles(new pcl::PointCloud<pcl::PointXYZ>);
ground->resize(occupiedCells_.size());
obstacles->resize(occupiedCells_.size());
ground->resize(octree_->size());
obstacles->resize(octree_->size());
int gi=0;
int oi=0;
for(std::map<octomap::ColorOcTreeNode*, OcTreeNodeInfo>::const_iterator iter = occupiedCells_.begin();
iter!=occupiedCells_.end();
++iter)
for (octomap::ColorOcTree::iterator it = octree_->begin(octree_->getTreeDepth()); it != octree_->end(); ++it)
{
if(iter->second.isObstacle_ && octree_->isNodeOccupied(iter->first))
if(octree_->isNodeOccupied(*it))
{
octomap::point3d pt = octree_->keyToCoord(iter->second.key_);
(*obstacles)[oi++] = pcl::PointXYZ(pt.x(), pt.y(), 0); // projected on ground
octomap::point3d pt = octree_->keyToCoord(it.getKey());
(*obstacles)[oi++] = pcl::PointXYZ(pt.x()-gridCellSize/2.0f, pt.y()-gridCellSize/2.0f, 0); // projected on ground
}
else if(!iter->second.isObstacle_)
else
{
octomap::point3d pt = octree_->keyToCoord(iter->second.key_);
(*ground)[gi++] = pcl::PointXYZ(pt.x(), pt.y(), 0); // projected on ground
octomap::point3d pt = octree_->keyToCoord(it.getKey());
(*ground)[gi++] = pcl::PointXYZ(pt.x()-gridCellSize/2.0f, pt.y()-gridCellSize/2.0f, 0); // projected on ground
}
}
obstacles->resize(oi);
@@ -525,11 +523,11 @@ cv::Mat OctoMap::createProjectionMap(float & xMin, float & yMin, float & gridCel
if(obstacles->size())
{
obstacles = util3d::voxelize(obstacles, gridCellSize);
obstacles = util3d::voxelize(obstacles, gridCellSize/2.0f);
}
if(ground->size())
{
ground = util3d::voxelize(ground, gridCellSize);
ground = util3d::voxelize(ground, gridCellSize/2.0f);
}
cv::Mat obstaclesMat = cv::Mat(1, (int)obstacles->size(), CV_32FC2);
corelib/src/util3d_mapping.cpp
+11 -9
View File
@@ -308,7 +308,9 @@ cv::Mat create2DMapFromOccupancyLocalMaps(
//Get map size
float margin = cellSize*10.0f;
xMin = minX-margin;
xMin -= cellSize/2.0f;
yMin = minY-margin;
yMin += cellSize/2.0f;
float xMax = maxX+margin;
float yMax = maxY+margin;
if(fabs((yMax - yMin) / cellSize) > 99999 ||
@@ -323,7 +325,7 @@ cv::Mat create2DMapFromOccupancyLocalMaps(
UDEBUG("map min=(%f, %f) max=(%f,%f)", xMin, yMin, xMax, yMax);
map = cv::Mat::ones((yMax - yMin) / cellSize + 0.5f, (xMax - xMin) / cellSize + 0.5f, CV_8S)*-1;
map = cv::Mat::ones((yMax - yMin) / cellSize, (xMax - xMin) / cellSize, CV_8S)*-1;
for(std::list<std::pair<int, Transform> >::const_iterator kter = poses.begin(); kter!=poses.end(); ++kter)
{
std::map<int, cv::Mat >::iterator iter = emptyLocalMaps.find(kter->first);
@@ -333,7 +335,7 @@ cv::Mat create2DMapFromOccupancyLocalMaps(
for(int i=0; i<iter->second.cols; ++i)
{
float * ptf = iter->second.ptr<float>(0, i);
cv::Point2i pt((ptf[0]-xMin)/cellSize + 0.5f, (ptf[1]-yMin)/cellSize + 0.5f);
cv::Point2i pt((ptf[0]-xMin)/cellSize, (ptf[1]-yMin)/cellSize);
char & value = map.at<char>(pt.y, pt.x);
if(value != -2)
{
@@ -345,8 +347,8 @@ cv::Mat create2DMapFromOccupancyLocalMaps(
if(footprintRadius >= cellSize*1.5f)
{
// place free space under the footprint of the robot
cv::Point2i ptBegin((kter->second.x()-footprintRadius-xMin)/cellSize + 0.5f, (kter->second.y()-footprintRadius-yMin)/cellSize + 0.5f);
cv::Point2i ptEnd((kter->second.x()+footprintRadius-xMin)/cellSize + 0.5f, (kter->second.y()+footprintRadius-yMin)/cellSize + 0.5f);
cv::Point2i ptBegin((kter->second.x()-footprintRadius-xMin)/cellSize, (kter->second.y()-footprintRadius-yMin)/cellSize);
cv::Point2i ptEnd((kter->second.x()+footprintRadius-xMin)/cellSize, (kter->second.y()+footprintRadius-yMin)/cellSize);
if(ptBegin.x < 0)
ptBegin.x = 0;
if(ptEnd.x >= map.cols)
@@ -370,7 +372,7 @@ cv::Mat create2DMapFromOccupancyLocalMaps(
for(int i=0; i<jter->second.cols; ++i)
{
float * ptf = jter->second.ptr<float>(0, i);
cv::Point2i pt((ptf[0]-xMin)/cellSize + 0.5f, (ptf[1]-yMin)/cellSize + 0.5f);
cv::Point2i pt((ptf[0]-xMin)/cellSize, (ptf[1]-yMin)/cellSize);
char & value = map.at<char>(pt.y, pt.x);
if(value != -2)
{
@@ -595,7 +597,7 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
UTimer timer;
map = cv::Mat::ones((yMax - yMin) / cellSize + 0.5f, (xMax - xMin) / cellSize + 0.5f, CV_8S)*-1;
map = cv::Mat::ones((yMax - yMin) / cellSize, (xMax - xMin) / cellSize, CV_8S)*-1;
int j=0;
for(std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr >::iterator iter = localScans.begin(); iter!=localScans.end(); ++iter)
{
@@ -606,7 +608,7 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
{
viewpoint = kter->second;
}
cv::Point2i start(((pose.x()+viewpoint.x)-xMin)/cellSize + 0.5f, ((pose.y()+viewpoint.y)-yMin)/cellSize + 0.5f);
cv::Point2i start(((pose.x()+viewpoint.x)-xMin)/cellSize, ((pose.y()+viewpoint.y)-yMin)/cellSize);
for(unsigned int i=0; i<iter->second->size(); ++i)
{
cv::Point2i end((iter->second->points[i].x-xMin)/cellSize, (iter->second->points[i].y-yMin)/cellSize);
@@ -638,7 +640,7 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
{
viewpoint = kter->second;
}
cv::Point2i start(((pose.x()+viewpoint.x)-xMin)/cellSize + 0.5f, ((pose.y()+viewpoint.y)-yMin)/cellSize + 0.5f);
cv::Point2i start(((pose.x()+viewpoint.x)-xMin)/cellSize, ((pose.y()+viewpoint.y)-yMin)/cellSize);
//UWARN("maxLength = %f", maxLength);
//rotate counterclockwise from the first point until we pass the last point
@@ -680,7 +682,7 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
angle = angle<-1.0f?-1.0f:angle>1.0f?1.0f:angle;
while(acos(angle) > M_PI_4 || endRotatedVector.cross(endLastVector).at<float>(2) > 0.0f)
{
cv::Point2i end((endRotated.at<float>(0)-xMin)/cellSize + 0.5f, (endRotated.at<float>(1)-yMin)/cellSize + 0.5f);
cv::Point2i end((endRotated.at<float>(0)-xMin)/cellSize, (endRotated.at<float>(1)-yMin)/cellSize);
//end must be inside the grid
end.x = end.x < 0?0:end.x;
end.x = end.x >= map.cols?map.cols-1:end.x;
guilib/include/rtabmap/gui/DatabaseViewer.h
+7
View File
@@ -54,6 +54,8 @@ class DBDriver;
class ImageView;
class SensorData;
class CloudViewer;
class OctoMap;
class ExportCloudsDialog;
class RTABMAPGUI_EXP DatabaseViewer : public QMainWindow
{
@@ -102,6 +104,7 @@ private slots:
void sliderLoopValueChanged(int);
void sliderIterationsValueChanged(int);
void updateGrid();
void updateOctomapView();
void updateGraphView();
void refineConstraint();
void addConstraint();
@@ -159,6 +162,7 @@ private:
CloudViewer * cloudViewerA_;
CloudViewer * cloudViewerB_;
CloudViewer * stereoViewer_;
CloudViewer * occupancyGridViewer_;
QList<int> ids_;
std::map<int, int> mapIds_;
QMap<int, int> idToIndex_;
@@ -176,8 +180,11 @@ private:
std::multimap<int, rtabmap::Link> linksAdded_;
std::multimap<int, rtabmap::Link> linksRemoved_;
std::map<int, std::pair<cv::Mat, cv::Mat> > localMaps_; // <ground, obstacles>
std::map<int, std::pair<float, cv::Point3f> > localMapsInfo_; // <cell size, viewpoint>
std::map<int, std::pair<cv::Mat, cv::Mat> > generatedLocalMaps_; // <ground, obstacles>
std::map<int, std::pair<float, cv::Point3f> > generatedLocalMapsInfo_; // <cell size, viewpoint>
OctoMap * octomap_;
ExportCloudsDialog * exportDialog_;
bool savedMaximized_;
bool firstCall_;
guilib/src/CloudViewer.cpp
+5 -1
View File
@@ -753,12 +753,16 @@ bool CloudViewer::addOctomap(const OctoMap * octomap, unsigned int treeDepth)
pcl::IndicesPtr obstacles(new std::vector<int>);
if(treeDepth > octomap->octree()->getTreeDepth())
if(treeDepth == 0 || treeDepth > octomap->octree()->getTreeDepth())
{
if(treeDepth>0)
{
UWARN("Tree depth requested (%d) is deeper than the "
"actual maximum tree depth of %d. Using maximum depth.",
(int)treeDepth, (int)octomap->octree()->getTreeDepth());
}
treeDepth = octomap->octree()->getTreeDepth();
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = octomap->createCloud(treeDepth, obstacles.get());
if(obstacles->size())
guilib/src/DatabaseViewer.cpp
+377 -246
View File
@@ -46,6 +46,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <opencv2/highgui/highgui.hpp>
#include <rtabmap/utilite/UTimer.h>
#include <rtabmap/utilite/UFile.h>
#include "rtabmap/utilite/UPlot.h"
#include "rtabmap/core/DBDriver.h"
#include "rtabmap/gui/KeypointItem.h"
#include "rtabmap/gui/CloudViewer.h"
@@ -68,6 +69,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/RegistrationIcp.h"
#include "rtabmap/core/OccupancyGrid.h"
#include "rtabmap/gui/DataRecorder.h"
#include "ExportCloudsDialog.h"
#include "rtabmap/core/SensorData.h"
#include "rtabmap/core/GainCompensator.h"
#include "ExportDialog.h"
@@ -80,11 +82,17 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <pcl/common/transforms.h>
#include <pcl/common/common.h>
#ifdef RTABMAP_OCTOMAP
#include "rtabmap/core/OctoMap.h"
#endif
namespace rtabmap {
DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) :
QMainWindow(parent),
dbDriver_(0),
octomap_(0),
exportDialog_(new ExportCloudsDialog(this)),
savedMaximized_(false),
firstCall_(true),
iniFilePath_(ini)
@@ -111,6 +119,7 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) :
ui_->dockWidget_constraints->setVisible(false);
ui_->dockWidget_graphView->setVisible(false);
ui_->dockWidget_occupancyGridView->setVisible(false);
ui_->dockWidget_guiparameters->setVisible(false);
ui_->dockWidget_coreparameters->setVisible(false);
ui_->dockWidget_info->setVisible(false);
@@ -123,20 +132,29 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) :
cloudViewerA_ = new CloudViewer(ui_->dockWidgetContents_3dviews);
cloudViewerB_ = new CloudViewer(ui_->dockWidgetContents_3dviews);
stereoViewer_ = new CloudViewer(ui_->dockWidgetContents_stereo);
occupancyGridViewer_ = new CloudViewer(ui_->dockWidgetContents_occupancyGrid);
constraintsViewer_->setObjectName("constraintsViewer");
cloudViewerA_->setObjectName("cloudViewerA");
cloudViewerB_->setObjectName("cloudViewerB");
stereoViewer_->setObjectName("stereoViewer");
occupancyGridViewer_->setObjectName("occupancyGridView");
ui_->layout_constraintsViewer->addWidget(constraintsViewer_);
ui_->horizontalLayout_3dviews->addWidget(cloudViewerA_, 1);
ui_->horizontalLayout_3dviews->addWidget(cloudViewerB_, 1);
ui_->horizontalLayout_stereo->addWidget(stereoViewer_, 1);
ui_->layout_occupancyGridView->addWidget(occupancyGridViewer_, 1);
constraintsViewer_->setCameraLockZ(false);
constraintsViewer_->setCameraFree();
occupancyGridViewer_->setCameraFree();
ui_->graphicsView_stereo->setAlpha(255);
#ifndef RTABMAP_OCTOMAP
ui_->checkBox_octomap->setEnabled(false);
ui_->checkBox_octomap->setChecked(false);
#endif
ParametersMap parameters;
uInsert(parameters, Parameters::getDefaultParameters("SURF"));
uInsert(parameters, Parameters::getDefaultParameters("SIFT"));
@@ -156,11 +174,16 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) :
uInsert(parameters, Parameters::getDefaultParameters("Grid"));
parameters.insert(*Parameters::getDefaultParameters().find(Parameters::kRGBDOptimizeMaxError()));
ui_->parameters_toolbox->setupUi(parameters);
exportDialog_->setObjectName("ExportCloudsDialog");
this->readSettings();
ui_->checkBox_grid_cubes->setVisible(ui_->checkBox_octomap->isChecked());
ui_->spinBox_grid_depth->setVisible(ui_->checkBox_octomap->isChecked());
ui_->checkBox_grid_empty->setVisible(ui_->checkBox_octomap->isChecked());
ui_->menuView->addAction(ui_->dockWidget_constraints->toggleViewAction());
ui_->menuView->addAction(ui_->dockWidget_graphView->toggleViewAction());
ui_->menuView->addAction(ui_->dockWidget_occupancyGridView->toggleViewAction());
ui_->menuView->addAction(ui_->dockWidget_stereoView->toggleViewAction());
ui_->menuView->addAction(ui_->dockWidget_view3d->toggleViewAction());
ui_->menuView->addAction(ui_->dockWidget_guiparameters->toggleViewAction());
@@ -168,6 +191,7 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) :
ui_->menuView->addAction(ui_->dockWidget_info->toggleViewAction());
ui_->menuView->addAction(ui_->dockWidget_statistics->toggleViewAction());
connect(ui_->dockWidget_graphView->toggleViewAction(), SIGNAL(triggered()), this, SLOT(updateGraphView()));
connect(ui_->dockWidget_occupancyGridView->toggleViewAction(), SIGNAL(triggered()), this, SLOT(updateGraphView()));
connect(ui_->parameters_toolbox, SIGNAL(parametersChanged(const QStringList &)), this, SLOT(notifyParametersChanged(const QStringList &)));
@@ -259,6 +283,11 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) :
connect(ui_->checkBox_ignoreUserLoop, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
connect(ui_->spinBox_optimizationDepth, SIGNAL(editingFinished()), this, SLOT(updateGraphView()));
connect(ui_->checkBox_gridErode, SIGNAL(stateChanged(int)), this, SLOT(updateGrid()));
connect(ui_->checkBox_octomap, SIGNAL(stateChanged(int)), this, SLOT(updateGrid()));
connect(ui_->checkBox_grid_2d, SIGNAL(stateChanged(int)), this, SLOT(updateGrid()));
connect(ui_->checkBox_grid_cubes, SIGNAL(stateChanged(int)), this, SLOT(updateOctomapView()));
connect(ui_->spinBox_grid_depth, SIGNAL(valueChanged(int)), this, SLOT(updateOctomapView()));
connect(ui_->checkBox_grid_empty, SIGNAL(stateChanged(int)), this, SLOT(updateOctomapView()));
connect(ui_->doubleSpinBox_gainCompensationRadius, SIGNAL(valueChanged(double)), this, SLOT(updateConstraintView()));
connect(ui_->groupBox_posefiltering, SIGNAL(clicked(bool)), this, SLOT(updateGraphView()));
connect(ui_->doubleSpinBox_posefilteringRadius, SIGNAL(editingFinished()), this, SLOT(updateGraphView()));
@@ -287,6 +316,7 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) :
connect(ui_->checkBox_ignoreUserLoop, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
connect(ui_->spinBox_optimizationDepth, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
connect(ui_->checkBox_gridErode, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
connect(ui_->checkBox_octomap, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
connect(ui_->doubleSpinBox_gainCompensationRadius, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
connect(ui_->doubleSpinBox_gridCellSize, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
connect(ui_->groupBox_posefiltering, SIGNAL(clicked(bool)), this, SLOT(configModified()));
@@ -302,6 +332,8 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) :
connect(ui_->doubleSpinBox_detectMore_angle, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
connect(ui_->spinBox_detectMore_iterations, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
connect(exportDialog_, SIGNAL(configChanged()), this, SLOT(configModified()));
// dockwidget
QList<QDockWidget*> dockWidgets = this->findChildren<QDockWidget*>();
for(int i=0; i<dockWidgets.size(); ++i)
@@ -311,6 +343,7 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) :
}
ui_->dockWidget_constraints->installEventFilter(this);
ui_->dockWidget_graphView->installEventFilter(this);
ui_->dockWidget_occupancyGridView->installEventFilter(this);
ui_->dockWidget_stereoView->installEventFilter(this);
ui_->dockWidget_view3d->installEventFilter(this);
ui_->dockWidget_guiparameters->installEventFilter(this);
@@ -326,6 +359,12 @@ DatabaseViewer::~DatabaseViewer()
{
delete dbDriver_;
}
#ifdef RTABMAP_OCTOMAP
if(octomap_)
{
delete octomap_;
}
#endif
}
void DatabaseViewer::setupMainLayout(int vertical)
@@ -400,7 +439,6 @@ void DatabaseViewer::readSettings()
ui_->checkBox_ignoreLocalLoopTime->setChecked(settings.value("ignoreLocalLoopTime", ui_->checkBox_ignoreLocalLoopTime->isChecked()).toBool());
ui_->checkBox_ignoreUserLoop->setChecked(settings.value("ignoreUserLoop", ui_->checkBox_ignoreUserLoop->isChecked()).toBool());
ui_->spinBox_optimizationDepth->setValue(settings.value("depth", ui_->spinBox_optimizationDepth->value()).toInt());
ui_->checkBox_gridErode->setChecked(settings.value("erode", ui_->checkBox_gridErode->isChecked()).toBool());
ui_->doubleSpinBox_gainCompensationRadius->setValue(settings.value("gainCompensationRadius", ui_->doubleSpinBox_gainCompensationRadius->value()).toDouble());
settings.endGroup();
@@ -409,6 +447,11 @@ void DatabaseViewer::readSettings()
ui_->groupBox_posefiltering->setChecked(settings.value("poseFiltering", ui_->groupBox_posefiltering->isChecked()).toBool());
ui_->doubleSpinBox_posefilteringRadius->setValue(settings.value("poseFilteringRadius", ui_->doubleSpinBox_posefilteringRadius->value()).toDouble());
ui_->doubleSpinBox_posefilteringAngle->setValue(settings.value("poseFilteringAngle", ui_->doubleSpinBox_posefilteringAngle->value()).toDouble());
ui_->checkBox_gridErode->setChecked(settings.value("erode", ui_->checkBox_gridErode->isChecked()).toBool());
if(ui_->checkBox_octomap->isEnabled())
{
ui_->checkBox_octomap->setChecked(settings.value("octomap", ui_->checkBox_octomap->isChecked()).toBool());
}
settings.endGroup();
settings.beginGroup("mesh");
@@ -440,6 +483,8 @@ void DatabaseViewer::readSettings()
settings.endGroup(); // DatabaseViewer
exportDialog_->loadSettings(settings);
ParametersMap parameters;
Parameters::readINI(path.toStdString(), parameters);
for(ParametersMap::iterator iter = parameters.begin(); iter!= parameters.end(); ++iter)
@@ -482,7 +527,6 @@ void DatabaseViewer::writeSettings()
//settings.setValue("strategy", ui_->comboBox_graphOptimizer->currentIndex());
//settings.setValue("slam2d", ui_->checkBox_2dslam->isChecked());
settings.setValue("depth", ui_->spinBox_optimizationDepth->value());
settings.setValue("erode", ui_->checkBox_gridErode->isChecked());
settings.setValue("gainCompensationRadius", ui_->doubleSpinBox_gainCompensationRadius->value());
settings.endGroup();
@@ -492,6 +536,8 @@ void DatabaseViewer::writeSettings()
settings.setValue("poseFiltering", ui_->groupBox_posefiltering->isChecked());
settings.setValue("poseFilteringRadius", ui_->doubleSpinBox_posefilteringRadius->value());
settings.setValue("poseFilteringAngle", ui_->doubleSpinBox_posefilteringAngle->value());
settings.setValue("erode", ui_->checkBox_gridErode->isChecked());
settings.setValue("octomap", ui_->checkBox_octomap->isChecked());
settings.endGroup();
settings.beginGroup("mesh");
@@ -524,6 +570,8 @@ void DatabaseViewer::writeSettings()
settings.endGroup(); // DatabaseViewer
exportDialog_->saveSettings(settings);
ParametersMap parameters = ui_->parameters_toolbox->getParameters();
for(ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end();)
{
@@ -573,9 +621,11 @@ bool DatabaseViewer::openDatabase(const QString & path)
linksRefined_.clear();
linksRemoved_.clear();
localMaps_.clear();
localMapsInfo_.clear();
generatedLocalMaps_.clear();
generatedLocalMapsInfo_.clear();
ui_->graphViewer->clearAll();
occupancyGridViewer_->clear();
ui_->actionGenerate_TORO_graph_graph->setEnabled(false);
ui_->actionGenerate_g2o_graph_g2o->setEnabled(false);
ui_->checkBox_showOptimized->setEnabled(false);
@@ -779,6 +829,7 @@ void DatabaseViewer::closeEvent(QCloseEvent* event)
generatedLocalMaps_.clear();
generatedLocalMapsInfo_.clear();
localMaps_.clear();
localMapsInfo_.clear();
}
if(button != QMessageBox::Yes && button != QMessageBox::No)
@@ -803,7 +854,7 @@ void DatabaseViewer::showEvent(QShowEvent* anEvent)
{
this->setWindowModified(false);
if(ui_->graphViewer->isVisible() && graphes_.size() && localMaps_.size()==0)
if((ui_->graphViewer->isVisible() || ui_->dockWidget_occupancyGridView->isVisible()) && graphes_.size() && localMaps_.size()==0)
{
sliderIterationsValueChanged((int)graphes_.size()-1);
}
@@ -1392,7 +1443,7 @@ void DatabaseViewer::updateIds()
if(ids_.size())
{
updateLoopClosuresSlider();
if(ui_->graphViewer->isVisible())
if(ui_->graphViewer->isVisible() || ui_->dockWidget_occupancyGridView->isVisible())
{
updateGraphView();
}
@@ -1597,7 +1648,6 @@ void DatabaseViewer::generateG2OGraph()
void DatabaseViewer::regenerateLocalMaps()
{
UTimer time;
OccupancyGrid grid(ui_->parameters_toolbox->getParameters());
generatedLocalMaps_.clear();
@@ -1607,11 +1657,24 @@ void DatabaseViewer::regenerateLocalMaps()
progressDialog.setMaximumSteps(ids_.size());
progressDialog.show();
UPlot * plot = new UPlot(this);
plot->setWindowFlags(Qt::Window);
plot->setWindowTitle("Local Occupancy Grid Generation Time (ms)");
plot->setAttribute(Qt::WA_DeleteOnClose);
UPlotCurve * decompressionCurve = plot->addCurve("Decompression");
UPlotCurve * gridCreationCurve = plot->addCurve("Grid Creation");
plot->show();
double decompressionTime = 0;
double gridCreationTime = 0;
for(int i =0; i<ids_.size(); ++i)
{
UTimer timer;
SensorData data;
dbDriver_->getNodeData(ids_.at(i), data);
data.uncompressData();
decompressionTime = timer.ticks()*1000.0;
int mapId, weight;
Transform odomPose, groundTruth;
@@ -1624,16 +1687,25 @@ void DatabaseViewer::regenerateLocalMaps()
s.setPose(odomPose);
cv::Mat ground, obstacles;
cv::Point3f viewpoint;
timer.ticks();
grid.createLocalMap(s, ground, obstacles, viewpoint);
gridCreationTime = timer.ticks()*1000.0;
uInsert(generatedLocalMaps_, std::make_pair(data.id(), std::make_pair(ground, obstacles)));
uInsert(generatedLocalMapsInfo_, std::make_pair(data.id(), std::make_pair(grid.getCellSize(), viewpoint)));
msg = QString("Generated local occupancy grid map %1/%2 (%3s)").arg(i+1).arg((int)ids_.size()).arg(time.ticks());
msg = QString("Generated local occupancy grid map %1/%2").arg(i+1).arg((int)ids_.size());
}
progressDialog.appendText(msg);
progressDialog.incrementStep();
decompressionCurve->addValue(ids_.at(i), decompressionTime);
gridCreationCurve->addValue(ids_.at(i), gridCreationTime);
if(ids_.size() < 50 || (i+1) % 25 == 0)
{
QApplication::processEvents();
}
}
progressDialog.setValue(progressDialog.maximumSteps());
updateGrid();
}
@@ -1709,27 +1781,7 @@ void DatabaseViewer::view3DMap()
return;
}
}
bool ok = false;
QStringList items;
items.append("1");
items.append("2");
items.append("4");
items.append("8");
items.append("16");
items.append("-2");
items.append("-4");
items.append("-8");
items.append("-16");
QString item = QInputDialog::getItem(this, tr("Decimation?"), tr("Image decimation. \n"
"Negative decimation is done from RGB size instead\n"
"of depth size (if depth is smaller than RGB, it may be\n"
"interpolated depending of the decimation value)."), items, 2, false, &ok);
if(ok)
{
int decimation = item.toInt();
double maxDepth = QInputDialog::getDouble(this, tr("Camera depth?"), tr("Maximum depth (m, 0=no max):"), 4.0, 0, 100, 2, &ok);
if(ok)
{
std::map<int, Transform> optimizedPoses = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
if(ui_->groupBox_posefiltering->isChecked())
{
@@ -1739,93 +1791,19 @@ void DatabaseViewer::view3DMap()
}
if(optimizedPoses.size() > 0)
{
rtabmap::ProgressDialog progressDialog(this);
progressDialog.setMaximumSteps((int)optimizedPoses.size());
progressDialog.show();
// create a window
QDialog * window = new QDialog(this, Qt::Window);
window->setModal(this->isModal());
window->setWindowTitle(tr("3D Map"));
window->setMinimumWidth(120);
window->setMinimumHeight(90);
window->resize(QDesktopWidget().availableGeometry(this).size() * 0.7);
rtabmap::CloudViewer * viewer = new rtabmap::CloudViewer(window);
QVBoxLayout *layout = new QVBoxLayout();
layout->addWidget(viewer);
layout->setContentsMargins(0,0,0,0);
viewer->setCameraLockZ(false);
window->setLayout(layout);
connect(window, SIGNAL(finished(int)), viewer, SLOT(clear()));
window->show();
int oi=0;
for(std::map<int, Transform>::const_iterator iter = optimizedPoses.begin(); iter!=optimizedPoses.end(); ++iter)
{
rtabmap::Transform pose = iter->second;
if(!pose.isNull())
{
SensorData data;
dbDriver_->getNodeData(iter->first, data);
data.uncompressData();
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
UASSERT(data.imageRaw().empty() || data.imageRaw().type()==CV_8UC3 || data.imageRaw().type() == CV_8UC1);
UASSERT(data.depthOrRightRaw().empty() || data.depthOrRightRaw().type()==CV_8UC1 || data.depthOrRightRaw().type() == CV_16UC1 || data.depthOrRightRaw().type() == CV_32FC1);
pcl::IndicesPtr indices(new std::vector<int>);
cloud = util3d::cloudRGBFromSensorData(data, decimation, maxDepth, 0, indices.get(), ui_->parameters_toolbox->getParameters());
oi++;
if(cloud->size())
{
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudWithNormals(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, indices);
pcl::concatenateFields(*cloud, *normals, *cloudWithNormals);
QColor color = Qt::red;
int mapId, weight;
Transform odomPose, groundTruth;
std::string label;
double stamp;
if(dbDriver_->getNodeInfo(iter->first, odomPose, mapId, weight, label, stamp, groundTruth))
{
color = (Qt::GlobalColor)(mapId % 12 + 7 );
}
viewer->addCloud(uFormat("cloud%d", iter->first), cloudWithNormals, pose, color);
UINFO("Generated %d (%d points)", iter->first, cloud->size());
if(optimizedPoses.size() > 150)
{
if(oi%50==0)
{
progressDialog.appendText(QString("Generated %1/%2").arg(oi).arg(optimizedPoses.size()));
}
}
else
{
progressDialog.appendText(QString("Generated %1 (%2 points)").arg(iter->first).arg(cloud->size()));
}
}
else
{
UINFO("Empty cloud %d", iter->first);
progressDialog.appendText(QString("Empty cloud %1").arg(iter->first));
}
progressDialog.incrementStep();
QApplication::processEvents();
}
}
progressDialog.setValue(progressDialog.maximumSteps());
exportDialog_->setDBDriver(dbDriver_);
exportDialog_->viewClouds(optimizedPoses,
updateLinksWithModifications(links_),
mapIds_,
QMap<int, Signature>(),
std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> >(),
pathDatabase_,
ui_->parameters_toolbox->getParameters());
}
else
{
QMessageBox::critical(this, tr("Error"), tr("No neighbors found for node %1.").arg(ui_->spinBox_optimizationsFrom->value()));
}
}
}
}
void DatabaseViewer::view3DLaserScans()
@@ -1957,48 +1935,6 @@ void DatabaseViewer::generate3DMap()
}
}
bool ok = false;
QStringList items;
items.append("1");
items.append("2");
items.append("4");
items.append("8");
items.append("16");
items.append("-2");
items.append("-4");
items.append("-8");
items.append("-16");
QString item = QInputDialog::getItem(this, tr("Decimation?"), tr("Image decimation.\n"
"Negative decimation is done from RGB size instead\n"
"of depth size (if depth is smaller than RGB, it may be\n"
"interpolated depending of the decimation value)."), items, 2, false, &ok);
if(ok)
{
int decimation = item.toInt();
double maxDepth = QInputDialog::getDouble(this, tr("Camera depth?"), tr("Maximum depth (m, 0=no max):"), 4.0, 0, 100, 2, &ok);
if(ok)
{
QMessageBox::StandardButton b = QMessageBox::question(
this,
tr("Assembling?"),
tr("Do you want to assemble all the point clouds (creating only one file with a density of 1pt/cm)?"),
QMessageBox::Yes|QMessageBox::No,
QMessageBox::Yes);
bool assemble = b == QMessageBox::Yes;
QString path;
if(assemble)
{
path = QFileDialog::getSaveFileName(this, tr("Save point cloud"),
pathDatabase_+QDir::separator()+"cloud.ply",
tr("Point Cloud (*.ply *.pcd)"));
}
else
{
path = QFileDialog::getExistingDirectory(this, tr("Save directory"), pathDatabase_);
}
if(!path.isEmpty())
{
std::map<int, Transform> optimizedPoses = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
if(ui_->groupBox_posefiltering->isChecked())
{
@@ -2008,96 +1944,19 @@ void DatabaseViewer::generate3DMap()
}
if(optimizedPoses.size() > 0)
{
rtabmap::ProgressDialog progressDialog;
progressDialog.setMaximumSteps((int)optimizedPoses.size());
progressDialog.show();
pcl::PointCloud<pcl::PointXYZRGB>::Ptr assembledCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
for(std::map<int, Transform>::const_iterator iter = optimizedPoses.begin(); iter!=optimizedPoses.end(); ++iter)
{
const rtabmap::Transform & pose = iter->second;
if(!pose.isNull())
{
SensorData data;
dbDriver_->getNodeData(iter->first, data);
data.uncompressData();
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
UASSERT(data.imageRaw().empty() || data.imageRaw().type()==CV_8UC3 || data.imageRaw().type() == CV_8UC1);
UASSERT(data.depthOrRightRaw().empty() || data.depthOrRightRaw().type()==CV_8UC1 || data.depthOrRightRaw().type() == CV_16UC1 || data.depthOrRightRaw().type() == CV_32FC1);
pcl::IndicesPtr validIndices(new std::vector<int>);
cloud = util3d::cloudRGBFromSensorData(data, decimation, maxDepth, 0, validIndices.get(), ui_->parameters_toolbox->getParameters());
if(assemble)
{
if(cloud->size())
{
cloud = util3d::voxelize(cloud, validIndices, 0.01);
if(cloud->size())
{
cloud = rtabmap::util3d::transformPointCloud(cloud, pose);
if(assembledCloud->size() == 0)
{
*assembledCloud = *cloud;
}
else
{
*assembledCloud += *cloud;
}
}
}
UINFO("Created cloud %d (%d points)", iter->first, (int)cloud->size());
progressDialog.appendText(QString("Created cloud %1 (%2 points)").arg(iter->first).arg(cloud->size()));
}
else
{
std::string name = uFormat("%s/node%d.pcd", path.toStdString().c_str(), iter->first);
if(cloud->size())
{
cloud = rtabmap::util3d::transformPointCloud(cloud, pose);
pcl::io::savePCDFile(name, *cloud);
UINFO("Saved %s (%d points)", name.c_str(), cloud->size());
progressDialog.appendText(QString("Saved %1 (%2 points)").arg(name.c_str()).arg(cloud->size()));
}
else
{
UINFO("Ignored empty cloud %s", name.c_str());
progressDialog.appendText(QString("Ignored empty cloud %1").arg(name.c_str()));
}
}
progressDialog.incrementStep();
QApplication::processEvents();
}
}
if(assemble && assembledCloud->size())
{
//voxelize by default to 1 cm
progressDialog.appendText(QString("Voxelize assembled cloud (%1 points)").arg(assembledCloud->size()));
QApplication::processEvents();
assembledCloud = util3d::voxelize(assembledCloud, 0.01);
if(QFileInfo(path).suffix() == "ply")
{
pcl::io::savePLYFile(path.toStdString(), *assembledCloud);
}
else
{
pcl::io::savePCDFile(path.toStdString(), *assembledCloud);
}
progressDialog.appendText(QString("Saved %1 (%2 points)").arg(path).arg(assembledCloud->size()));
QApplication::processEvents();
}
QMessageBox::information(this, tr("Finished"), tr("%1 clouds generated to %2.").arg(optimizedPoses.size()).arg(path));
progressDialog.setValue(progressDialog.maximumSteps());
exportDialog_->setDBDriver(dbDriver_);
exportDialog_->exportClouds(optimizedPoses,
updateLinksWithModifications(links_),
mapIds_,
QMap<int, Signature>(),
std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> >(),
pathDatabase_,
ui_->parameters_toolbox->getParameters());
}
else
{
QMessageBox::critical(this, tr("Error"), tr("No neighbors found for node %1.").arg(ui_->spinBox_optimizationsFrom->value()));
}
}
}
}
}
void DatabaseViewer::generate3DLaserScans()
@@ -2524,6 +2383,7 @@ void DatabaseViewer::update(int value,
view3D->removeCloud("map");
view3D->removeCloud("ground");
view3D->removeCloud("obstacles");
view3D->removeOctomap();
if(ui_->checkBox_showCloud->isChecked() || ui_->checkBox_showMesh->isChecked())
{
if(!data.depthOrRightRaw().empty())
@@ -2642,37 +2502,93 @@ void DatabaseViewer::update(int value,
if(ui_->checkBox_showMap->isChecked() || ui_->checkBox_showGrid->isChecked())
{
std::map<int, std::pair<cv::Mat, cv::Mat> > localMaps;
std::map<int, std::pair<float, cv::Point3f> > localMapsInfo;
if(generatedLocalMaps_.find(data.id()) != generatedLocalMaps_.end())
{
localMaps.insert(*generatedLocalMaps_.find(data.id()));
localMapsInfo.insert(*generatedLocalMapsInfo_.find(data.id()));
}
else if(!data.gridGroundCellsRaw().empty() && !data.gridObstacleCellsRaw().empty())
{
localMaps.insert(std::make_pair(data.id(), std::make_pair(data.gridGroundCellsRaw(), data.gridObstacleCellsRaw())));
localMapsInfo.insert(std::make_pair(data.id(), std::make_pair(data.gridCellSize(), data.gridViewPoint())));
}
if(!localMaps.empty())
{
std::map<int, Transform> poses;
poses.insert(std::make_pair(data.id(), Transform::getIdentity()));
#ifdef RTABMAP_OCTOMAP
OctoMap * octomap = 0;
if(ui_->checkBox_octomap->isChecked() &&
localMaps.begin()->second.first.channels() > 2 &&
localMaps.begin()->second.second.channels() > 2 &&
localMapsInfo.begin()->second.first > 0.0f)
{
//create local octomap
octomap = new OctoMap(localMapsInfo.begin()->second.first);
octomap->addToCache(data.id(), localMaps.begin()->second.first, localMaps.begin()->second.second, localMapsInfo.begin()->second.second);
octomap->update(poses);
}
#endif
if(ui_->checkBox_showMap->isChecked())
{
float xMin=0.0f, yMin=0.0f;
cv::Mat map8S = util3d::create2DMapFromOccupancyLocalMaps(
cv::Mat map8S;
float gridCellSize = ui_->doubleSpinBox_gridCellSize->value();
#ifdef RTABMAP_OCTOMAP
if(octomap)
{
map8S = octomap->createProjectionMap(xMin, yMin, gridCellSize, 0);
}
else
#endif
{
map8S = util3d::create2DMapFromOccupancyLocalMaps(
poses,
localMaps,
ui_->doubleSpinBox_gridCellSize->value(),
xMin, yMin);
}
if(!map8S.empty())
{
//convert to gray scaled map
cv::Mat map8U = util3d::convertMap2Image8U(map8S);
view3D->addOccupancyGridMap(map8U, ui_->doubleSpinBox_gridCellSize->value(), xMin, yMin, 1);
view3D->addOccupancyGridMap(util3d::convertMap2Image8U(map8S), gridCellSize, xMin, yMin, 1);
}
}
if(ui_->checkBox_showGrid->isChecked())
{
#ifdef RTABMAP_OCTOMAP
if(octomap)
{
if(!ui_->checkBox_grid_cubes->isChecked())
{
pcl::IndicesPtr obstacles(new std::vector<int>);
pcl::IndicesPtr empty(new std::vector<int>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = octomap->createCloud(0, obstacles.get(), empty.get());
pcl::PointCloud<pcl::PointXYZRGB>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::copyPointCloud(*cloud, *obstacles, *obstaclesCloud);
view3D->addCloud("obstacles", obstaclesCloud);
view3D->setCloudPointSize("obstacles", 5);
if(ui_->checkBox_grid_empty->isChecked())
{
pcl::PointCloud<pcl::PointXYZ>::Ptr emptyCloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::copyPointCloud(*cloud, *empty, *emptyCloud);
view3D->addCloud("ground", emptyCloud, Transform::getIdentity(), Qt::white);
view3D->setCloudOpacity("ground", 0.5);
view3D->setCloudPointSize("ground", 5);
}
}
else
{
view3D->addOctomap(octomap);
}
}
else
#endif
{
// occupancy cloud
view3D->addCloud("ground",
util3d::laserScanToPointCloud(localMaps.begin()->second.first),
@@ -2686,6 +2602,13 @@ void DatabaseViewer::update(int value,
view3D->setCloudPointSize("obstacles", 5);
}
}
#ifdef RTABMAP_OCTOMAP
if(octomap)
{
delete octomap;
}
#endif
}
}
view3D->update();
}
@@ -3741,7 +3664,15 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
ui_->doubleSpinBox_posefilteringAngle->value()*CV_PI/180.0);
}
std::map<int, std::pair<cv::Mat, cv::Mat> > localMaps;
if(ui_->dockWidget_graphView->isVisible())
std::map<int, std::pair<float, cv::Point3f> > localMapsInfo;
#ifdef RTABMAP_OCTOMAP
if(octomap_)
{
delete octomap_;
octomap_ = 0;
}
#endif
if(ui_->dockWidget_graphView->isVisible() || ui_->dockWidget_occupancyGridView->isVisible())
{
//update scans
UINFO("Update local maps list...");
@@ -3751,12 +3682,14 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
if(generatedLocalMaps_.find(ids[i]) != generatedLocalMaps_.end())
{
localMaps.insert(*generatedLocalMaps_.find(ids[i]));
localMapsInfo.insert(*generatedLocalMapsInfo_.find(ids[i]));
}
else if(localMaps_.find(ids[i]) != localMaps_.end())
{
if(!localMaps_.find(ids[i])->second.first.empty() || !localMaps_.find(ids[i])->second.first.empty())
{
localMaps.insert(*localMaps_.find(ids.at(i)));
localMapsInfo.insert(*localMapsInfo_.find(ids[i]));
}
}
else
@@ -3766,9 +3699,11 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
cv::Mat ground, obstacles;
data.uncompressData(0, 0, 0, 0, &ground, &obstacles);
localMaps_.insert(std::make_pair(ids.at(i), std::make_pair(ground, obstacles)));
localMapsInfo_.insert(std::make_pair(ids.at(i), std::make_pair(data.gridCellSize(), data.gridViewPoint())));
if(!ground.empty() || !obstacles.empty())
{
localMaps.insert(std::make_pair(ids.at(i), std::make_pair(ground, obstacles)));
localMapsInfo.insert(std::make_pair(ids.at(i), std::make_pair(data.gridCellSize(), data.gridViewPoint())));
}
}
}
@@ -3777,6 +3712,7 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
{
if(graphFiltered.find(iter->first) == graphFiltered.end())
{
localMapsInfo_.erase(iter->first);
localMaps_.erase(iter++);
}
else
@@ -3789,19 +3725,159 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
ui_->graphViewer->updateGTGraph(groundTruthPoses_);
ui_->graphViewer->updateGraph(graph, graphLinks_, mapIds_);
if(graph.size() && localMaps.size() && ui_->graphViewer->isGridMapVisible())
ui_->graphViewer->clearMap();
occupancyGridViewer_->clear();
if(graph.size() && localMaps.size() &&
(ui_->graphViewer->isGridMapVisible() || ui_->dockWidget_occupancyGridView->isVisible()))
{
QTime time;
time.start();
#ifdef RTABMAP_OCTOMAP
if(ui_->checkBox_octomap->isChecked())
{
octomap_ = new OctoMap(ui_->doubleSpinBox_gridCellSize->value());
bool updateAborted = false;
for(std::map<int, std::pair<cv::Mat, cv::Mat> >::iterator iter=localMaps.begin(); iter!=localMaps.end(); ++iter)
{
if(iter->second.first.channels() == 2 || iter->second.second.channels() == 2)
{
QMessageBox::warning(this, tr(""),
tr("Some local occupancy grids are 2D, but OctoMap requires 3D local "
"occupancy grids. Uncheck OctoMap under GUI parameters or generate "
"3D local occupancy grids (\"Grid/3D\" core parameter)."));
updateAborted = true;
break;
}
octomap_->addToCache(iter->first, iter->second.first, iter->second.second, localMapsInfo.at(iter->first).second);
}
if(!updateAborted)
{
octomap_->update(graphFiltered);
}
}
#endif
// Generate 2d grid map?
if((ui_->dockWidget_graphView->isVisible() && ui_->graphViewer->isGridMapVisible()) ||
(ui_->dockWidget_occupancyGridView->isVisible() && ui_->checkBox_grid_2d->isChecked()))
{
float xMin, yMin;
float cell = ui_->doubleSpinBox_gridCellSize->value();
cv::Mat map;
QTime time;
time.start();
#ifdef RTABMAP_OCTOMAP
if(ui_->checkBox_octomap->isChecked())
{
map = octomap_->createProjectionMap(xMin, yMin, cell, 0);
}
else
#endif
{
map = rtabmap::util3d::create2DMapFromOccupancyLocalMaps(graphFiltered, localMaps, cell, xMin, yMin, 0, ui_->checkBox_gridErode->isChecked());
}
ui_->label_timeGrid->setNum(double(time.elapsed())/1000.0);
if(!map.empty())
{
ui_->graphViewer->updateMap(rtabmap::util3d::convertMap2Image8U(map), cell, xMin, yMin);
cv::Mat map8U = rtabmap::util3d::convertMap2Image8U(map);
if(ui_->dockWidget_graphView->isVisible() && ui_->graphViewer->isGridMapVisible())
{
ui_->graphViewer->updateMap(map8U, cell, xMin, yMin);
}
if(ui_->dockWidget_occupancyGridView->isVisible() && ui_->checkBox_grid_2d->isChecked())
{
occupancyGridViewer_->addOccupancyGridMap(map8U, cell, xMin, yMin, 1.0f);
occupancyGridViewer_->update();
}
}
}
// Generate 3d grid map?
if(ui_->dockWidget_occupancyGridView->isVisible())
{
#ifdef RTABMAP_OCTOMAP
if(ui_->checkBox_octomap->isChecked())
{
updateOctomapView();
}
else
#endif
{
pcl::PointCloud<pcl::PointXYZ>::Ptr groundXYZ(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesXYZ(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr groundRGB(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr obstaclesRGB(new pcl::PointCloud<pcl::PointXYZRGB>);
for(std::map<int, std::pair<cv::Mat, cv::Mat> >::iterator iter=localMaps.begin(); iter!=localMaps.end(); ++iter)
{
Transform pose = graphFiltered.at(iter->first);
float x,y,z,roll,pitch,yaw;
pose.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
Transform pose2d(x,y, 0, 0, 0, yaw);
if(!iter->second.first.empty())
{
if(iter->second.first.channels() == 4)
{
*groundRGB += *util3d::laserScanToPointCloudRGB(iter->second.first, pose);
}
else
{
*groundXYZ += *util3d::laserScanToPointCloud(iter->second.first, iter->second.first.channels()==2?pose2d:pose);
}
}
if(!iter->second.second.empty())
{
if(iter->second.second.channels() == 4)
{
*obstaclesRGB += *util3d::laserScanToPointCloudRGB(iter->second.second, pose);
}
else
{
*obstaclesXYZ += *util3d::laserScanToPointCloud(iter->second.second, iter->second.second.channels()==2?pose2d:pose);
}
}
}
// occupancy cloud
if(groundRGB->size())
{
groundRGB = util3d::voxelize(groundRGB, ui_->doubleSpinBox_gridCellSize->value());
occupancyGridViewer_->addCloud("groundRGB",
groundRGB,
Transform::getIdentity(),
Qt::green);
occupancyGridViewer_->setCloudPointSize("groundRGB", 5);
}
if(groundXYZ->size())
{
groundXYZ = util3d::voxelize(groundXYZ, ui_->doubleSpinBox_gridCellSize->value());
occupancyGridViewer_->addCloud("groundXYZ",
groundXYZ,
Transform::getIdentity(),
Qt::green);
occupancyGridViewer_->setCloudPointSize("groundXYZ", 5);
}
if(obstaclesRGB->size())
{
obstaclesRGB = util3d::voxelize(obstaclesRGB, ui_->doubleSpinBox_gridCellSize->value());
occupancyGridViewer_->addCloud("obstaclesRGB",
obstaclesRGB,
Transform::getIdentity(),
Qt::red);
occupancyGridViewer_->setCloudPointSize("obstaclesRGB", 5);
}
if(obstaclesXYZ->size())
{
obstaclesXYZ = util3d::voxelize(obstaclesXYZ, ui_->doubleSpinBox_gridCellSize->value());
occupancyGridViewer_->addCloud("obstaclesXYZ",
obstaclesXYZ,
Transform::getIdentity(),
Qt::red);
occupancyGridViewer_->setCloudPointSize("obstaclesXYZ", 5);
}
}
}
ui_->label_timeGrid->setNum(double(time.elapsed())/1000.0);
}
ui_->graphViewer->update();
ui_->label_iterations->setNum(value);
@@ -4047,8 +4123,63 @@ void DatabaseViewer::updateGraphView()
void DatabaseViewer::updateGrid()
{
if(sender() == ui_->checkBox_grid_2d && !ui_->checkBox_grid_2d->isChecked())
{
//just remove map in occupancy grid view
occupancyGridViewer_->removeOccupancyGridMap();
occupancyGridViewer_->update();
}
else
{
ui_->checkBox_grid_cubes->setVisible(ui_->checkBox_octomap->isChecked());
ui_->spinBox_grid_depth->setVisible(ui_->checkBox_octomap->isChecked());
ui_->checkBox_grid_empty->setVisible(ui_->checkBox_octomap->isChecked());
update3dView();
updateGraphView();
}
}
void DatabaseViewer::updateOctomapView()
{
#ifdef RTABMAP_OCTOMAP
if(ui_->checkBox_octomap->isChecked())
{
if(octomap_)
{
occupancyGridViewer_->removeOctomap();
occupancyGridViewer_->removeCloud("octomap_obstacles");
occupancyGridViewer_->removeCloud("octomap_empty");
if(ui_->checkBox_grid_cubes->isChecked())
{
occupancyGridViewer_->addOctomap(octomap_, ui_->spinBox_grid_depth->value());
}
else
{
pcl::IndicesPtr obstacles(new std::vector<int>);
pcl::IndicesPtr empty(new std::vector<int>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = octomap_->createCloud(ui_->spinBox_grid_depth->value(), obstacles.get(), empty.get());
pcl::PointCloud<pcl::PointXYZRGB>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::copyPointCloud(*cloud, *obstacles, *obstaclesCloud);
occupancyGridViewer_->addCloud("octomap_obstacles", obstaclesCloud);
occupancyGridViewer_->setCloudPointSize("octomap_obstacles", 5);
if(ui_->checkBox_grid_empty->isChecked())
{
pcl::PointCloud<pcl::PointXYZ>::Ptr emptyCloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::copyPointCloud(*cloud, *empty, *emptyCloud);
occupancyGridViewer_->addCloud("octomap_empty", emptyCloud, Transform::getIdentity(), Qt::white);
occupancyGridViewer_->setCloudOpacity("octomap_empty", 0.5);
occupancyGridViewer_->setCloudPointSize("octomap_empty", 5);
}
}
occupancyGridViewer_->update();
}
if(ui_->dockWidget_view3d->isVisible() && ui_->checkBox_showGrid->isChecked())
{
this->update3dView();
}
}
#endif
}
Link DatabaseViewer::findActiveLink(int from, int to)
guilib/src/ExportCloudsDialog.cpp
+205 -83
View File
@@ -43,6 +43,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/Graph.h"
#include "rtabmap/core/GainCompensator.h"
#include "rtabmap/core/clams/discrete_depth_distortion_model.h"
#include "rtabmap/core/DBDriver.h"
#include <pcl/conversions.h>
#include <pcl/io/pcd_io.h>
@@ -65,7 +66,8 @@ namespace rtabmap {
ExportCloudsDialog::ExportCloudsDialog(QWidget *parent) :
QDialog(parent),
_canceled(false),
_compensator(0)
_compensator(0),
_dbDriver(0)
{
_ui = new Ui_ExportCloudsDialog();
_ui->setupUi(this);
@@ -392,9 +394,9 @@ void ExportCloudsDialog::restoreDefaults()
{
_ui->comboBox_pipeline->setCurrentIndex(1);
_ui->checkBox_binary->setChecked(true);
_ui->spinBox_normalKSearch->setValue(10);
_ui->spinBox_normalKSearch->setValue(20);
_ui->checkBox_regenerate->setChecked(false);
_ui->checkBox_regenerate->setChecked(_dbDriver!=0?true:false);
_ui->spinBox_decimation->setValue(1);
_ui->doubleSpinBox_maxDepth->setValue(4);
_ui->doubleSpinBox_minDepth->setValue(0);
@@ -749,8 +751,17 @@ void ExportCloudsDialog::viewClouds(
uIsInteger(mesh->tex_materials[0].tex_file, false))
{
int textureId = uStr2Int(mesh->tex_materials[0].tex_file);
UASSERT(cachedSignatures.contains(textureId) && !cachedSignatures.value(textureId).sensorData().imageCompressed().empty());
cachedSignatures.value(textureId).sensorData().uncompressDataConst(&globalTexture, 0);
SensorData data;
if(cachedSignatures.contains(textureId) && !cachedSignatures.value(textureId).sensorData().imageCompressed().empty())
{
data = cachedSignatures.value(textureId).sensorData();
}
else if(_dbDriver)
{
_dbDriver->getNodeData(textureId, data, true, false, false, false);
}
UASSERT(!data.imageCompressed().empty());
data.uncompressDataConst(&globalTexture, 0);
UASSERT(!globalTexture.empty());
if (_ui->checkBox_gainCompensation->isChecked() && _compensator && _compensator->getIndex(textureId) >= 0)
{
@@ -864,7 +875,11 @@ bool ExportCloudsDialog::getExportedClouds(
{
_canceled = false;
_workingDirectory = workingDirectory;
enableRegeneration(cachedSignatures.size());
enableRegeneration(_dbDriver || cachedSignatures.size());
if(cachedSignatures.empty() && _dbDriver)
{
_ui->checkBox_regenerate->setChecked(true);
}
if(_compensator)
{
delete _compensator;
@@ -1073,17 +1088,26 @@ bool ExportCloudsDialog::getExportedClouds(
// Adjust view points with local transforms
for(std::map<int, Transform>::iterator iter= viewPoints.begin(); iter!=viewPoints.end(); ++iter)
{
std::vector<CameraModel> models;
StereoCameraModel stereoModel;
if(cachedSignatures.contains(iter->first))
{
const SensorData & data = cachedSignatures.find(iter->first)->sensorData();
if(data.cameraModels().size() && !data.cameraModels()[0].localTransform().isNull())
{
iter->second *= data.cameraModels()[0].localTransform();
models = data.cameraModels();
stereoModel = data.stereoCameraModel();
}
else if(!data.stereoCameraModel().localTransform().isNull())
else if(_dbDriver)
{
iter->second *= data.stereoCameraModel().localTransform();
_dbDriver->getCalibration(iter->first, models, stereoModel);
}
if(models.size() && !models[0].localTransform().isNull())
{
iter->second *= models[0].localTransform();
}
else if(!stereoModel.localTransform().isNull())
{
iter->second *= stereoModel.localTransform();
}
}
}
@@ -1194,22 +1218,33 @@ bool ExportCloudsDialog::getExportedClouds(
if(iter->second->size())
{
Eigen::Vector3f viewpoint(0.0f,0.0f,0.0f);
std::vector<CameraModel> models;
StereoCameraModel stereoModel;
if(cachedSignatures.contains(iter->first))
{
const SensorData & data = cachedSignatures.find(iter->first)->sensorData();
if(data.cameraModels().size() && !data.cameraModels()[0].localTransform().isNull())
models = data.cameraModels();
stereoModel = data.stereoCameraModel();
}
else if(_dbDriver)
{
viewpoint[0] = data.cameraModels()[0].localTransform().x();
viewpoint[1] = data.cameraModels()[0].localTransform().y();
viewpoint[2] = data.cameraModels()[0].localTransform().z();
_dbDriver->getCalibration(iter->first, models, stereoModel);
}
else if(!data.stereoCameraModel().localTransform().isNull())
if(models.size() && !models[0].localTransform().isNull())
{
viewpoint[0] = data.stereoCameraModel().localTransform().x();
viewpoint[1] = data.stereoCameraModel().localTransform().y();
viewpoint[2] = data.stereoCameraModel().localTransform().z();
viewpoint[0] = models[0].localTransform().x();
viewpoint[1] = models[0].localTransform().y();
viewpoint[2] = models[0].localTransform().z();
}
else if(!stereoModel.localTransform().isNull())
{
viewpoint[0] = stereoModel.localTransform().x();
viewpoint[1] = stereoModel.localTransform().y();
viewpoint[2] = stereoModel.localTransform().z();
}
std::vector<pcl::Vertices> polygons = util3d::organizedFastMesh(
iter->second,
_ui->doubleSpinBox_mesh_angleTolerance->value()*M_PI/180.0,
@@ -1766,29 +1801,52 @@ bool ExportCloudsDialog::getExportedClouds(
std::map<int, CameraModel> cameraModels;
for(std::map<int, Transform>::iterator jter=cameras.begin(); jter!=cameras.end(); ++jter)
{
std::vector<CameraModel> models;
StereoCameraModel stereoModel;
bool cacheHasCompressedImage = false;
if(cachedSignatures.contains(jter->first))
{
const Signature & s = cachedSignatures.value(jter->first);
const SensorData & data = cachedSignatures.find(jter->first)->sensorData();
models = data.cameraModels();
stereoModel = data.stereoCameraModel();
cacheHasCompressedImage = !data.imageCompressed().empty();
}
else if(_dbDriver)
{
_dbDriver->getCalibration(jter->first, models, stereoModel);
}
CameraModel model;
if(s.sensorData().stereoCameraModel().isValidForProjection())
if(stereoModel.isValidForProjection())
{
model = s.sensorData().stereoCameraModel().left();
model = stereoModel.left();
}
else if(s.sensorData().cameraModels().size() == 1 && s.sensorData().cameraModels()[0].isValidForProjection())
else if(models.size() == 1 && models[0].isValidForProjection())
{
model = s.sensorData().cameraModels()[0];
model = models[0];
}
if(!jter->second.isNull() && model.isValidForProjection() && !s.sensorData().imageCompressed().empty())
if(!jter->second.isNull() && model.isValidForProjection())
{
if(model.imageWidth() == 0 || model.imageHeight() == 0)
{
// we are using an old database format (image size not saved in calibrations), we should
// uncompress images to get their size
cv::Mat img;
s.sensorData().uncompressDataConst(&img, 0);
if(cacheHasCompressedImage)
{
cachedSignatures.find(jter->first)->sensorData().uncompressDataConst(&img, 0);
}
else if(_dbDriver)
{
SensorData data;
_dbDriver->getNodeData(jter->first, data, true, false, false, false);
data.uncompressDataConst(&img, 0);
}
model.setImageSize(img.size());
}
if(model.imageWidth() != 0 && model.imageHeight() != 0)
{
cameraPoses.insert(std::make_pair(jter->first, jter->second));
cameraModels.insert(std::make_pair(jter->first, model));
}
@@ -2096,12 +2154,20 @@ std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr, pcl::Indic
pcl::IndicesPtr indices(new std::vector<int>);
if(_ui->checkBox_regenerate->isChecked())
{
SensorData data;
cv::Mat image, depth;
if(cachedSignatures.contains(iter->first))
{
const Signature & s = cachedSignatures.find(iter->first).value();
SensorData d = s.sensorData();
cv::Mat image, depth;
d.uncompressData(&image, &depth, 0);
data = s.sensorData();
data.uncompressData(&image, &depth, 0);
}
else if(_dbDriver)
{
_dbDriver->getNodeData(iter->first, data, true, false, false, false);
data.uncompressData(&image, &depth, 0);
}
if(!image.empty() && !depth.empty())
{
if(_ui->spinBox_fillDepthHoles->value() > 0)
@@ -2116,7 +2182,7 @@ std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr, pcl::Indic
model.load(_ui->lineEdit_distortionModel->text().toStdString());
depth = depth.clone();// make sure we are not modifying data in cached signatures.
model.undistort(depth);
d.setDepthOrRightRaw(depth);
data.setDepthOrRightRaw(depth);
}
// bilateral filtering
@@ -2125,10 +2191,10 @@ std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr, pcl::Indic
depth = util2d::fastBilateralFiltering(depth,
_ui->doubleSpinBox_bilateral_sigmaS->value(),
_ui->doubleSpinBox_bilateral_sigmaR->value());
d.setDepthOrRightRaw(depth);
data.setDepthOrRightRaw(depth);
}
UASSERT(iter->first == d.id());
UASSERT(iter->first == data.id());
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudWithoutNormals;
std::vector<float> roiRatios;
if(!_ui->lineEdit_roiRatios->text().isEmpty())
@@ -2144,7 +2210,7 @@ std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr, pcl::Indic
}
}
cloudWithoutNormals = util3d::cloudRGBFromSensorData(
d,
data,
_ui->spinBox_decimation->value() == 0?1:_ui->spinBox_decimation->value(),
_ui->doubleSpinBox_maxDepth->value(),
_ui->doubleSpinBox_minDepth->value(),
@@ -2167,17 +2233,17 @@ std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr, pcl::Indic
// view point
Eigen::Vector3f viewPoint(0.0f,0.0f,0.0f);
if(d.cameraModels().size() && !d.cameraModels()[0].localTransform().isNull())
if(data.cameraModels().size() && !data.cameraModels()[0].localTransform().isNull())
{
viewPoint[0] = d.cameraModels()[0].localTransform().x();
viewPoint[1] = d.cameraModels()[0].localTransform().y();
viewPoint[2] = d.cameraModels()[0].localTransform().z();
viewPoint[0] = data.cameraModels()[0].localTransform().x();
viewPoint[1] = data.cameraModels()[0].localTransform().y();
viewPoint[2] = data.cameraModels()[0].localTransform().z();
}
else if(!d.stereoCameraModel().localTransform().isNull())
else if(!data.stereoCameraModel().localTransform().isNull())
{
viewPoint[0] = d.stereoCameraModel().localTransform().x();
viewPoint[1] = d.stereoCameraModel().localTransform().y();
viewPoint[2] = d.stereoCameraModel().localTransform().z();
viewPoint[0] = data.stereoCameraModel().localTransform().x();
viewPoint[1] = data.stereoCameraModel().localTransform().y();
viewPoint[2] = data.stereoCameraModel().localTransform().z();
}
pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloudWithoutNormals, indices, _ui->spinBox_normalKSearch->value(), viewPoint);
@@ -2210,7 +2276,6 @@ std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr, pcl::Indic
}
}
}
}
else
{
UERROR("Cloud %d not found in cache!", iter->first);
@@ -2242,22 +2307,30 @@ std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr, pcl::Indic
// view point
Eigen::Vector3f viewPoint(0.0f,0.0f,0.0f);
std::vector<CameraModel> models;
StereoCameraModel stereoModel;
if(cachedSignatures.contains(iter->first))
{
const Signature & s = cachedSignatures.find(iter->first).value();
SensorData d = s.sensorData();
if(d.cameraModels().size() && !d.cameraModels()[0].localTransform().isNull())
{
viewPoint[0] = d.cameraModels()[0].localTransform().x();
viewPoint[1] = d.cameraModels()[0].localTransform().y();
viewPoint[2] = d.cameraModels()[0].localTransform().z();
models = s.sensorData().cameraModels();
stereoModel = s.sensorData().stereoCameraModel();
}
else if(!d.stereoCameraModel().localTransform().isNull())
else if(_dbDriver)
{
viewPoint[0] = d.stereoCameraModel().localTransform().x();
viewPoint[1] = d.stereoCameraModel().localTransform().y();
viewPoint[2] = d.stereoCameraModel().localTransform().z();
_dbDriver->getCalibration(iter->first, models, stereoModel);
}
if(models.size() && !models[0].localTransform().isNull())
{
viewPoint[0] = models[0].localTransform().x();
viewPoint[1] = models[0].localTransform().y();
viewPoint[2] = models[0].localTransform().z();
}
else if(!stereoModel.localTransform().isNull())
{
viewPoint[0] = stereoModel.localTransform().x();
viewPoint[1] = stereoModel.localTransform().y();
viewPoint[2] = stereoModel.localTransform().z();
}
else
{
@@ -2605,7 +2678,7 @@ cv::Mat ExportCloudsDialog::mergeTextures(pcl::TextureMesh & mesh, const QMap<in
{
std::vector<int> textures(mesh.tex_materials.size(), -1);
cv::Size imageSize;
int imageType=CV_8UC1;
const int imageType=CV_8UC3;
UDEBUG("");
bool mergeTextures = true;
for(unsigned int i=0; i<mesh.tex_materials.size(); ++i)
@@ -2617,42 +2690,57 @@ cv::Mat ExportCloudsDialog::mergeTextures(pcl::TextureMesh & mesh, const QMap<in
int textureId = uStr2Int(mesh.tex_materials[i].tex_file);
textures[i] = textureId;
QMap<int, Signature>::const_iterator iter = cachedSignatures.find(textureId);
UASSERT(iter!=cachedSignatures.end() && !iter->sensorData().imageCompressed().empty());
cv::Size tmpImageSize;
if(iter->sensorData().cameraModels().size()==1 &&
iter->sensorData().cameraModels()[0].imageHeight()>0 &&
iter->sensorData().cameraModels()[0].imageWidth()>0)
if(imageSize.width == 0 || imageSize.height == 0)
{
tmpImageSize = iter->sensorData().cameraModels()[0].imageSize();
if(imageSize.height == 0 && imageSize.width == 0)
if(cachedSignatures.find(textureId)!=cachedSignatures.end() && !cachedSignatures.find(textureId)->sensorData().imageCompressed().empty())
{
// just for the first image, get the type, assuming all others have the same type
cv::Mat image;
iter->sensorData().uncompressDataConst(&image, 0);
UASSERT(!image.empty());
imageType = image.type();
SensorData data = cachedSignatures.find(textureId).value().sensorData();
if(data.cameraModels().size()==1 &&
data.cameraModels()[0].imageHeight()>0 &&
data.cameraModels()[0].imageWidth()>0)
{
imageSize = data.cameraModels()[0].imageSize();
}
else if(data.stereoCameraModel().left().imageHeight() > 0 &&
data.stereoCameraModel().left().imageWidth() > 0)
{
imageSize = data.stereoCameraModel().left().imageSize();
}
else // backward compatibility for image size not set in CameraModel
{
cv::Mat image;
iter->sensorData().uncompressDataConst(&image, 0);
data.uncompressDataConst(&image, 0);
UASSERT(!image.empty());
tmpImageSize = image.size();
if(imageSize.height == 0 && imageSize.width == 0)
imageSize = image.size();
}
}
else if(_dbDriver)
{
imageType = image.type();
}
}
if(imageSize.width>0 && imageSize.height>0 && imageSize.width != tmpImageSize.width)
std::vector<CameraModel> models;
StereoCameraModel stereoModel;
_dbDriver->getCalibration(textureId, models, stereoModel);
if(models.size()==1 &&
models[0].imageHeight()>0 &&
models[0].imageWidth()>0)
{
UWARN("All images should have the same dimensions to merge the textures!");
mergeTextures = false;
break;
imageSize = models[0].imageSize();
}
else if(stereoModel.left().imageHeight() > 0 &&
stereoModel.left().imageWidth() > 0)
{
imageSize = stereoModel.left().imageSize();
}
else // backward compatibility for image size not set in CameraModel
{
SensorData data;
_dbDriver->getNodeData(textureId, data, true, false, false, false);
cv::Mat image;
data.uncompressDataConst(&image, 0);
UASSERT(!image.empty());
imageSize = image.size();
}
}
}
imageSize = tmpImageSize;
}
}
if(mergeTextures && textures.size() && imageSize.height>0 && imageSize.width>0)
@@ -2680,13 +2768,30 @@ cv::Mat ExportCloudsDialog::mergeTextures(pcl::TextureMesh & mesh, const QMap<in
UASSERT(v < textureSize-emptyImage.rows);
if(textures[t]>=0)
{
QMap<int, Signature>::const_iterator iter = cachedSignatures.find(textures[t]);
UASSERT(iter!=cachedSignatures.end() && !iter->sensorData().imageCompressed().empty());
cv::Mat image;
iter->sensorData().uncompressDataConst(&image, 0);
if(cachedSignatures.find(textures[t]) != cachedSignatures.end() &&
!cachedSignatures.find(textures[t])->sensorData().imageCompressed().empty())
{
cachedSignatures.find(textures[t])->sensorData().uncompressDataConst(&image, 0);
}
else if(_dbDriver)
{
SensorData data;
_dbDriver->getNodeData(textures[t], data, true, false, false, false);
data.uncompressDataConst(&image, 0);
}
UASSERT(!image.empty());
cv::Mat resizedImage;
cv::resize(image, resizedImage, emptyImage.size(), 0.0f, 0.0f, cv::INTER_AREA);
UASSERT(resizedImage.type() == CV_8UC1 || resizedImage.type() == CV_8UC3);
if(resizedImage.type() == CV_8UC1)
{
cv::Mat resizedImageColor;
cv::cvtColor(resizedImage, resizedImageColor, CV_GRAY2BGR);
resizedImage = resizedImageColor;
}
if(_ui->checkBox_gainCompensation->isChecked() && _compensator && _compensator->getIndex(textures[t]) >= 0)
{
_compensator->apply(textures[t], resizedImage);
@@ -2766,9 +2871,17 @@ void ExportCloudsDialog::saveTextureMeshes(
if(uIsInteger(mesh->tex_materials[i].tex_file, false))
{
int textureId = uStr2Int(mesh->tex_materials[i].tex_file);
UASSERT(cachedSignatures.contains(textureId) && !cachedSignatures.value(textureId).sensorData().imageCompressed().empty());
cv::Mat image;
if(cachedSignatures.contains(textureId) && !cachedSignatures.value(textureId).sensorData().imageCompressed().empty())
{
cachedSignatures.value(textureId).sensorData().uncompressDataConst(&image, 0);
}
else if(_dbDriver)
{
SensorData data;
_dbDriver->getNodeData(textureId, data, true, false, false, false);
data.uncompressDataConst(&image, 0);
}
UASSERT(!image.empty());
imageSize = image.size();
if(_ui->checkBox_gainCompensation->isChecked() && _compensator && _compensator->getIndex(textureId) >= 0)
@@ -2886,9 +2999,18 @@ void ExportCloudsDialog::saveTextureMeshes(
if(uIsInteger(mesh->tex_materials[i].tex_file, false))
{
int textureId = uStr2Int(mesh->tex_materials[i].tex_file);
UASSERT(cachedSignatures.contains(textureId) && !cachedSignatures.value(textureId).sensorData().imageCompressed().empty());
cv::Mat image;
if(cachedSignatures.contains(textureId) && !cachedSignatures.value(textureId).sensorData().imageCompressed().empty())
{
cachedSignatures.value(textureId).sensorData().uncompressDataConst(&image, 0);
}
else if(_dbDriver)
{
SensorData data;
_dbDriver->getNodeData(textureId, data, true, false, false, false);
data.uncompressDataConst(&image, 0);
}
UASSERT(!image.empty());
imageSize = image.size();
if(_ui->checkBox_gainCompensation->isChecked() && _compensator && _compensator->getIndex(textureId) >= 0)
guilib/src/ExportCloudsDialog.h
+4
View File
@@ -47,6 +47,7 @@ class QAbstractButton;
namespace rtabmap {
class ProgressDialog;
class GainCompensator;
class DBDriver;
class ExportCloudsDialog : public QDialog
{
@@ -60,6 +61,8 @@ public:
void saveSettings(QSettings & settings, const QString & group = "") const;
void loadSettings(QSettings & settings, const QString & group = "");
void setDBDriver(const DBDriver * dbDriver) {_dbDriver = dbDriver;}
void exportClouds(
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & links,
@@ -124,6 +127,7 @@ private:
QString _workingDirectory;
bool _canceled;
GainCompensator * _compensator;
const DBDriver * _dbDriver;
};
}
guilib/src/ui/DatabaseViewer.ui
+112 -10
View File
@@ -6,7 +6,7 @@
<rect>
<x>0</x>
<y>0</y>
<width>1285</width>
<width>1392</width>
<height>965</height>
</rect>
</property>
@@ -52,7 +52,7 @@
<rect>
<x>0</x>
<y>0</y>
<width>253</width>
<width>339</width>
<height>208</height>
</rect>
</property>
@@ -210,7 +210,7 @@
<rect>
<x>0</x>
<y>0</y>
<width>253</width>
<width>339</width>
<height>208</height>
</rect>
</property>
@@ -483,7 +483,7 @@
<rect>
<x>0</x>
<y>0</y>
<width>1285</width>
<width>1392</width>
<height>22</height>
</rect>
</property>
@@ -987,7 +987,7 @@
<item>
<widget class="QToolBox" name="toolBox">
<property name="currentIndex">
<number>3</number>
<number>1</number>
</property>
<widget class="QWidget" name="page_3">
<property name="geometry">
@@ -1131,7 +1131,7 @@
<x>0</x>
<y>0</y>
<width>280</width>
<height>584</height>
<height>608</height>
</rect>
</property>
<attribute name="label">
@@ -1174,7 +1174,7 @@
</property>
</widget>
</item>
<item row="2" column="1">
<item row="3" column="1">
<widget class="QLabel" name="label_51">
<property name="text">
<string>Gain compensation radius (Constraints view)</string>
@@ -1184,7 +1184,7 @@
</property>
</widget>
</item>
<item row="2" column="0">
<item row="3" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_gainCompensationRadius">
<property name="suffix">
<string> m</string>
@@ -1206,6 +1206,20 @@
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="label_53">
<property name="text">
<string>OctoMap</string>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QCheckBox" name="checkBox_octomap">
<property name="text">
<string/>
</property>
</widget>
</item>
</layout>
</item>
<item>
@@ -1547,8 +1561,8 @@
<rect>
<x>0</x>
<y>0</y>
<width>333</width>
<height>256</height>
<width>186</width>
<height>496</height>
</rect>
</property>
<attribute name="label">
@@ -1911,6 +1925,94 @@
</layout>
</widget>
</widget>
<widget class="QDockWidget" name="dockWidget_occupancyGridView">
<property name="windowTitle">
<string>Occupancy Grid</string>
</property>
<attribute name="dockWidgetArea">
<number>4</number>
</attribute>
<widget class="QWidget" name="dockWidgetContents_occupancyGrid">
<layout class="QVBoxLayout" name="verticalLayout_14" stretch="1,0">
<property name="spacing">
<number>0</number>
</property>
<property name="margin">
<number>0</number>
</property>
<item>
<layout class="QVBoxLayout" name="layout_occupancyGridView">
<property name="spacing">
<number>0</number>
</property>
</layout>
</item>
<item>
<layout class="QHBoxLayout" name="horizontalLayout_8">
<item>
<widget class="QCheckBox" name="checkBox_grid_2d">
<property name="text">
<string>2d Map</string>
</property>
<property name="checked">
<bool>false</bool>
</property>
</widget>
</item>
<item>
<widget class="QCheckBox" name="checkBox_grid_cubes">
<property name="text">
<string>Cubes Rendering</string>
</property>
<property name="checked">
<bool>false</bool>
</property>
</widget>
</item>
<item>
<widget class="QCheckBox" name="checkBox_grid_empty">
<property name="text">
<string>Empty Space</string>
</property>
<property name="checked">
<bool>false</bool>
</property>
</widget>
</item>
<item>
<widget class="QSpinBox" name="spinBox_grid_depth">
<property name="prefix">
<string>Depth </string>
</property>
<property name="minimum">
<number>0</number>
</property>
<property name="maximum">
<number>16</number>
</property>
<property name="value">
<number>16</number>
</property>
</widget>
</item>
<item>
<spacer name="horizontalSpacer_5">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>40</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
</layout>
</item>
</layout>
</widget>
</widget>
<action name="actionOpen_database">
<property name="text">
<string>Open database</string>