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

Added parameter Rtabmap/LoopGPS (default true) to enabled/disable the use of GPS in likelihood computation

Browse Source
This commit is contained in:
matlabbe
2018-10-22 14:39:30 -04:00
parent 3bc8fc4c11
commit d2abc3a237
5 changed files with 54 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/include/rtabmap/core/Parameters.h
+1
View File
@@ -193,6 +193,7 @@ class RTABMAP_EXP Parameters
// Hypotheses selection
RTABMAP_PARAM(Rtabmap, LoopThr, float, 0.11, "Loop closing threshold.");
RTABMAP_PARAM(Rtabmap, LoopRatio, float, 0, "The loop closure hypothesis must be over LoopRatio x lastHypothesisValue.");
RTABMAP_PARAM(Rtabmap, LoopGPS, bool, true, uFormat("Use GPS to filter likelihood (if GPS is recorded). Only locations inside the local radius \"%s\" of the current GPS location are considered for loop closure detection.", kRGBDLocalRadius().c_str()));
// Memory
RTABMAP_PARAM(Mem, RehearsalSimilarity, float, 0.6, "Rehearsal similarity.");
corelib/include/rtabmap/core/Rtabmap.h
+1
View File
@@ -261,6 +261,7 @@ private:
float _pathAngularVelocity;
bool _savedLocalizationIgnored;
bool _loopCovLimited;
bool _loopGPS;
std::pair<int, float> _loopClosureHypothesis;
std::pair<int, float> _highestHypothesis;
corelib/src/Rtabmap.cpp
+29 -23
View File
@@ -124,6 +124,7 @@ Rtabmap::Rtabmap() :
_pathAngularVelocity(Parameters::defaultRGBDPlanAngularVelocity()),
_savedLocalizationIgnored(Parameters::defaultRGBDSavedLocalizationIgnored()),
_loopCovLimited(Parameters::defaultRGBDLoopCovLimited()),
_loopGPS(Parameters::defaultRtabmapLoopGPS()),
_loopClosureHypothesis(0,0.0f),
_highestHypothesis(0,0.0f),
_lastProcessTime(0.0),
@@ -469,6 +470,7 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kRGBDPlanAngularVelocity(), _pathAngularVelocity);
Parameters::parse(parameters, Parameters::kRGBDSavedLocalizationIgnored(), _savedLocalizationIgnored);
Parameters::parse(parameters, Parameters::kRGBDLoopCovLimited(), _loopCovLimited);
Parameters::parse(parameters, Parameters::kRtabmapLoopGPS(), _loopGPS);
UASSERT(_rgbdLinearUpdate >= 0.0f);
UASSERT(_rgbdAngularUpdate >= 0.0f);
@@ -1399,37 +1401,41 @@ bool Rtabmap::process(
ULOGGER_INFO("computing likelihood...");
std::list<int> signaturesToCompare;
GPS originGPS = signature->sensorData().gps();
GPS originGPS;
Transform originOffsetENU = Transform::getIdentity();
if(originGPS.stamp() == 0.0 && _currentSessionHasGPS)
if(_loopGPS)
{
UTimer tmpT;
if(_optimizedPoses.size() && _memory->isIncremental())
originGPS = signature->sensorData().gps();
if(originGPS.stamp() == 0.0 && _currentSessionHasGPS)
{
//Search for latest node having GPS linked to current signature not too far.
std::map<int, float> nearestIds = graph::getNodesInRadius(signature->id(), _optimizedPoses, _localRadius);
for(std::map<int, float>::reverse_iterator iter=nearestIds.rbegin(); iter!=nearestIds.rend(); ++iter)
UTimer tmpT;
if(_optimizedPoses.size() && _memory->isIncremental())
{
const Signature * s = _memory->getSignature(iter->first);
UASSERT(s!=0);
if(s->sensorData().gps().stamp() > 0.0)
//Search for latest node having GPS linked to current signature not too far.
std::map<int, float> nearestIds = graph::getNodesInRadius(signature->id(), _optimizedPoses, _localRadius);
for(std::map<int, float>::reverse_iterator iter=nearestIds.rbegin(); iter!=nearestIds.rend(); ++iter)
{
originGPS = s->sensorData().gps();
const Transform & sPose = _optimizedPoses.at(s->id());
Transform localToENU(0,0,(float)((-(originGPS.bearing()-90))*M_PI/180.0) - sPose.theta());
originOffsetENU = localToENU * (sPose.rotation()*(sPose.inverse()*_optimizedPoses.at(signature->id())));
break;
const Signature * s = _memory->getSignature(iter->first);
UASSERT(s!=0);
if(s->sensorData().gps().stamp() > 0.0)
{
originGPS = s->sensorData().gps();
const Transform & sPose = _optimizedPoses.at(s->id());
Transform localToENU(0,0,(float)((-(originGPS.bearing()-90))*M_PI/180.0) - sPose.theta());
originOffsetENU = localToENU * (sPose.rotation()*(sPose.inverse()*_optimizedPoses.at(signature->id())));
break;
}
}
}
//else if(!_memory->isIncremental()) // TODO, how can we estimate current GPS position in localization?
//{
//}
}
if(originGPS.stamp() > 0.0)
{
// no need to save it if it is in localization mode
_gpsGeocentricCache.insert(std::make_pair(signature->id(), std::make_pair(originGPS.toGeodeticCoords().toGeocentric_WGS84(), originOffsetENU)));
}
//else if(!_memory->isIncremental()) // TODO, how can we estimate current GPS position in localization?
//{
//}
}
if(originGPS.stamp() > 0.0)
{
// no need to save it if it is in localization mode
_gpsGeocentricCache.insert(std::make_pair(signature->id(), std::make_pair(originGPS.toGeodeticCoords().toGeocentric_WGS84(), originOffsetENU)));
}
for(std::map<int, double>::const_iterator iter=_memory->getWorkingMem().begin();
guilib/src/PreferencesDialog.cpp
+1
View File
@@ -709,6 +709,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->general_checkBox_startNewMapOnGoodSignature->setObjectName(Parameters::kRtabmapStartNewMapOnGoodSignature().c_str());
_ui->general_checkBox_imagesAlreadyRectified->setObjectName(Parameters::kRtabmapImagesAlreadyRectified().c_str());
_ui->general_checkBox_rectifyOnlyFeatures->setObjectName(Parameters::kRtabmapRectifyOnlyFeatures().c_str());
_ui->checkBox_rtabmap_loopGPS->setObjectName(Parameters::kRtabmapLoopGPS().c_str());
_ui->lineEdit_workingDirectory->setObjectName(Parameters::kRtabmapWorkingDirectory().c_str());
connect(_ui->toolButton_workingDirectory, SIGNAL(clicked()), this, SLOT(changeWorkingDirectory()));
guilib/src/ui/preferencesDialog.ui
+22 -2
View File
@@ -94,7 +94,7 @@
<property name="geometry">
<rect>
<x>0</x>
<y>0</y>
<y>-57</y>
<width>681</width>
<height>2983</height>
</rect>
@@ -126,7 +126,7 @@
<enum>QFrame::Raised</enum>
</property>
<property name="currentIndex">
<number>24</number>
<number>11</number>
</property>
<widget class="QWidget" name="page_22">
<layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1">
@@ -8704,6 +8704,26 @@ see Sqlite3 doc 'PRAGMA temp_store'.</string>
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QLabel" name="label_502">
<property name="text">
<string>Use GPS to filter likelihood (if GPS is recorded). Only locations inside the Local Radius (see RGBD-SLAM panel) of the current GPS location are considered for loop closure detection.</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="QCheckBox" name="checkBox_rtabmap_loopGPS">
<property name="text">
<string/>
</property>
</widget>
</item>
</layout>
</item>
<item>