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

Added occupancy grid footprintRadius parameter to clear obstacles under path

Browse Source
This commit is contained in:
matlabbe
2016-08-14 14:52:42 -04:00
parent ae7436a41b
commit 18b1931e7c
6 changed files with 142 additions and 57 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
corelib/include/rtabmap/core/util3d_mapping.h
View File

@@ -58,7 +58,8 @@ cv::Mat RTABMAP_EXP create2DMapFromOccupancyLocalMaps(
float & xMin, float & xMin,
float & yMin, float & yMin,
float minMapSize = 0.0f, float minMapSize = 0.0f,
bool erode = false); bool erode = false,
float footprintRadius = 0.0f);
cv::Mat RTABMAP_EXP create2DMap(const std::map<int, Transform> & poses, cv::Mat RTABMAP_EXP create2DMap(const std::map<int, Transform> & poses,
const std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > & scans, const std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > & scans,

144
corelib/src/util3d_mapping.cpp
View File

@@ -129,7 +129,8 @@ cv::Mat create2DMapFromOccupancyLocalMaps(
float & xMin, float & xMin,
float & yMin, float & yMin,
float minMapSize, float minMapSize,
bool erode) bool erode,
float footprintRadius)
{ {
UASSERT(minMapSize >= 0.0f); UASSERT(minMapSize >= 0.0f);
UDEBUG("cellSize=%f m, minMapSize=%f m, erode=%d", cellSize, minMapSize, erode?1:0); UDEBUG("cellSize=%f m, minMapSize=%f m, erode=%d", cellSize, minMapSize, erode?1:0);
@@ -269,15 +270,45 @@ cv::Mat create2DMapFromOccupancyLocalMaps(
for(int i=0; i<iter->second.rows; ++i) for(int i=0; i<iter->second.rows; ++i)
{ {
cv::Point2i pt((iter->second.at<float>(i,0)-xMin)/cellSize + 0.5f, (iter->second.at<float>(i,1)-yMin)/cellSize + 0.5f); cv::Point2i pt((iter->second.at<float>(i,0)-xMin)/cellSize + 0.5f, (iter->second.at<float>(i,1)-yMin)/cellSize + 0.5f);
map.at<char>(pt.y, pt.x) = 0; // free space if(map.at<char>(pt.y, pt.x) != -2)
{
map.at<char>(pt.y, pt.x) = 0; // free space
}
} }
} }
if(footprintRadius >= cellSize*1.5f)
{
// place free space under the footprint of the robot
cv::Point2i ptBegin((kter->second.x()-footprintRadius-xMin)/cellSize + 0.5f, (kter->second.y()-footprintRadius-yMin)/cellSize + 0.5f);
cv::Point2i ptEnd((kter->second.x()+footprintRadius-xMin)/cellSize + 0.5f, (kter->second.y()+footprintRadius-yMin)/cellSize + 0.5f);
if(ptBegin.x < 0)
ptBegin.x = 0;
if(ptEnd.x >= map.cols)
ptEnd.x = map.cols-1;
if(ptBegin.y < 0)
ptBegin.y = 0;
if(ptEnd.y >= map.rows)
ptEnd.y = map.rows-1;
for(int i=ptBegin.x; i<ptEnd.x; ++i)
{
for(int j=ptBegin.y; j<ptEnd.y; ++j)
{
map.at<char>(j, i) = -2; // free space (footprint)
}
}
}
if(jter!=occupiedLocalMaps.end()) if(jter!=occupiedLocalMaps.end())
{ {
for(int i=0; i<jter->second.rows; ++i) for(int i=0; i<jter->second.rows; ++i)
{ {
cv::Point2i pt((jter->second.at<float>(i,0)-xMin)/cellSize + 0.5f, (jter->second.at<float>(i,1)-yMin)/cellSize + 0.5f); cv::Point2i pt((jter->second.at<float>(i,0)-xMin)/cellSize + 0.5f, (jter->second.at<float>(i,1)-yMin)/cellSize + 0.5f);
map.at<char>(pt.y, pt.x) = 100; // obstacles if(map.at<char>(pt.y, pt.x) != -2)
{
map.at<char>(pt.y, pt.x) = 100; // obstacles
}
} }
} }
@@ -287,63 +318,70 @@ cv::Mat create2DMapFromOccupancyLocalMaps(
// fill holes and remove empty from obstacle borders // fill holes and remove empty from obstacle borders
cv::Mat updatedMap = map.clone(); cv::Mat updatedMap = map.clone();
std::list<std::pair<int, int> > obstacleIndices; std::list<std::pair<int, int> > obstacleIndices;
for(int i=2; i<map.rows-2; ++i) for(int i=0; i<map.rows; ++i)
{ {
for(int j=2; j<map.cols-2; ++j) for(int j=0; j<map.cols; ++j)
{ {
if(map.at<char>(i, j) == -1 && if(map.at<char>(i, j) == -2)
map.at<char>(i+1, j) != -1 &&
map.at<char>(i-1, j) != -1 &&
map.at<char>(i, j+1) != -1 &&
map.at<char>(i, j-1) != -1)
{ {
updatedMap.at<char>(i, j) = 0; updatedMap.at<char>(i, j) = 0;
} }
else if(map.at<char>(i, j) == 100)
{
// obstacle/empty/unknown -> remove empty
// unknown/empty/obstacle -> remove empty
if(map.at<char>(i-1, j) == 0 &&
map.at<char>(i-2, j) == -1)
{
updatedMap.at<char>(i-1, j) = -1;
}
else if(map.at<char>(i+1, j) == 0 &&
map.at<char>(i+2, j) == -1)
{
updatedMap.at<char>(i+1, j) = -1;
}
if(map.at<char>(i, j-1) == 0 &&
map.at<char>(i, j-2) == -1)
{
updatedMap.at<char>(i, j-1) = -1;
}
else if(map.at<char>(i, j+1) == 0 &&
map.at<char>(i, j+2) == -1)
{
updatedMap.at<char>(i, j+1) = -1;
}
if(erode) if(i >=2 && i<map.rows-2 && j>=2 && j<map.cols-2)
{
if(map.at<char>(i, j) == -1 &&
map.at<char>(i+1, j) != -1 &&
map.at<char>(i-1, j) != -1 &&
map.at<char>(i, j+1) != -1 &&
map.at<char>(i, j-1) != -1)
{ {
obstacleIndices.push_back(std::make_pair(i, j)); updatedMap.at<char>(i, j) = 0;
}
else if(map.at<char>(i, j) == 100)
{
// obstacle/empty/unknown -> remove empty
// unknown/empty/obstacle -> remove empty
if((map.at<char>(i-1, j) == 0 || map.at<char>(i-1, j) == -2) &&
map.at<char>(i-2, j) == -1)
{
updatedMap.at<char>(i-1, j) = -1;
}
else if((map.at<char>(i+1, j) == 0 || map.at<char>(i+1, j) == -2) &&
map.at<char>(i+2, j) == -1)
{
updatedMap.at<char>(i+1, j) = -1;
}
if((map.at<char>(i, j-1) == 0 || map.at<char>(i, j-1) == -2) &&
map.at<char>(i, j-2) == -1)
{
updatedMap.at<char>(i, j-1) = -1;
}
else if((map.at<char>(i, j+1) == 0 || map.at<char>(i, j+1) == -2) &&
map.at<char>(i, j+2) == -1)
{
updatedMap.at<char>(i, j+1) = -1;
}
if(erode)
{
obstacleIndices.push_back(std::make_pair(i, j));
}
}
else if(map.at<char>(i, j) == 0)
{
// obstacle/empty/obstacle -> remove empty
if(map.at<char>(i-1, j) == 100 &&
map.at<char>(i+1, j) == 100)
{
updatedMap.at<char>(i, j) = -1;
}
else if(map.at<char>(i, j-1) == 100 &&
map.at<char>(i, j+1) == 100)
{
updatedMap.at<char>(i, j) = -1;
}
} }
} }
else if(map.at<char>(i, j) == 0)
{
// obstacle/empty/obstacle -> remove empty
if(map.at<char>(i-1, j) == 100 &&
map.at<char>(i+1, j) == 100)
{
updatedMap.at<char>(i, j) = -1;
}
else if(map.at<char>(i, j-1) == 100 &&
map.at<char>(i, j+1) == 100)
{
updatedMap.at<char>(i, j) = -1;
}
}
} }
} }
map = updatedMap; map = updatedMap;
@@ -649,6 +687,10 @@ cv::Mat convertMap2Image8U(const cv::Mat & map8S)
{ {
gray = 0; gray = 0;
} }
else if(v == -2)
{
gray = 200;
}
else // -1 else // -1
{ {
gray = 89; gray = 89;

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

@@ -184,6 +184,7 @@ public:
bool getGridMapShown() const; bool getGridMapShown() const;
double getGridMapResolution() const;; double getGridMapResolution() const;;
bool isGridMapEroded() const; bool isGridMapEroded() const;
double getGridMapFootprintRadius() const;
bool isGridMapFrom3DCloud() const; bool isGridMapFrom3DCloud() const;
bool projMapFrame() const; bool projMapFrame() const;
double projMaxGroundAngle() const; double projMaxGroundAngle() const;

3
guilib/src/MainWindow.cpp
View File

@@ -2207,7 +2207,8 @@ void MainWindow::updateMapCloud(
resolution, resolution,
xMin, yMin, xMin, yMin,
0, 0,
_preferencesDialog->isGridMapEroded()); _preferencesDialog->isGridMapEroded(),
_preferencesDialog->getGridMapFootprintRadius());
if(!map8S.empty()) if(!map8S.empty())
{ {
//convert to gray scaled map //convert to gray scaled map

8
guilib/src/PreferencesDialog.cpp
View File

@@ -377,6 +377,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
connect(_ui->doubleSpinBox_map_resolution, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel())); connect(_ui->doubleSpinBox_map_resolution, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
connect(_ui->doubleSpinBox_map_opacity, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel())); connect(_ui->doubleSpinBox_map_opacity, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
connect(_ui->checkBox_map_erode, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel())); connect(_ui->checkBox_map_erode, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
connect(_ui->doubleSpinBox_map_footprintRadius, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
connect(_ui->groupBox_map_occupancyFrom3DCloud, SIGNAL(toggled(bool)), this, SLOT(makeObsoleteCloudRenderingPanel())); connect(_ui->groupBox_map_occupancyFrom3DCloud, SIGNAL(toggled(bool)), this, SLOT(makeObsoleteCloudRenderingPanel()));
connect(_ui->checkBox_projMapFrame, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel())); connect(_ui->checkBox_projMapFrame, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
connect(_ui->doubleSpinBox_projMaxGroundAngle, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel())); connect(_ui->doubleSpinBox_projMaxGroundAngle, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
@@ -1220,6 +1221,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
_ui->checkBox_map_shown->setChecked(false); _ui->checkBox_map_shown->setChecked(false);
_ui->doubleSpinBox_map_resolution->setValue(0.05); _ui->doubleSpinBox_map_resolution->setValue(0.05);
_ui->checkBox_map_erode->setChecked(false); _ui->checkBox_map_erode->setChecked(false);
_ui->doubleSpinBox_map_footprintRadius->setValue(0);
_ui->doubleSpinBox_map_opacity->setValue(0.75); _ui->doubleSpinBox_map_opacity->setValue(0.75);
_ui->groupBox_map_occupancyFrom3DCloud->setChecked(false); _ui->groupBox_map_occupancyFrom3DCloud->setChecked(false);
_ui->checkBox_projMapFrame->setChecked(true); _ui->checkBox_projMapFrame->setChecked(true);
@@ -1575,6 +1577,7 @@ void PreferencesDialog::readGuiSettings(const QString & filePath)
_ui->checkBox_map_shown->setChecked(settings.value("gridMapShown", _ui->checkBox_map_shown->isChecked()).toBool()); _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->doubleSpinBox_map_resolution->setValue(settings.value("gridMapResolution", _ui->doubleSpinBox_map_resolution->value()).toDouble());
_ui->checkBox_map_erode->setChecked(settings.value("gridMapEroded", _ui->checkBox_map_erode->isChecked()).toBool()); _ui->checkBox_map_erode->setChecked(settings.value("gridMapEroded", _ui->checkBox_map_erode->isChecked()).toBool());
_ui->doubleSpinBox_map_footprintRadius->setValue(settings.value("gridMapFootprintRadius", _ui->doubleSpinBox_map_footprintRadius->value()).toDouble());
_ui->doubleSpinBox_map_opacity->setValue(settings.value("gridMapOpacity", _ui->doubleSpinBox_map_opacity->value()).toDouble()); _ui->doubleSpinBox_map_opacity->setValue(settings.value("gridMapOpacity", _ui->doubleSpinBox_map_opacity->value()).toDouble());
_ui->groupBox_map_occupancyFrom3DCloud->setChecked(settings.value("gridMapOccupancyFrom3DCloud", _ui->groupBox_map_occupancyFrom3DCloud->isChecked()).toBool()); _ui->groupBox_map_occupancyFrom3DCloud->setChecked(settings.value("gridMapOccupancyFrom3DCloud", _ui->groupBox_map_occupancyFrom3DCloud->isChecked()).toBool());
_ui->checkBox_projMapFrame->setChecked(settings.value("projMapFrame", _ui->checkBox_projMapFrame->isChecked()).toBool()); _ui->checkBox_projMapFrame->setChecked(settings.value("projMapFrame", _ui->checkBox_projMapFrame->isChecked()).toBool());
@@ -1991,6 +1994,7 @@ void PreferencesDialog::writeGuiSettings(const QString & filePath) const
settings.setValue("gridMapShown", _ui->checkBox_map_shown->isChecked()); settings.setValue("gridMapShown", _ui->checkBox_map_shown->isChecked());
settings.setValue("gridMapResolution", _ui->doubleSpinBox_map_resolution->value()); settings.setValue("gridMapResolution", _ui->doubleSpinBox_map_resolution->value());
settings.setValue("gridMapEroded", _ui->checkBox_map_erode->isChecked()); settings.setValue("gridMapEroded", _ui->checkBox_map_erode->isChecked());
settings.setValue("gridMapFootprintRadius", _ui->doubleSpinBox_map_footprintRadius->value());
settings.setValue("gridMapOpacity", _ui->doubleSpinBox_map_opacity->value()); settings.setValue("gridMapOpacity", _ui->doubleSpinBox_map_opacity->value());
settings.setValue("gridMapOccupancyFrom3DCloud", _ui->groupBox_map_occupancyFrom3DCloud->isChecked()); settings.setValue("gridMapOccupancyFrom3DCloud", _ui->groupBox_map_occupancyFrom3DCloud->isChecked());
@@ -3843,6 +3847,10 @@ bool PreferencesDialog::isGridMapEroded() const
{ {
return _ui->checkBox_map_erode->isChecked(); return _ui->checkBox_map_erode->isChecked();
} }
double PreferencesDialog::getGridMapFootprintRadius() const
{
return _ui->doubleSpinBox_map_footprintRadius->value();
}
bool PreferencesDialog::isGridMapFrom3DCloud() const bool PreferencesDialog::isGridMapFrom3DCloud() const
{ {
return _ui->groupBox_map_occupancyFrom3DCloud->isChecked(); return _ui->groupBox_map_occupancyFrom3DCloud->isChecked();

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

@@ -65,7 +65,7 @@
<x>0</x> <x>0</x>
<y>0</y> <y>0</y>
<width>686</width> <width>686</width>
<height>2023</height> <height>2053</height>
</rect> </rect>
</property> </property>
<layout class="QVBoxLayout" name="verticalLayout_16"> <layout class="QVBoxLayout" name="verticalLayout_16">
@@ -86,7 +86,7 @@
<enum>QFrame::Raised</enum> <enum>QFrame::Raised</enum>
</property> </property>
<property name="currentIndex"> <property name="currentIndex">
<number>13</number> <number>3</number>
</property> </property>
<widget class="QWidget" name="page_22"> <widget class="QWidget" name="page_22">
<layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1"> <layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1">
@@ -1598,7 +1598,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
</property> </property>
</widget> </widget>
</item> </item>
<item row="3" column="1"> <item row="4" column="1">
<widget class="QLabel" name="label_224"> <widget class="QLabel" name="label_224">
<property name="text"> <property name="text">
<string>Erode.</string> <string>Erode.</string>
@@ -1611,7 +1611,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
</property> </property>
</widget> </widget>
</item> </item>
<item row="3" column="0"> <item row="4" column="0">
<widget class="QCheckBox" name="checkBox_map_erode"> <widget class="QCheckBox" name="checkBox_map_erode">
<property name="text"> <property name="text">
<string/> <string/>
@@ -1621,6 +1621,38 @@ Show a yellow background when the number of odometry inliers goes under this thr
</property> </property>
</widget> </widget>
</item> </item>
<item row="3" column="1">
<widget class="QLabel" name="label_319">
<property name="text">
<string>Footprint radius.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_map_footprintRadius">
<property name="suffix">
<string> m</string>
</property>
<property name="minimum">
<double>0.000000000000000</double>
</property>
<property name="maximum">
<double>10.000000000000000</double>
</property>
<property name="singleStep">
<double>0.100000000000000</double>
</property>
<property name="value">
<double>0.000000000000000</double>
</property>
</widget>
</item>
</layout> </layout>
</item> </item>
<item> <item>