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

Optimizer: fixed bad g2o optimizations on 2d slam when there are 3D landmarks (#384). DBViewer: landmarks can be now visualized.

Browse Source
This commit is contained in:
matlabbe
2019-04-26 14:03:25 -04:00
parent f9b7b54454
commit a5350c4891
11 changed files with 289 additions and 285 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
corelib/include/rtabmap/core/Optimizer.h
View File

@@ -69,13 +69,12 @@ public:
static Optimizer * create(Optimizer::Type type, const ParametersMap & parameters = ParametersMap());
// Get connected poses and constraints from a set of links
static void getConnectedGraph(
void getConnectedGraph(
int fromId,
const std::map<int, Transform> & posesIn,
const std::multimap<int, Link> & linksIn, // only one link between two poses
const std::multimap<int, Link> & linksIn,
std::map<int, Transform> & posesOut,
std::multimap<int, Link> & linksOut,
int depth = 0);
std::multimap<int, Link> & linksOut) const;
public:
virtual ~Optimizer() {}

99
corelib/src/Optimizer.cpp
View File

@@ -159,94 +159,107 @@ void Optimizer::getConnectedGraph(
const std::map<int, Transform> & posesIn,
const std::multimap<int, Link> & linksIn,
std::map<int, Transform> & posesOut,
std::multimap<int, Link> & linksOut,
int depth)
std::multimap<int, Link> & linksOut) const
{
UASSERT(depth >= 0);
UDEBUG("IN: fromId=%d poses=%d links=%d priorsIgnored=%d landmarksIgnored=%d", fromId, (int)posesIn.size(), (int)linksIn.size(), priorsIgnored()?1:0, landmarksIgnored()?1:0);
UASSERT(fromId>0);
UASSERT(uContains(posesIn, fromId));
posesOut.clear();
linksOut.clear();
std::set<int> curentPoses;
std::set<int> nextPoses;
nextPoses.insert(fromId);
int d = 0;
std::multimap<int, int> biLinks;
for(std::multimap<int, Link>::const_iterator iter=linksIn.begin(); iter!=linksIn.end(); ++iter)
{
if(iter->second.from() != iter->second.to())
{
UASSERT_MSG(graph::findLink(biLinks, iter->second.from(), iter->second.to()) == biLinks.end(),
uFormat("Input links should be unique between two poses (%d->%d).",
iter->second.from(), iter->second.to()).c_str());
biLinks.insert(std::make_pair(iter->second.from(), iter->second.to()));
if(iter->second.from() != iter->second.to())
if(graph::findLink(biLinks, iter->second.from(), iter->second.to()) == biLinks.end())
{
biLinks.insert(std::make_pair(iter->second.from(), iter->second.to()));
biLinks.insert(std::make_pair(iter->second.to(), iter->second.from()));
}
}
}
while((depth == 0 || d < depth) && nextPoses.size())
while(nextPoses.size())
{
curentPoses = nextPoses;
nextPoses.clear();
int fromId = *nextPoses.rbegin(); // fill up all nodes before landmarks
nextPoses.erase(*nextPoses.rbegin());
for(std::set<int>::iterator jter = curentPoses.begin(); jter!=curentPoses.end(); ++jter)
if(posesOut.empty())
{
int fromId = *jter;
if(posesOut.empty())
posesOut.insert(std::make_pair(fromId, posesIn.find(fromId)->second));
// add prior links
for(std::multimap<int, Link>::const_iterator pter=linksIn.find(fromId); pter!=linksIn.end() && pter->first==fromId; ++pter)
{
posesOut.insert(*posesIn.find(fromId));
// add prior links
for(std::multimap<int, Link>::const_iterator pter=linksIn.find(fromId); pter!=linksIn.end() && pter->first==fromId; ++pter)
if(pter->second.from() == pter->second.to() && (!priorsIgnored() || pter->second.type() != Link::kPosePrior))
{
if(pter->second.from() == pter->second.to())
{
linksOut.insert(*pter);
}
linksOut.insert(*pter);
}
}
}
for(std::multimap<int, int>::const_iterator iter=biLinks.find(fromId); iter!=biLinks.end() && iter->first==fromId; ++iter)
for(std::multimap<int, int>::const_iterator iter=biLinks.find(fromId); iter!=biLinks.end() && iter->first==fromId; ++iter)
{
int toId = iter->second;
if(posesIn.find(toId) != posesIn.end() && (!landmarksIgnored() || toId>0))
{
int toId = iter->second;
if(posesIn.find(toId) != posesIn.end())
std::multimap<int, Link>::const_iterator kter = graph::findLink(linksIn, fromId, toId);
if(nextPoses.find(toId) == nextPoses.end())
{
std::multimap<int, Link>::const_iterator kter = graph::findLink(linksIn, fromId, toId);
int nextDepth = toId!=fromId?depth-1:depth;
if(depth == 0 || d < nextDepth || curentPoses.find(toId) != curentPoses.end())
if(!uContains(posesOut, toId))
{
if(!uContains(posesOut, toId))
if(isSlam2d() && kter->second.type() == Link::kLandmark && toId>0)
{
posesOut.insert(std::make_pair(toId, posesOut.at(fromId) * (kter->second.from()==fromId?kter->second.transform():kter->second.transform().inverse())));
// add prior links
for(std::multimap<int, Link>::const_iterator pter=linksIn.find(toId); pter!=linksIn.end() && pter->first==toId; ++pter)
Transform t;
if(kter->second.from()==fromId)
{
if(pter->second.from() == pter->second.to())
{
linksOut.insert(*pter);
}
t = kter->second.transform();
}
if(curentPoses.find(toId) == curentPoses.end())
else
{
nextPoses.insert(toId);
t = kter->second.transform().inverse();
}
posesOut.insert(std::make_pair(toId, (posesOut.at(fromId) * t).to3DoF()));
}
else
{
Transform t = posesOut.at(fromId) * (kter->second.from()==fromId?kter->second.transform():kter->second.transform().inverse());
posesOut.insert(std::make_pair(toId, t));
}
// 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);
}
}
if(graph::findLink(linksOut, fromId, toId) == linksOut.end())
nextPoses.insert(toId);
}
// only add unique links
if(graph::findLink(linksOut, fromId, toId) == linksOut.end())
{
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
{
// only add unique links
linksOut.insert(*kter);
}
}
}
}
}
++d;
}
UDEBUG("OUT: poses=%d links=%d", (int)posesOut.size(), (int)linksOut.size());
}
Optimizer::Optimizer(int iterations, bool slam2d, bool covarianceIgnored, double epsilon, bool robust, bool priorsIgnored, bool landmarksIgnored, float gravitySigma) :

76
corelib/src/Rtabmap.cpp
View File

@@ -3970,10 +3970,10 @@ std::map<int, Transform> Rtabmap::optimizeGraph(
{
UTimer timer;
std::map<int, Transform> optimizedPoses;
std::map<int, Transform> poses, posesOut;
std::multimap<int, Link> edgeConstraints, linksOut;
std::map<int, Transform> poses;
std::multimap<int, Link> edgeConstraints;
UDEBUG("ids=%d", (int)ids.size());
_memory->getMetricConstraints(ids, poses, edgeConstraints, lookInDatabase, true);
_memory->getMetricConstraints(ids, poses, edgeConstraints, lookInDatabase, !_graphOptimizer->landmarksIgnored());
UINFO("get constraints (ids=%d, %d poses, %d edges) time %f s", (int)ids.size(), (int)poses.size(), (int)edgeConstraints.size(), timer.ticks());
if(_graphOptimizer->iterations() > 0)
@@ -3995,78 +3995,40 @@ std::map<int, Transform> Rtabmap::optimizeGraph(
}
}
bool hasLandmarks = poses.begin()->first < 0;
// The constraints must be all already connected! Only check in debug
if(ULogger::level() == ULogger::kDebug)
{
_graphOptimizer->getConnectedGraph(fromId, poses, edgeConstraints, posesOut, linksOut);
if(poses.size() != posesOut.size())
{
for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{
if(posesOut.find(iter->first) == posesOut.end())
{
UERROR("Not found %d in posesOut", iter->first);
for(std::multimap<int, Link>::iterator jter=edgeConstraints.begin(); jter!=edgeConstraints.end(); ++jter)
{
if(jter->second.from() == iter->first || jter->second.to()==iter->first)
{
UERROR("Found link %d->%d", jter->second.from(), jter->second.to());
}
}
}
}
}
int ignoredLinks = 0;
if(edgeConstraints.size() != linksOut.size())
{
for(std::multimap<int, Link>::iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
{
if(graph::findLink(linksOut, iter->second.from(), iter->second.to()) == linksOut.end())
{
if(iter->second.type() == Link::kPosePrior)
{
++ignoredLinks;
}
else
{
UERROR("Not found link %d->%d in linksOut", iter->second.from(), iter->second.to());
}
}
}
}
UDEBUG("nodes %d->%d, links %d->%d (ignored=%d)", poses.size(), posesOut.size(), edgeConstraints.size(), linksOut.size(), ignoredLinks);
UASSERT_MSG(poses.size() == posesOut.size() && edgeConstraints.size()-ignoredLinks == linksOut.size(),
uFormat("nodes %d->%d, links %d->%d (ignored=%d)", poses.size(), posesOut.size(), edgeConstraints.size(), linksOut.size(), ignoredLinks).c_str());
}
if(constraints)
{
*constraints = edgeConstraints;
}
UASSERT(_graphOptimizer!=0);
if(_graphOptimizer->iterations() == 0)
{
// Optimization disabled! Return not optimized poses.
optimizedPoses = poses;
if(constraints)
{
*constraints = edgeConstraints;
}
}
else
{
bool hasLandmarks = edgeConstraints.begin()->first < 0;
if(poses.size() != guessPoses.size() || hasLandmarks)
{
// recompute poses using only links (robust to multi-session)
UDEBUG("recompute poses using only links (robust to multi-session)");
std::map<int, Transform> posesOut;
std::multimap<int, Link> edgeConstraintsOut;
_graphOptimizer->getConnectedGraph(fromId, poses, edgeConstraints, posesOut, edgeConstraintsOut);
UASSERT(edgeConstraintsOut.size() == edgeConstraints.size());
optimizedPoses = _graphOptimizer->optimize(fromId, posesOut, edgeConstraints, covariance, 0, error, iterationsDone);
optimizedPoses = _graphOptimizer->optimize(fromId, posesOut, edgeConstraintsOut, covariance, 0, error, iterationsDone);
if(constraints)
{
*constraints = edgeConstraintsOut;
}
}
else
{
// use input guess poses
UDEBUG("use input guess poses");
optimizedPoses = _graphOptimizer->optimize(fromId, poses, edgeConstraints, covariance, 0, error, iterationsDone);
if(constraints)
{
*constraints = edgeConstraints;
}
}
if(!poses.empty() && optimizedPoses.empty())

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

@@ -341,28 +341,23 @@ std::map<int, Transform> OptimizerG2O::optimize(
{
// check if it is SE2 or only PointXY
std::multimap<int, Link>::const_iterator jter=edgeConstraints.find(id);
if(jter != edgeConstraints.end())
UASSERT(jter != edgeConstraints.end());
if (1 / static_cast<double>(jter->second.infMatrix().at<double>(5,5)) >= 9999.0)
{
if (1 / static_cast<double>(jter->second.infMatrix().at<double>(5,5)) >= 9999.0)
{
g2o::VertexPointXY * v2 = new g2o::VertexPointXY();
v2->setEstimate(Eigen::Vector2d(iter->second.x(), iter->second.y()));
vertex = v2;
isLandmarkWithRotation.insert(std::make_pair(id, false));
id = landmarkVertexOffset - id;
}
else
{
g2o::VertexSE2 * v2 = new g2o::VertexSE2();
v2->setEstimate(g2o::SE2(iter->second.x(), iter->second.y(), iter->second.theta()));
vertex = v2;
isLandmarkWithRotation.insert(std::make_pair(id, true));
id = landmarkVertexOffset - id;
}
g2o::VertexPointXY * v2 = new g2o::VertexPointXY();
v2->setEstimate(Eigen::Vector2d(iter->second.x(), iter->second.y()));
vertex = v2;
isLandmarkWithRotation.insert(std::make_pair(id, false));
id = landmarkVertexOffset - id;
}
else
{
continue;
g2o::VertexSE2 * v2 = new g2o::VertexSE2();
v2->setEstimate(g2o::SE2(iter->second.x(), iter->second.y(), iter->second.theta()));
vertex = v2;
isLandmarkWithRotation.insert(std::make_pair(id, true));
id = landmarkVertexOffset - id;
}
}
else
@@ -391,34 +386,29 @@ std::map<int, Transform> OptimizerG2O::optimize(
{
// check if it is SE3 or only PointXYZ
std::multimap<int, Link>::const_iterator jter=edgeConstraints.find(id);
if(jter != edgeConstraints.end())
UASSERT(jter != edgeConstraints.end());
if (1 / static_cast<double>(jter->second.infMatrix().at<double>(3,3)) >= 9999.0 ||
1 / static_cast<double>(jter->second.infMatrix().at<double>(4,4)) >= 9999.0 ||
1 / static_cast<double>(jter->second.infMatrix().at<double>(5,5)) >= 9999.0)
{
if (1 / static_cast<double>(jter->second.infMatrix().at<double>(3,3)) >= 9999.0 ||
1 / static_cast<double>(jter->second.infMatrix().at<double>(4,4)) >= 9999.0 ||
1 / static_cast<double>(jter->second.infMatrix().at<double>(5,5)) >= 9999.0)
{
g2o::VertexPointXYZ * v3 = new g2o::VertexPointXYZ();
v3->setEstimate(Eigen::Vector3d(iter->second.x(), iter->second.y(), iter->second.z()));
vertex = v3;
isLandmarkWithRotation.insert(std::make_pair(id, false));
id = landmarkVertexOffset - id;
}
else
{
g2o::VertexSE3 * v3 = new g2o::VertexSE3();
Eigen::Affine3d a = iter->second.toEigen3d();
Eigen::Isometry3d pose;
pose = a.linear();
pose.translation() = a.translation();
v3->setEstimate(pose);
vertex = v3;
isLandmarkWithRotation.insert(std::make_pair(id, true));
id = landmarkVertexOffset - id;
}
g2o::VertexPointXYZ * v3 = new g2o::VertexPointXYZ();
v3->setEstimate(Eigen::Vector3d(iter->second.x(), iter->second.y(), iter->second.z()));
vertex = v3;
isLandmarkWithRotation.insert(std::make_pair(id, false));
id = landmarkVertexOffset - id;
}
else
{
continue;
g2o::VertexSE3 * v3 = new g2o::VertexSE3();
Eigen::Affine3d a = iter->second.toEigen3d();
Eigen::Isometry3d pose;
pose = a.linear();
pose.translation() = a.translation();
v3->setEstimate(pose);
vertex = v3;
isLandmarkWithRotation.insert(std::make_pair(id, true));
id = landmarkVertexOffset - id;
}
}
else

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

@@ -168,18 +168,17 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
{
// check if it is SE2 or only PointXY
std::multimap<int, Link>::const_iterator jter=edgeConstraints.find(iter->first);
if(jter != edgeConstraints.end())
UASSERT_MSG(jter != edgeConstraints.end(), uFormat("Not found landmark %d in edges!", iter->first).c_str());
if (1 / static_cast<double>(jter->second.infMatrix().at<double>(5,5)) >= 9999.0)
{
if (1 / static_cast<double>(jter->second.infMatrix().at<double>(5,5)) >= 9999.0)
{
initialEstimate.insert(iter->first, gtsam::Point2(iter->second.x(), iter->second.y()));
isLandmarkWithRotation.insert(std::make_pair(iter->first, false));
}
else
{
initialEstimate.insert(iter->first, gtsam::Pose2(iter->second.x(), iter->second.y(), iter->second.theta()));
isLandmarkWithRotation.insert(std::make_pair(iter->first, true));
}
initialEstimate.insert(iter->first, gtsam::Point2(iter->second.x(), iter->second.y()));
isLandmarkWithRotation.insert(std::make_pair(iter->first, false));
}
else
{
initialEstimate.insert(iter->first, gtsam::Pose2(iter->second.x(), iter->second.y(), iter->second.theta()));
isLandmarkWithRotation.insert(std::make_pair(iter->first, true));
}
}
@@ -194,20 +193,19 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
{
// check if it is SE3 or only PointXYZ
std::multimap<int, Link>::const_iterator jter=edgeConstraints.find(iter->first);
if(jter != edgeConstraints.end())
UASSERT_MSG(jter != edgeConstraints.end(), uFormat("Not found landmark %d in edges!", iter->first).c_str());
if (1 / static_cast<double>(jter->second.infMatrix().at<double>(3,3)) >= 9999.0 ||
1 / static_cast<double>(jter->second.infMatrix().at<double>(4,4)) >= 9999.0 ||
1 / static_cast<double>(jter->second.infMatrix().at<double>(5,5)) >= 9999.0)
{
if (1 / static_cast<double>(jter->second.infMatrix().at<double>(3,3)) >= 9999.0 ||
1 / static_cast<double>(jter->second.infMatrix().at<double>(4,4)) >= 9999.0 ||
1 / static_cast<double>(jter->second.infMatrix().at<double>(5,5)) >= 9999.0)
{
initialEstimate.insert(iter->first, gtsam::Point3(iter->second.x(), iter->second.y(), iter->second.z()));
isLandmarkWithRotation.insert(std::make_pair(iter->first, false));
}
else
{
initialEstimate.insert(iter->first, gtsam::Pose3(iter->second.toEigen4d()));
isLandmarkWithRotation.insert(std::make_pair(iter->first, true));
}
initialEstimate.insert(iter->first, gtsam::Point3(iter->second.x(), iter->second.y(), iter->second.z()));
isLandmarkWithRotation.insert(std::make_pair(iter->first, false));
}
else
{
initialEstimate.insert(iter->first, gtsam::Pose3(iter->second.toEigen4d()));
isLandmarkWithRotation.insert(std::make_pair(iter->first, true));
}
}
}