Skip to content

Commit e9c6180

Browse files
matlabbematlabbe
authored
Prioritize neighbor links in Optimizer::getConnectedGraph() (#1610)
* Prioritize neighbor links in Optimizer::getConnectedGraph() to avoid odometry jumps. * Fixed landmark order * report: added search for first valid id * Fixed non-neighbor comparison logic * RGBD/LocalizationPriorError can now be 0 (disabled) to avoid using priors to fix the graph (introlab/rtabmap_ros#1371) * amend previous commit
1 parent 5e4fd17 commit e9c6180

File tree

6 files changed

+119
-59
lines changed

6 files changed

+119
-59
lines changed

‎corelib/include/rtabmap/core/Parameters.h‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -399,7 +399,7 @@ class RTABMAP_CORE_EXPORT Parameters
399399
RTABMAP_PARAM(RGBD, LoopCovLimited, bool, false, "Limit covariance of non-neighbor links to minimum covariance of neighbor links. In other words, if covariance of a loop closure link is smaller than the minimum covariance of odometry links, its covariance is set to minimum covariance of odometry links.");
400400
RTABMAP_PARAM(RGBD, MaxOdomCacheSize, int, 10, uFormat("Maximum odometry cache size. Used only in localization mode (when %s=false). This is used to get smoother localizations and to verify localization transforms (when %s!=0) to make sure we don't teleport to a location very similar to one we previously localized on. Set 0 to disable caching.", kMemIncrementalMemory().c_str(), kRGBDOptimizeMaxError().c_str()));
401401
RTABMAP_PARAM(RGBD, LocalizationSmoothing, bool, true, uFormat("Adjust localization constraints based on optimized odometry cache poses (when %s>0).", kRGBDMaxOdomCacheSize().c_str()));
402-
RTABMAP_PARAM(RGBD, LocalizationPriorError, double, 0.001, uFormat("The corresponding variance (error x error) set to priors of the map's poses during localization (when %s>0).", kRGBDMaxOdomCacheSize().c_str()));
402+
RTABMAP_PARAM(RGBD, LocalizationPriorError, double, 0.001, uFormat("The corresponding variance (error x error) set to priors of the map's poses during localization (when %s>0). Set to 0 to only fix the first map's pose linked in odometry cache (i.e., other map's poses will be allowed to move during the optimization).", kRGBDMaxOdomCacheSize().c_str()));
403403
RTABMAP_PARAM(RGBD, LocalizationSecondTryWithoutProximityLinks, bool, true, uFormat("When localization is rejected by graph optimization validation, try a second time without proximity links if landmark or loop closure links are also present in odometry cache (see %s). If it succeeds, the proximity links are removed. This assumes that global loop closure and landmark links are more accurate than proximity links.", kRGBDMaxOdomCacheSize().c_str()));
404404

405405
// Local/Proximity loop closure detection

‎corelib/src/Optimizer.cpp‎

Lines changed: 85 additions & 48 deletions
Original file line numberDiff line numberDiff line change
@@ -185,6 +185,48 @@ Optimizer * Optimizer::create(Optimizer::Type type, const ParametersMap & parame
185185
return optimizer;
186186
}
187187

188+
class LinkIdKey
189+
{
190+
public:
191+
LinkIdKey(int id, Link::Type type) :
192+
id_(id),
193+
type_(type) {}
194+
bool operator<(const LinkIdKey & k) const
195+
{
196+
// landmark, sort by smallest to largest landmark id, after normal links
197+
if(id_ < 0 && k.id_ < 0)
198+
{
199+
return id_ > k.id_;
200+
}
201+
else if(id_ < 0) {
202+
return false;
203+
}
204+
else if(k.id_ < 0) {
205+
return true;
206+
}
207+
208+
if(type_ == Link::kNeighbor && k.type_ != Link::kNeighbor)
209+
{
210+
return true;
211+
}
212+
else if(type_ != Link::kNeighbor && k.type_ == Link::kNeighbor)
213+
{
214+
return false;
215+
}
216+
else if(type_ == Link::kNeighborMerged && k.type_ != Link::kNeighbor && k.type_ != Link::kNeighborMerged)
217+
{
218+
return true;
219+
}
220+
else
221+
{
222+
// normal link, sort by smallest to largest id
223+
return id_ < k.id_;
224+
}
225+
}
226+
int id_;
227+
Link::Type type_;
228+
};
229+
188230
void Optimizer::getConnectedGraph(
189231
int fromId,
190232
const std::map<int, Transform> & posesIn,
@@ -199,8 +241,8 @@ void Optimizer::getConnectedGraph(
199241
posesOut.clear();
200242
linksOut.clear();
201243

202-
std::set<int> nextPoses;
203-
nextPoses.insert(fromId);
244+
std::map<LinkIdKey, Transform> nextPoses;
245+
nextPoses.insert(std::make_pair(LinkIdKey(fromId, Link::kUndef), posesIn.find(fromId)->second));
204246
std::multimap<int, std::pair<int, Link::Type> > biLinks;
205247
for(std::multimap<int, Link>::const_iterator iter=linksIn.begin(); iter!=linksIn.end(); ++iter)
206248
{
@@ -216,20 +258,25 @@ void Optimizer::getConnectedGraph(
216258

217259
while(nextPoses.size())
218260
{
219-
int currentId = *nextPoses.rbegin(); // fill up all nodes before landmarks
220-
nextPoses.erase(*nextPoses.rbegin());
261+
// Fill up all nodes before landmarks
262+
// For nodes, fill up all neightbor nodes before loop closure ones
263+
int currentId = nextPoses.begin()->first.id_;
264+
Transform currentPose = nextPoses.begin()->second;
265+
nextPoses.erase(nextPoses.begin());
266+
267+
if(posesOut.find(currentId) != posesOut.end()) {
268+
// Already added from priority list
269+
continue;
270+
}
221271

222-
if(posesOut.empty())
223-
{
224-
posesOut.insert(std::make_pair(currentId, posesIn.find(currentId)->second));
272+
posesOut.insert(std::make_pair(currentId, currentPose));
225273

226-
// add prior links
227-
for(std::multimap<int, Link>::const_iterator pter=linksIn.find(currentId); pter!=linksIn.end() && pter->first==currentId; ++pter)
274+
// add prior links
275+
for(std::multimap<int, Link>::const_iterator pter=linksIn.find(currentId); pter!=linksIn.end() && pter->first==currentId; ++pter)
276+
{
277+
if(pter->second.from() == pter->second.to() && (!priorsIgnored() || pter->second.type() != Link::kPosePrior))
228278
{
229-
if(pter->second.from() == pter->second.to() && (!priorsIgnored() || pter->second.type() != Link::kPosePrior))
230-
{
231-
linksOut.insert(*pter);
232-
}
279+
linksOut.insert(*pter);
233280
}
234281
}
235282

@@ -240,52 +287,42 @@ void Optimizer::getConnectedGraph(
240287
if(posesIn.find(toId) != posesIn.end() && (!landmarksIgnored() || toId>0))
241288
{
242289
std::multimap<int, Link>::const_iterator kter = graph::findLink(linksIn, currentId, toId, true, type);
243-
if(nextPoses.find(toId) == nextPoses.end())
290+
UASSERT(kter!=linksIn.end());
291+
if(!uContains(posesOut, toId))
244292
{
245-
if(!uContains(posesOut, toId))
293+
const Transform & poseToIn = posesIn.at(toId);
294+
Transform t = kter->second.from()==currentId?kter->second.transform():kter->second.transform().inverse();
295+
Transform pose;
296+
if(isSlam2d() && kter->second.type() == Link::kLandmark && toId>0 && (poseToIn.is3DoF() || poseToIn.is4DoF()))
246297
{
247-
const Transform & poseToIn = posesIn.at(toId);
248-
Transform t = kter->second.from()==currentId?kter->second.transform():kter->second.transform().inverse();
249-
if(isSlam2d() && kter->second.type() == Link::kLandmark && toId>0 && (poseToIn.is3DoF() || poseToIn.is4DoF()))
298+
if(poseToIn.is3DoF())
250299
{
251-
if(poseToIn.is3DoF())
252-
{
253-
posesOut.insert(std::make_pair(toId, (posesOut.at(currentId) * t).to3DoF()));
254-
}
255-
else
256-
{
257-
posesOut.insert(std::make_pair(toId, (posesOut.at(currentId) * t).to4DoF()));
258-
}
300+
pose = (posesOut.at(currentId) * t).to3DoF();
259301
}
260302
else
261303
{
262-
posesOut.insert(std::make_pair(toId, posesOut.at(currentId)* t));
304+
pose = (posesOut.at(currentId) * t).to4DoF();
263305
}
306+
}
307+
else
308+
{
309+
pose = posesOut.at(currentId)* t;
310+
}
264311

265-
// add prior links
266-
for(std::multimap<int, Link>::const_iterator pter=linksIn.find(toId); pter!=linksIn.end() && pter->first==toId; ++pter)
267-
{
268-
if(pter->second.from() == pter->second.to() && (!priorsIgnored() || pter->second.type() != Link::kPosePrior))
269-
{
270-
linksOut.insert(*pter);
271-
}
272-
}
312+
nextPoses.insert(std::make_pair(LinkIdKey(toId, type), pose));
313+
}
273314

274-
nextPoses.insert(toId);
315+
// only add unique links
316+
if(graph::findLink(linksOut, currentId, toId, true, kter->second.type()) == linksOut.end())
317+
{
318+
if(kter->second.to() < 0)
319+
{
320+
// For landmarks, make sure fromId is the landmark
321+
linksOut.insert(std::make_pair(kter->second.to(), kter->second.inverse()));
275322
}
276-
277-
// only add unique links
278-
if(graph::findLink(linksOut, currentId, toId, true, kter->second.type()) == linksOut.end())
323+
else
279324
{
280-
if(kter->second.to() < 0)
281-
{
282-
// For landmarks, make sure fromId is the landmark
283-
linksOut.insert(std::make_pair(kter->second.to(), kter->second.inverse()));
284-
}
285-
else
286-
{
287-
linksOut.insert(*kter);
288-
}
325+
linksOut.insert(*kter);
289326
}
290327
}
291328
}

‎corelib/src/Rtabmap.cpp‎

Lines changed: 21 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -631,8 +631,8 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
631631
Parameters::parse(parameters, Parameters::kRGBDLocalizationSmoothing(), _localizationSmoothing);
632632
double localizationPriorError = Parameters::defaultRGBDLocalizationPriorError();
633633
Parameters::parse(parameters, Parameters::kRGBDLocalizationPriorError(), localizationPriorError);
634-
UASSERT(localizationPriorError>0.0);
635-
_localizationPriorInf = 1.0/(localizationPriorError*localizationPriorError);
634+
UASSERT(localizationPriorError>=0.0);
635+
_localizationPriorInf = localizationPriorError>0?1.0/(localizationPriorError*localizationPriorError):0.0;
636636
Parameters::parse(parameters, Parameters::kRGBDLocalizationSecondTryWithoutProximityLinks(), _localizationSecondTryWithoutProximityLinks);
637637
Parameters::parse(parameters, Parameters::kRGBDProximityGlobalScanMap(), _createGlobalScanMap);
638638

@@ -3258,25 +3258,39 @@ bool Rtabmap::process(
32583258
{
32593259
constraints.insert(std::make_pair(iter->second.from(), iter->second));
32603260
}
3261+
32613262
cv::Mat priorInfMat = cv::Mat::eye(6,6, CV_64FC1)*_localizationPriorInf;
3263+
std::list<int> addedPriors;
32623264
for(std::multimap<int, Link>::iterator iter=constraints.begin(); iter!=constraints.end(); ++iter)
32633265
{
32643266
std::map<int, Transform>::iterator iterPose = _optimizedPoses.find(iter->second.to());
32653267
if(iterPose != _optimizedPoses.end() && poses.find(iterPose->first) == poses.end())
32663268
{
32673269
poses.insert(*iterPose);
3268-
// make the poses in the map fixed
3269-
constraints.insert(std::make_pair(iterPose->first, Link(iterPose->first, iterPose->first, Link::kPosePrior, iterPose->second, priorInfMat)));
3270-
UDEBUG("Constraint %d->%d: %s (type=%s, var=%f)", iterPose->first, iterPose->first, iterPose->second.prettyPrint().c_str(), Link::typeName(Link::kPosePrior).c_str(), 1./_localizationPriorInf);
3270+
if(_localizationPriorInf > 0)
3271+
{
3272+
// make the poses in the map fixed
3273+
constraints.insert(std::make_pair(iterPose->first, Link(iterPose->first, iterPose->first, Link::kPosePrior, iterPose->second, priorInfMat)));
3274+
UDEBUG("Constraint %d->%d: %s (type=%s, var=%f)", iterPose->first, iterPose->first, iterPose->second.prettyPrint().c_str(), Link::typeName(Link::kPosePrior).c_str(), 1./_localizationPriorInf);
3275+
addedPriors.push_back(iterPose->first);
3276+
}
32713277
}
32723278
UDEBUG("Constraint %d->%d: %s (type=%s, var = %f %f)", iter->second.from(), iter->second.to(), iter->second.transform().prettyPrint().c_str(), iter->second.typeName().c_str(), iter->second.transVariance(), iter->second.rotVariance());
32733279
}
3280+
if(addedPriors.size() == 1) {
3281+
// When there is only one map node, remove the prior to use fixed constraint in g2o (https://github.com/introlab/rtabmap_ros/issues/1371)
3282+
UDEBUG("Currently localizing on a single map node, removing prior on %d", addedPriors.front());
3283+
constraints.erase(graph::findLink(constraints, addedPriors.front(), addedPriors.front(), false, Link::kPosePrior));
3284+
}
32743285

32753286
std::map<int, Transform> posesOut;
32763287
std::multimap<int, Link> edgeConstraintsOut;
32773288
bool priorsIgnored = _graphOptimizer->priorsIgnored();
3278-
UDEBUG("priorsIgnored was %s", priorsIgnored?"true":"false");
3279-
_graphOptimizer->setPriorsIgnored(false); //temporary set false to use priors above to fix nodes of the map
3289+
if(_localizationPriorInf > 0)
3290+
{
3291+
UDEBUG("priorsIgnored was %s", priorsIgnored?"true":"false");
3292+
_graphOptimizer->setPriorsIgnored(false); //temporary set false to use priors above to fix nodes of the map
3293+
}
32803294
// If slam2d: get connected graph while keeping original roll,pitch,z values.
32813295
_graphOptimizer->getConnectedGraph(signature->id(), poses, constraints, posesOut, edgeConstraintsOut);
32823296
if(ULogger::level() == ULogger::kDebug)

‎corelib/src/optimizer/OptimizerG2O.cpp‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -1004,8 +1004,8 @@ std::map<int, Transform> OptimizerG2O::optimize(
10041004
g2o::EdgeSE3 * e = new g2o::EdgeSE3();
10051005
g2o::VertexSE3* v1 = (g2o::VertexSE3*)optimizer.vertex(id1);
10061006
g2o::VertexSE3* v2 = (g2o::VertexSE3*)optimizer.vertex(id2);
1007-
UASSERT(v1 != 0);
1008-
UASSERT(v2 != 0);
1007+
UASSERT_MSG(v1 != 0, uFormat("v1=%d v2=%d", id1, id2).c_str());
1008+
UASSERT_MSG(v2 != 0, uFormat("v1=%d v2=%d", id1, id2).c_str());
10091009
e->setVertex(0, v1);
10101010
e->setVertex(1, v2);
10111011
e->setMeasurement(constraint);

‎guilib/src/ui/preferencesDialog.ui‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -12888,7 +12888,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
1288812888
<item row="21" column="1">
1288912889
<widget class="QLabel" name="label_space2_19">
1289012890
<property name="text">
12891-
<string>Localization prior error. Used only in localization mode. The corresponding variance (error x error) set to priors of the map's poses during localization.</string>
12891+
<string>Localization prior error. Used only in localization mode. The corresponding variance (error x error) set to priors of the map's poses during localization. Set to 0 to only fix the first map's pose linked in odometry cache (i.e., other map's poses will be allowed to move during the optimization).</string>
1289212892
</property>
1289312893
<property name="wordWrap">
1289412894
<bool>true</bool>

‎tools/Report/main.cpp‎

Lines changed: 9 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -908,7 +908,16 @@ int main(int argc, char * argv[])
908908
{
909909
std::map<int, Transform> posesOut;
910910
std::multimap<int, Link> linksOut;
911+
// Find first valid id
911912
int firstId = *ids.begin();
913+
for(std::multimap<int, Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
914+
{
915+
if(iter->second.type() == Link::kNeighbor || iter->second.type() == Link::kNeighborMerged)
916+
{
917+
firstId = iter->first;
918+
break;
919+
}
920+
}
912921
rtabmap::Optimizer * optimizer = rtabmap::Optimizer::create(params);
913922
bool useOdomGravity = Parameters::defaultMemUseOdomGravity();
914923
Parameters::parse(params, Parameters::kMemUseOdomGravity(), useOdomGravity);

0 commit comments

Comments
 (0)