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

Added util3d::occupancy2DFromCloud3D() and util3d::create2DMapFromOccupancyLocalMaps() methods to create 2D occupancy maps from 3D clouds

Browse Source 
git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1899 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2014-10-20 02:14:31 +00:00
parent b97426c83d
commit d0325f3171
11 changed files with 910 additions and 91 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -258,8 +258,9 @@ private:
QMap<int, int> _mapIds;
QMap<int, Transform> _localTransformsMap;
std::map<int, Transform> _currentPosesMap;
QMap<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr > _createdClouds;
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>
Transform _odometryCorrection;
Transform _lastOdomPose;
bool _lastOdometryProcessed;

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

@@ -145,6 +145,8 @@ public:
bool getGridMapShown() const;
double getGridMapResolution() const;
bool getGridMapFillEmptySpace() const;
bool isGridMapFrom3DCloud() const;
int getGridMapFillEmptyRadius() const;
double getGridMapOpacity() const;
QString getWorkingDirectory() const;

75
guilib/src/MainWindow.cpp
View File

@@ -267,7 +267,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
connect(_ui->action720p, SIGNAL(triggered()), this, SLOT(setAspectRatio720p()));
connect(_ui->action1080p, SIGNAL(triggered()), this, SLOT(setAspectRatio1080p()));
connect(_ui->actionSave_point_cloud, SIGNAL(triggered()), this, SLOT(exportClouds()));
connect(_ui->actionExport_2D_scans_ply_bmp, SIGNAL(triggered()), this, SLOT(exportScans()));
connect(_ui->actionExport_2D_scans_ply_pcd, SIGNAL(triggered()), this, SLOT(exportScans()));
connect(_ui->actionExport_2D_Grid_map_bmp_png, SIGNAL(triggered()), this, SLOT(exportGridMap()));
connect(_ui->actionView_scans, SIGNAL(triggered()), this, SLOT(viewScans()));
connect(_ui->actionView_high_res_point_cloud, SIGNAL(triggered()), this, SLOT(viewClouds()));
@@ -277,7 +277,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
_ui->actionPause->setShortcut(Qt::Key_Space);
_ui->actionSave_point_cloud->setEnabled(false);
_ui->actionExport_2D_scans_ply_bmp->setEnabled(false);
_ui->actionExport_2D_scans_ply_pcd->setEnabled(false);
_ui->actionExport_2D_Grid_map_bmp_png->setEnabled(false);
_ui->actionView_scans->setEnabled(false);
_ui->actionView_high_res_point_cloud->setEnabled(false);
@@ -1087,10 +1087,14 @@ void MainWindow::updateMapCloud(
if(_depths2DMap.size())
{
_ui->actionExport_2D_scans_ply_bmp->setEnabled(true);
_ui->actionExport_2D_scans_ply_pcd->setEnabled(true);
_ui->actionExport_2D_Grid_map_bmp_png->setEnabled(true);
_ui->actionView_scans->setEnabled(true);
}
else if(_preferencesDialog->isGridMapFrom3DCloud() && _occupancyLocalMaps.size())
{
_ui->actionExport_2D_Grid_map_bmp_png->setEnabled(true);
}
}
}
@@ -1219,12 +1223,21 @@ void MainWindow::updateMapCloud(
_ui->graphicsView_graphView->updateGraph(poses, constraints);
}
cv::Mat map8U;
if((_ui->graphicsView_graphView->isVisible() || _preferencesDialog->getGridMapShown()) && _depths2DMap.size())
if((_ui->graphicsView_graphView->isVisible() || _preferencesDialog->getGridMapShown()) && (_createdScans.size() || _preferencesDialog->isGridMapFrom3DCloud()))
{
float xMin, yMin;
float resolution = _preferencesDialog->getGridMapResolution();
bool fillEmptySpace = _preferencesDialog->getGridMapFillEmptySpace();
cv::Mat map8S = util3d::create2DMap(poses, _createdScans, resolution, fillEmptySpace, xMin, yMin);
cv::Mat map8S;
if(_preferencesDialog->isGridMapFrom3DCloud())
{
int fillEmptyRadius = _preferencesDialog->getGridMapFillEmptyRadius();
map8S = util3d::create2DMapFromOccupancyLocalMaps(poses, _occupancyLocalMaps, resolution, xMin, yMin, fillEmptyRadius);
}
else
{
bool fillEmptySpace = _preferencesDialog->getGridMapFillEmptySpace();
map8S = util3d::create2DMap(poses, _createdScans, resolution, fillEmptySpace, xMin, yMin);
}
if(!map8S.empty())
{
//convert to gray scaled map
@@ -1303,6 +1316,18 @@ void MainWindow::createAndAddCloudToMap(int nodeId, const Transform & pose)
_preferencesDialog->getCloudDecimation(0),
_preferencesDialog->getCloudMaxDepth(0));
if(cloud->size() && _preferencesDialog->isGridMapFrom3DCloud())
{
float cellSize = _preferencesDialog->getGridMapResolution();
float groundNormalMaxAngle = M_PI_4;
int minClusterSize = 20;
cv::Mat ground, obstacles;
if(util3d::occupancy2DFromCloud3D(cloud, ground, obstacles, cellSize, groundNormalMaxAngle, minClusterSize))
{
_occupancyLocalMaps.insert(std::make_pair(nodeId, std::make_pair(ground, obstacles)));
}
}
if(_preferencesDialog->isCloudMeshing())
{
pcl::PolygonMesh::Ptr mesh(new pcl::PolygonMesh);
@@ -1330,7 +1355,7 @@ void MainWindow::createAndAddCloudToMap(int nodeId, const Transform & pose)
}
else
{
_createdClouds.insert(nodeId, tmp);
_createdClouds.insert(std::make_pair(nodeId, tmp));
}
}
}
@@ -1355,7 +1380,7 @@ void MainWindow::createAndAddCloudToMap(int nodeId, const Transform & pose)
}
else
{
_createdClouds.insert(nodeId, cloud);
_createdClouds.insert(std::make_pair(nodeId, cloud));
}
}
@@ -2765,6 +2790,7 @@ void MainWindow::clearTheCache()
_localTransformsMap.clear();
_createdClouds.clear();
_createdScans.clear();
_occupancyLocalMaps.clear();
_ui->widget_cloudViewer->removeAllClouds();
_ui->widget_cloudViewer->setBackgroundColor(Qt::black);
_ui->widget_cloudViewer->clearTrajectory();
@@ -2773,7 +2799,7 @@ void MainWindow::clearTheCache()
_lastOdomPose.setNull();
//disable save cloud action
_ui->actionExport_2D_Grid_map_bmp_png->setEnabled(false);
_ui->actionExport_2D_scans_ply_bmp->setEnabled(false);
_ui->actionExport_2D_scans_ply_pcd->setEnabled(false);
_ui->actionSave_point_cloud->setEnabled(false);
_ui->actionView_scans->setEnabled(false);
_ui->actionView_high_res_point_cloud->setEnabled(false);
@@ -2992,22 +3018,19 @@ void MainWindow::exportGridMap()
}
gridUnknownSpaceFilled = b == QMessageBox::Yes;
std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > scans;
std::map<int, Transform> posesIn = _ui->widget_mapVisibility->getVisiblePoses();
std::map<int, Transform> poses;
for(std::map<int, Transform>::const_iterator iter = posesIn.begin(); iter!=posesIn.end(); ++iter)
{
if(_depths2DMap.contains(iter->first))
{
cv::Mat depth2d = util3d::uncompressData(_depths2DMap.value(iter->first));
scans.insert(std::make_pair(iter->first, util3d::depth2DToPointCloud(depth2d)));
poses.insert(*iter);
}
}
std::map<int, Transform> poses = _ui->widget_mapVisibility->getVisiblePoses();
// create the map
float xMin=0.0f, yMin=0.0f;
cv::Mat pixels = util3d::create2DMap(poses, scans, gridCellSize, gridUnknownSpaceFilled, xMin, yMin);
cv::Mat pixels;
if(_preferencesDialog->isGridMapFrom3DCloud())
{
pixels = util3d::create2DMapFromOccupancyLocalMaps(poses, _occupancyLocalMaps, gridCellSize, xMin, yMin, gridUnknownSpaceFilled?1:0);
}
else
{
pixels = util3d::create2DMap(poses, _createdScans, gridCellSize, gridUnknownSpaceFilled, xMin, yMin);
}
if(!pixels.empty())
{
@@ -3849,9 +3872,9 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr MainWindow::getAssembledCloud(
regenerateDecimation,
regenerateMaxDepth);
}
else if(_createdClouds.contains(iter->first))
else if(uContains(_createdClouds, iter->first))
{
cloud = util3d::transformPointCloud(_createdClouds.value(iter->first), iter->second);
cloud = util3d::transformPointCloud(_createdClouds.at(iter->first), iter->second);
}
if(cloud->size())
@@ -3932,9 +3955,9 @@ std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr > MainWindow::getClouds(
regenerateDecimation,
regenerateMaxDepth);
}
else if(_createdClouds.contains(iter->first))
else if(uContains(_createdClouds, iter->first))
{
cloud = _createdClouds.value(iter->first);
cloud = _createdClouds.at(iter->first);
}
if(cloud->size())

14
guilib/src/PreferencesDialog.cpp
View File

@@ -818,6 +818,8 @@ 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->checkBox_map_fillEmptyRadius->setValue(0);
_ui->doubleSpinBox_map_opacity->setValue(0.75);
}
else if(groupBox->objectName() == _ui->groupBox_logging1->objectName())
@@ -1053,6 +1055,8 @@ 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->checkBox_map_fillEmptyRadius->setValue(settings.value("gridMapFillEmptyRadius", _ui->checkBox_map_fillEmptyRadius->value()).toInt());
_ui->doubleSpinBox_map_opacity->setValue(settings.value("gridMapOpacity", _ui->doubleSpinBox_map_opacity->value()).toDouble());
settings.endGroup(); // General
@@ -1293,6 +1297,8 @@ 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->checkBox_map_fillEmptyRadius->value());
settings.setValue("gridMapOpacity", _ui->doubleSpinBox_map_opacity->value());
settings.endGroup(); // General
@@ -2653,6 +2659,14 @@ 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->checkBox_map_fillEmptyRadius->value();
}
double PreferencesDialog::getGridMapOpacity() const
{
return _ui->doubleSpinBox_map_opacity->value();

6
guilib/src/ui/mainWindow.ui
View File

@@ -27,7 +27,7 @@
<x>0</x>
<y>0</y>
<width>1012</width>
<height>21</height>
<height>25</height>
</rect>
</property>
<widget class="QMenu" name="menuFile">
@@ -35,7 +35,7 @@
<string>File</string>
</property>
<addaction name="actionSave_point_cloud"/>
<addaction name="actionExport_2D_scans_ply_bmp"/>
<addaction name="actionExport_2D_scans_ply_pcd"/>
<addaction name="actionExport_2D_Grid_map_bmp_png"/>
<addaction name="separator"/>
<addaction name="actionExit"/>
@@ -1053,7 +1053,7 @@
<string>Export 2D grid map (*.png *.bmp)...</string>
</property>
</action>
<action name="actionExport_2D_scans_ply_bmp">
<action name="actionExport_2D_scans_ply_pcd">
<property name="text">
<string>Export 2D scans (*.ply *.pcd)...</string>
</property>

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

@@ -65,7 +65,7 @@
<x>0</x>
<y>0</y>
<width>744</width>
<height>900</height>
<height>1047</height>
</rect>
</property>
<layout class="QVBoxLayout" name="verticalLayout_16">
@@ -86,7 +86,7 @@
<enum>QFrame::Raised</enum>
</property>
<property name="currentIndex">
<number>5</number>
<number>1</number>
</property>
<widget class="QWidget" name="page_22">
<layout class="QVBoxLayout" name="verticalLayout_29">
@@ -391,7 +391,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
<item row="0" column="1">
<widget class="QLabel" name="label_37">
<property name="text">
<string>Radius</string>
<string>Radius.</string>
</property>
</widget>
</item>
@@ -414,7 +414,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
<item row="1" column="1">
<widget class="QLabel" name="label_48">
<property name="text">
<string>Angle</string>
<string>Angle.</string>
</property>
</widget>
</item>
@@ -438,7 +438,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
<item>
<widget class="QLabel" name="label_167">
<property name="text">
<string>When laser scans are used, a 2D grid map can be created. When the Graph view is visible or if the &quot;Show in 3D map view&quot; below is checked, the grid map is generated.</string>
<string>When the Graph view is visible or if the &quot;Show in 3D map view&quot; below is checked, the grid map is generated using the laser scans.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
@@ -466,18 +466,18 @@ Show a yellow background when the number of odometry inliers goes under this thr
<item row="1" column="1">
<widget class="QLabel" name="label_159">
<property name="text">
<string>Resolution (cell size)</string>
<string>Resolution (cell size).</string>
</property>
</widget>
</item>
<item row="2" column="1">
<item row="3" column="1">
<widget class="QLabel" name="label_164">
<property name="text">
<string>Fill empty space</string>
<string>Fill empty space.</string>
</property>
</widget>
</item>
<item row="2" column="0">
<item row="3" column="0">
<widget class="QCheckBox" name="checkBox_map_fillEmptySpace">
<property name="text">
<string/>
@@ -487,26 +487,10 @@ Show a yellow background when the number of odometry inliers goes under this thr
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_map_opacity">
<property name="suffix">
<string/>
</property>
<property name="minimum">
<double>0.750000000000000</double>
</property>
<property name="maximum">
<double>1.000000000000000</double>
</property>
<property name="value">
<double>0.750000000000000</double>
</property>
</widget>
</item>
<item row="3" column="1">
<item row="2" column="1">
<widget class="QLabel" name="label_170">
<property name="text">
<string>Opacity</string>
<string>Opacity.</string>
</property>
</widget>
</item>
@@ -523,7 +507,69 @@ Show a yellow background when the number of odometry inliers goes under this thr
<item row="0" column="1">
<widget class="QLabel" name="label_map_shown">
<property name="text">
<string>Show in 3D map view</string>
<string>Show in 3D map view.</string>
</property>
</widget>
</item>
<item row="4" column="0">
<widget class="QCheckBox" name="checkBox_map_occupancyFrom3DCloud">
<property name="text">
<string/>
</property>
<property name="checked">
<bool>false</bool>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_map_opacity">
<property name="suffix">
<string/>
</property>
<property name="minimum">
<double>0.750000000000000</double>
</property>
<property name="maximum">
<double>1.000000000000000</double>
</property>
<property name="value">
<double>0.750000000000000</double>
</property>
</widget>
</item>
<item row="4" 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>
</property>
<property name="wordWrap">
<bool>true</bool>
</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="checkBox_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>0</number>
</property>
</widget>
</item>