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

DatabaseViewer: Added ExportCloudsDialog, added Occupancy Grid View

Browse Source
This commit is contained in:
matlabbe
2017-03-21 21:51:39 -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

32
corelib/src/OctoMap.cpp
View File

@@ -456,8 +456,8 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr OctoMap::createCloud(
(*cloud)[oi].g = g*255.0f; (*cloud)[oi].g = g*255.0f;
(*cloud)[oi].b = b*255.0f; (*cloud)[oi].b = b*255.0f;
} }
(*cloud)[oi].x = pt.x(); (*cloud)[oi].x = pt.x()-octree_->getResolution()/2.0;
(*cloud)[oi].y = pt.y(); (*cloud)[oi].y = pt.y()-octree_->getResolution()/2.0;
(*cloud)[oi].z = pt.z(); (*cloud)[oi].z = pt.z();
if(obstacleIndices) if(obstacleIndices)
{ {
@@ -469,8 +469,8 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr OctoMap::createCloud(
{ {
octomap::point3d pt = octree_->keyToCoord(it.getKey()); octomap::point3d pt = octree_->keyToCoord(it.getKey());
(*cloud)[oi] = pcl::PointXYZRGB(it->getColor().r, it->getColor().g, it->getColor().b); (*cloud)[oi] = pcl::PointXYZRGB(it->getColor().r, it->getColor().g, it->getColor().b);
(*cloud)[oi].x = pt.x(); (*cloud)[oi].x = pt.x()-octree_->getResolution()/2.0f;
(*cloud)[oi].y = pt.y(); (*cloud)[oi].y = pt.y()-octree_->getResolution()/2.0f;
(*cloud)[oi].z = pt.z(); (*cloud)[oi].z = pt.z();
if(emptyIndices) 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 ground(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr obstacles(new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr obstacles(new pcl::PointCloud<pcl::PointXYZ>);
ground->resize(occupiedCells_.size()); ground->resize(octree_->size());
obstacles->resize(occupiedCells_.size()); obstacles->resize(octree_->size());
int gi=0; int gi=0;
int oi=0; int oi=0;
for(std::map<octomap::ColorOcTreeNode*, OcTreeNodeInfo>::const_iterator iter = occupiedCells_.begin(); for (octomap::ColorOcTree::iterator it = octree_->begin(octree_->getTreeDepth()); it != octree_->end(); ++it)
iter!=occupiedCells_.end();
++iter)
{ {
if(iter->second.isObstacle_ && octree_->isNodeOccupied(iter->first)) if(octree_->isNodeOccupied(*it))
{ {
octomap::point3d pt = octree_->keyToCoord(iter->second.key_); octomap::point3d pt = octree_->keyToCoord(it.getKey());
(*obstacles)[oi++] = pcl::PointXYZ(pt.x(), pt.y(), 0); // projected on ground (*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_); octomap::point3d pt = octree_->keyToCoord(it.getKey());
(*ground)[gi++] = pcl::PointXYZ(pt.x(), pt.y(), 0); // projected on ground (*ground)[gi++] = pcl::PointXYZ(pt.x()-gridCellSize/2.0f, pt.y()-gridCellSize/2.0f, 0); // projected on ground
} }
} }
obstacles->resize(oi); obstacles->resize(oi);
@@ -525,11 +523,11 @@ cv::Mat OctoMap::createProjectionMap(float & xMin, float & yMin, float & gridCel
if(obstacles->size()) if(obstacles->size())
{ {
obstacles = util3d::voxelize(obstacles, gridCellSize); obstacles = util3d::voxelize(obstacles, gridCellSize/2.0f);
} }
if(ground->size()) 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); cv::Mat obstaclesMat = cv::Mat(1, (int)obstacles->size(), CV_32FC2);

20
corelib/src/util3d_mapping.cpp
View File

@@ -308,7 +308,9 @@ cv::Mat create2DMapFromOccupancyLocalMaps(
//Get map size //Get map size
float margin = cellSize*10.0f; float margin = cellSize*10.0f;
xMin = minX-margin; xMin = minX-margin;
xMin -= cellSize/2.0f;
yMin = minY-margin; yMin = minY-margin;
yMin += cellSize/2.0f;
float xMax = maxX+margin; float xMax = maxX+margin;
float yMax = maxY+margin; float yMax = maxY+margin;
if(fabs((yMax - yMin) / cellSize) > 99999 || 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); 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) 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); 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) for(int i=0; i<iter->second.cols; ++i)
{ {
float * ptf = iter->second.ptr<float>(0, 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); char & value = map.at<char>(pt.y, pt.x);
if(value != -2) if(value != -2)
{ {
@@ -345,8 +347,8 @@ cv::Mat create2DMapFromOccupancyLocalMaps(
if(footprintRadius >= cellSize*1.5f) if(footprintRadius >= cellSize*1.5f)
{ {
// place free space under the footprint of the robot // 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 ptBegin((kter->second.x()-footprintRadius-xMin)/cellSize, (kter->second.y()-footprintRadius-yMin)/cellSize);
cv::Point2i ptEnd((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, (kter->second.y()+footprintRadius-yMin)/cellSize);
if(ptBegin.x < 0) if(ptBegin.x < 0)
ptBegin.x = 0; ptBegin.x = 0;
if(ptEnd.x >= map.cols) if(ptEnd.x >= map.cols)
@@ -370,7 +372,7 @@ cv::Mat create2DMapFromOccupancyLocalMaps(
for(int i=0; i<jter->second.cols; ++i) for(int i=0; i<jter->second.cols; ++i)
{ {
float * ptf = jter->second.ptr<float>(0, 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); char & value = map.at<char>(pt.y, pt.x);
if(value != -2) if(value != -2)
{ {
@@ -595,7 +597,7 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
UTimer timer; 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; int j=0;
for(std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr >::iterator iter = localScans.begin(); iter!=localScans.end(); ++iter) 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; 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) 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); 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; 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); //UWARN("maxLength = %f", maxLength);
//rotate counterclockwise from the first point until we pass the last point //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; 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) 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 must be inside the grid
end.x = end.x < 0?0:end.x; end.x = end.x < 0?0:end.x;
end.x = end.x >= map.cols?map.cols-1:end.x; end.x = end.x >= map.cols?map.cols-1:end.x;

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

@@ -54,6 +54,8 @@ class DBDriver;
class ImageView; class ImageView;
class SensorData; class SensorData;
class CloudViewer; class CloudViewer;
class OctoMap;
class ExportCloudsDialog;
class RTABMAPGUI_EXP DatabaseViewer : public QMainWindow class RTABMAPGUI_EXP DatabaseViewer : public QMainWindow
{ {
@@ -102,6 +104,7 @@ private slots:
void sliderLoopValueChanged(int); void sliderLoopValueChanged(int);
void sliderIterationsValueChanged(int); void sliderIterationsValueChanged(int);
void updateGrid(); void updateGrid();
void updateOctomapView();
void updateGraphView(); void updateGraphView();
void refineConstraint(); void refineConstraint();
void addConstraint(); void addConstraint();
@@ -159,6 +162,7 @@ private:
CloudViewer * cloudViewerA_; CloudViewer * cloudViewerA_;
CloudViewer * cloudViewerB_; CloudViewer * cloudViewerB_;
CloudViewer * stereoViewer_; CloudViewer * stereoViewer_;
CloudViewer * occupancyGridViewer_;
QList<int> ids_; QList<int> ids_;
std::map<int, int> mapIds_; std::map<int, int> mapIds_;
QMap<int, int> idToIndex_; QMap<int, int> idToIndex_;
@@ -176,8 +180,11 @@ private:
std::multimap<int, rtabmap::Link> linksAdded_; std::multimap<int, rtabmap::Link> linksAdded_;
std::multimap<int, rtabmap::Link> linksRemoved_; std::multimap<int, rtabmap::Link> linksRemoved_;
std::map<int, std::pair<cv::Mat, cv::Mat> > localMaps_; // <ground, obstacles> 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<cv::Mat, cv::Mat> > generatedLocalMaps_; // <ground, obstacles>
std::map<int, std::pair<float, cv::Point3f> > generatedLocalMapsInfo_; // <cell size, viewpoint> std::map<int, std::pair<float, cv::Point3f> > generatedLocalMapsInfo_; // <cell size, viewpoint>
OctoMap * octomap_;
ExportCloudsDialog * exportDialog_;
bool savedMaximized_; bool savedMaximized_;
bool firstCall_; bool firstCall_;

6
guilib/src/CloudViewer.cpp
View File

@@ -753,12 +753,16 @@ bool CloudViewer::addOctomap(const OctoMap * octomap, unsigned int treeDepth)
pcl::IndicesPtr obstacles(new std::vector<int>); 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 " UWARN("Tree depth requested (%d) is deeper than the "
"actual maximum tree depth of %d. Using maximum depth.", "actual maximum tree depth of %d. Using maximum depth.",
(int)treeDepth, (int)octomap->octree()->getTreeDepth()); (int)treeDepth, (int)octomap->octree()->getTreeDepth());
} }
treeDepth = octomap->octree()->getTreeDepth();
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = octomap->createCloud(treeDepth, obstacles.get()); pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = octomap->createCloud(treeDepth, obstacles.get());
if(obstacles->size()) if(obstacles->size())

623
guilib/src/DatabaseViewer.cpp
View File

@@ -46,6 +46,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <opencv2/highgui/highgui.hpp> #include <opencv2/highgui/highgui.hpp>
#include <rtabmap/utilite/UTimer.h> #include <rtabmap/utilite/UTimer.h>
#include <rtabmap/utilite/UFile.h> #include <rtabmap/utilite/UFile.h>
#include "rtabmap/utilite/UPlot.h"
#include "rtabmap/core/DBDriver.h" #include "rtabmap/core/DBDriver.h"
#include "rtabmap/gui/KeypointItem.h" #include "rtabmap/gui/KeypointItem.h"
#include "rtabmap/gui/CloudViewer.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/RegistrationIcp.h"
#include "rtabmap/core/OccupancyGrid.h" #include "rtabmap/core/OccupancyGrid.h"
#include "rtabmap/gui/DataRecorder.h" #include "rtabmap/gui/DataRecorder.h"
#include "ExportCloudsDialog.h"
#include "rtabmap/core/SensorData.h" #include "rtabmap/core/SensorData.h"
#include "rtabmap/core/GainCompensator.h" #include "rtabmap/core/GainCompensator.h"
#include "ExportDialog.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/transforms.h>
#include <pcl/common/common.h> #include <pcl/common/common.h>
#ifdef RTABMAP_OCTOMAP
#include "rtabmap/core/OctoMap.h"
#endif
namespace rtabmap { namespace rtabmap {
DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) : DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) :
QMainWindow(parent), QMainWindow(parent),
dbDriver_(0), dbDriver_(0),
octomap_(0),
exportDialog_(new ExportCloudsDialog(this)),
savedMaximized_(false), savedMaximized_(false),
firstCall_(true), firstCall_(true),
iniFilePath_(ini) iniFilePath_(ini)
@@ -111,6 +119,7 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) :
ui_->dockWidget_constraints->setVisible(false); ui_->dockWidget_constraints->setVisible(false);
ui_->dockWidget_graphView->setVisible(false); ui_->dockWidget_graphView->setVisible(false);
ui_->dockWidget_occupancyGridView->setVisible(false);
ui_->dockWidget_guiparameters->setVisible(false); ui_->dockWidget_guiparameters->setVisible(false);
ui_->dockWidget_coreparameters->setVisible(false); ui_->dockWidget_coreparameters->setVisible(false);
ui_->dockWidget_info->setVisible(false); ui_->dockWidget_info->setVisible(false);
@@ -123,20 +132,29 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) :
cloudViewerA_ = new CloudViewer(ui_->dockWidgetContents_3dviews); cloudViewerA_ = new CloudViewer(ui_->dockWidgetContents_3dviews);
cloudViewerB_ = new CloudViewer(ui_->dockWidgetContents_3dviews); cloudViewerB_ = new CloudViewer(ui_->dockWidgetContents_3dviews);
stereoViewer_ = new CloudViewer(ui_->dockWidgetContents_stereo); stereoViewer_ = new CloudViewer(ui_->dockWidgetContents_stereo);
occupancyGridViewer_ = new CloudViewer(ui_->dockWidgetContents_occupancyGrid);
constraintsViewer_->setObjectName("constraintsViewer"); constraintsViewer_->setObjectName("constraintsViewer");
cloudViewerA_->setObjectName("cloudViewerA"); cloudViewerA_->setObjectName("cloudViewerA");
cloudViewerB_->setObjectName("cloudViewerB"); cloudViewerB_->setObjectName("cloudViewerB");
stereoViewer_->setObjectName("stereoViewer"); stereoViewer_->setObjectName("stereoViewer");
occupancyGridViewer_->setObjectName("occupancyGridView");
ui_->layout_constraintsViewer->addWidget(constraintsViewer_); ui_->layout_constraintsViewer->addWidget(constraintsViewer_);
ui_->horizontalLayout_3dviews->addWidget(cloudViewerA_, 1); ui_->horizontalLayout_3dviews->addWidget(cloudViewerA_, 1);
ui_->horizontalLayout_3dviews->addWidget(cloudViewerB_, 1); ui_->horizontalLayout_3dviews->addWidget(cloudViewerB_, 1);
ui_->horizontalLayout_stereo->addWidget(stereoViewer_, 1); ui_->horizontalLayout_stereo->addWidget(stereoViewer_, 1);
ui_->layout_occupancyGridView->addWidget(occupancyGridViewer_, 1);
constraintsViewer_->setCameraLockZ(false); constraintsViewer_->setCameraLockZ(false);
constraintsViewer_->setCameraFree(); constraintsViewer_->setCameraFree();
occupancyGridViewer_->setCameraFree();
ui_->graphicsView_stereo->setAlpha(255); ui_->graphicsView_stereo->setAlpha(255);
#ifndef RTABMAP_OCTOMAP
ui_->checkBox_octomap->setEnabled(false);
ui_->checkBox_octomap->setChecked(false);
#endif
ParametersMap parameters; ParametersMap parameters;
uInsert(parameters, Parameters::getDefaultParameters("SURF")); uInsert(parameters, Parameters::getDefaultParameters("SURF"));
uInsert(parameters, Parameters::getDefaultParameters("SIFT")); uInsert(parameters, Parameters::getDefaultParameters("SIFT"));
@@ -156,11 +174,16 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) :
uInsert(parameters, Parameters::getDefaultParameters("Grid")); uInsert(parameters, Parameters::getDefaultParameters("Grid"));
parameters.insert(*Parameters::getDefaultParameters().find(Parameters::kRGBDOptimizeMaxError())); parameters.insert(*Parameters::getDefaultParameters().find(Parameters::kRGBDOptimizeMaxError()));
ui_->parameters_toolbox->setupUi(parameters); ui_->parameters_toolbox->setupUi(parameters);
exportDialog_->setObjectName("ExportCloudsDialog");
this->readSettings(); 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_constraints->toggleViewAction());
ui_->menuView->addAction(ui_->dockWidget_graphView->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_stereoView->toggleViewAction());
ui_->menuView->addAction(ui_->dockWidget_view3d->toggleViewAction()); ui_->menuView->addAction(ui_->dockWidget_view3d->toggleViewAction());
ui_->menuView->addAction(ui_->dockWidget_guiparameters->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_info->toggleViewAction());
ui_->menuView->addAction(ui_->dockWidget_statistics->toggleViewAction()); ui_->menuView->addAction(ui_->dockWidget_statistics->toggleViewAction());
connect(ui_->dockWidget_graphView->toggleViewAction(), SIGNAL(triggered()), this, SLOT(updateGraphView())); 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 &))); 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_->checkBox_ignoreUserLoop, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
connect(ui_->spinBox_optimizationDepth, SIGNAL(editingFinished()), 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_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_->doubleSpinBox_gainCompensationRadius, SIGNAL(valueChanged(double)), this, SLOT(updateConstraintView()));
connect(ui_->groupBox_posefiltering, SIGNAL(clicked(bool)), this, SLOT(updateGraphView())); connect(ui_->groupBox_posefiltering, SIGNAL(clicked(bool)), this, SLOT(updateGraphView()));
connect(ui_->doubleSpinBox_posefilteringRadius, SIGNAL(editingFinished()), 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_->checkBox_ignoreUserLoop, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
connect(ui_->spinBox_optimizationDepth, SIGNAL(valueChanged(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_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_gainCompensationRadius, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
connect(ui_->doubleSpinBox_gridCellSize, 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())); 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_->doubleSpinBox_detectMore_angle, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
connect(ui_->spinBox_detectMore_iterations, SIGNAL(valueChanged(int)), this, SLOT(configModified())); connect(ui_->spinBox_detectMore_iterations, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
connect(exportDialog_, SIGNAL(configChanged()), this, SLOT(configModified()));
// dockwidget // dockwidget
QList<QDockWidget*> dockWidgets = this->findChildren<QDockWidget*>(); QList<QDockWidget*> dockWidgets = this->findChildren<QDockWidget*>();
for(int i=0; i<dockWidgets.size(); ++i) 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_constraints->installEventFilter(this);
ui_->dockWidget_graphView->installEventFilter(this); ui_->dockWidget_graphView->installEventFilter(this);
ui_->dockWidget_occupancyGridView->installEventFilter(this);
ui_->dockWidget_stereoView->installEventFilter(this); ui_->dockWidget_stereoView->installEventFilter(this);
ui_->dockWidget_view3d->installEventFilter(this); ui_->dockWidget_view3d->installEventFilter(this);
ui_->dockWidget_guiparameters->installEventFilter(this); ui_->dockWidget_guiparameters->installEventFilter(this);
@@ -326,6 +359,12 @@ DatabaseViewer::~DatabaseViewer()
{ {
delete dbDriver_; delete dbDriver_;
} }
#ifdef RTABMAP_OCTOMAP
if(octomap_)
{
delete octomap_;
}
#endif
} }
void DatabaseViewer::setupMainLayout(int vertical) 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_ignoreLocalLoopTime->setChecked(settings.value("ignoreLocalLoopTime", ui_->checkBox_ignoreLocalLoopTime->isChecked()).toBool());
ui_->checkBox_ignoreUserLoop->setChecked(settings.value("ignoreUserLoop", ui_->checkBox_ignoreUserLoop->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_->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()); ui_->doubleSpinBox_gainCompensationRadius->setValue(settings.value("gainCompensationRadius", ui_->doubleSpinBox_gainCompensationRadius->value()).toDouble());
settings.endGroup(); settings.endGroup();
@@ -409,6 +447,11 @@ void DatabaseViewer::readSettings()
ui_->groupBox_posefiltering->setChecked(settings.value("poseFiltering", ui_->groupBox_posefiltering->isChecked()).toBool()); 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_posefilteringRadius->setValue(settings.value("poseFilteringRadius", ui_->doubleSpinBox_posefilteringRadius->value()).toDouble());
ui_->doubleSpinBox_posefilteringAngle->setValue(settings.value("poseFilteringAngle", ui_->doubleSpinBox_posefilteringAngle->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.endGroup();
settings.beginGroup("mesh"); settings.beginGroup("mesh");
@@ -440,6 +483,8 @@ void DatabaseViewer::readSettings()
settings.endGroup(); // DatabaseViewer settings.endGroup(); // DatabaseViewer
exportDialog_->loadSettings(settings);
ParametersMap parameters; ParametersMap parameters;
Parameters::readINI(path.toStdString(), parameters); Parameters::readINI(path.toStdString(), parameters);
for(ParametersMap::iterator iter = parameters.begin(); iter!= parameters.end(); ++iter) 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("strategy", ui_->comboBox_graphOptimizer->currentIndex());
//settings.setValue("slam2d", ui_->checkBox_2dslam->isChecked()); //settings.setValue("slam2d", ui_->checkBox_2dslam->isChecked());
settings.setValue("depth", ui_->spinBox_optimizationDepth->value()); settings.setValue("depth", ui_->spinBox_optimizationDepth->value());
settings.setValue("erode", ui_->checkBox_gridErode->isChecked());
settings.setValue("gainCompensationRadius", ui_->doubleSpinBox_gainCompensationRadius->value()); settings.setValue("gainCompensationRadius", ui_->doubleSpinBox_gainCompensationRadius->value());
settings.endGroup(); settings.endGroup();
@@ -492,6 +536,8 @@ void DatabaseViewer::writeSettings()
settings.setValue("poseFiltering", ui_->groupBox_posefiltering->isChecked()); settings.setValue("poseFiltering", ui_->groupBox_posefiltering->isChecked());
settings.setValue("poseFilteringRadius", ui_->doubleSpinBox_posefilteringRadius->value()); settings.setValue("poseFilteringRadius", ui_->doubleSpinBox_posefilteringRadius->value());
settings.setValue("poseFilteringAngle", ui_->doubleSpinBox_posefilteringAngle->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.endGroup();
settings.beginGroup("mesh"); settings.beginGroup("mesh");
@@ -524,6 +570,8 @@ void DatabaseViewer::writeSettings()
settings.endGroup(); // DatabaseViewer settings.endGroup(); // DatabaseViewer
exportDialog_->saveSettings(settings);
ParametersMap parameters = ui_->parameters_toolbox->getParameters(); ParametersMap parameters = ui_->parameters_toolbox->getParameters();
for(ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end();) for(ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end();)
{ {
@@ -573,9 +621,11 @@ bool DatabaseViewer::openDatabase(const QString & path)
linksRefined_.clear(); linksRefined_.clear();
linksRemoved_.clear(); linksRemoved_.clear();
localMaps_.clear(); localMaps_.clear();
localMapsInfo_.clear();
generatedLocalMaps_.clear(); generatedLocalMaps_.clear();
generatedLocalMapsInfo_.clear(); generatedLocalMapsInfo_.clear();
ui_->graphViewer->clearAll(); ui_->graphViewer->clearAll();
occupancyGridViewer_->clear();
ui_->actionGenerate_TORO_graph_graph->setEnabled(false); ui_->actionGenerate_TORO_graph_graph->setEnabled(false);
ui_->actionGenerate_g2o_graph_g2o->setEnabled(false); ui_->actionGenerate_g2o_graph_g2o->setEnabled(false);
ui_->checkBox_showOptimized->setEnabled(false); ui_->checkBox_showOptimized->setEnabled(false);
@@ -779,6 +829,7 @@ void DatabaseViewer::closeEvent(QCloseEvent* event)
generatedLocalMaps_.clear(); generatedLocalMaps_.clear();
generatedLocalMapsInfo_.clear(); generatedLocalMapsInfo_.clear();
localMaps_.clear(); localMaps_.clear();
localMapsInfo_.clear();
} }
if(button != QMessageBox::Yes && button != QMessageBox::No) if(button != QMessageBox::Yes && button != QMessageBox::No)
@@ -803,7 +854,7 @@ void DatabaseViewer::showEvent(QShowEvent* anEvent)
{ {
this->setWindowModified(false); 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); sliderIterationsValueChanged((int)graphes_.size()-1);
} }
@@ -1392,7 +1443,7 @@ void DatabaseViewer::updateIds()
if(ids_.size()) if(ids_.size())
{ {
updateLoopClosuresSlider(); updateLoopClosuresSlider();
if(ui_->graphViewer->isVisible()) if(ui_->graphViewer->isVisible() || ui_->dockWidget_occupancyGridView->isVisible())
{ {
updateGraphView(); updateGraphView();
} }
@@ -1597,7 +1648,6 @@ void DatabaseViewer::generateG2OGraph()
void DatabaseViewer::regenerateLocalMaps() void DatabaseViewer::regenerateLocalMaps()
{ {
UTimer time;
OccupancyGrid grid(ui_->parameters_toolbox->getParameters()); OccupancyGrid grid(ui_->parameters_toolbox->getParameters());
generatedLocalMaps_.clear(); generatedLocalMaps_.clear();
@@ -1607,11 +1657,24 @@ void DatabaseViewer::regenerateLocalMaps()
progressDialog.setMaximumSteps(ids_.size()); progressDialog.setMaximumSteps(ids_.size());
progressDialog.show(); 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) for(int i =0; i<ids_.size(); ++i)
{ {
UTimer timer;
SensorData data; SensorData data;
dbDriver_->getNodeData(ids_.at(i), data); dbDriver_->getNodeData(ids_.at(i), data);
data.uncompressData(); data.uncompressData();
decompressionTime = timer.ticks()*1000.0;
int mapId, weight; int mapId, weight;
Transform odomPose, groundTruth; Transform odomPose, groundTruth;
@@ -1624,16 +1687,25 @@ void DatabaseViewer::regenerateLocalMaps()
s.setPose(odomPose); s.setPose(odomPose);
cv::Mat ground, obstacles; cv::Mat ground, obstacles;
cv::Point3f viewpoint; cv::Point3f viewpoint;
timer.ticks();
grid.createLocalMap(s, ground, obstacles, viewpoint); grid.createLocalMap(s, ground, obstacles, viewpoint);
gridCreationTime = timer.ticks()*1000.0;
uInsert(generatedLocalMaps_, std::make_pair(data.id(), std::make_pair(ground, obstacles))); 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))); 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.appendText(msg);
progressDialog.incrementStep(); progressDialog.incrementStep();
decompressionCurve->addValue(ids_.at(i), decompressionTime);
gridCreationCurve->addValue(ids_.at(i), gridCreationTime);
if(ids_.size() < 50 || (i+1) % 25 == 0)
{
QApplication::processEvents(); QApplication::processEvents();
} }
}
progressDialog.setValue(progressDialog.maximumSteps()); progressDialog.setValue(progressDialog.maximumSteps());
updateGrid(); updateGrid();
} }
@@ -1709,27 +1781,7 @@ void DatabaseViewer::view3DMap()
return; 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()); std::map<int, Transform> optimizedPoses = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
if(ui_->groupBox_posefiltering->isChecked()) if(ui_->groupBox_posefiltering->isChecked())
{ {
@@ -1739,94 +1791,20 @@ void DatabaseViewer::view3DMap()
} }
if(optimizedPoses.size() > 0) if(optimizedPoses.size() > 0)
{ {
rtabmap::ProgressDialog progressDialog(this); exportDialog_->setDBDriver(dbDriver_);
progressDialog.setMaximumSteps((int)optimizedPoses.size()); exportDialog_->viewClouds(optimizedPoses,
progressDialog.show(); updateLinksWithModifications(links_),
mapIds_,
// create a window QMap<int, Signature>(),
QDialog * window = new QDialog(this, Qt::Window); std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> >(),
window->setModal(this->isModal()); pathDatabase_,
window->setWindowTitle(tr("3D Map")); ui_->parameters_toolbox->getParameters());
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());
} }
else else
{ {
QMessageBox::critical(this, tr("Error"), tr("No neighbors found for node %1.").arg(ui_->spinBox_optimizationsFrom->value())); QMessageBox::critical(this, tr("Error"), tr("No neighbors found for node %1.").arg(ui_->spinBox_optimizationsFrom->value()));
} }
} }
}
}
void DatabaseViewer::view3DLaserScans() 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()); std::map<int, Transform> optimizedPoses = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
if(ui_->groupBox_posefiltering->isChecked()) if(ui_->groupBox_posefiltering->isChecked())
{ {
@@ -2008,97 +1944,20 @@ void DatabaseViewer::generate3DMap()
} }
if(optimizedPoses.size() > 0) if(optimizedPoses.size() > 0)
{ {
rtabmap::ProgressDialog progressDialog; exportDialog_->setDBDriver(dbDriver_);
progressDialog.setMaximumSteps((int)optimizedPoses.size()); exportDialog_->exportClouds(optimizedPoses,
progressDialog.show(); updateLinksWithModifications(links_),
mapIds_,
pcl::PointCloud<pcl::PointXYZRGB>::Ptr assembledCloud(new pcl::PointCloud<pcl::PointXYZRGB>); QMap<int, Signature>(),
for(std::map<int, Transform>::const_iterator iter = optimizedPoses.begin(); iter!=optimizedPoses.end(); ++iter) std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> >(),
{ pathDatabase_,
const rtabmap::Transform & pose = iter->second; ui_->parameters_toolbox->getParameters());
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());
} }
else else
{ {
QMessageBox::critical(this, tr("Error"), tr("No neighbors found for node %1.").arg(ui_->spinBox_optimizationsFrom->value())); QMessageBox::critical(this, tr("Error"), tr("No neighbors found for node %1.").arg(ui_->spinBox_optimizationsFrom->value()));
} }
} }
}
}
}
void DatabaseViewer::generate3DLaserScans() void DatabaseViewer::generate3DLaserScans()
{ {
@@ -2524,6 +2383,7 @@ void DatabaseViewer::update(int value,
view3D->removeCloud("map"); view3D->removeCloud("map");
view3D->removeCloud("ground"); view3D->removeCloud("ground");
view3D->removeCloud("obstacles"); view3D->removeCloud("obstacles");
view3D->removeOctomap();
if(ui_->checkBox_showCloud->isChecked() || ui_->checkBox_showMesh->isChecked()) if(ui_->checkBox_showCloud->isChecked() || ui_->checkBox_showMesh->isChecked())
{ {
if(!data.depthOrRightRaw().empty()) if(!data.depthOrRightRaw().empty())
@@ -2642,37 +2502,93 @@ void DatabaseViewer::update(int value,
if(ui_->checkBox_showMap->isChecked() || ui_->checkBox_showGrid->isChecked()) if(ui_->checkBox_showMap->isChecked() || ui_->checkBox_showGrid->isChecked())
{ {
std::map<int, std::pair<cv::Mat, cv::Mat> > localMaps; 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()) if(generatedLocalMaps_.find(data.id()) != generatedLocalMaps_.end())
{ {
localMaps.insert(*generatedLocalMaps_.find(data.id())); localMaps.insert(*generatedLocalMaps_.find(data.id()));
localMapsInfo.insert(*generatedLocalMapsInfo_.find(data.id()));
} }
else if(!data.gridGroundCellsRaw().empty() && !data.gridObstacleCellsRaw().empty()) else if(!data.gridGroundCellsRaw().empty() && !data.gridObstacleCellsRaw().empty())
{ {
localMaps.insert(std::make_pair(data.id(), std::make_pair(data.gridGroundCellsRaw(), data.gridObstacleCellsRaw()))); 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()) if(!localMaps.empty())
{ {
std::map<int, Transform> poses; std::map<int, Transform> poses;
poses.insert(std::make_pair(data.id(), Transform::getIdentity())); 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()) if(ui_->checkBox_showMap->isChecked())
{ {
float xMin=0.0f, yMin=0.0f; 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, poses,
localMaps, localMaps,
ui_->doubleSpinBox_gridCellSize->value(), ui_->doubleSpinBox_gridCellSize->value(),
xMin, yMin); xMin, yMin);
}
if(!map8S.empty()) if(!map8S.empty())
{ {
//convert to gray scaled map //convert to gray scaled map
cv::Mat map8U = util3d::convertMap2Image8U(map8S); view3D->addOccupancyGridMap(util3d::convertMap2Image8U(map8S), gridCellSize, xMin, yMin, 1);
view3D->addOccupancyGridMap(map8U, ui_->doubleSpinBox_gridCellSize->value(), xMin, yMin, 1);
} }
} }
if(ui_->checkBox_showGrid->isChecked()) 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 // occupancy cloud
view3D->addCloud("ground", view3D->addCloud("ground",
util3d::laserScanToPointCloud(localMaps.begin()->second.first), util3d::laserScanToPointCloud(localMaps.begin()->second.first),
@@ -2686,6 +2602,13 @@ void DatabaseViewer::update(int value,
view3D->setCloudPointSize("obstacles", 5); view3D->setCloudPointSize("obstacles", 5);
} }
} }
#ifdef RTABMAP_OCTOMAP
if(octomap)
{
delete octomap;
}
#endif
}
} }
view3D->update(); view3D->update();
} }
@@ -3741,7 +3664,15 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
ui_->doubleSpinBox_posefilteringAngle->value()*CV_PI/180.0); ui_->doubleSpinBox_posefilteringAngle->value()*CV_PI/180.0);
} }
std::map<int, std::pair<cv::Mat, cv::Mat> > localMaps; 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 //update scans
UINFO("Update local maps list..."); UINFO("Update local maps list...");
@@ -3751,12 +3682,14 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
if(generatedLocalMaps_.find(ids[i]) != generatedLocalMaps_.end()) if(generatedLocalMaps_.find(ids[i]) != generatedLocalMaps_.end())
{ {
localMaps.insert(*generatedLocalMaps_.find(ids[i])); localMaps.insert(*generatedLocalMaps_.find(ids[i]));
localMapsInfo.insert(*generatedLocalMapsInfo_.find(ids[i]));
} }
else if(localMaps_.find(ids[i]) != localMaps_.end()) else if(localMaps_.find(ids[i]) != localMaps_.end())
{ {
if(!localMaps_.find(ids[i])->second.first.empty() || !localMaps_.find(ids[i])->second.first.empty()) if(!localMaps_.find(ids[i])->second.first.empty() || !localMaps_.find(ids[i])->second.first.empty())
{ {
localMaps.insert(*localMaps_.find(ids.at(i))); localMaps.insert(*localMaps_.find(ids.at(i)));
localMapsInfo.insert(*localMapsInfo_.find(ids[i]));
} }
} }
else else
@@ -3766,9 +3699,11 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
cv::Mat ground, obstacles; cv::Mat ground, obstacles;
data.uncompressData(0, 0, 0, 0, &ground, &obstacles); data.uncompressData(0, 0, 0, 0, &ground, &obstacles);
localMaps_.insert(std::make_pair(ids.at(i), std::make_pair(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()) if(!ground.empty() || !obstacles.empty())
{ {
localMaps.insert(std::make_pair(ids.at(i), std::make_pair(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())));
} }
} }
} }
@@ -3777,6 +3712,7 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
{ {
if(graphFiltered.find(iter->first) == graphFiltered.end()) if(graphFiltered.find(iter->first) == graphFiltered.end())
{ {
localMapsInfo_.erase(iter->first);
localMaps_.erase(iter++); localMaps_.erase(iter++);
} }
else else
@@ -3789,19 +3725,159 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
ui_->graphViewer->updateGTGraph(groundTruthPoses_); ui_->graphViewer->updateGTGraph(groundTruthPoses_);
ui_->graphViewer->updateGraph(graph, graphLinks_, mapIds_); 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 xMin, yMin;
float cell = ui_->doubleSpinBox_gridCellSize->value(); float cell = ui_->doubleSpinBox_gridCellSize->value();
cv::Mat map; 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()); 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()) 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_->graphViewer->update();
ui_->label_iterations->setNum(value); ui_->label_iterations->setNum(value);
@@ -4047,9 +4123,64 @@ void DatabaseViewer::updateGraphView()
void DatabaseViewer::updateGrid() 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(); update3dView();
updateGraphView(); 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) Link DatabaseViewer::findActiveLink(int from, int to)
{ {

288
guilib/src/ExportCloudsDialog.cpp
View File

@@ -43,6 +43,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/Graph.h" #include "rtabmap/core/Graph.h"
#include "rtabmap/core/GainCompensator.h" #include "rtabmap/core/GainCompensator.h"
#include "rtabmap/core/clams/discrete_depth_distortion_model.h" #include "rtabmap/core/clams/discrete_depth_distortion_model.h"
#include "rtabmap/core/DBDriver.h"
#include <pcl/conversions.h> #include <pcl/conversions.h>
#include <pcl/io/pcd_io.h> #include <pcl/io/pcd_io.h>
@@ -65,7 +66,8 @@ namespace rtabmap {
ExportCloudsDialog::ExportCloudsDialog(QWidget *parent) : ExportCloudsDialog::ExportCloudsDialog(QWidget *parent) :
QDialog(parent), QDialog(parent),
_canceled(false), _canceled(false),
_compensator(0) _compensator(0),
_dbDriver(0)
{ {
_ui = new Ui_ExportCloudsDialog(); _ui = new Ui_ExportCloudsDialog();
_ui->setupUi(this); _ui->setupUi(this);
@@ -392,9 +394,9 @@ void ExportCloudsDialog::restoreDefaults()
{ {
_ui->comboBox_pipeline->setCurrentIndex(1); _ui->comboBox_pipeline->setCurrentIndex(1);
_ui->checkBox_binary->setChecked(true); _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->spinBox_decimation->setValue(1);
_ui->doubleSpinBox_maxDepth->setValue(4); _ui->doubleSpinBox_maxDepth->setValue(4);
_ui->doubleSpinBox_minDepth->setValue(0); _ui->doubleSpinBox_minDepth->setValue(0);
@@ -749,8 +751,17 @@ void ExportCloudsDialog::viewClouds(
uIsInteger(mesh->tex_materials[0].tex_file, false)) uIsInteger(mesh->tex_materials[0].tex_file, false))
{ {
int textureId = uStr2Int(mesh->tex_materials[0].tex_file); int textureId = uStr2Int(mesh->tex_materials[0].tex_file);
UASSERT(cachedSignatures.contains(textureId) && !cachedSignatures.value(textureId).sensorData().imageCompressed().empty()); SensorData data;
cachedSignatures.value(textureId).sensorData().uncompressDataConst(&globalTexture, 0); 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()); UASSERT(!globalTexture.empty());
if (_ui->checkBox_gainCompensation->isChecked() && _compensator && _compensator->getIndex(textureId) >= 0) if (_ui->checkBox_gainCompensation->isChecked() && _compensator && _compensator->getIndex(textureId) >= 0)
{ {
@@ -864,7 +875,11 @@ bool ExportCloudsDialog::getExportedClouds(
{ {
_canceled = false; _canceled = false;
_workingDirectory = workingDirectory; _workingDirectory = workingDirectory;
enableRegeneration(cachedSignatures.size()); enableRegeneration(_dbDriver || cachedSignatures.size());
if(cachedSignatures.empty() && _dbDriver)
{
_ui->checkBox_regenerate->setChecked(true);
}
if(_compensator) if(_compensator)
{ {
delete _compensator; delete _compensator;
@@ -1073,17 +1088,26 @@ bool ExportCloudsDialog::getExportedClouds(
// Adjust view points with local transforms // Adjust view points with local transforms
for(std::map<int, Transform>::iterator iter= viewPoints.begin(); iter!=viewPoints.end(); ++iter) for(std::map<int, Transform>::iterator iter= viewPoints.begin(); iter!=viewPoints.end(); ++iter)
{ {
std::vector<CameraModel> models;
StereoCameraModel stereoModel;
if(cachedSignatures.contains(iter->first)) if(cachedSignatures.contains(iter->first))
{ {
const SensorData & data = cachedSignatures.find(iter->first)->sensorData(); const SensorData & data = cachedSignatures.find(iter->first)->sensorData();
if(data.cameraModels().size() && !data.cameraModels()[0].localTransform().isNull()) models = data.cameraModels();
{ stereoModel = data.stereoCameraModel();
iter->second *= data.cameraModels()[0].localTransform();
} }
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()) if(iter->second->size())
{ {
Eigen::Vector3f viewpoint(0.0f,0.0f,0.0f); Eigen::Vector3f viewpoint(0.0f,0.0f,0.0f);
std::vector<CameraModel> models;
StereoCameraModel stereoModel;
if(cachedSignatures.contains(iter->first)) if(cachedSignatures.contains(iter->first))
{ {
const SensorData & data = cachedSignatures.find(iter->first)->sensorData(); 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(); _dbDriver->getCalibration(iter->first, models, stereoModel);
viewpoint[1] = data.cameraModels()[0].localTransform().y();
viewpoint[2] = data.cameraModels()[0].localTransform().z();
} }
else if(!data.stereoCameraModel().localTransform().isNull())
if(models.size() && !models[0].localTransform().isNull())
{ {
viewpoint[0] = data.stereoCameraModel().localTransform().x(); viewpoint[0] = models[0].localTransform().x();
viewpoint[1] = data.stereoCameraModel().localTransform().y(); viewpoint[1] = models[0].localTransform().y();
viewpoint[2] = data.stereoCameraModel().localTransform().z(); 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( std::vector<pcl::Vertices> polygons = util3d::organizedFastMesh(
iter->second, iter->second,
_ui->doubleSpinBox_mesh_angleTolerance->value()*M_PI/180.0, _ui->doubleSpinBox_mesh_angleTolerance->value()*M_PI/180.0,
@@ -1766,29 +1801,52 @@ bool ExportCloudsDialog::getExportedClouds(
std::map<int, CameraModel> cameraModels; std::map<int, CameraModel> cameraModels;
for(std::map<int, Transform>::iterator jter=cameras.begin(); jter!=cameras.end(); ++jter) 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)) 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; 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) if(model.imageWidth() == 0 || model.imageHeight() == 0)
{ {
// we are using an old database format (image size not saved in calibrations), we should // we are using an old database format (image size not saved in calibrations), we should
// uncompress images to get their size // uncompress images to get their size
cv::Mat img; 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()); model.setImageSize(img.size());
} }
if(model.imageWidth() != 0 && model.imageHeight() != 0)
{
cameraPoses.insert(std::make_pair(jter->first, jter->second)); cameraPoses.insert(std::make_pair(jter->first, jter->second));
cameraModels.insert(std::make_pair(jter->first, model)); 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>); pcl::IndicesPtr indices(new std::vector<int>);
if(_ui->checkBox_regenerate->isChecked()) if(_ui->checkBox_regenerate->isChecked())
{ {
SensorData data;
cv::Mat image, depth;
if(cachedSignatures.contains(iter->first)) if(cachedSignatures.contains(iter->first))
{ {
const Signature & s = cachedSignatures.find(iter->first).value(); const Signature & s = cachedSignatures.find(iter->first).value();
SensorData d = s.sensorData(); data = s.sensorData();
cv::Mat image, depth; data.uncompressData(&image, &depth, 0);
d.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(!image.empty() && !depth.empty())
{ {
if(_ui->spinBox_fillDepthHoles->value() > 0) 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()); model.load(_ui->lineEdit_distortionModel->text().toStdString());
depth = depth.clone();// make sure we are not modifying data in cached signatures. depth = depth.clone();// make sure we are not modifying data in cached signatures.
model.undistort(depth); model.undistort(depth);
d.setDepthOrRightRaw(depth); data.setDepthOrRightRaw(depth);
} }
// bilateral filtering // bilateral filtering
@@ -2125,10 +2191,10 @@ std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr, pcl::Indic
depth = util2d::fastBilateralFiltering(depth, depth = util2d::fastBilateralFiltering(depth,
_ui->doubleSpinBox_bilateral_sigmaS->value(), _ui->doubleSpinBox_bilateral_sigmaS->value(),
_ui->doubleSpinBox_bilateral_sigmaR->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; pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudWithoutNormals;
std::vector<float> roiRatios; std::vector<float> roiRatios;
if(!_ui->lineEdit_roiRatios->text().isEmpty()) 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( cloudWithoutNormals = util3d::cloudRGBFromSensorData(
d, data,
_ui->spinBox_decimation->value() == 0?1:_ui->spinBox_decimation->value(), _ui->spinBox_decimation->value() == 0?1:_ui->spinBox_decimation->value(),
_ui->doubleSpinBox_maxDepth->value(), _ui->doubleSpinBox_maxDepth->value(),
_ui->doubleSpinBox_minDepth->value(), _ui->doubleSpinBox_minDepth->value(),
@@ -2167,17 +2233,17 @@ std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr, pcl::Indic
// view point // view point
Eigen::Vector3f viewPoint(0.0f,0.0f,0.0f); 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[0] = data.cameraModels()[0].localTransform().x();
viewPoint[1] = d.cameraModels()[0].localTransform().y(); viewPoint[1] = data.cameraModels()[0].localTransform().y();
viewPoint[2] = d.cameraModels()[0].localTransform().z(); 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[0] = data.stereoCameraModel().localTransform().x();
viewPoint[1] = d.stereoCameraModel().localTransform().y(); viewPoint[1] = data.stereoCameraModel().localTransform().y();
viewPoint[2] = d.stereoCameraModel().localTransform().z(); viewPoint[2] = data.stereoCameraModel().localTransform().z();
} }
pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloudWithoutNormals, indices, _ui->spinBox_normalKSearch->value(), viewPoint); 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 else
{ {
UERROR("Cloud %d not found in cache!", iter->first); 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 // view point
Eigen::Vector3f viewPoint(0.0f,0.0f,0.0f); Eigen::Vector3f viewPoint(0.0f,0.0f,0.0f);
std::vector<CameraModel> models;
StereoCameraModel stereoModel;
if(cachedSignatures.contains(iter->first)) if(cachedSignatures.contains(iter->first))
{ {
const Signature & s = cachedSignatures.find(iter->first).value(); const Signature & s = cachedSignatures.find(iter->first).value();
SensorData d = s.sensorData(); models = s.sensorData().cameraModels();
if(d.cameraModels().size() && !d.cameraModels()[0].localTransform().isNull()) stereoModel = s.sensorData().stereoCameraModel();
{
viewPoint[0] = d.cameraModels()[0].localTransform().x();
viewPoint[1] = d.cameraModels()[0].localTransform().y();
viewPoint[2] = d.cameraModels()[0].localTransform().z();
} }
else if(!d.stereoCameraModel().localTransform().isNull()) else if(_dbDriver)
{ {
viewPoint[0] = d.stereoCameraModel().localTransform().x(); _dbDriver->getCalibration(iter->first, models, stereoModel);
viewPoint[1] = d.stereoCameraModel().localTransform().y();
viewPoint[2] = d.stereoCameraModel().localTransform().z();
} }
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 else
{ {
@@ -2605,7 +2678,7 @@ cv::Mat ExportCloudsDialog::mergeTextures(pcl::TextureMesh & mesh, const QMap<in
{ {
std::vector<int> textures(mesh.tex_materials.size(), -1); std::vector<int> textures(mesh.tex_materials.size(), -1);
cv::Size imageSize; cv::Size imageSize;
int imageType=CV_8UC1; const int imageType=CV_8UC3;
UDEBUG(""); UDEBUG("");
bool mergeTextures = true; bool mergeTextures = true;
for(unsigned int i=0; i<mesh.tex_materials.size(); ++i) 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); int textureId = uStr2Int(mesh.tex_materials[i].tex_file);
textures[i] = textureId; textures[i] = textureId;
QMap<int, Signature>::const_iterator iter = cachedSignatures.find(textureId); if(imageSize.width == 0 || imageSize.height == 0)
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)
{ {
tmpImageSize = iter->sensorData().cameraModels()[0].imageSize(); if(cachedSignatures.find(textureId)!=cachedSignatures.end() && !cachedSignatures.find(textureId)->sensorData().imageCompressed().empty())
if(imageSize.height == 0 && imageSize.width == 0)
{ {
// just for the first image, get the type, assuming all others have the same type SensorData data = cachedSignatures.find(textureId).value().sensorData();
cv::Mat image; if(data.cameraModels().size()==1 &&
iter->sensorData().uncompressDataConst(&image, 0); data.cameraModels()[0].imageHeight()>0 &&
UASSERT(!image.empty()); data.cameraModels()[0].imageWidth()>0)
imageType = image.type(); {
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 else // backward compatibility for image size not set in CameraModel
{ {
cv::Mat image; cv::Mat image;
iter->sensorData().uncompressDataConst(&image, 0); data.uncompressDataConst(&image, 0);
UASSERT(!image.empty()); UASSERT(!image.empty());
tmpImageSize = image.size(); imageSize = image.size();
if(imageSize.height == 0 && imageSize.width == 0) }
}
else if(_dbDriver)
{ {
imageType = image.type(); std::vector<CameraModel> models;
} StereoCameraModel stereoModel;
} _dbDriver->getCalibration(textureId, models, stereoModel);
if(imageSize.width>0 && imageSize.height>0 && imageSize.width != tmpImageSize.width) if(models.size()==1 &&
models[0].imageHeight()>0 &&
models[0].imageWidth()>0)
{ {
UWARN("All images should have the same dimensions to merge the textures!"); imageSize = models[0].imageSize();
mergeTextures = false; }
break; 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) 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); UASSERT(v < textureSize-emptyImage.rows);
if(textures[t]>=0) 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; 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()); UASSERT(!image.empty());
cv::Mat resizedImage; cv::Mat resizedImage;
cv::resize(image, resizedImage, emptyImage.size(), 0.0f, 0.0f, cv::INTER_AREA); 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) if(_ui->checkBox_gainCompensation->isChecked() && _compensator && _compensator->getIndex(textures[t]) >= 0)
{ {
_compensator->apply(textures[t], resizedImage); _compensator->apply(textures[t], resizedImage);
@@ -2766,9 +2871,17 @@ void ExportCloudsDialog::saveTextureMeshes(
if(uIsInteger(mesh->tex_materials[i].tex_file, false)) if(uIsInteger(mesh->tex_materials[i].tex_file, false))
{ {
int textureId = uStr2Int(mesh->tex_materials[i].tex_file); int textureId = uStr2Int(mesh->tex_materials[i].tex_file);
UASSERT(cachedSignatures.contains(textureId) && !cachedSignatures.value(textureId).sensorData().imageCompressed().empty());
cv::Mat image; cv::Mat image;
if(cachedSignatures.contains(textureId) && !cachedSignatures.value(textureId).sensorData().imageCompressed().empty())
{
cachedSignatures.value(textureId).sensorData().uncompressDataConst(&image, 0); 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()); UASSERT(!image.empty());
imageSize = image.size(); imageSize = image.size();
if(_ui->checkBox_gainCompensation->isChecked() && _compensator && _compensator->getIndex(textureId) >= 0) 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)) if(uIsInteger(mesh->tex_materials[i].tex_file, false))
{ {
int textureId = uStr2Int(mesh->tex_materials[i].tex_file); int textureId = uStr2Int(mesh->tex_materials[i].tex_file);
UASSERT(cachedSignatures.contains(textureId) && !cachedSignatures.value(textureId).sensorData().imageCompressed().empty());
cv::Mat image; cv::Mat image;
if(cachedSignatures.contains(textureId) && !cachedSignatures.value(textureId).sensorData().imageCompressed().empty())
{
cachedSignatures.value(textureId).sensorData().uncompressDataConst(&image, 0); 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()); UASSERT(!image.empty());
imageSize = image.size(); imageSize = image.size();
if(_ui->checkBox_gainCompensation->isChecked() && _compensator && _compensator->getIndex(textureId) >= 0) if(_ui->checkBox_gainCompensation->isChecked() && _compensator && _compensator->getIndex(textureId) >= 0)

4
guilib/src/ExportCloudsDialog.h
View File

@@ -47,6 +47,7 @@ class QAbstractButton;
namespace rtabmap { namespace rtabmap {
class ProgressDialog; class ProgressDialog;
class GainCompensator; class GainCompensator;
class DBDriver;
class ExportCloudsDialog : public QDialog class ExportCloudsDialog : public QDialog
{ {
@@ -60,6 +61,8 @@ public:
void saveSettings(QSettings & settings, const QString & group = "") const; void saveSettings(QSettings & settings, const QString & group = "") const;
void loadSettings(QSettings & settings, const QString & group = ""); void loadSettings(QSettings & settings, const QString & group = "");
void setDBDriver(const DBDriver * dbDriver) {_dbDriver = dbDriver;}
void exportClouds( void exportClouds(
const std::map<int, Transform> & poses, const std::map<int, Transform> & poses,
const std::multimap<int, Link> & links, const std::multimap<int, Link> & links,
@@ -124,6 +127,7 @@ private:
QString _workingDirectory; QString _workingDirectory;
bool _canceled; bool _canceled;
GainCompensator * _compensator; GainCompensator * _compensator;
const DBDriver * _dbDriver;
}; };
} }

122
guilib/src/ui/DatabaseViewer.ui
View File

@@ -6,7 +6,7 @@
<rect> <rect>
<x>0</x> <x>0</x>
<y>0</y> <y>0</y>
<width>1285</width> <width>1392</width>
<height>965</height> <height>965</height>
</rect> </rect>
</property> </property>
@@ -52,7 +52,7 @@
<rect> <rect>
<x>0</x> <x>0</x>
<y>0</y> <y>0</y>
<width>253</width> <width>339</width>
<height>208</height> <height>208</height>
</rect> </rect>
</property> </property>
@@ -210,7 +210,7 @@
<rect> <rect>
<x>0</x> <x>0</x>
<y>0</y> <y>0</y>
<width>253</width> <width>339</width>
<height>208</height> <height>208</height>
</rect> </rect>
</property> </property>
@@ -483,7 +483,7 @@
<rect> <rect>
<x>0</x> <x>0</x>
<y>0</y> <y>0</y>
<width>1285</width> <width>1392</width>
<height>22</height> <height>22</height>
</rect> </rect>
</property> </property>
@@ -987,7 +987,7 @@
<item> <item>
<widget class="QToolBox" name="toolBox"> <widget class="QToolBox" name="toolBox">
<property name="currentIndex"> <property name="currentIndex">
<number>3</number> <number>1</number>
</property> </property>
<widget class="QWidget" name="page_3"> <widget class="QWidget" name="page_3">
<property name="geometry"> <property name="geometry">
@@ -1131,7 +1131,7 @@
<x>0</x> <x>0</x>
<y>0</y> <y>0</y>
<width>280</width> <width>280</width>
<height>584</height> <height>608</height>
</rect> </rect>
</property> </property>
<attribute name="label"> <attribute name="label">
@@ -1174,7 +1174,7 @@
</property> </property>
</widget> </widget>
</item> </item>
<item row="2" column="1"> <item row="3" column="1">
<widget class="QLabel" name="label_51"> <widget class="QLabel" name="label_51">
<property name="text"> <property name="text">
<string>Gain compensation radius (Constraints view)</string> <string>Gain compensation radius (Constraints view)</string>
@@ -1184,7 +1184,7 @@
</property> </property>
</widget> </widget>
</item> </item>
<item row="2" column="0"> <item row="3" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_gainCompensationRadius"> <widget class="QDoubleSpinBox" name="doubleSpinBox_gainCompensationRadius">
<property name="suffix"> <property name="suffix">
<string> m</string> <string> m</string>
@@ -1206,6 +1206,20 @@
</property> </property>
</widget> </widget>
</item> </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> </layout>
</item> </item>
<item> <item>
@@ -1547,8 +1561,8 @@
<rect> <rect>
<x>0</x> <x>0</x>
<y>0</y> <y>0</y>
<width>333</width> <width>186</width>
<height>256</height> <height>496</height>
</rect> </rect>
</property> </property>
<attribute name="label"> <attribute name="label">
@@ -1911,6 +1925,94 @@
</layout> </layout>
</widget> </widget>
</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"> <action name="actionOpen_database">
<property name="text"> <property name="text">
<string>Open database</string> <string>Open database</string>