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

Added Optimizer/GravitySigma parameter (with GTSAM support).

Browse Source
This commit is contained in:
matlabbe
2019-03-31 15:51:50 -04:00
parent 35933cafba
commit fc9c762516
17 changed files with 565 additions and 40 deletions
Download Patch File Download Diff File Expand all files Collapse all files

23
guilib/src/DatabaseViewer.cpp
View File

@@ -5250,6 +5250,14 @@ void DatabaseViewer::updateConstraintView(
}
}
if(optimizer->gravitySigma() > 0)
{
for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{
linksOut.insert(std::make_pair(iter->first, Link(iter->first, iter->first, Link::kPoseOdom, odomPoses_.at(iter->first))));
}
}
QTime time;
time.start();
std::map<int, rtabmap::Transform> finalPoses = optimizer->optimize(link.to(), posesOut, linksOut);
@@ -6397,6 +6405,14 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent)
}
}
if(optimizer->gravitySigma() > 0)
{
for(std::map<int, Transform>::iterator iter=scanPoses.begin(); iter!=scanPoses.end(); ++iter)
{
linksOut.insert(std::make_pair(iter->first, Link(iter->first, iter->first, Link::kPoseOdom, odomPoses_.at(iter->first))));
}
}
scanPoses = optimizer->optimize(currentLink.to(), posesOut, linksOut);
delete optimizer;
@@ -6788,6 +6804,13 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent)
UASSERT(odomPoses_.find(fromId) != odomPoses_.end());
UASSERT_MSG(odomPoses_.find(newLink.from()) != odomPoses_.end(), uFormat("id=%d poses=%d links=%d", newLink.from(), (int)odomPoses_.size(), (int)linksIn.size()).c_str());
UASSERT_MSG(odomPoses_.find(newLink.to()) != odomPoses_.end(), uFormat("id=%d poses=%d links=%d", newLink.to(), (int)odomPoses_.size(), (int)linksIn.size()).c_str());
if(optimizer->gravitySigma() > 0)
{
for(std::map<int, Transform>::iterator iter=odomPoses_.begin(); iter!=odomPoses_.end(); ++iter)
{
linksIn.insert(std::make_pair(iter->first, Link(iter->first, iter->first, Link::kPoseOdom, iter->second)));
}
}
optimizer->getConnectedGraph(fromId, odomPoses_, linksIn, poses, links);
// use already optimized poses
if(graphes_.size())

39
guilib/src/MainWindow.cpp
View File

@@ -5523,6 +5523,7 @@ void MainWindow::postProcessing()
}
// First, verify that we have all data required in the GUI
std::map<int, Transform> odomPoses;
bool allDataAvailable = true;
for(std::map<int, Transform>::iterator iter = _currentPosesMap.lower_bound(1);
iter!=_currentPosesMap.end() && allDataAvailable;
@@ -5534,6 +5535,10 @@ void MainWindow::postProcessing()
UWARN("Node %d missing.", iter->first);
allDataAvailable = false;
}
else if(!jter.value().getPose().isNull())
{
odomPoses.insert(std::make_pair(iter->first, jter.value().getPose()));
}
}
if(!allDataAvailable)
@@ -5747,6 +5752,13 @@ void MainWindow::postProcessing()
UASSERT(_currentPosesMap.find(fromId) != _currentPosesMap.end());
UASSERT_MSG(_currentPosesMap.find(from) != _currentPosesMap.end(), uFormat("id=%d poses=%d links=%d", from, (int)poses.size(), (int)links.size()).c_str());
UASSERT_MSG(_currentPosesMap.find(to) != _currentPosesMap.end(), uFormat("id=%d poses=%d links=%d", to, (int)poses.size(), (int)links.size()).c_str());
if(optimizer->gravitySigma() > 0)
{
for(std::map<int, Transform>::iterator jter=odomPoses.begin(); jter!=odomPoses.end(); ++jter)
{
linksIn.insert(std::make_pair(jter->first, Link(jter->first, jter->first, Link::kPoseOdom, jter->second)));
}
}
optimizer->getConnectedGraph(fromId, _currentPosesMap, linksIn, poses, links);
UASSERT(poses.find(fromId) != poses.end());
UASSERT_MSG(poses.find(from) != poses.end(), uFormat("id=%d poses=%d links=%d", from, (int)poses.size(), (int)links.size()).c_str());
@@ -5857,10 +5869,18 @@ void MainWindow::postProcessing()
std::map<int, rtabmap::Transform> posesOut;
std::multimap<int, rtabmap::Link> linksOut;
std::map<int, rtabmap::Transform> optimizedPoses;
std::multimap<int, rtabmap::Link> linksIn = _currentLinksMap;
if(optimizer->gravitySigma() > 0)
{
for(std::map<int, Transform>::iterator iter=odomPoses.begin(); iter!=odomPoses.end(); ++iter)
{
linksIn.insert(std::make_pair(iter->first, Link(iter->first, iter->first, Link::kPoseOdom, iter->second)));
}
}
optimizer->getConnectedGraph(
fromId,
_currentPosesMap,
_currentLinksMap,
linksIn,
posesOut,
linksOut);
optimizedPoses = optimizer->optimize(fromId, posesOut, linksOut);
@@ -5889,13 +5909,12 @@ void MainWindow::postProcessing()
for(std::multimap<int, Link>::iterator iter = _currentLinksMap.lower_bound(1); iter!=_currentLinksMap.end() && !_progressCanceled; ++iter, ++i)
{
int type = iter->second.type();
int from = iter->second.from();
int to = iter->second.to();
if((refineNeighborLinks && type==Link::kNeighbor) ||
(refineLoopClosureLinks && type!=Link::kNeighbor && type!=Link::kLandmark))
(refineLoopClosureLinks && type!=Link::kNeighbor && type!=Link::kLandmark && from!=to))
{
int from = iter->second.from();
int to = iter->second.to();
_progressDialog->appendText(tr("Refining link %1->%2 (%3/%4)").arg(from).arg(to).arg(i+1).arg(_currentLinksMap.size()));
_progressDialog->incrementStep();
QApplication::processEvents();
@@ -5966,10 +5985,18 @@ void MainWindow::postProcessing()
std::map<int, rtabmap::Transform> posesOut;
std::multimap<int, rtabmap::Link> linksOut;
std::map<int, rtabmap::Transform> optimizedPoses;
std::multimap<int, rtabmap::Link> linksIn = _currentLinksMap;
if(optimizer->gravitySigma() > 0)
{
for(std::map<int, Transform>::iterator iter=odomPoses.begin(); iter!=odomPoses.end(); ++iter)
{
linksIn.insert(std::make_pair(iter->first, Link(iter->first, iter->first, Link::kPoseOdom, iter->second)));
}
}
optimizer->getConnectedGraph(
fromId,
_currentPosesMap,
_currentLinksMap,
linksIn,
posesOut,
linksOut);
optimizedPoses = optimizer->optimize(fromId, posesOut, linksOut);

1
guilib/src/PreferencesDialog.cpp
View File

@@ -895,6 +895,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->graphOptimization_covarianceIgnored->setObjectName(Parameters::kOptimizerVarianceIgnored().c_str());
_ui->graphOptimization_fromGraphEnd->setObjectName(Parameters::kRGBDOptimizeFromGraphEnd().c_str());
_ui->graphOptimization_maxError->setObjectName(Parameters::kRGBDOptimizeMaxError().c_str());
_ui->graphOptimization_gravitySigma->setObjectName(Parameters::kOptimizerGravitySigma().c_str());
_ui->graphOptimization_stopEpsilon->setObjectName(Parameters::kOptimizerEpsilon().c_str());
_ui->graphOptimization_robust->setObjectName(Parameters::kOptimizerRobust().c_str());
_ui->graphOptimization_priorsIgnored->setObjectName(Parameters::kOptimizerPriorsIgnored().c_str());

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

@@ -94,16 +94,25 @@
<property name="geometry">
<rect>
<x>0</x>
<y>-631</y>
<width>681</width>
<height>3071</height>
<y>0</y>
<width>680</width>
<height>3082</height>
</rect>
</property>
<layout class="QVBoxLayout" name="verticalLayout_16">
<property name="spacing">
<number>0</number>
</property>
<property name="margin">
<property name="leftMargin">
<number>0</number>
</property>
<property name="topMargin">
<number>0</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<number>0</number>
</property>
<item>
@@ -117,7 +126,7 @@
<enum>QFrame::Raised</enum>
</property>
<property name="currentIndex">
<number>19</number>
<number>14</number>
</property>
<widget class="QWidget" name="page_22">
<layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1">
@@ -2955,7 +2964,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
<item>
<widget class="QStackedWidget" name="stackedWidget_src">
<property name="currentIndex">
<number>0</number>
<number>3</number>
</property>
<widget class="QWidget" name="page_41">
<layout class="QVBoxLayout" name="verticalLayout_64">
@@ -2979,6 +2988,9 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item>
@@ -3048,6 +3060,9 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="1" column="1">
@@ -3058,6 +3073,9 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="1" column="0">
@@ -4121,6 +4139,9 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item>
@@ -4175,6 +4196,9 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="1" column="1">
@@ -4192,6 +4216,9 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="2" column="2">
@@ -5164,7 +5191,16 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
<string>Directory of images (optional settings)</string>
</property>
<layout class="QVBoxLayout" name="verticalLayout_93">
<property name="margin">
<property name="leftMargin">
<number>0</number>
</property>
<property name="topMargin">
<number>0</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<number>0</number>
</property>
<item>
@@ -9646,21 +9682,21 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="5" column="0" rowspan="2">
<item row="6" column="0" rowspan="2">
<widget class="QCheckBox" name="graphOptimization_fromGraphEnd">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="5" column="1">
<item row="6" column="1">
<widget class="Line" name="line">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
</widget>
</item>
<item row="6" column="1">
<item row="7" column="1">
<widget class="QLabel" name="label_151">
<property name="text">
<string>Optimize graph from the newest node.</string>
@@ -9673,7 +9709,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_211">
<property name="text">
<string>-If true, there is no odometry correction computed. All previous poses in the map are corrected instead, not the last one (which corresponds to latest odometry value). So, the transform between frames /map to /odom will be always Identity even on loop closures.</string>
@@ -9686,7 +9722,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="8" column="1">
<item row="9" column="1">
<widget class="QLabel" name="label_183">
<property name="text">
<string>-If false, the graph is optimized from the oldest node of the current graph. It can be useful to preserve the map referential from the oldest node. An odometry correction between frames /map to /odom is computed. Warning: when some nodes are transferred, the first referential of the local map may change, resulting in momentary changes in robot/map position (which are annoying in teleoperation).</string>
@@ -9699,7 +9735,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_431">
<property name="text">
<string>Ignore pose priors.</string>
@@ -9712,14 +9748,21 @@ 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="QCheckBox" name="graphOptimization_priorsIgnored">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="4" column="1">
<item row="5" column="0">
<widget class="QCheckBox" name="graphOptimization_landmarksIgnored">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="5" column="1">
<widget class="QLabel" name="label_521">
<property name="text">
<string>Ignore landmarks.</string>
@@ -9732,11 +9775,39 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="4" column="0">
<widget class="QCheckBox" name="graphOptimization_landmarksIgnored">
<item row="3" column="1">
<widget class="QLabel" name="label_527">
<property name="text">
<string>Gravity sigma value (&gt;=0, typically between 0.1 and 0.3). Optimization is done while preserving gravity orientation of the poses. This should be used only with visual/lidar inertial odometry approaches, for which we assume that all odometry poses are aligned with gravity. Set to 0 to disable gravity constraints. Currently supported only with GTSAM optimization strategy.</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="graphOptimization_gravitySigma">
<property name="suffix">
<string/>
</property>
<property name="decimals">
<number>2</number>
</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>
@@ -15939,7 +16010,16 @@ Lower the ratio -&gt; higher the precision.</string>
<property name="spacing">
<number>0</number>
</property>
<property name="margin">
<property name="leftMargin">
<number>0</number>
</property>
<property name="topMargin">
<number>0</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<number>0</number>
</property>
<item>
@@ -16019,7 +16099,16 @@ Lower the ratio -&gt; higher the precision.</string>
<property name="spacing">
<number>0</number>
</property>
<property name="margin">
<property name="leftMargin">
<number>0</number>
</property>
<property name="topMargin">
<number>0</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<number>0</number>
</property>
<item>
@@ -16131,7 +16220,16 @@ Lower the ratio -&gt; higher the precision.</string>
<property name="spacing">
<number>0</number>
</property>
<property name="margin">
<property name="leftMargin">
<number>0</number>
</property>
<property name="topMargin">
<number>0</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<number>0</number>
</property>
<item>