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

Improved occupancy grid map construction performance

Browse Source
This commit is contained in:
Mathieu Labbe
2015-02-11 17:03:46 -05:00
parent 530fd5d2c1
commit 7c65dbf6bb
11 changed files with 324 additions and 205 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -275,7 +275,8 @@ private:
std::map<int, int> _currentMapIds; // <nodeId, mapId>
std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr > _createdClouds;
std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > _createdScans;
std::map<int, std::pair<cv::Mat, cv::Mat> > _occupancyLocalMaps; // <ground, obstacles>
std::map<int, std::pair<cv::Mat, cv::Mat> > _projectionLocalMaps; // <ground, obstacles>
std::map<int, std::pair<cv::Mat, cv::Mat> > _gridLocalMaps; // <ground, obstacles>
Transform _odometryCorrection;
Transform _lastOdomPose;
bool _lastOdometryProcessed;

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

@@ -147,9 +147,7 @@ public:
bool getGridMapShown() const;
double getGridMapResolution() const;
bool getGridMapFillEmptySpace() const;
bool isGridMapFrom3DCloud() const;
int getGridMapFillEmptyRadius() const;
double getGridMapOpacity() const;
QString getWorkingDirectory() const;

65
guilib/src/MainWindow.cpp
View File

@@ -1191,7 +1191,7 @@ void MainWindow::updateMapCloud(
_ui->actionExport_2D_Grid_map_bmp_png->setEnabled(true);
_ui->actionView_scans->setEnabled(true);
}
else if(_preferencesDialog->isGridMapFrom3DCloud() && _occupancyLocalMaps.size())
else if(_preferencesDialog->isGridMapFrom3DCloud() && _projectionLocalMaps.size())
{
_ui->actionExport_2D_Grid_map_bmp_png->setEnabled(true);
}
@@ -1394,13 +1394,19 @@ void MainWindow::updateMapCloud(
cv::Mat map8S;
if(_preferencesDialog->isGridMapFrom3DCloud())
{
int fillEmptyRadius = _preferencesDialog->getGridMapFillEmptyRadius();
map8S = util3d::create2DMapFromOccupancyLocalMaps(poses, _occupancyLocalMaps, resolution, xMin, yMin, fillEmptyRadius);
map8S = util3d::create2DMapFromOccupancyLocalMaps(
poses,
_projectionLocalMaps,
resolution,
xMin, yMin);
}
else if(_createdScans.size())
else if(_gridLocalMaps.size())
{
bool fillEmptySpace = _preferencesDialog->getGridMapFillEmptySpace();
map8S = util3d::create2DMap(poses, _createdScans, resolution, fillEmptySpace, xMin, yMin);
map8S = util3d::create2DMapFromOccupancyLocalMaps(
poses,
_gridLocalMaps,
resolution,
xMin, yMin);
}
if(!map8S.empty())
{
@@ -1506,9 +1512,20 @@ void MainWindow::createAndAddCloudToMap(int nodeId, const Transform & pose, int
float groundNormalMaxAngle = M_PI_4;
int minClusterSize = 20;
cv::Mat ground, obstacles;
if(util3d::occupancy2DFromCloud3D(cloud, ground, obstacles, cellSize, groundNormalMaxAngle, minClusterSize))
pcl::PointCloud<pcl::PointXYZRGB>::Ptr voxelizedCloud = cloud;
if(voxelizedCloud->size())
{
_occupancyLocalMaps.insert(std::make_pair(nodeId, std::make_pair(ground, obstacles)));
voxelizedCloud = util3d::voxelize<pcl::PointXYZRGB>(cloud, cellSize);
}
util3d::occupancy2DFromCloud3D<pcl::PointXYZRGB>(
voxelizedCloud,
ground, obstacles,
cellSize,
groundNormalMaxAngle,
minClusterSize);
if(!ground.empty() || !obstacles.empty())
{
_projectionLocalMaps.insert(std::make_pair(nodeId, std::make_pair(ground, obstacles)));
}
UDEBUG("time gridMapFrom2DCloud = %f s", timer.ticks());
}
@@ -1607,6 +1624,10 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int m
else
{
_createdScans.insert(std::make_pair(nodeId, cloud));
cv::Mat ground, obstacles;
util3d::occupancy2DFromLaserScan(depth2D, ground, obstacles, _preferencesDialog->getGridMapResolution());
_gridLocalMaps.insert(std::make_pair(nodeId, std::make_pair(ground, obstacles)));
}
_ui->widget_cloudViewer->setCloudOpacity(scanName, _preferencesDialog->getScanOpacity(0));
_ui->widget_cloudViewer->setCloudPointSize(scanName, _preferencesDialog->getScanPointSize(0));
@@ -3504,7 +3525,8 @@ void MainWindow::clearTheCache()
_cachedSignatures.clear();
_createdClouds.clear();
_createdScans.clear();
_occupancyLocalMaps.clear();
_gridLocalMaps.clear();
_projectionLocalMaps.clear();
_ui->widget_cloudViewer->removeAllClouds();
_ui->widget_cloudViewer->removeAllGraphs();
_ui->widget_cloudViewer->setBackgroundColor(Qt::black);
@@ -3724,7 +3746,6 @@ void MainWindow::setAspectRatio1080p()
void MainWindow::exportGridMap()
{
double gridCellSize = 0.05;
bool gridUnknownSpaceFilled = true;
bool ok;
gridCellSize = QInputDialog::getDouble(this, tr("Grid cell size"), tr("Size (m):"), gridCellSize, 0.01, 1, 2, &ok);
if(!ok)
@@ -3732,18 +3753,6 @@ void MainWindow::exportGridMap()
return;
}
QMessageBox::StandardButton b = QMessageBox::question(this,
tr("Fill empty space?"),
tr("Do you want to fill empty space?"),
QMessageBox::No | QMessageBox::Yes,
QMessageBox::Yes);
if(b != QMessageBox::Yes && b != QMessageBox::No)
{
return;
}
gridUnknownSpaceFilled = b == QMessageBox::Yes;
std::map<int, Transform> poses = _ui->widget_mapVisibility->getVisiblePoses();
// create the map
@@ -3751,11 +3760,19 @@ void MainWindow::exportGridMap()
cv::Mat pixels;
if(_preferencesDialog->isGridMapFrom3DCloud())
{
pixels = util3d::create2DMapFromOccupancyLocalMaps(poses, _occupancyLocalMaps, gridCellSize, xMin, yMin, gridUnknownSpaceFilled?1:0);
pixels = util3d::create2DMapFromOccupancyLocalMaps(
poses,
_projectionLocalMaps,
gridCellSize,
xMin, yMin);
}
else
{
pixels = util3d::create2DMap(poses, _createdScans, gridCellSize, gridUnknownSpaceFilled, xMin, yMin);
pixels = util3d::create2DMapFromOccupancyLocalMaps(
poses,
_gridLocalMaps,
gridCellSize,
xMin, yMin);
}
if(!pixels.empty())

16
guilib/src/PreferencesDialog.cpp
View File

@@ -224,9 +224,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
connect(_ui->checkBox_map_shown, SIGNAL(clicked(bool)), this, SLOT(makeObsoleteCloudRenderingPanel()));
connect(_ui->doubleSpinBox_map_resolution, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
connect(_ui->checkBox_map_fillEmptySpace, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
connect(_ui->doubleSpinBox_map_opacity, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
connect(_ui->spinbox_map_fillEmptyRadius, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
connect(_ui->checkBox_map_occupancyFrom3DCloud, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
//Logging panel
@@ -854,9 +852,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
_ui->checkBox_map_shown->setChecked(false);
_ui->doubleSpinBox_map_resolution->setValue(0.05);
_ui->checkBox_map_fillEmptySpace->setChecked(true);
_ui->checkBox_map_occupancyFrom3DCloud->setChecked(false);
_ui->spinbox_map_fillEmptyRadius->setValue(1);
_ui->doubleSpinBox_map_opacity->setValue(0.75);
}
else if(groupBox->objectName() == _ui->groupBox_logging1->objectName())
@@ -1094,9 +1090,7 @@ void PreferencesDialog::readGuiSettings(const QString & filePath)
_ui->checkBox_map_shown->setChecked(settings.value("gridMapShown", _ui->checkBox_map_shown->isChecked()).toBool());
_ui->doubleSpinBox_map_resolution->setValue(settings.value("gridMapResolution", _ui->doubleSpinBox_map_resolution->value()).toDouble());
_ui->checkBox_map_fillEmptySpace->setChecked(settings.value("gridMapFillEmptySpace", _ui->checkBox_map_fillEmptySpace->isChecked()).toBool());
_ui->checkBox_map_occupancyFrom3DCloud->setChecked(settings.value("gridMapOccupancyFrom3DCloud", _ui->checkBox_map_occupancyFrom3DCloud->isChecked()).toBool());
_ui->spinbox_map_fillEmptyRadius->setValue(settings.value("gridMapFillEmptyRadius", _ui->spinbox_map_fillEmptyRadius->value()).toInt());
_ui->doubleSpinBox_map_opacity->setValue(settings.value("gridMapOpacity", _ui->doubleSpinBox_map_opacity->value()).toDouble());
settings.endGroup(); // General
@@ -1343,9 +1337,7 @@ void PreferencesDialog::writeGuiSettings(const QString & filePath)
settings.setValue("gridMapShown", _ui->checkBox_map_shown->isChecked());
settings.setValue("gridMapResolution", _ui->doubleSpinBox_map_resolution->value());
settings.setValue("gridMapFillEmptySpace", _ui->checkBox_map_fillEmptySpace->isChecked());
settings.setValue("gridMapOccupancyFrom3DCloud", _ui->checkBox_map_occupancyFrom3DCloud->isChecked());
settings.setValue("gridMapFillEmptyRadius", _ui->spinbox_map_fillEmptyRadius->value());
settings.setValue("gridMapOpacity", _ui->doubleSpinBox_map_opacity->value());
settings.endGroup(); // General
@@ -3000,18 +2992,10 @@ double PreferencesDialog::getGridMapResolution() const
{
return _ui->doubleSpinBox_map_resolution->value();
}
bool PreferencesDialog::getGridMapFillEmptySpace() const
{
return _ui->checkBox_map_fillEmptySpace->isChecked();
}
bool PreferencesDialog::isGridMapFrom3DCloud() const
{
return _ui->checkBox_map_occupancyFrom3DCloud->isChecked();
}
int PreferencesDialog::getGridMapFillEmptyRadius() const
{
return _ui->spinbox_map_fillEmptyRadius->value();
}
double PreferencesDialog::getGridMapOpacity() const
{
return _ui->doubleSpinBox_map_opacity->value();

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

@@ -65,7 +65,7 @@
<x>0</x>
<y>0</y>
<width>744</width>
<height>1074</height>
<height>1056</height>
</rect>
</property>
<layout class="QVBoxLayout" name="verticalLayout_16">
@@ -421,6 +421,23 @@ Show a yellow background when the number of odometry inliers goes under this thr
</item>
<item>
<layout class="QGridLayout" name="gridLayout_20" columnstretch="0,1">
<item row="0" column="0">
<widget class="QCheckBox" name="checkBox_map_shown">
<property name="text">
<string/>
</property>
<property name="checked">
<bool>false</bool>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLabel" name="label_map_shown">
<property name="text">
<string>Show in 3D map view.</string>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_map_resolution">
<property name="suffix">
@@ -444,23 +461,6 @@ Show a yellow background when the number of odometry inliers goes under this thr
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QLabel" name="label_164">
<property name="text">
<string>Fill empty space.</string>
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QCheckBox" name="checkBox_map_fillEmptySpace">
<property name="text">
<string/>
</property>
<property name="checked">
<bool>true</bool>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="label_170">
<property name="text">
@@ -468,24 +468,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
</property>
</widget>
</item>
<item row="0" column="0">
<widget class="QCheckBox" name="checkBox_map_shown">
<property name="text">
<string/>
</property>
<property name="checked">
<bool>false</bool>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLabel" name="label_map_shown">
<property name="text">
<string>Show in 3D map view.</string>
</property>
</widget>
</item>
<item row="4" column="0">
<item row="3" column="0">
<widget class="QCheckBox" name="checkBox_map_occupancyFrom3DCloud">
<property name="text">
<string/>
@@ -511,7 +494,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
</property>
</widget>
</item>
<item row="4" column="1">
<item row="3" column="1">
<widget class="QLabel" name="label_210">
<property name="text">
<string>Occupancy from 3D cloud projection on the ground. Laser scans are ignored when activated.</string>
@@ -521,32 +504,6 @@ Show a yellow background when the number of odometry inliers goes under this thr
</property>
</widget>
</item>
<item row="5" column="1">
<widget class="QLabel" name="label_211">
<property name="text">
<string>Fill empty radius. Used when occupancy from 3D projection is activated.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="5" column="0">
<widget class="QSpinBox" name="spinbox_map_fillEmptyRadius">
<property name="suffix">
<string> cells</string>
</property>
<property name="minimum">
<number>0</number>
</property>
<property name="maximum">
<number>10</number>
</property>
<property name="value">
<number>1</number>
</property>
</widget>
</item>
</layout>
</item>
</layout>