i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-10-06 18:17:47 +08:00
Code Issues Packages Projects Releases Wiki Activity

Prioritize neighbor links in Optimizer::getConnectedGraph() (#1610)

Browse Source 
* 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 (https://github.com/introlab/rtabmap_ros/issues/1371)

* amend previous commit
This commit is contained in:
matlabbe
2025-11-08 10:58:34 -08:00
committed by GitHub
parent 5e4fd171e2
commit e9c6180d56
6 changed files with 120 additions and 60 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/Optimizer.cpp
+86 -49
View File
@@ -185,6 +185,48 @@ Optimizer * Optimizer::create(Optimizer::Type type, const ParametersMap & parame
return optimizer;
}
class LinkIdKey
{
public:
LinkIdKey(int id, Link::Type type) :
id_(id),
type_(type) {}
bool operator<(const LinkIdKey & k) const
{
// landmark, sort by smallest to largest landmark id, after normal links
if(id_ < 0 && k.id_ < 0)
{
return id_ > k.id_;
}
else if(id_ < 0) {
return false;
}
else if(k.id_ < 0) {
return true;
}
if(type_ == Link::kNeighbor && k.type_ != Link::kNeighbor)
{
return true;
}
else if(type_ != Link::kNeighbor && k.type_ == Link::kNeighbor)
{
return false;
}
else if(type_ == Link::kNeighborMerged && k.type_ != Link::kNeighbor && k.type_ != Link::kNeighborMerged)
{
return true;
}
else
{
// normal link, sort by smallest to largest id
return id_ < k.id_;
}
}
int id_;
Link::Type type_;
};
void Optimizer::getConnectedGraph(
int fromId,
const std::map<int, Transform> & posesIn,
@@ -199,8 +241,8 @@ void Optimizer::getConnectedGraph(
posesOut.clear();
linksOut.clear();
std::set<int> nextPoses;
nextPoses.insert(fromId);
std::map<LinkIdKey, Transform> nextPoses;
nextPoses.insert(std::make_pair(LinkIdKey(fromId, Link::kUndef), posesIn.find(fromId)->second));
std::multimap<int, std::pair<int, Link::Type> > biLinks;
for(std::multimap<int, Link>::const_iterator iter=linksIn.begin(); iter!=linksIn.end(); ++iter)
{
@@ -216,20 +258,25 @@ void Optimizer::getConnectedGraph(
while(nextPoses.size())
{
int currentId = *nextPoses.rbegin(); // fill up all nodes before landmarks
nextPoses.erase(*nextPoses.rbegin());
// Fill up all nodes before landmarks
// For nodes, fill up all neightbor nodes before loop closure ones
int currentId = nextPoses.begin()->first.id_;
Transform currentPose = nextPoses.begin()->second;
nextPoses.erase(nextPoses.begin());
if(posesOut.empty())
if(posesOut.find(currentId) != posesOut.end()) {
// Already added from priority list
continue;
}
posesOut.insert(std::make_pair(currentId, currentPose));
// add prior links
for(std::multimap<int, Link>::const_iterator pter=linksIn.find(currentId); pter!=linksIn.end() && pter->first==currentId; ++pter)
{
posesOut.insert(std::make_pair(currentId, posesIn.find(currentId)->second));
// add prior links
for(std::multimap<int, Link>::const_iterator pter=linksIn.find(currentId); pter!=linksIn.end() && pter->first==currentId; ++pter)
if(pter->second.from() == pter->second.to() && (!priorsIgnored() || pter->second.type() != Link::kPosePrior))
{
if(pter->second.from() == pter->second.to() && (!priorsIgnored() || pter->second.type() != Link::kPosePrior))
{
linksOut.insert(*pter);
}
linksOut.insert(*pter);
}
}
@@ -240,52 +287,42 @@ void Optimizer::getConnectedGraph(
if(posesIn.find(toId) != posesIn.end() && (!landmarksIgnored() || toId>0))
{
std::multimap<int, Link>::const_iterator kter = graph::findLink(linksIn, currentId, toId, true, type);
if(nextPoses.find(toId) == nextPoses.end())
UASSERT(kter!=linksIn.end());
if(!uContains(posesOut, toId))
{
if(!uContains(posesOut, toId))
const Transform & poseToIn = posesIn.at(toId);
Transform t = kter->second.from()==currentId?kter->second.transform():kter->second.transform().inverse();
Transform pose;
if(isSlam2d() && kter->second.type() == Link::kLandmark && toId>0 && (poseToIn.is3DoF() || poseToIn.is4DoF()))
{
const Transform & poseToIn = posesIn.at(toId);
Transform t = kter->second.from()==currentId?kter->second.transform():kter->second.transform().inverse();
if(isSlam2d() && kter->second.type() == Link::kLandmark && toId>0 && (poseToIn.is3DoF() || poseToIn.is4DoF()))
if(poseToIn.is3DoF())
{
if(poseToIn.is3DoF())
{
posesOut.insert(std::make_pair(toId, (posesOut.at(currentId) * t).to3DoF()));
}
else
{
posesOut.insert(std::make_pair(toId, (posesOut.at(currentId) * t).to4DoF()));
}
pose = (posesOut.at(currentId) * t).to3DoF();
}
else
{
posesOut.insert(std::make_pair(toId, posesOut.at(currentId)* t));
pose = (posesOut.at(currentId) * t).to4DoF();
}
// add prior links
for(std::multimap<int, Link>::const_iterator pter=linksIn.find(toId); pter!=linksIn.end() && pter->first==toId; ++pter)
{
if(pter->second.from() == pter->second.to() && (!priorsIgnored() || pter->second.type() != Link::kPosePrior))
{
linksOut.insert(*pter);
}
}
nextPoses.insert(toId);
}
else
{
pose = posesOut.at(currentId)* t;
}
// only add unique links
if(graph::findLink(linksOut, currentId, toId, true, kter->second.type()) == linksOut.end())
nextPoses.insert(std::make_pair(LinkIdKey(toId, type), pose));
}
// only add unique links
if(graph::findLink(linksOut, currentId, toId, true, kter->second.type()) == linksOut.end())
{
if(kter->second.to() < 0)
{
if(kter->second.to() < 0)
{
// For landmarks, make sure fromId is the landmark
linksOut.insert(std::make_pair(kter->second.to(), kter->second.inverse()));
}
else
{
linksOut.insert(*kter);
}
// For landmarks, make sure fromId is the landmark
linksOut.insert(std::make_pair(kter->second.to(), kter->second.inverse()));
}
else
{
linksOut.insert(*kter);
}
}
}