Skip to content

Commit

Permalink
Landmark constraints can now be rejected (in case we see same landmar…
Browse files Browse the repository at this point in the history 
…k at two different spots or constraint is poorly estimated). Optimizer/Robust can now be used with landmark constraints with orientation.
  • Loading branch information
@matlabbe
matlabbe committed Apr 6, 2024
1 parent 92edae3 commit c33e995
Show file tree
Hide file tree
Showing 6 changed files with 190 additions and 26 deletions.
5 changes: 3 additions & 2 deletions corelib/include/rtabmap/core/Signature.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -90,9 +90,10 @@ class RTABMAP_CORE_EXPORT Signature
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;}
Expand Down
13 changes: 13 additions & 0 deletions corelib/src/Memory.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down
1 change: 1 addition & 0 deletions corelib/src/Rtabmap.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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));
}
}
}
Expand Down
28 changes: 28 additions & 0 deletions corelib/src/Signature.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
111 changes: 91 additions & 20 deletions corelib/src/optimizer/OptimizerG2O.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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))
Expand All @@ -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
{
Expand Down Expand Up @@ -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
{
Expand Down
58 changes: 54 additions & 4 deletions corelib/src/optimizer/OptimizerGTSAM.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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))
Expand All @@ -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
{
Expand Down Expand Up @@ -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
{
Expand Down

0 comments on commit c33e995

Please sign in to comment.