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 parameter GridGlobal/AltitudeDelta

Browse Source
This commit is contained in:
matlabbe
2021-02-21 13:00:16 -05:00
parent e59aad03ed
commit 8696a38343
4 changed files with 82 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files

38
guilib/src/MainWindow.cpp
View File

@@ -258,7 +258,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent, bool sh
setupMainLayout(_preferencesDialog->isVerticalLayoutUsed());
ParametersMap parameters = _preferencesDialog->getAllParameters();
_occupancyGrid = new OccupancyGrid();
_occupancyGrid = new OccupancyGrid(parameters);
#ifdef RTABMAP_OCTOMAP
_octomap = new OctoMap(parameters);
#endif
@@ -2466,19 +2466,39 @@ void MainWindow::updateMapCloud(
}
int maxNodes = uStr2Int(_preferencesDialog->getParameter(Parameters::kGridGlobalMaxNodes()));
if(maxNodes > 0 && poses.size()>1)
int altitudeDelta = uStr2Int(_preferencesDialog->getParameter(Parameters::kGridGlobalAltitudeDelta()));
if((maxNodes > 0 || altitudeDelta>0.0) && poses.size()>1)
{
std::map<int, float> nodes = graph::findNearestNodes(poses, poses.rbegin()->second, maxNodes);
std::map<int, Transform> nearestPoses;
nearestPoses.insert(*poses.rbegin());
for(std::map<int, float>::iterator iter=nodes.begin(); iter!=nodes.end(); ++iter)
Transform currentPose = poses.rbegin()->second;
if(poses.find(0) != poses.end())
{
std::map<int, Transform>::iterator pter = poses.find(iter->first);
if(pter != poses.end())
currentPose = poses.at(0);
}
std::map<int, Transform> nearestPoses;
if(maxNodes > 0)
{
std::map<int, float> nodes = graph::findNearestNodes(poses, currentPose, maxNodes);
for(std::map<int, float>::iterator iter=nodes.begin(); iter!=nodes.end(); ++iter)
{
nearestPoses.insert(*pter);
if(altitudeDelta<=0.0 ||
fabs(poses.at(iter->first).z()-currentPose.z())<altitudeDelta)
{
nearestPoses.insert(*poses.find(iter->first));
}
}
}
else // altitudeDelta>0.0
{
for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{
if(fabs(iter->second.z()-currentPose.z())<altitudeDelta)
{
nearestPoses.insert(*iter);
}
}
}
//add zero...
if(poses.find(0) != poses.end())
{

1
guilib/src/PreferencesDialog.cpp
View File

@@ -1187,6 +1187,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_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_altitudeDelta->setObjectName(Parameters::kGridGlobalAltitudeDelta().c_str());
_ui->doubleSpinBox_grid_footprintRadius->setObjectName(Parameters::kGridGlobalFootprintRadius().c_str());
_ui->doubleSpinBox_grid_occThr->setObjectName(Parameters::kGridGlobalOccupancyThr().c_str());
_ui->doubleSpinBox_grid_probHit->setObjectName(Parameters::kGridGlobalProbHit().c_str());

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

@@ -63,7 +63,7 @@
<property name="geometry">
<rect>
<x>0</x>
<y>-678</y>
<y>0</y>
<width>686</width>
<height>3357</height>
</rect>
@@ -95,7 +95,7 @@
<enum>QFrame::Raised</enum>
</property>
<property name="currentIndex">
<number>5</number>
<number>16</number>
</property>
<widget class="QWidget" name="page_22">
<layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1">
@@ -12238,7 +12238,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
<layout class="QVBoxLayout" name="verticalLayout_115">
<item>
<layout class="QGridLayout" name="gridLayout_88" columnstretch="0,1">
<item row="8" column="1">
<item row="9" column="1">
<widget class="QLabel" name="label_484">
<property name="text">
<string>Probability clamping threshold minimum.</string>
@@ -12274,7 +12274,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="10" column="1">
<item row="11" column="1">
<widget class="QLabel" name="label_224">
<property name="text">
<string>Erode obstacle cells.</string>
@@ -12287,7 +12287,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="6" column="1">
<item row="7" column="1">
<widget class="QLabel" name="label_482">
<property name="text">
<string>Probability of a hit.</string>
@@ -12300,7 +12300,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_319">
<property name="text">
<string>Footprint radius used to clear all obstacles under the graph.</string>
@@ -12313,7 +12313,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="10" column="0">
<item row="11" column="0">
<widget class="QCheckBox" name="checkBox_grid_erode">
<property name="text">
<string/>
@@ -12330,7 +12330,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="QDoubleSpinBox" name="doubleSpinBox_grid_footprintRadius">
<property name="suffix">
<string> m</string>
@@ -12432,7 +12432,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="5" column="1">
<item row="6" column="1">
<widget class="QLabel" name="label_457">
<property name="text">
<string>Occupancy threshold.</string>
@@ -12445,7 +12445,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="5" column="0">
<item row="6" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_grid_occThr">
<property name="suffix">
<string/>
@@ -12464,7 +12464,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="7" column="1">
<item row="8" column="1">
<widget class="QLabel" name="label_483">
<property name="text">
<string>Probability of a miss.</string>
@@ -12477,7 +12477,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="6" column="0">
<item row="7" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_grid_probHit">
<property name="suffix">
<string/>
@@ -12496,7 +12496,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="7" column="0">
<item row="8" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_grid_probMiss">
<property name="suffix">
<string/>
@@ -12515,7 +12515,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="9" column="1">
<item row="10" column="1">
<widget class="QLabel" name="label_485">
<property name="text">
<string>Probability clamping threshold maximum.</string>
@@ -12528,7 +12528,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="8" column="0">
<item row="9" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_grid_clampingMin">
<property name="suffix">
<string/>
@@ -12550,7 +12550,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="9" column="0">
<item row="10" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_grid_clampingMax">
<property name="suffix">
<string/>
@@ -12572,6 +12572,41 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="4" column="1">
<widget class="QLabel" name="label_616">
<property name="text">
<string>Altitude delta. Assemble only nodes that have the same altitude of +-delta meters of the current pose (0=disabled). This is used to generate 2D occupancy grid based on the current altitude (e.g., multi-floor building).</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="4" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_grid_altitudeDelta">
<property name="suffix">
<string> m</string>
</property>
<property name="decimals">
<number>2</number>
</property>
<property name="minimum">
<double>0.000000000000000</double>
</property>
<property name="maximum">
<double>99.000000000000000</double>
</property>
<property name="singleStep">
<double>0.100000000000000</double>
</property>
<property name="value">
<double>0.000000000000000</double>
</property>
</widget>
</item>
</layout>
</item>
</layout>