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

Landmark constraints can now be rejected (in case we see same landmark at two different spots or constraint is poorly estimated). Optimizer/Robust can now be used with landmark constraints with orientation.

Browse Source
This commit is contained in:
matlabbe
2024-04-06 13:41:03 -07:00
parent 92edae35fc
commit c33e995e83
6 changed files with 190 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
corelib/include/rtabmap/core/Signature.h
View File

@@ -90,9 +90,10 @@ public:
void removeLink(int idTo);
void removeVirtualLinks();
void addLandmark(const Link & landmark) {_landmarks.insert(std::make_pair(landmark.to(), landmark));}
void addLandmark(const Link & landmark);
const std::map<int, Link> & getLandmarks() const {return _landmarks;}
void removeLandmarks() {_landmarks.clear();}
void removeLandmarks();
void removeLandmark(int landmarkId);
void setSaved(bool saved) {_saved = saved;}
void setModified(bool modified) {_modified = modified; _linksModified = modified;}

13
corelib/src/Memory.cpp
View File

@@ -2770,6 +2770,19 @@ void Memory::removeLink(int oldId, int newId)
UERROR("Signatures %d and %d don't have bidirectional link!", oldS->id(), newS->id());
}
}
else if(this->_getSignature(newId<0?oldId:newId))
{
int landmarkId = newId<0?newId:oldId;
Signature * s = this->_getSignature(newId<0?oldId:newId);
s->removeLandmark(newId<0?newId:oldId);
_linksChanged = true;
// Update landmark index
std::map<int, std::set<int> >::iterator nter = _landmarksIndex.find(landmarkId);
if(nter!=_landmarksIndex.end())
{
nter->second.erase(s->id());
}
}
else
{
if(!newS)

1
corelib/src/Rtabmap.cpp
View File

@@ -3097,6 +3097,7 @@ bool Rtabmap::process(
UINFO("Landmark %d observed again! Seen the first time by node %d.", -iter->first, *_memory->getLandmarksIndex().find(iter->first)->second.begin());
landmarksDetected.insert(std::make_pair(iter->first, _memory->getLandmarksIndex().find(iter->first)->second));
rejectedGlobalLoopClosure = false; // If it was true, it will be set back to false if landmarks are rejected on graph optimization
loopClosureLinksAdded.push_back(std::make_pair(signature->id(), iter->first));
}
}
}

28
corelib/src/Signature.cpp
View File

@@ -219,6 +219,34 @@ void Signature::removeVirtualLinks()
}
}
void Signature::addLandmark(const Link & landmark)
{
UDEBUG("Add landmark %d to %d (type=%d/%s var=%f,%f)", landmark.to(), this->id(), (int)landmark.type(), landmark.typeName().c_str(), landmark.transVariance(), landmark.rotVariance());
UASSERT_MSG(landmark.from() == this->id(), uFormat("%d->%d for signature %d (type=%d)", landmark.from(), landmark.to(), this->id(), landmark.type()).c_str());
UASSERT_MSG(landmark.to() < 0, uFormat("%d->%d for signature %d (type=%d)", landmark.from(), landmark.to(), this->id(), landmark.type()).c_str());
UASSERT_MSG(_landmarks.find(landmark.to()) == _landmarks.end(), uFormat("Landmark %d (type=%d) already added to signature %d!", landmark.to(), landmark.type(), this->id()).c_str());
_landmarks.insert(std::make_pair(landmark.to(), landmark));
_linksModified = true;
}
void Signature::removeLandmarks()
{
size_t sizeBefore = _landmarks.size();
_landmarks.clear();
if(_landmarks.size() != sizeBefore)
_linksModified = true;
}
void Signature::removeLandmark(int landmarkId)
{
int count = (int)_landmarks.erase(landmarkId);
if(count)
{
UDEBUG("Removed landmark %d from %d", landmarkId, this->id());
_linksModified = true;
}
}
float Signature::compareTo(const Signature & s) const
{
float similarity = 0.0f;

111
corelib/src/optimizer/OptimizerG2O.cpp
View File

@@ -720,6 +720,39 @@ std::map<int, Transform> OptimizerG2O::optimize(
int idTag= id2;
id2 = landmarkVertexOffset - id2;
#if defined(RTABMAP_VERTIGO)
VertexSwitchLinear * v = 0;
if(this->isRobust() && isLandmarkWithRotation.at(idTag))
{
// For landmark links, add switchable edges
// Currently supporting only constraints with rotation
// create new switch variable
// Sunderhauf IROS 2012:
// "Since it is reasonable to initially accept all loop closure constraints,
// a proper and convenient initial value for all switch variables would be
// sij = 1 when using the linear switch function"
v = new VertexSwitchLinear();
v->setEstimate(1.0);
v->setId(vertigoVertexId++);
UASSERT_MSG(optimizer.addVertex(v), uFormat("cannot insert switchable vertex %d!?", v->id()).c_str());
// create switch prior factor
// "If the front-end is not able to assign sound individual values
// for Ξij , it is save to set all Ξij = 1, since this value is close
// to the individual optimal choice of Ξij for a large range of
// outliers."
EdgeSwitchPrior * prior = new EdgeSwitchPrior();
prior->setMeasurement(1.0);
prior->setVertex(0, v);
UASSERT_MSG(optimizer.addEdge(prior), uFormat("cannot insert switchable prior edge %d!?", v->id()).c_str());
}
else if(this->isRobust() && !isLandmarkWithRotation.at(idTag))
{
UWARN("%s cannot be used for landmark constraints without orientation.", Parameters::kOptimizerRobust().c_str());
}
#endif
if(isSlam2d())
{
if(isLandmarkWithRotation.at(idTag))
@@ -737,16 +770,35 @@ std::map<int, Transform> OptimizerG2O::optimize(
information(2,1) = iter->second.infMatrix().at<double>(5,1); // theta-y
information(2,2) = iter->second.infMatrix().at<double>(5,5); // theta-theta
}
g2o::EdgeSE2 * e = new g2o::EdgeSE2();
g2o::VertexSE2* v1 = (g2o::VertexSE2*)optimizer.vertex(id1);
g2o::VertexSE2* v2 = (g2o::VertexSE2*)optimizer.vertex(id2);
UASSERT(v1 != 0);
UASSERT(v2 != 0);
e->setVertex(0, v1);
e->setVertex(1, v2);
e->setMeasurement(g2o::SE2(t.x(), t.y(), t.theta()));
e->setInformation(information);
edge = e;
#if defined(RTABMAP_VERTIGO)
if(this->isRobust())
{
EdgeSE2Switchable * e = new EdgeSE2Switchable();
g2o::VertexSE2* v1 = (g2o::VertexSE2*)optimizer.vertex(id1);
g2o::VertexSE2* v2 = (g2o::VertexSE2*)optimizer.vertex(id2);
UASSERT(v1 != 0);
UASSERT(v2 != 0);
e->setVertex(0, v1);
e->setVertex(1, v2);
e->setVertex(2, v);
e->setMeasurement(g2o::SE2(t.x(), t.y(), t.theta()));
e->setInformation(information);
edge = e;
}
else
#endif
{
g2o::EdgeSE2 * e = new g2o::EdgeSE2();
g2o::VertexSE2* v1 = (g2o::VertexSE2*)optimizer.vertex(id1);
g2o::VertexSE2* v2 = (g2o::VertexSE2*)optimizer.vertex(id2);
UASSERT(v1 != 0);
UASSERT(v2 != 0);
e->setVertex(0, v1);
e->setVertex(1, v2);
e->setMeasurement(g2o::SE2(t.x(), t.y(), t.theta()));
e->setInformation(information);
edge = e;
}
}
else
{
@@ -780,16 +832,35 @@ std::map<int, Transform> OptimizerG2O::optimize(
constraint = a.linear();
constraint.translation() = a.translation();
g2o::EdgeSE3 * e = new g2o::EdgeSE3();
g2o::VertexSE3* v1 = (g2o::VertexSE3*)optimizer.vertex(id1);
g2o::VertexSE3* v2 = (g2o::VertexSE3*)optimizer.vertex(id2);
UASSERT(v1 != 0);
UASSERT(v2 != 0);
e->setVertex(0, v1);
e->setVertex(1, v2);
e->setMeasurement(constraint);
e->setInformation(information);
edge = e;
#if defined(RTABMAP_VERTIGO)
if(this->isRobust())
{
EdgeSE3Switchable * e = new EdgeSE3Switchable();
g2o::VertexSE3* v1 = (g2o::VertexSE3*)optimizer.vertex(id1);
g2o::VertexSE3* v2 = (g2o::VertexSE3*)optimizer.vertex(id2);
UASSERT(v1 != 0);
UASSERT(v2 != 0);
e->setVertex(0, v1);
e->setVertex(1, v2);
e->setVertex(2, v);
e->setMeasurement(constraint);
e->setInformation(information);
edge = e;
}
else
#endif
{
g2o::EdgeSE3 * e = new g2o::EdgeSE3();
g2o::VertexSE3* v1 = (g2o::VertexSE3*)optimizer.vertex(id1);
g2o::VertexSE3* v2 = (g2o::VertexSE3*)optimizer.vertex(id2);
UASSERT(v1 != 0);
UASSERT(v2 != 0);
e->setVertex(0, v1);
e->setVertex(1, v2);
e->setMeasurement(constraint);
e->setInformation(information);
edge = e;
}
}
else
{

58
corelib/src/optimizer/OptimizerGTSAM.cpp
View File

@@ -493,6 +493,32 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
t = iter->second.transform().inverse();
std::swap(id1, id2); // should be node -> landmark
}
#ifdef RTABMAP_VERTIGO
if(this->isRobust() && isLandmarkWithRotation.at(id2))
{
// create new switch variable
// Sunderhauf IROS 2012:
// "Since it is reasonable to initially accept all loop closure constraints,
// a proper and convenient initial value for all switch variables would be
// sij = 1 when using the linear switch function"
double prior = 1.0;
initialEstimate.insert(gtsam::Symbol('s',lastSwitchId_), vertigo::SwitchVariableLinear(prior));
// create switch prior factor
// "If the front-end is not able to assign sound individual values
// for Ξij , it is save to set all Ξij = 1, since this value is close
// to the individual optimal choice of Ξij for a large range of
// outliers."
gtsam::noiseModel::Diagonal::shared_ptr switchPriorModel = gtsam::noiseModel::Diagonal::Sigmas(gtsam::Vector1(1.0));
graph.add(gtsam::PriorFactor<vertigo::SwitchVariableLinear> (gtsam::Symbol('s',lastSwitchId_), vertigo::SwitchVariableLinear(prior), switchPriorModel));
}
else if(this->isRobust() && !isLandmarkWithRotation.at(id2))
{
UWARN("%s cannot be used for landmark constraints without orientation.", Parameters::kOptimizerRobust().c_str());
}
#endif
if(isSlam2d())
{
if(isLandmarkWithRotation.at(id2))
@@ -511,8 +537,19 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
information(2,2) = iter->second.infMatrix().at<double>(5,5); // theta-theta
}
gtsam::noiseModel::Gaussian::shared_ptr model = gtsam::noiseModel::Gaussian::Information(information);
graph.add(gtsam::BetweenFactor<gtsam::Pose2>(id1, id2, gtsam::Pose2(t.x(), t.y(), t.theta()), model));
lastAddedConstraints_.push_back(ConstraintToFactor(id1, id2, -1));
#ifdef RTABMAP_VERTIGO
if(this->isRobust())
{
// create switchable edge factor
graph.add(vertigo::BetweenFactorSwitchableLinear<gtsam::Pose2>(id1, id2, gtsam::Symbol('s', lastSwitchId_++), gtsam::Pose2(t.x(), t.y(), t.theta()), model));
}
else
#endif
{
graph.add(gtsam::BetweenFactor<gtsam::Pose2>(id1, id2, gtsam::Pose2(t.x(), t.y(), t.theta()), model));
lastAddedConstraints_.push_back(ConstraintToFactor(id1, id2, -1));
}
}
else
{
@@ -546,8 +583,21 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
mgtsam.block(0,3,3,3) = information.block(3,0,3,3); // off diagonal
mgtsam.block(3,0,3,3) = information.block(0,3,3,3); // off diagonal
gtsam::SharedNoiseModel model = gtsam::noiseModel::Gaussian::Information(mgtsam);
graph.add(gtsam::BetweenFactor<gtsam::Pose3>(id1, id2, gtsam::Pose3(t.toEigen4d()), model));
lastAddedConstraints_.push_back(ConstraintToFactor(id1, id2, -1));
#ifdef RTABMAP_VERTIGO
if(this->isRobust() &&
iter->second.type() != Link::kNeighbor &&
iter->second.type() != Link::kNeighborMerged)
{
// create switchable edge factor
graph.add(vertigo::BetweenFactorSwitchableLinear<gtsam::Pose3>(id1, id2, gtsam::Symbol('s', lastSwitchId_++), gtsam::Pose3(t.toEigen4d()), model));
}
else
#endif
{
graph.add(gtsam::BetweenFactor<gtsam::Pose3>(id1, id2, gtsam::Pose3(t.toEigen4d()), model));
lastAddedConstraints_.push_back(ConstraintToFactor(id1, id2, -1));
}
}
else
{