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

0.16.0: Database updated with Data.empty_cells, Admin.opt_map, Admin.opt_map_x_min and Admin.opt_map_y_min fields. Changed Parameter Grid/ProjRayTracing to Grid/RayTracing (OctoMap ray tracing done for 3D local grids). Improved OctoMap performance.

Browse Source
This commit is contained in:
matlabbe
2018-02-08 21:40:17 -05:00
parent e7ceacc215
commit fced2c521c
32 changed files with 1642 additions and 600 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -142,7 +142,7 @@ public:
const cv::Mat & texture,
const Transform & pose = Transform::getIdentity());
bool addOctomap(const OctoMap * octomap, unsigned int treeDepth = 0);
bool addOctomap(const OctoMap * octomap, unsigned int treeDepth = 0, bool volumeRepresentation = true);
void removeOctomap();
// Only one texture per mesh is supported!

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

@@ -196,9 +196,9 @@ private:
std::multimap<int, rtabmap::Link> linksRefined_;
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<std::pair<cv::Mat, cv::Mat>, cv::Mat> > localMaps_; // < <ground, obstacles>, empty>
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<std::pair<cv::Mat, cv::Mat>, cv::Mat> > generatedLocalMaps_; // < <ground, obstacles>, empty>
std::map<int, std::pair<float, cv::Point3f> > generatedLocalMapsInfo_; // <cell size, viewpoint>
std::map<int, cv::Mat> modifiedDepthImages_;
OctoMap * octomap_;

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

@@ -255,7 +255,7 @@ private:
std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> createAndAddCloudToMap(int nodeId, const Transform & pose, int mapId);
void createAndAddScanToMap(int nodeId, const Transform & pose, int mapId);
void createAndAddFeaturesToMap(int nodeId, const Transform & pose, int mapId);
Transform alignPosesToGroundTruth(std::map<int, Transform> & poses, const std::map<int, Transform> & groundTruth, double stamp = 0.0, int refId = -1);
Transform alignPosesToGroundTruth(const std::map<int, Transform> & poses, const std::map<int, Transform> & groundTruth);
void drawKeypoints(const std::multimap<int, cv::KeyPoint> & refWords, const std::multimap<int, cv::KeyPoint> & loopWords);
void setupMainLayout(bool vertical);
void updateSelectSourceMenu();

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

@@ -173,10 +173,9 @@ public:
bool isCloudsShown(int index) const; // 0=map, 1=odom
bool isOctomapUpdated() const;
bool isOctomapShown() const;
bool isOctomapCubeRendering() const;
int getOctomapRenderingType() const;
bool isOctomap2dGrid() const;
int getOctomapTreeDepth() const;
bool isOctomapFullUpdate() const;
double getOctomapOccupancyThr() const;
int getOctomapPointSize() const;
int getCloudDecimation(int index) const; // 0=map, 1=odom
@@ -205,7 +204,6 @@ public:
double getSubtractFilteringAngle() const;
bool getGridMapShown() const;
double getGridMapResolution() const;;
bool isGridMapFrom3DCloud() const;
bool projMapFrame() const;
double projMaxGroundAngle() const;
@@ -305,7 +303,6 @@ private slots:
void updateKpROI();
void updateStereoDisparityVisibility();
void useOdomFeatures();
void useGridProjRayTracing();
void changeWorkingDirectory();
void changeDictionaryPath();
void changeOdometryORBSLAM2Vocabulary();

243
guilib/src/CloudViewer.cpp
View File

@@ -52,6 +52,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <vtkCubeSource.h>
#include <vtkGlyph3D.h>
#include <vtkGlyph3DMapper.h>
#include <vtkSmartVolumeMapper.h>
#include <vtkVolumeProperty.h>
#include <vtkColorTransferFunction.h>
#include <vtkPiecewiseFunction.h>
#include <vtkImageData.h>
#include <vtkLookupTable.h>
#include <vtkTextureUnitManager.h>
#include <vtkJPEGReader.h>
@@ -792,7 +797,7 @@ bool CloudViewer::addCloudTextureMesh(
return false;
}
bool CloudViewer::addOctomap(const OctoMap * octomap, unsigned int treeDepth)
bool CloudViewer::addOctomap(const OctoMap * octomap, unsigned int treeDepth, bool volumeRepresentation)
{
UDEBUG("");
#ifdef RTABMAP_OCTOMAP
@@ -811,78 +816,190 @@ bool CloudViewer::addOctomap(const OctoMap * octomap, unsigned int treeDepth)
treeDepth = octomap->octree()->getTreeDepth();
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = octomap->createCloud(treeDepth, obstacles.get());
if(obstacles->size())
//get the renderer of the visualizer object
vtkRenderer *renderer = _visualizer->getRenderWindow()->GetRenderers()->GetFirstRenderer();
if(!volumeRepresentation)
{
//get the renderer of the visualizer object
vtkRenderer *renderer = _visualizer->getRenderWindow()->GetRenderers()->GetFirstRenderer();
if(_octomapActor)
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = octomap->createCloud(treeDepth, obstacles.get(), 0, 0, false);
if(obstacles->size())
{
renderer->RemoveActor(_octomapActor);
_octomapActor = 0;
if(_octomapActor)
{
renderer->RemoveActor(_octomapActor);
_octomapActor = 0;
}
//vtkSmartPointer<vtkUnsignedCharArray> colors = vtkSmartPointer<vtkUnsignedCharArray>::New();
//colors->SetName("colors");
//colors->SetNumberOfComponents(3);
vtkSmartPointer<vtkFloatArray> colors = vtkSmartPointer<vtkFloatArray>::New();
colors->SetName("colors");
colors->SetNumberOfValues(obstacles->size());
vtkSmartPointer<vtkLookupTable> lut = vtkSmartPointer<vtkLookupTable>::New();
lut->SetNumberOfTableValues(obstacles->size());
lut->Build();
// Create points
vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New();
points->SetNumberOfPoints(obstacles->size());
double s = octomap->octree()->getNodeSize(treeDepth) / 2.0;
for (unsigned int i = 0; i < obstacles->size(); i++)
{
points->InsertPoint(i,
cloud->at(obstacles->at(i)).x,
cloud->at(obstacles->at(i)).y,
cloud->at(obstacles->at(i)).z);
colors->InsertValue(i,i);
lut->SetTableValue(i,
double(cloud->at(obstacles->at(i)).r) / 255.0,
double(cloud->at(obstacles->at(i)).g) / 255.0,
double(cloud->at(obstacles->at(i)).b) / 255.0);
}
// Combine into a polydata
vtkSmartPointer<vtkPolyData> polydata = vtkSmartPointer<vtkPolyData>::New();
polydata->SetPoints(points);
polydata->GetPointData()->SetScalars(colors);
// Create anything you want here, we will use a cube for the demo.
vtkSmartPointer<vtkCubeSource> cubeSource = vtkSmartPointer<vtkCubeSource>::New();
cubeSource->SetBounds(-s, s, -s, s, -s, s);
vtkSmartPointer<vtkGlyph3DMapper> mapper = vtkSmartPointer<vtkGlyph3DMapper>::New();
mapper->SetSourceConnection(cubeSource->GetOutputPort());
#if VTK_MAJOR_VERSION <= 5
mapper->SetInputConnection(polydata->GetProducerPort());
#else
mapper->SetInputData(polydata);
#endif
mapper->SetScalarRange(0, obstacles->size() - 1);
mapper->SetLookupTable(lut);
mapper->ScalingOff();
mapper->Update();
vtkSmartPointer<vtkActor> octomapActor = vtkSmartPointer<vtkActor>::New();
octomapActor->SetMapper(mapper);
octomapActor->GetProperty()->SetRepresentationToSurface();
octomapActor->GetProperty()->SetEdgeVisibility(_aSetEdgeVisibility->isChecked());
octomapActor->GetProperty()->SetLighting(_aSetLighting->isChecked());
renderer->AddActor(octomapActor);
_octomapActor = octomapActor.GetPointer();
return true;
}
//vtkSmartPointer<vtkUnsignedCharArray> colors = vtkSmartPointer<vtkUnsignedCharArray>::New();
//colors->SetName("colors");
//colors->SetNumberOfComponents(3);
vtkSmartPointer<vtkFloatArray> colors = vtkSmartPointer<vtkFloatArray>::New();
colors->SetName("colors");
colors->SetNumberOfValues(obstacles->size());
vtkSmartPointer<vtkLookupTable> lut = vtkSmartPointer<vtkLookupTable>::New();
lut->SetNumberOfTableValues(obstacles->size());
lut->Build();
// Create points
vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New();
double s = octomap->octree()->getNodeSize(treeDepth) / 2.0;
for (unsigned int i = 0; i < obstacles->size(); i++)
}
else
{
if(octomap->octree()->size())
{
points->InsertNextPoint(
cloud->at(obstacles->at(i)).x,
cloud->at(obstacles->at(i)).y,
cloud->at(obstacles->at(i)).z);
colors->InsertValue(i,i);
if(_octomapActor)
{
renderer->RemoveActor(_octomapActor);
_octomapActor = 0;
}
// Create an image data
vtkSmartPointer<vtkImageData> imageData =
vtkSmartPointer<vtkImageData>::New();
lut->SetTableValue(i,
double(cloud->at(obstacles->at(i)).r) / 255.0,
double(cloud->at(obstacles->at(i)).g) / 255.0,
double(cloud->at(obstacles->at(i)).b) / 255.0);
}
double sizeX, sizeY, sizeZ;
double minX, minY, minZ;
double maxX, maxY, maxZ;
octomap->getGridMin(minX, minY, minZ);
octomap->getGridMax(maxX, maxY, maxZ);
sizeX = maxX-minX;
sizeY = maxY-minY;
sizeZ = maxZ-minZ;
double cellSize = octomap->octree()->getNodeSize(treeDepth);
// Combine into a polydata
vtkSmartPointer<vtkPolyData> polydata = vtkSmartPointer<vtkPolyData>::New();
polydata->SetPoints(points);
polydata->GetPointData()->SetScalars(colors);
// Create anything you want here, we will use a cube for the demo.
vtkSmartPointer<vtkCubeSource> cubeSource = vtkSmartPointer<vtkCubeSource>::New();
cubeSource->SetBounds(-s, s, -s, s, -s, s);
vtkSmartPointer<vtkGlyph3DMapper> mapper = vtkSmartPointer<vtkGlyph3DMapper>::New();
mapper->SetSourceConnection(cubeSource->GetOutputPort());
UTimer t;
// Specify the size of the image data
imageData->SetExtent(0, int(sizeX/cellSize+0.5), 0, int(sizeY/cellSize+0.5), 0, int(sizeZ/cellSize+0.5)); // 3D image
#if VTK_MAJOR_VERSION <= 5
mapper->SetInputConnection(polydata->GetProducerPort());
imageData->SetNumberOfScalarComponents(4);
imageData->SetScalarTypeToUnsignedChar();
#else
mapper->SetInputData(polydata);
imageData->AllocateScalars(VTK_UNSIGNED_CHAR,4);
#endif
mapper->SetScalarRange(0, obstacles->size() - 1);
mapper->SetLookupTable(lut);
mapper->ScalingOff();
mapper->Update();
vtkSmartPointer<vtkActor> octomapActor = vtkSmartPointer<vtkActor>::New();
octomapActor->SetMapper(mapper);
int dims[3];
imageData->GetDimensions(dims);
octomapActor->GetProperty()->SetRepresentationToSurface();
octomapActor->GetProperty()->SetEdgeVisibility(_aSetEdgeVisibility->isChecked());
octomapActor->GetProperty()->SetLighting(_aSetLighting->isChecked());
memset(imageData->GetScalarPointer(), 0, imageData->GetScalarSize()*imageData->GetNumberOfScalarComponents()*dims[0]*dims[1]*dims[2]);
renderer->AddActor(octomapActor);
_octomapActor = octomapActor.GetPointer();
for (RtabmapColorOcTree::iterator it = octomap->octree()->begin(treeDepth); it != octomap->octree()->end(); ++it)
{
if(octomap->octree()->isNodeOccupied(*it))
{
octomap::point3d pt = octomap->octree()->keyToCoord(it.getKey());
int x = (pt.x()-minX) / cellSize;
int y = (pt.y()-minY) / cellSize;
int z = (pt.z()-minZ) / cellSize;
if(x>=0 && x<dims[0] && y>=0 && y<dims[1] && z>=0 && z<dims[2])
{
unsigned char* pixel = static_cast<unsigned char*>(imageData->GetScalarPointer(x,y,z));
if(octomap->octree()->getTreeDepth() == it.getDepth() && it->isColorSet())
{
pixel[0] = it->getColor().r;
pixel[1] = it->getColor().g;
pixel[2] = it->getColor().b;
}
else
{
// Gradiant color on z axis
float H = (maxZ - pt.z())*299.0f/(maxZ-minZ);
float r,g,b;
OctoMap::HSVtoRGB(&r, &g, &b, H, 1, 1);
pixel[0] = r*255.0f;
pixel[1] = g*255.0f;
pixel[2] = b*255.0f;
}
pixel[3] = 255;
}
}
}
vtkSmartPointer<vtkSmartVolumeMapper> volumeMapper =
vtkSmartPointer<vtkSmartVolumeMapper>::New();
volumeMapper->SetBlendModeToComposite(); // composite first
#if VTK_MAJOR_VERSION <= 5
volumeMapper->SetInputConnection(imageData->GetProducerPort());
#else
volumeMapper->SetInputData(imageData);
#endif
vtkSmartPointer<vtkVolumeProperty> volumeProperty =
vtkSmartPointer<vtkVolumeProperty>::New();
volumeProperty->ShadeOff();
volumeProperty->IndependentComponentsOff();
return true;
vtkSmartPointer<vtkPiecewiseFunction> compositeOpacity =
vtkSmartPointer<vtkPiecewiseFunction>::New();
compositeOpacity->AddPoint(0.0,0.0);
compositeOpacity->AddPoint(255.0,1.0);
volumeProperty->SetScalarOpacity(0, compositeOpacity); // composite first.
vtkSmartPointer<vtkVolume> volume =
vtkSmartPointer<vtkVolume>::New();
volume->SetMapper(volumeMapper);
volume->SetProperty(volumeProperty);
volume->SetScale(cellSize);
volume->SetPosition(minX, minY, minZ);
renderer->AddViewProp(volume);
// 3D texture mode. For coverage.
#if !defined(VTK_LEGACY_REMOVE) && !defined(VTK_OPENGL2)
volumeMapper->SetRequestedRenderModeToRayCastAndTexture();
#endif // VTK_LEGACY_REMOVE
// Software mode, for coverage. It also makes sure we will get the same
// regression image on all platforms.
volumeMapper->SetRequestedRenderModeToRayCast();
_octomapActor = volume.GetPointer();
return true;
}
}
#endif
return false;
@@ -1913,6 +2030,12 @@ void CloudViewer::updateCameraFrustum(const Transform & pose, const StereoCamera
{
std::vector<CameraModel> models;
models.push_back(model.left());
CameraModel right = model.right();
if(!model.left().localTransform().isNull())
{
right.setLocalTransform(model.left().localTransform() * Transform(model.baseline(), 0, 0, 0, 0, 0));
}
models.push_back(right);
updateCameraFrustums(pose, models);
}

346
guilib/src/DatabaseViewer.cpp
View File

@@ -188,6 +188,7 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) :
uInsert(parameters, Parameters::getDefaultParameters("Stereo"));
uInsert(parameters, Parameters::getDefaultParameters("StereoBM"));
uInsert(parameters, Parameters::getDefaultParameters("Grid"));
uInsert(parameters, Parameters::getDefaultParameters("GridGlobal"));
parameters.insert(*Parameters::getDefaultParameters().find(Parameters::kRGBDOptimizeMaxError()));
parameters.insert(*Parameters::getDefaultParameters().find(Parameters::kRGBDLoopClosureReextractFeatures()));
ui_->parameters_toolbox->setupUi(parameters);
@@ -196,12 +197,12 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) :
this->readSettings();
setupMainLayout(ui_->actionVertical_Layout->isChecked());
ui_->checkBox_grid_cubes->setVisible(ui_->checkBox_octomap->isChecked());
ui_->comboBox_octomap_rendering_type->setVisible(ui_->checkBox_octomap->isChecked());
ui_->spinBox_grid_depth->setVisible(ui_->checkBox_octomap->isChecked());
ui_->checkBox_grid_empty->setVisible(ui_->checkBox_octomap->isChecked() && !ui_->checkBox_grid_cubes->isChecked());
ui_->checkBox_grid_empty->setVisible(!ui_->checkBox_octomap->isChecked() || ui_->comboBox_octomap_rendering_type->currentIndex()==0);
ui_->label_octomap_cubes->setVisible(ui_->checkBox_octomap->isChecked());
ui_->label_octomap_depth->setVisible(ui_->checkBox_octomap->isChecked());
ui_->label_octomap_empty->setVisible(ui_->checkBox_octomap->isChecked() && !ui_->checkBox_grid_cubes->isChecked());
ui_->label_octomap_empty->setVisible(!ui_->checkBox_octomap->isChecked() || ui_->comboBox_octomap_rendering_type->currentIndex()==0);
ui_->menuView->addAction(ui_->dockWidget_constraints->toggleViewAction());
ui_->menuView->addAction(ui_->dockWidget_graphView->toggleViewAction());
@@ -321,10 +322,9 @@ 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_->doubleSpinBox_optimizationScale, 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_->comboBox_octomap_rendering_type, SIGNAL(currentIndexChanged(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()));
@@ -334,8 +334,6 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) :
connect(ui_->doubleSpinBox_posefilteringRadius, SIGNAL(editingFinished()), this, SLOT(updateGraphView()));
connect(ui_->doubleSpinBox_posefilteringAngle, SIGNAL(editingFinished()), this, SLOT(updateGraphView()));
connect(ui_->doubleSpinBox_gridCellSize, SIGNAL(editingFinished()), this, SLOT(updateGrid()));
ui_->label_stereo_inliers_name->setStyleSheet("QLabel {color : blue; }");
ui_->label_stereo_flowOutliers_name->setStyleSheet("QLabel {color : red; }");
ui_->label_stereo_slopeOutliers_name->setStyleSheet("QLabel {color : yellow; }");
@@ -355,10 +353,8 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) :
connect(ui_->checkBox_timeStats, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
connect(ui_->checkBox_timeStats, SIGNAL(stateChanged(int)), this, SLOT(updateStatistics()));
// Graph view
connect(ui_->checkBox_gridErode, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
connect(ui_->doubleSpinBox_gainCompensationRadius, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
connect(ui_->doubleSpinBox_voxelSize, 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_->doubleSpinBox_posefilteringRadius, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
connect(ui_->doubleSpinBox_posefilteringAngle, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
@@ -483,11 +479,9 @@ void DatabaseViewer::readSettings()
settings.endGroup();
settings.beginGroup("grid");
ui_->doubleSpinBox_gridCellSize->setValue(settings.value("gridCellSize", ui_->doubleSpinBox_gridCellSize->value()).toDouble());
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());
settings.endGroup();
settings.beginGroup("mesh");
@@ -564,11 +558,9 @@ void DatabaseViewer::writeSettings()
// save Grid settings
settings.beginGroup("grid");
settings.setValue("gridCellSize", ui_->doubleSpinBox_gridCellSize->value());
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.endGroup();
settings.beginGroup("mesh");
@@ -640,11 +632,9 @@ void DatabaseViewer::restoreDefaultSettings()
ui_->doubleSpinBox_gainCompensationRadius->setValue(0.0);
ui_->doubleSpinBox_voxelSize->setValue(0.0);
ui_->doubleSpinBox_gridCellSize->setValue(0.05);
ui_->groupBox_posefiltering->setChecked(false);
ui_->doubleSpinBox_posefilteringRadius->setValue(0.1);
ui_->doubleSpinBox_posefilteringAngle->setValue(30);
ui_->checkBox_gridErode->setChecked(false);
ui_->checkBox_octomap->setChecked(false);
ui_->checkBox_mesh_quad->setChecked(true);
@@ -845,14 +835,15 @@ bool DatabaseViewer::closeDatabase()
{
// Rejected links
UASSERT(generatedLocalMaps_.size() == generatedLocalMapsInfo_.size());
std::map<int, std::pair<cv::Mat, cv::Mat> >::iterator mapIter = generatedLocalMaps_.begin();
std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat > >::iterator mapIter = generatedLocalMaps_.begin();
std::map<int, std::pair<float, cv::Point3f> >::iterator infoIter = generatedLocalMapsInfo_.begin();
for(; mapIter!=generatedLocalMaps_.end(); ++mapIter, ++infoIter)
{
UASSERT(mapIter->first == infoIter->first);
dbDriver_->updateOccupancyGrid(
mapIter->first,
mapIter->second.first,
mapIter->second.first.first,
mapIter->second.first.second,
mapIter->second.second,
infoIter->second.first,
infoIter->second.second);
@@ -2308,8 +2299,9 @@ void DatabaseViewer::regenerateLocalMaps()
plotCells->setWindowTitle("Occupancy Cells");
plotCells->setAttribute(Qt::WA_DeleteOnClose);
UPlotCurve * totalCurve = plotCells->addCurve("Total");
UPlotCurve * groundCurve = plotCells->addCurve("Empty");
UPlotCurve * obstaclesCurve = plotCells->addCurve("Occupied");
UPlotCurve * emptyCurve = plotCells->addCurve("Empty");
UPlotCurve * obstaclesCurve = plotCells->addCurve("Obstacles");
UPlotCurve * groundCurve = plotCells->addCurve("Ground");
plotCells->show();
double decompressionTime = 0;
@@ -2334,18 +2326,63 @@ void DatabaseViewer::regenerateLocalMaps()
{
Signature s = data;
s.setPose(odomPose);
cv::Mat ground, obstacles;
cv::Mat ground, obstacles, empty;
cv::Point3f viewpoint;
timer.ticks();
grid.createLocalMap(s, ground, obstacles, viewpoint);
if(ui_->checkBox_grid_regenerateFromSavedGrid->isChecked() && s.sensorData().gridCellSize() > 0.0f)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::laserScanToPointCloudRGB(s.sensorData().gridObstacleCellsRaw());
*cloud+=*util3d::laserScanToPointCloudRGB(s.sensorData().gridGroundCellsRaw());
if(cloud->size())
{
// update viewpoint
if(s.sensorData().cameraModels().size())
{
// average of all local transforms
float sum = 0;
for(unsigned int i=0; i<s.sensorData().cameraModels().size(); ++i)
{
const Transform & t = s.sensorData().cameraModels()[i].localTransform();
if(!t.isNull())
{
viewpoint.x += t.x();
viewpoint.y += t.y();
viewpoint.z += t.z();
sum += 1.0f;
}
}
if(sum > 0.0f)
{
viewpoint.x /= sum;
viewpoint.y /= sum;
viewpoint.z /= sum;
}
}
else
{
const Transform & t = s.sensorData().stereoCameraModel().localTransform();
viewpoint = cv::Point3f(t.x(), t.y(), t.z());
}
grid.createLocalMap(cloud, s.getPose(), ground, obstacles, empty, viewpoint);
}
}
else
{
grid.createLocalMap(s, ground, obstacles, empty, 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(std::make_pair(ground, obstacles), empty)));
uInsert(generatedLocalMapsInfo_, std::make_pair(data.id(), std::make_pair(grid.getCellSize(), viewpoint)));
msg = QString("Generated local occupancy grid map %1/%2").arg(i+1).arg((int)ids_.size());
totalCurve->addValue(ids_.at(i), obstacles.cols+ground.cols);
groundCurve->addValue(ids_.at(i), ground.cols);
totalCurve->addValue(ids_.at(i), obstacles.cols+ground.cols+empty.cols);
emptyCurve->addValue(ids_.at(i), empty.cols);
obstaclesCurve->addValue(ids_.at(i), obstacles.cols);
groundCurve->addValue(ids_.at(i), ground.cols);
}
progressDialog.appendText(msg);
@@ -2412,10 +2449,55 @@ void DatabaseViewer::regenerateCurrentLocalMaps()
{
Signature s = data;
s.setPose(odomPose);
cv::Mat ground, obstacles;
cv::Mat ground, obstacles, empty;
cv::Point3f viewpoint;
grid.createLocalMap(s, ground, obstacles, viewpoint);
uInsert(generatedLocalMaps_, std::make_pair(data.id(), std::make_pair(ground, obstacles)));
if(ui_->checkBox_grid_regenerateFromSavedGrid->isChecked() && s.sensorData().gridCellSize() > 0.0f)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::laserScanToPointCloudRGB(s.sensorData().gridObstacleCellsRaw());
*cloud+=*util3d::laserScanToPointCloudRGB(s.sensorData().gridGroundCellsRaw());
if(cloud->size())
{
// update viewpoint
if(s.sensorData().cameraModels().size())
{
// average of all local transforms
float sum = 0;
for(unsigned int i=0; i<s.sensorData().cameraModels().size(); ++i)
{
const Transform & t = s.sensorData().cameraModels()[i].localTransform();
if(!t.isNull())
{
viewpoint.x += t.x();
viewpoint.y += t.y();
viewpoint.z += t.z();
sum += 1.0f;
}
}
if(sum > 0.0f)
{
viewpoint.x /= sum;
viewpoint.y /= sum;
viewpoint.z /= sum;
}
}
else
{
const Transform & t = s.sensorData().stereoCameraModel().localTransform();
viewpoint = cv::Point3f(t.x(), t.y(), t.z());
}
grid.createLocalMap(cloud, s.getPose(), ground, obstacles, empty, viewpoint);
}
}
else
{
grid.createLocalMap(s, ground, obstacles, empty, viewpoint);
}
uInsert(generatedLocalMaps_, std::make_pair(data.id(), std::make_pair(std::make_pair(ground, obstacles),empty)));
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());
}
@@ -2915,6 +2997,7 @@ void DatabaseViewer::update(int value,
cloudViewer_->removeCloud("map");
cloudViewer_->removeCloud("ground");
cloudViewer_->removeCloud("obstacles");
cloudViewer_->removeCloud("empty_cells");
cloudViewer_->removeCloud("words");
cloudViewer_->removeOctomap();
if(ui_->checkBox_showCloud->isChecked() || ui_->checkBox_showMesh->isChecked())
@@ -3022,7 +3105,6 @@ void DatabaseViewer::update(int value,
}
cloudViewer_->addCloud("cloud", cloud, pose);
cloudViewer_->updateCameraFrustum(pose, data.stereoCameraModel());
}
}
}
@@ -3031,7 +3113,14 @@ void DatabaseViewer::update(int value,
//frustums
if(cloudViewer_->isFrustumShown())
{
cloudViewer_->updateCameraFrustums(pose, data.cameraModels());
if(data.cameraModels().size())
{
cloudViewer_->updateCameraFrustums(pose, data.cameraModels());
}
else
{
cloudViewer_->updateCameraFrustum(pose, data.stereoCameraModel());
}
}
//words
@@ -3102,7 +3191,7 @@ void DatabaseViewer::update(int value,
//add occupancy grid
if(ui_->checkBox_showMap->isChecked() || ui_->checkBox_showGrid->isChecked())
{
std::map<int, std::pair<cv::Mat, cv::Mat> > localMaps;
std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> > localMaps;
std::map<int, std::pair<float, cv::Point3f> > localMapsInfo;
if(generatedLocalMaps_.find(data.id()) != generatedLocalMaps_.end())
{
@@ -3111,7 +3200,7 @@ void DatabaseViewer::update(int value,
}
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(std::make_pair(data.gridGroundCellsRaw(), data.gridObstacleCellsRaw()), data.gridEmptyCellsRaw())));
localMapsInfo.insert(std::make_pair(data.id(), std::make_pair(data.gridCellSize(), data.gridViewPoint())));
}
if(!localMaps.empty())
@@ -3122,14 +3211,15 @@ void DatabaseViewer::update(int value,
#ifdef RTABMAP_OCTOMAP
OctoMap * octomap = 0;
if(ui_->checkBox_octomap->isChecked() &&
(!localMaps.begin()->second.first.empty() || !localMaps.begin()->second.second.empty()) &&
(localMaps.begin()->second.first.empty() || localMaps.begin()->second.first.channels() > 2) &&
(!localMaps.begin()->second.first.first.empty() || !localMaps.begin()->second.first.second.empty()) &&
(localMaps.begin()->second.first.first.empty() || localMaps.begin()->second.first.first.channels() > 2) &&
(localMaps.begin()->second.first.second.empty() || localMaps.begin()->second.first.second.channels() > 2) &&
(localMaps.begin()->second.second.empty() || 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->addToCache(data.id(), localMaps.begin()->second.first.first, localMaps.begin()->second.first.second, localMaps.begin()->second.second, localMapsInfo.begin()->second.second);
octomap->update(poses);
}
#endif
@@ -3138,7 +3228,9 @@ void DatabaseViewer::update(int value,
{
float xMin=0.0f, yMin=0.0f;
cv::Mat map8S;
float gridCellSize = ui_->doubleSpinBox_gridCellSize->value();
ParametersMap parameters = ui_->parameters_toolbox->getParameters();
float gridCellSize = Parameters::defaultGridCellSize();
Parameters::parse(parameters, Parameters::kGridCellSize(), gridCellSize);
#ifdef RTABMAP_OCTOMAP
if(octomap)
{
@@ -3147,15 +3239,11 @@ void DatabaseViewer::update(int value,
else
#endif
{
map8S = util3d::create2DMapFromOccupancyLocalMaps(
poses,
localMaps,
ui_->doubleSpinBox_gridCellSize->value(),
xMin, yMin);
//OccupancyGrid grid(ui_->parameters_toolbox->getParameters());
//grid.addToCache(data.id(), localMaps.begin()->second.first, localMaps.begin()->second.second);
//grid.update(poses);
//map8S = grid.getMap(xMin, yMin);
OccupancyGrid grid(ui_->parameters_toolbox->getParameters());
grid.setCellSize(gridCellSize);
grid.addToCache(data.id(), localMaps.begin()->second.first.first, localMaps.begin()->second.first.second, localMaps.begin()->second.second);
grid.update(poses);
map8S = grid.getMap(xMin, yMin);
}
if(!map8S.empty())
{
@@ -3169,27 +3257,34 @@ void DatabaseViewer::update(int value,
#ifdef RTABMAP_OCTOMAP
if(octomap)
{
if(!ui_->checkBox_grid_cubes->isChecked())
if(ui_->comboBox_octomap_rendering_type->currentIndex()== 0)
{
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::IndicesPtr ground(new std::vector<int>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = octomap->createCloud(ui_->spinBox_grid_depth->value(), obstacles.get(), empty.get(), ground.get());
pcl::PointCloud<pcl::PointXYZRGB>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::copyPointCloud(*cloud, *obstacles, *obstaclesCloud);
cloudViewer_->addCloud("obstacles", obstaclesCloud);
cloudViewer_->setCloudPointSize("obstacles", 5);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::copyPointCloud(*cloud, *ground, *groundCloud);
cloudViewer_->addCloud("ground", groundCloud);
cloudViewer_->setCloudPointSize("ground", 5);
if(ui_->checkBox_grid_empty->isChecked())
{
pcl::PointCloud<pcl::PointXYZ>::Ptr emptyCloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::copyPointCloud(*cloud, *empty, *emptyCloud);
cloudViewer_->addCloud("ground", emptyCloud, Transform::getIdentity(), Qt::white);
cloudViewer_->setCloudOpacity("ground", 0.5);
cloudViewer_->setCloudPointSize("ground", 5);
cloudViewer_->addCloud("empty_cells", emptyCloud, Transform::getIdentity(), Qt::white);
cloudViewer_->setCloudOpacity("empty_cells", 0.5);
cloudViewer_->setCloudPointSize("empty_cells", 1);
}
}
else
{
cloudViewer_->addOctomap(octomap, ui_->spinBox_grid_depth->value());
cloudViewer_->addOctomap(octomap, ui_->spinBox_grid_depth->value(), ui_->comboBox_octomap_rendering_type->currentIndex()>1);
}
}
else
@@ -3197,15 +3292,25 @@ void DatabaseViewer::update(int value,
{
// occupancy cloud
cloudViewer_->addCloud("ground",
util3d::laserScanToPointCloud(localMaps.begin()->second.first),
util3d::laserScanToPointCloud(localMaps.begin()->second.first.first),
pose,
Qt::green);
cloudViewer_->addCloud("obstacles",
util3d::laserScanToPointCloud(localMaps.begin()->second.second),
util3d::laserScanToPointCloud(localMaps.begin()->second.first.second),
pose,
Qt::red);
cloudViewer_->setCloudPointSize("ground", 5);
cloudViewer_->setCloudPointSize("obstacles", 5);
if(ui_->checkBox_grid_empty->isChecked())
{
cloudViewer_->addCloud("empty_cells",
util3d::laserScanToPointCloud(localMaps.begin()->second.second),
pose,
Qt::white);
cloudViewer_->setCloudPointSize("empty_cells", 1);
cloudViewer_->setCloudOpacity("empty_cells", 0.5);
}
}
}
#ifdef RTABMAP_OCTOMAP
@@ -4347,7 +4452,7 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
ui_->doubleSpinBox_posefilteringRadius->value(),
ui_->doubleSpinBox_posefilteringAngle->value()*CV_PI/180.0);
}
std::map<int, std::pair<cv::Mat, cv::Mat> > localMaps;
std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> > localMaps;
std::map<int, std::pair<float, cv::Point3f> > localMapsInfo;
#ifdef RTABMAP_OCTOMAP
if(octomap_)
@@ -4370,7 +4475,7 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
}
else if(localMaps_.find(ids[i]) != localMaps_.end())
{
if(!localMaps_.find(ids[i])->second.first.empty() || !localMaps_.find(ids[i])->second.second.empty())
if(!localMaps_.find(ids[i])->second.first.first.empty() || !localMaps_.find(ids[i])->second.first.second.empty())
{
localMaps.insert(*localMaps_.find(ids.at(i)));
localMapsInfo.insert(*localMapsInfo_.find(ids[i]));
@@ -4380,19 +4485,19 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
{
SensorData data;
dbDriver_->getNodeData(ids.at(i), data);
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)));
cv::Mat ground, obstacles, empty;
data.uncompressData(0, 0, 0, 0, &ground, &obstacles, &empty);
localMaps_.insert(std::make_pair(ids.at(i), std::make_pair(std::make_pair(ground, obstacles), empty)));
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)));
localMaps.insert(std::make_pair(ids.at(i), std::make_pair(std::make_pair(ground, obstacles), empty)));
localMapsInfo.insert(std::make_pair(ids.at(i), std::make_pair(data.gridCellSize(), data.gridViewPoint())));
}
}
}
//cleanup
for(std::map<int, std::pair<cv::Mat, cv::Mat> >::iterator iter=localMaps_.begin(); iter!=localMaps_.end();)
for(std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> >::iterator iter=localMaps_.begin(); iter!=localMaps_.end();)
{
if(graphFiltered.find(iter->first) == graphFiltered.end())
{
@@ -4407,6 +4512,10 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
UINFO("Update local maps list... done (%d local maps, graph size=%d)", (int)localMaps.size(), (int)graph.size());
}
ParametersMap parameters = ui_->parameters_toolbox->getParameters();
float cellSize = Parameters::defaultGridCellSize();
Parameters::parse(parameters, Parameters::kGridCellSize(), cellSize);
ui_->graphViewer->updateGTGraph(groundTruthPoses_);
ui_->graphViewer->updateGPSGraph(gpsPoses_, gpsValues_);
ui_->graphViewer->updateGraph(graph, graphLinks_, mapIds_);
@@ -4421,11 +4530,11 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
#ifdef RTABMAP_OCTOMAP
if(ui_->checkBox_octomap->isChecked())
{
octomap_ = new OctoMap(ui_->doubleSpinBox_gridCellSize->value());
octomap_ = new OctoMap(cellSize);
bool updateAborted = false;
for(std::map<int, std::pair<cv::Mat, cv::Mat> >::iterator iter=localMaps.begin(); iter!=localMaps.end(); ++iter)
for(std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> >::iterator iter=localMaps.begin(); iter!=localMaps.end(); ++iter)
{
if(iter->second.first.channels() == 2 || iter->second.second.channels() == 2)
if(iter->second.first.first.channels() == 2 || iter->second.first.second.channels() == 2)
{
QMessageBox::warning(this, tr(""),
tr("Some local occupancy grids are 2D, but OctoMap requires 3D local "
@@ -4434,7 +4543,7 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
updateAborted = true;
break;
}
octomap_->addToCache(iter->first, iter->second.first, iter->second.second, localMapsInfo.at(iter->first).second);
octomap_->addToCache(iter->first, iter->second.first.first, iter->second.first.second, iter->second.second, localMapsInfo.at(iter->first).second);
}
if(!updateAborted)
{
@@ -4447,19 +4556,31 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
if((ui_->dockWidget_graphView->isVisible() && ui_->graphViewer->isGridMapVisible()) ||
(ui_->dockWidget_occupancyGridView->isVisible() && ui_->checkBox_grid_2d->isChecked()))
{
bool eroded = Parameters::defaultGridGlobalEroded();
Parameters::parse(parameters, Parameters::kGridGlobalEroded(), eroded);
float xMin, yMin;
float cell = ui_->doubleSpinBox_gridCellSize->value();
cv::Mat map;
#ifdef RTABMAP_OCTOMAP
if(ui_->checkBox_octomap->isChecked())
{
map = octomap_->createProjectionMap(xMin, yMin, cell, 0, ui_->spinBox_grid_depth->value());
map = octomap_->createProjectionMap(xMin, yMin, cellSize, 0, ui_->spinBox_grid_depth->value());
}
else
#endif
{
map = rtabmap::util3d::create2DMapFromOccupancyLocalMaps(graphFiltered, localMaps, cell, xMin, yMin, 0, ui_->checkBox_gridErode->isChecked());
if(eroded)
{
uInsert(parameters, ParametersPair(Parameters::kGridGlobalEroded(), "true"));
}
OccupancyGrid grid(parameters);
grid.setCellSize(cellSize);
for(std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> >::iterator iter=localMaps.begin(); iter!=localMaps.end(); ++iter)
{
grid.addToCache(iter->first, iter->second.first.first, iter->second.first.second, iter->second.second);
}
grid.update(graphFiltered);
map = grid.getMap(xMin, yMin);
}
ui_->label_timeGrid->setNum(double(time.elapsed())/1000.0);
@@ -4469,11 +4590,11 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
cv::Mat map8U = rtabmap::util3d::convertMap2Image8U(map);
if(ui_->dockWidget_graphView->isVisible() && ui_->graphViewer->isGridMapVisible())
{
ui_->graphViewer->updateMap(map8U, cell, xMin, yMin);
ui_->graphViewer->updateMap(map8U, cellSize, xMin, yMin);
}
if(ui_->dockWidget_occupancyGridView->isVisible() && ui_->checkBox_grid_2d->isChecked())
{
occupancyGridViewer_->addOccupancyGridMap(map8U, cell, xMin, yMin, 1.0f);
occupancyGridViewer_->addOccupancyGridMap(map8U, cellSize, xMin, yMin, 1.0f);
occupancyGridViewer_->update();
}
}
@@ -4492,42 +4613,58 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
{
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::PointXYZ>::Ptr emptyCellsXYZ(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>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr emptyCellsRGB(new pcl::PointCloud<pcl::PointXYZRGB>);
for(std::map<int, std::pair<cv::Mat, cv::Mat> >::iterator iter=localMaps.begin(); iter!=localMaps.end(); ++iter)
for(std::map<int, std::pair<std::pair<cv::Mat, 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.first.empty())
{
if(iter->second.first.channels() == 4)
if(iter->second.first.first.channels() == 4)
{
*groundRGB += *util3d::laserScanToPointCloudRGB(iter->second.first, pose);
*groundRGB += *util3d::laserScanToPointCloudRGB(iter->second.first.first, pose);
}
else
{
*groundXYZ += *util3d::laserScanToPointCloud(iter->second.first, iter->second.first.channels()==2?pose2d:pose);
*groundXYZ += *util3d::laserScanToPointCloud(iter->second.first.first, iter->second.first.first.channels()==2?pose2d:pose);
}
}
if(!iter->second.second.empty())
if(!iter->second.first.second.empty())
{
if(iter->second.second.channels() == 4)
if(iter->second.first.second.channels() == 4)
{
*obstaclesRGB += *util3d::laserScanToPointCloudRGB(iter->second.second, pose);
*obstaclesRGB += *util3d::laserScanToPointCloudRGB(iter->second.first.second, pose);
}
else
{
*obstaclesXYZ += *util3d::laserScanToPointCloud(iter->second.second, iter->second.second.channels()==2?pose2d:pose);
*obstaclesXYZ += *util3d::laserScanToPointCloud(iter->second.first.second, iter->second.first.second.channels()==2?pose2d:pose);
}
}
if(ui_->checkBox_grid_empty->isChecked())
{
if(!iter->second.second.empty())
{
if(iter->second.second.channels() == 4)
{
*emptyCellsRGB += *util3d::laserScanToPointCloudRGB(iter->second.second, pose);
}
else
{
*emptyCellsXYZ += *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());
groundRGB = util3d::voxelize(groundRGB, cellSize);
occupancyGridViewer_->addCloud("groundRGB",
groundRGB,
Transform::getIdentity(),
@@ -4536,7 +4673,7 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
}
if(groundXYZ->size())
{
groundXYZ = util3d::voxelize(groundXYZ, ui_->doubleSpinBox_gridCellSize->value());
groundXYZ = util3d::voxelize(groundXYZ, cellSize);
occupancyGridViewer_->addCloud("groundXYZ",
groundXYZ,
Transform::getIdentity(),
@@ -4545,7 +4682,7 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
}
if(obstaclesRGB->size())
{
obstaclesRGB = util3d::voxelize(obstaclesRGB, ui_->doubleSpinBox_gridCellSize->value());
obstaclesRGB = util3d::voxelize(obstaclesRGB, cellSize);
occupancyGridViewer_->addCloud("obstaclesRGB",
obstaclesRGB,
Transform::getIdentity(),
@@ -4554,13 +4691,33 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
}
if(obstaclesXYZ->size())
{
obstaclesXYZ = util3d::voxelize(obstaclesXYZ, ui_->doubleSpinBox_gridCellSize->value());
obstaclesXYZ = util3d::voxelize(obstaclesXYZ, cellSize);
occupancyGridViewer_->addCloud("obstaclesXYZ",
obstaclesXYZ,
Transform::getIdentity(),
Qt::red);
occupancyGridViewer_->setCloudPointSize("obstaclesXYZ", 5);
}
if(emptyCellsRGB->size())
{
emptyCellsRGB = util3d::voxelize(emptyCellsRGB, cellSize);
occupancyGridViewer_->addCloud("emptyCellsRGB",
emptyCellsRGB,
Transform::getIdentity(),
Qt::white);
occupancyGridViewer_->setCloudPointSize("emptyCellsRGB", 1);
occupancyGridViewer_->setCloudOpacity("emptyCellsRGB", 0.5);
}
if(emptyCellsXYZ->size())
{
emptyCellsXYZ = util3d::voxelize(emptyCellsXYZ, cellSize);
occupancyGridViewer_->addCloud("emptyCellsXYZ",
emptyCellsXYZ,
Transform::getIdentity(),
Qt::white);
occupancyGridViewer_->setCloudPointSize("emptyCellsXYZ", 1);
occupancyGridViewer_->setCloudOpacity("emptyCellsXYZ", 0.5);
}
occupancyGridViewer_->update();
}
}
@@ -4876,12 +5033,12 @@ void DatabaseViewer::updateGrid()
}
else
{
ui_->checkBox_grid_cubes->setVisible(ui_->checkBox_octomap->isChecked());
ui_->comboBox_octomap_rendering_type->setVisible(ui_->checkBox_octomap->isChecked());
ui_->spinBox_grid_depth->setVisible(ui_->checkBox_octomap->isChecked());
ui_->checkBox_grid_empty->setVisible(ui_->checkBox_octomap->isChecked() && !ui_->checkBox_grid_cubes->isChecked());
ui_->checkBox_grid_empty->setVisible(!ui_->checkBox_octomap->isChecked() || ui_->comboBox_octomap_rendering_type->currentIndex()==0);
ui_->label_octomap_cubes->setVisible(ui_->checkBox_octomap->isChecked());
ui_->label_octomap_depth->setVisible(ui_->checkBox_octomap->isChecked());
ui_->label_octomap_empty->setVisible(ui_->checkBox_octomap->isChecked() && !ui_->checkBox_grid_cubes->isChecked());
ui_->label_octomap_empty->setVisible(!ui_->checkBox_octomap->isChecked() || ui_->comboBox_octomap_rendering_type->currentIndex()==0);
update3dView();
updateGraphView();
@@ -4891,12 +5048,12 @@ void DatabaseViewer::updateGrid()
void DatabaseViewer::updateOctomapView()
{
#ifdef RTABMAP_OCTOMAP
ui_->checkBox_grid_cubes->setVisible(ui_->checkBox_octomap->isChecked());
ui_->comboBox_octomap_rendering_type->setVisible(ui_->checkBox_octomap->isChecked());
ui_->spinBox_grid_depth->setVisible(ui_->checkBox_octomap->isChecked());
ui_->checkBox_grid_empty->setVisible(ui_->checkBox_octomap->isChecked() && !ui_->checkBox_grid_cubes->isChecked());
ui_->checkBox_grid_empty->setVisible(!ui_->checkBox_octomap->isChecked() || ui_->comboBox_octomap_rendering_type->currentIndex()==0);
ui_->label_octomap_cubes->setVisible(ui_->checkBox_octomap->isChecked());
ui_->label_octomap_depth->setVisible(ui_->checkBox_octomap->isChecked());
ui_->label_octomap_empty->setVisible(ui_->checkBox_octomap->isChecked() && !ui_->checkBox_grid_cubes->isChecked());
ui_->label_octomap_empty->setVisible(!ui_->checkBox_octomap->isChecked() || ui_->comboBox_octomap_rendering_type->currentIndex()==0);
if(ui_->checkBox_octomap->isChecked())
{
@@ -4905,19 +5062,26 @@ void DatabaseViewer::updateOctomapView()
occupancyGridViewer_->removeOctomap();
occupancyGridViewer_->removeCloud("octomap_obstacles");
occupancyGridViewer_->removeCloud("octomap_empty");
if(ui_->checkBox_grid_cubes->isChecked())
if(ui_->comboBox_octomap_rendering_type->currentIndex()>0)
{
occupancyGridViewer_->addOctomap(octomap_, ui_->spinBox_grid_depth->value());
occupancyGridViewer_->addOctomap(octomap_, ui_->spinBox_grid_depth->value(), ui_->comboBox_octomap_rendering_type->currentIndex()>1);
}
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::IndicesPtr ground(new std::vector<int>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = octomap_->createCloud(ui_->spinBox_grid_depth->value(), obstacles.get(), empty.get(), ground.get());
pcl::PointCloud<pcl::PointXYZRGB>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::copyPointCloud(*cloud, *obstacles, *obstaclesCloud);
occupancyGridViewer_->addCloud("octomap_obstacles", obstaclesCloud);
occupancyGridViewer_->addCloud("octomap_obstacles", obstaclesCloud, Transform::getIdentity(), Qt::red);
occupancyGridViewer_->setCloudPointSize("octomap_obstacles", 5);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::copyPointCloud(*cloud, *ground, *groundCloud);
occupancyGridViewer_->addCloud("octomap_ground", groundCloud, Transform::getIdentity(), Qt::green);
occupancyGridViewer_->setCloudPointSize("octomap_ground", 5);
if(ui_->checkBox_grid_empty->isChecked())
{
pcl::PointCloud<pcl::PointXYZ>::Ptr emptyCloud(new pcl::PointCloud<pcl::PointXYZ>);

75
guilib/src/MainWindow.cpp
View File

@@ -254,9 +254,10 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent, bool sh
_occupancyGrid = new OccupancyGrid(_preferencesDialog->getAllParameters());
#ifdef RTABMAP_OCTOMAP
_octomap = new OctoMap(
_preferencesDialog->getGridMapResolution(),
_occupancyGrid->getCellSize(),
_preferencesDialog->getOctomapOccupancyThr(),
_preferencesDialog->isOctomapFullUpdate());
_occupancyGrid->isFullUpdate(),
_occupancyGrid->getUpdateError());
#endif
// Timer
@@ -1860,7 +1861,6 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
}
std::map<int, Transform> poses = stat.poses();
Transform groundTruthOffset = alignPosesToGroundTruth(poses, groundTruth, stat.stamp(), stat.refImageId());
UDEBUG("time= %d ms", time.restart());
if(!_odometryReceived && poses.size() && poses.rbegin()->first == stat.refImageId())
@@ -1913,7 +1913,7 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
_odometryReceived = false;
_odometryCorrection = groundTruthOffset * stat.mapCorrection();
_odometryCorrection = stat.mapCorrection();
UDEBUG("time= %d ms", time.restart());
@@ -2243,10 +2243,11 @@ void MainWindow::updateMapCloud(
{
cv::Mat ground;
cv::Mat obstacles;
cv::Mat empty;
jter->sensorData().uncompressDataConst(0, 0, 0, 0, &ground, &obstacles);
jter->sensorData().uncompressDataConst(0, 0, 0, 0, &ground, &obstacles, &empty);
_occupancyGrid->addToCache(iter->first, ground, obstacles);
_occupancyGrid->addToCache(iter->first, ground, obstacles, empty);
#ifdef RTABMAP_OCTOMAP
if(updateOctomap)
@@ -2255,7 +2256,7 @@ void MainWindow::updateMapCloud(
(obstacles.empty() || obstacles.channels() > 2))
{
cv::Point3f viewpoint = jter->sensorData().gridViewPoint();
_octomap->addToCache(iter->first, ground, obstacles, viewpoint);
_octomap->addToCache(iter->first, ground, obstacles, empty, viewpoint);
}
else if(!ground.empty() || !obstacles.empty())
{
@@ -2370,6 +2371,13 @@ void MainWindow::updateMapCloud(
}
}
}
Transform mapToGt = Transform::getIdentity();
if(_preferencesDialog->isGroundTruthAligned() && _currentGTPosesMap.size())
{
mapToGt = alignPosesToGroundTruth(_currentPosesMap, _currentGTPosesMap).inverse();
}
if((_preferencesDialog->isGraphsShown() || _preferencesDialog->isFrustumsShown(0)) && _currentPosesMap.size())
{
UTimer timerGraph;
@@ -2436,7 +2444,8 @@ void MainWindow::updateMapCloud(
{
kter = graphs.insert(std::make_pair(mapId, pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>))).first;
}
pcl::PointXYZ pt(iter->second.x(), iter->second.y(), iter->second.z());
Transform t = mapToGt*iter->second;
pcl::PointXYZ pt(t.x(), t.y(), t.z());
kter->second->push_back(pt);
}
}
@@ -2524,9 +2533,9 @@ void MainWindow::updateMapCloud(
{
UDEBUG("");
UTimer time;
if(_preferencesDialog->isOctomapCubeRendering())
if(_preferencesDialog->getOctomapRenderingType() > 0)
{
_cloudViewer->addOctomap(_octomap, _preferencesDialog->getOctomapTreeDepth());
_cloudViewer->addOctomap(_octomap, _preferencesDialog->getOctomapTreeDepth(), _preferencesDialog->getOctomapRenderingType()>1);
}
else
{
@@ -2551,13 +2560,25 @@ void MainWindow::updateMapCloud(
if(_ui->graphicsView_graphView->isVisible())
{
_ui->graphicsView_graphView->updateGraph(posesIn, constraints, mapIdsIn);
_ui->graphicsView_graphView->updateGTGraph(_currentGTPosesMap);
if(_preferencesDialog->isGroundTruthAligned() && !mapToGt.isIdentity())
{
std::map<int, Transform> gtPoses = _currentGTPosesMap;
for(std::map<int, Transform>::iterator iter=gtPoses.begin(); iter!=gtPoses.end(); ++iter)
{
iter->second = mapToGt * iter->second;
}
_ui->graphicsView_graphView->updateGTGraph(gtPoses);
}
else
{
_ui->graphicsView_graphView->updateGTGraph(_currentGTPosesMap);
}
}
cv::Mat map8U;
if((_ui->graphicsView_graphView->isVisible() || _preferencesDialog->getGridMapShown()))
{
float xMin, yMin;
float resolution = _preferencesDialog->getGridMapResolution();
float resolution = _occupancyGrid->getCellSize();
cv::Mat map8S;
#ifdef RTABMAP_OCTOMAP
if(_preferencesDialog->isOctomap2dGrid())
@@ -3323,10 +3344,8 @@ void MainWindow::createAndAddFeaturesToMap(int nodeId, const Transform & pose, i
}
Transform MainWindow::alignPosesToGroundTruth(
std::map<int, Transform> & poses,
const std::map<int, Transform> & groundTruth,
double stamp,
int refId)
const std::map<int, Transform> & poses,
const std::map<int, Transform> & groundTruth)
{
Transform t = Transform::getIdentity();
if(groundTruth.size() && poses.size())
@@ -3344,7 +3363,7 @@ Transform MainWindow::alignPosesToGroundTruth(
float rotational_min = 0.0f;
float rotational_max = 0.0f;
Transform gtToMap = graph::calcRMSE(
t = graph::calcRMSE(
groundTruth,
poses,
translational_rmse,
@@ -3360,16 +3379,6 @@ Transform MainWindow::alignPosesToGroundTruth(
rotational_min,
rotational_max);
if(_preferencesDialog->isGroundTruthAligned())
{
t = gtToMap;
for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{
iter->second = gtToMap * iter->second;
}
}
// ground truth live statistics
UINFO("translational_rmse=%f", translational_rmse);
UINFO("translational_mean=%f", translational_mean);
@@ -3653,7 +3662,6 @@ void MainWindow::processRtabmapEvent3DMap(const rtabmap::RtabmapEvent3DMap & eve
_progressCanceled = false;
QApplication::processEvents();
std::map<int, Transform> poses = event.getPoses();
alignPosesToGroundTruth(poses, groundTruth);
this->updateMapCloud(poses, event.getConstraints(), mapIds, labels, groundTruth, true);
_progressDialog->appendText("Updating the 3D map cloud... done.");
}
@@ -4725,9 +4733,10 @@ void MainWindow::startDetection()
UASSERT(_octomap != 0);
delete _octomap;
_octomap = new OctoMap(
_preferencesDialog->getGridMapResolution(),
_occupancyGrid->getCellSize(),
_preferencesDialog->getOctomapOccupancyThr(),
_preferencesDialog->isOctomapFullUpdate());
_occupancyGrid->isFullUpdate(),
_occupancyGrid->getUpdateError());
#endif
// clear odometry visual stuff
@@ -5577,7 +5586,6 @@ void MainWindow::postProcessing()
}
_progressDialog->appendText(tr("Updating map..."));
alignPosesToGroundTruth(optimizedPoses, _currentGTPosesMap);
this->updateMapCloud(
optimizedPoses,
std::multimap<int, Link>(_currentLinksMap),
@@ -6043,9 +6051,10 @@ void MainWindow::clearTheCache()
UASSERT(_octomap != 0);
delete _octomap;
_octomap = new OctoMap(
_preferencesDialog->getGridMapResolution(),
_occupancyGrid->getCellSize(),
_preferencesDialog->getOctomapOccupancyThr(),
_preferencesDialog->isOctomapFullUpdate());
_occupancyGrid->isFullUpdate(),
_occupancyGrid->getUpdateError());
#endif
_occupancyGrid->clear();
}

40
guilib/src/PreferencesDialog.cpp
View File

@@ -453,7 +453,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
connect(_ui->spinBox_octomap_treeDepth, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
connect(_ui->checkBox_octomap_2dgrid, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
connect(_ui->checkBox_octomap_show3dMap, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
connect(_ui->checkBox_octomap_cubeRendering, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
connect(_ui->comboBox_octomap_renderingType, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
connect(_ui->spinBox_octomap_pointSize, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
connect(_ui->doubleSpinBox_octomap_occupancyThr, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
@@ -915,8 +915,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->doubleSpinBox_grid_maxDepth->setObjectName(Parameters::kGridDepthMax().c_str());
_ui->doubleSpinBox_grid_minDepth->setObjectName(Parameters::kGridDepthMin().c_str());
_ui->lineEdit_grid_roi->setObjectName(Parameters::kGridDepthRoiRatios().c_str());
_ui->checkBox_grid_projRayTracing->setObjectName(Parameters::kGridProjRayTracing().c_str());
connect(_ui->checkBox_grid_projRayTracing, SIGNAL(stateChanged(int)), this, SLOT(useGridProjRayTracing()));
_ui->checkBox_grid_projRayTracing->setObjectName(Parameters::kGridRayTracing().c_str());
_ui->doubleSpinBox_grid_footprintLength->setObjectName(Parameters::kGridFootprintLength().c_str());
_ui->doubleSpinBox_grid_footprintWidth->setObjectName(Parameters::kGridFootprintWidth().c_str());
_ui->doubleSpinBox_grid_footprintHeight->setObjectName(Parameters::kGridFootprintHeight().c_str());
@@ -938,6 +937,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->spinBox_grid_scanDecimation->setObjectName(Parameters::kGridScanDecimation().c_str());
_ui->checkBox_grid_fullUpdate->setObjectName(Parameters::kGridGlobalFullUpdate().c_str());
_ui->doubleSpinBox_grid_updateError->setObjectName(Parameters::kGridGlobalUpdateError().c_str());
_ui->doubleSpinBox_grid_minMapSize->setObjectName(Parameters::kGridGlobalMinSize().c_str());
_ui->spinBox_grid_maxNodes->setObjectName(Parameters::kGridGlobalMaxNodes().c_str());
_ui->doubleSpinBox_grid_footprintRadius->setObjectName(Parameters::kGridGlobalFootprintRadius().c_str());
@@ -1466,7 +1466,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
_ui->spinBox_octomap_treeDepth->setValue(16);
_ui->checkBox_octomap_2dgrid->setChecked(true);
_ui->checkBox_octomap_show3dMap->setChecked(true);
_ui->checkBox_octomap_cubeRendering->setChecked(false);
_ui->comboBox_octomap_renderingType->setCurrentIndex(0);
_ui->spinBox_octomap_pointSize->setValue(5);
_ui->doubleSpinBox_octomap_occupancyThr->setValue(0.5);
}
@@ -1854,7 +1854,7 @@ void PreferencesDialog::readGuiSettings(const QString & filePath)
_ui->spinBox_octomap_treeDepth->setValue(settings.value("octomap_depth", _ui->spinBox_octomap_treeDepth->value()).toInt());
_ui->checkBox_octomap_2dgrid->setChecked(settings.value("octomap_2dgrid", _ui->checkBox_octomap_2dgrid->isChecked()).toBool());
_ui->checkBox_octomap_show3dMap->setChecked(settings.value("octomap_3dmap", _ui->checkBox_octomap_show3dMap->isChecked()).toBool());
_ui->checkBox_octomap_cubeRendering->setChecked(settings.value("octomap_cube", _ui->checkBox_octomap_cubeRendering->isChecked()).toBool());
_ui->comboBox_octomap_renderingType->setCurrentIndex(settings.value("octomap_rendering_type", _ui->comboBox_octomap_renderingType->currentIndex()).toInt());
_ui->doubleSpinBox_octomap_occupancyThr->setValue(settings.value("octomap_occupancy_thr", _ui->doubleSpinBox_octomap_occupancyThr->value()).toDouble());
_ui->spinBox_octomap_pointSize->setValue(settings.value("octomap_point_size", _ui->spinBox_octomap_pointSize->value()).toInt());
@@ -2257,7 +2257,7 @@ void PreferencesDialog::writeGuiSettings(const QString & filePath) const
settings.setValue("octomap_depth", _ui->spinBox_octomap_treeDepth->value());
settings.setValue("octomap_2dgrid", _ui->checkBox_octomap_2dgrid->isChecked());
settings.setValue("octomap_3dmap", _ui->checkBox_octomap_show3dMap->isChecked());
settings.setValue("octomap_cube", _ui->checkBox_octomap_cubeRendering->isChecked());
settings.setValue("octomap_rendering_type", _ui->comboBox_octomap_renderingType->currentIndex());
settings.setValue("octomap_occupancy_thr", _ui->doubleSpinBox_octomap_occupancyThr->value());
settings.setValue("octomap_point_size", _ui->spinBox_octomap_pointSize->value());
@@ -4082,22 +4082,6 @@ void PreferencesDialog::useOdomFeatures()
}
}
void PreferencesDialog::useGridProjRayTracing()
{
if(this->isVisible() && _ui->checkBox_grid_projRayTracing->isChecked() && _ui->groupBox_grid_3d->isChecked())
{
int r = QMessageBox::question(this, tr("Using ray tracing for 2D projection..."),
tr("Currently the 3D occupancy grid parameter is checked, but 2D ray tracing "
"only works with 2D occupancy grids. Do you want to uncheck 3D occupancy grid?"), QMessageBox::Yes | QMessageBox::No, QMessageBox::Yes);
if(r == QMessageBox::Yes)
{
_ui->groupBox_grid_3d->setChecked(false);
}
}
}
void PreferencesDialog::changeWorkingDirectory()
{
QString directory = QFileDialog::getExistingDirectory(this, tr("Working directory"), _ui->lineEdit_workingDirectory->text());
@@ -4319,9 +4303,9 @@ bool PreferencesDialog::isOctomapShown() const
#endif
return false;
}
bool PreferencesDialog::isOctomapCubeRendering() const
int PreferencesDialog::getOctomapRenderingType() const
{
return _ui->checkBox_octomap_cubeRendering->isChecked();
return _ui->comboBox_octomap_renderingType->currentIndex();
}
bool PreferencesDialog::isOctomap2dGrid() const
{
@@ -4334,10 +4318,6 @@ int PreferencesDialog::getOctomapTreeDepth() const
{
return _ui->spinBox_octomap_treeDepth->value();
}
bool PreferencesDialog::isOctomapFullUpdate() const
{
return uStr2Bool(this->getParameter(Parameters::kGridGlobalFullUpdate()));
}
double PreferencesDialog::getOctomapOccupancyThr() const
{
return _ui->doubleSpinBox_octomap_occupancyThr->value();
@@ -4538,10 +4518,6 @@ bool PreferencesDialog::getGridMapShown() const
{
return _ui->checkBox_map_shown->isChecked();
}
double PreferencesDialog::getGridMapResolution() const
{
return _ui->doubleSpinBox_grid_resolution->value();
}
bool PreferencesDialog::isGridMapFrom3DCloud() const
{
return _ui->groupBox_grid_fromDepthImage->isChecked();

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

@@ -61,7 +61,7 @@
<rect>
<x>0</x>
<y>0</y>
<width>395</width>
<width>326</width>
<height>242</height>
</rect>
</property>
@@ -253,7 +253,7 @@
<rect>
<x>0</x>
<y>0</y>
<width>394</width>
<width>325</width>
<height>242</height>
</rect>
</property>
@@ -1163,7 +1163,7 @@
<item>
<widget class="QToolBox" name="toolBox">
<property name="currentIndex">
<number>0</number>
<number>1</number>
</property>
<widget class="QWidget" name="page_3">
<property name="geometry">
@@ -1332,8 +1332,8 @@
<rect>
<x>0</x>
<y>0</y>
<width>302</width>
<height>696</height>
<width>451</width>
<height>668</height>
</rect>
</property>
<attribute name="label">
@@ -1341,7 +1341,7 @@
</attribute>
<layout class="QVBoxLayout" name="verticalLayout_16">
<item>
<layout class="QGridLayout" name="gridLayout_9" columnstretch="0,1">
<layout class="QGridLayout" name="gridLayout_9" columnstretch="0,0">
<item row="7" column="1">
<widget class="QLabel" name="label_51">
<property name="text">
@@ -1374,7 +1374,7 @@
</property>
</widget>
</item>
<item row="2" column="1">
<item row="1" column="1">
<widget class="QLabel" name="label_53">
<property name="text">
<string>OctoMap</string>
@@ -1391,41 +1391,7 @@
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="label_44">
<property name="text">
<string>Errode</string>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QCheckBox" name="checkBox_gridErode">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLabel" name="label_46">
<property name="text">
<string>Grid cell size</string>
</property>
</widget>
</item>
<item row="0" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_gridCellSize">
<property name="decimals">
<number>3</number>
</property>
<property name="minimum">
<double>0.001000000000000</double>
</property>
<property name="singleStep">
<double>0.010000000000000</double>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QCheckBox" name="checkBox_octomap">
<property name="text">
<string/>
@@ -1455,26 +1421,6 @@
</widget>
</item>
<item row="5" column="0">
<widget class="QCheckBox" name="checkBox_grid_cubes">
<property name="text">
<string/>
</property>
<property name="checked">
<bool>false</bool>
</property>
</widget>
</item>
<item row="4" column="0">
<widget class="QCheckBox" name="checkBox_grid_empty">
<property name="text">
<string/>
</property>
<property name="checked">
<bool>false</bool>
</property>
</widget>
</item>
<item row="6" column="0">
<widget class="QSpinBox" name="spinBox_grid_depth">
<property name="prefix">
<string/>
@@ -1490,25 +1436,83 @@
</property>
</widget>
</item>
<item row="4" column="1">
<widget class="QLabel" name="label_octomap_empty">
<item row="6" column="0">
<widget class="QCheckBox" name="checkBox_grid_regenerateFromSavedGrid">
<property name="text">
<string>OctoMap: Empty space</string>
<string/>
</property>
<property name="checked">
<bool>false</bool>
</property>
</widget>
</item>
<item row="4" column="1">
<widget class="QLabel" name="label_octomap_cubes">
<property name="text">
<string>OctoMap: Rendering type</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="5" column="1">
<widget class="QLabel" name="label_octomap_cubes">
<property name="text">
<string>OctoMap: Cubes rendering</string>
</property>
</widget>
</item>
<item row="6" column="1">
<widget class="QLabel" name="label_octomap_depth">
<property name="text">
<string>OctoMap: Tree depth</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="0" column="0">
<widget class="QCheckBox" name="checkBox_grid_empty">
<property name="text">
<string/>
</property>
<property name="checked">
<bool>false</bool>
</property>
</widget>
</item>
<item row="6" column="1">
<widget class="QLabel" name="label_55">
<property name="text">
<string>Local grid: regenerate from saved grid instead of sensors</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLabel" name="label_octomap_empty">
<property name="text">
<string>Global grid/OctoMap: show empty space</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="4" column="0">
<widget class="QComboBox" name="comboBox_octomap_rendering_type">
<item>
<property name="text">
<string>Point Cloud</string>
</property>
</item>
<item>
<property name="text">
<string>Cube</string>
</property>
</item>
<item>
<property name="text">
<string>Volume</string>
</property>
</item>
</widget>
</item>
</layout>

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

@@ -65,7 +65,7 @@
<x>0</x>
<y>0</y>
<width>678</width>
<height>2739</height>
<height>2778</height>
</rect>
</property>
<layout class="QVBoxLayout" name="verticalLayout_16">
@@ -95,7 +95,7 @@
<enum>QFrame::Raised</enum>
</property>
<property name="currentIndex">
<number>16</number>
<number>3</number>
</property>
<widget class="QWidget" name="page_22">
<layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1">
@@ -259,7 +259,7 @@
<item row="7" column="1">
<widget class="QLabel" name="label_347">
<property name="text">
<string>When a ground truth is provided, align the map with it.</string>
<string>When a ground truth is provided, align it with the map.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
@@ -2107,20 +2107,10 @@ Show a yellow background when the number of odometry inliers goes under this thr
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QCheckBox" name="checkBox_octomap_cubeRendering">
<property name="text">
<string/>
</property>
<property name="checked">
<bool>false</bool>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="label_octomap_treeDepth_5">
<property name="text">
<string>Cube rendering. Warning: this requires significant more GPU power.</string>
<string>OctoMap rendering type. Cube rendering requires significant more GPU power.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
@@ -2179,6 +2169,28 @@ Show a yellow background when the number of odometry inliers goes under this thr
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QComboBox" name="comboBox_octomap_renderingType">
<property name="sizeAdjustPolicy">
<enum>QComboBox::AdjustToContents</enum>
</property>
<item>
<property name="text">
<string>Point Cloud</string>
</property>
</item>
<item>
<property name="text">
<string>Cube</string>
</property>
</item>
<item>
<property name="text">
<string>Volume</string>
</property>
</item>
</widget>
</item>
</layout>
</widget>
</item>
@@ -9499,7 +9511,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
<item row="4" column="1">
<widget class="QLabel" name="label_363">
<property name="text">
<string>2D ray tracing is done for each projected obstacle (when 3D is not checked below), filling unknown space between the sensor and obstacles.</string>
<string>Ray tracing is done for each obstacle, filling unknown space between the sensor and obstacles. If RTAB-Map is not built with OctoMap and 3D is checked below, 3D ray tracing cannot be done.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
@@ -9876,7 +9888,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="4" column="1">
<item row="5" column="1">
<widget class="QLabel" name="label_224">
<property name="text">
<string>Erode obstacle cells.</string>
@@ -9889,7 +9901,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="3" column="1">
<item row="4" column="1">
<widget class="QLabel" name="label_319">
<property name="text">
<string>Footprint radius used to clear all obstacles under the graph.</string>
@@ -9902,7 +9914,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="4" column="0">
<item row="5" column="0">
<widget class="QCheckBox" name="checkBox_grid_erode">
<property name="text">
<string/>
@@ -9912,7 +9924,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="3" column="0">
<item row="4" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_grid_footprintRadius">
<property name="suffix">
<string> m</string>
@@ -9931,7 +9943,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="1" column="1">
<item row="2" column="1">
<widget class="QLabel" name="label_366">
<property name="text">
<string>Minimum map size.</string>
@@ -9944,7 +9956,14 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="1" column="0">
<item row="3" column="0">
<widget class="QSpinBox" name="spinBox_grid_maxNodes">
<property name="maximum">
<number>9999</number>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_grid_minMapSize">
<property name="suffix">
<string> m</string>
@@ -9966,7 +9985,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="2" column="1">
<item row="3" column="1">
<widget class="QLabel" name="label_455">
<property name="text">
<string>Maximum nodes assembled in the map starting from the last node (0=unlimited).</string>
@@ -9979,10 +9998,38 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QSpinBox" name="spinBox_grid_maxNodes">
<item row="1" column="1">
<widget class="QLabel" name="label_454">
<property name="text">
<string>Graph changed detection error. Update map only if poses in new optimized graph have moved more than this value.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_grid_updateError">
<property name="suffix">
<string> m</string>
</property>
<property name="decimals">
<number>3</number>
</property>
<property name="minimum">
<double>0.001000000000000</double>
</property>
<property name="maximum">
<number>9999</number>
<double>99.000000000000000</double>
</property>
<property name="singleStep">
<double>0.100000000000000</double>
</property>
<property name="value">
<double>0.010000000000000</double>
</property>
</widget>
</item>