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

Replaced all optimizeIncremental() by optimizeMultiSession() to fix GTSAM errors when merging multiple maps together. DBReader: always publish saved stamp. DBViewer: added actions to set covariance of all neighbor or loop closure links.

Browse Source
This commit is contained in:
matlabbe
2018-10-15 17:07:18 -04:00
parent b2c012d8bc
commit f1546c1fca
15 changed files with 285 additions and 82 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
corelib/include/rtabmap/core/DBDriver.h
View File

@@ -103,7 +103,7 @@ public:
void savePreviewImage(const cv::Mat & image) const;
cv::Mat loadPreviewImage() const;
void saveOptimizedPoses(const std::map<int, Transform> & optimizedPoses, const Transform & lastlocalizationPose) const;
std::map<int, Transform> loadOptimizedPoses(Transform * lastlocalizationPose) const;
std::map<int, Transform> loadOptimizedPoses(Transform * lastlocalizationPose = 0) const;
void save2DMap(const cv::Mat & map, float xMin, float yMin, float cellSize) const;
cv::Mat load2DMap(float & xMin, float & yMin, float & cellSize) const;
void saveOptimizedMesh(
@@ -232,7 +232,7 @@ protected:
virtual void savePreviewImageQuery(const cv::Mat & image) const = 0;
virtual cv::Mat loadPreviewImageQuery() const = 0;
virtual void saveOptimizedPosesQuery(const std::map<int, Transform> & optimizedPoses, const Transform & lastlocalizationPose) const = 0;
virtual std::map<int, Transform> loadOptimizedPosesQuery(Transform * lastlocalizationPose) const = 0;
virtual std::map<int, Transform> loadOptimizedPosesQuery(Transform * lastlocalizationPose = 0) const = 0;
virtual void save2DMapQuery(const cv::Mat & map, float xMin, float yMin, float cellSize) const = 0;
virtual cv::Mat load2DMapQuery(float & xMin, float & yMin, float & cellSize) const = 0;
virtual void saveOptimizedMeshQuery(

2
corelib/include/rtabmap/core/DBDriverSqlite3.h
View File

@@ -104,7 +104,7 @@ protected:
virtual void savePreviewImageQuery(const cv::Mat & image) const;
virtual cv::Mat loadPreviewImageQuery() const;
virtual void saveOptimizedPosesQuery(const std::map<int, Transform> & optimizedPoses, const Transform & lastlocalizationPose) const;
virtual std::map<int, Transform> loadOptimizedPosesQuery(Transform * lastlocalizationPose) const;
virtual std::map<int, Transform> loadOptimizedPosesQuery(Transform * lastlocalizationPose = 0) const;
virtual void save2DMapQuery(const cv::Mat & map, float xMin, float yMin, float cellSize) const;
virtual cv::Mat load2DMapQuery(float & xMin, float & yMin, float & cellSize) const;
virtual void saveOptimizedMeshQuery(

3
corelib/include/rtabmap/core/Graph.h
View File

@@ -135,6 +135,9 @@ std::multimap<int, int>::const_iterator RTABMAP_EXP findLink(
int from,
int to,
bool checkBothWays = true);
std::list<Link> RTABMAP_EXP findLinks(
const std::multimap<int, Link> & links,
int from);
std::multimap<int, Link> RTABMAP_EXP filterDuplicateLinks(
const std::multimap<int, Link> & links);

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

@@ -95,6 +95,14 @@ public:
double * finalError = 0,
int * iterationsDone = 0);
std::map<int, Transform> optimizeMultiSession(
int rootId,
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & constraints,
std::list<std::map<int, Transform> > * intermediateGraphes = 0,
double * finalError = 0,
int * iterationsDone = 0);
std::map<int, Transform> optimize(
int rootId,
const std::map<int, Transform> & poses,

4
corelib/src/DBReader.cpp
View File

@@ -400,10 +400,6 @@ SensorData DBReader::getNextData(CameraInfo * info)
_previousStamp = stamp;
_previousMapID = mapId;
}
else
{
stamp = 0;
}
data.uncompressData();
if(data.cameraModels().size() > 1 &&

19
corelib/src/Graph.cpp
View File

@@ -1037,6 +1037,25 @@ std::multimap<int, int>::const_iterator findLink(
return links.end();
}
std::list<Link> findLinks(
const std::multimap<int, Link> & links,
int from)
{
std::list<Link> output;
for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter != links.end(); ++iter)
{
if(iter->second.from() == from)
{
output.push_back(iter->second);
}
else if(iter->second.to() == from)
{
output.push_back(iter->second.inverse());
}
}
return output;
}
std::multimap<int, Link> filterDuplicateLinks(
const std::multimap<int, Link> & links)
{

68
corelib/src/Optimizer.cpp
View File

@@ -276,7 +276,7 @@ std::map<int, Transform> Optimizer::optimizeIncremental(
incGraph.insert(*poses.begin());
int i=0;
std::multimap<int, Link> constraintsCpy = constraints;
UDEBUG("Incremental optimization... poses=%d comstraints=%d", (int)poses.size(), (int)constraints.size());
UDEBUG("Incremental optimization... poses=%d constraints=%d", (int)poses.size(), (int)constraints.size());
for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{
incGraph.insert(*iter);
@@ -310,7 +310,7 @@ std::map<int, Transform> Optimizer::optimizeIncremental(
incGraph = this->optimize(incGraph.begin()->first, incGraph, incGraphLinks);
if(incGraph.empty())
{
UWARN("Failed incremental optimization...");
UWARN("Failed incremental optimization... last pose added is %d", iter->first);
break;
}
}
@@ -328,6 +328,70 @@ std::map<int, Transform> Optimizer::optimizeIncremental(
return std::map<int, Transform>();
}
std::map<int, Transform> Optimizer::optimizeMultiSession(
int rootId,
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & constraints,
std::list<std::map<int, Transform> > * intermediateGraphes,
double * finalError,
int * iterationsDone)
{
std::map<int, Transform> incGraph;
if(poses.empty())
{
return incGraph;
}
UDEBUG("Incremental optimization... poses=%d constraints=%d", (int)poses.size(), (int)constraints.size());
std::set<int> nextPoses;
nextPoses.insert(rootId);
UASSERT(uContains(poses, rootId));
while(!nextPoses.empty())
{
std::set<int> currentPoses = nextPoses;
nextPoses.clear();
for(std::set<int>::iterator iter=currentPoses.begin(); iter!=currentPoses.end(); ++iter)
{
int fromId = *iter;
if(incGraph.empty())
{
std::map<int, Transform>::const_iterator cter = poses.find(fromId);
UASSERT(cter != poses.end());
incGraph.insert(*cter);
}
std::list<Link> links = graph::findLinks(constraints, fromId);
for(std::list<Link>::iterator jter = links.begin(); jter!=links.end(); ++jter)
{
if(!uContains(incGraph, jter->to()))
{
incGraph.insert(std::make_pair(jter->to(), incGraph.at(jter->from()) * jter->transform()));
nextPoses.insert(jter->to());
}
}
}
}
UASSERT(!incGraph.empty());
if(intermediateGraphes)
{
intermediateGraphes->push_back(incGraph);
}
if(incGraph.size() != poses.size())
{
UWARN("Failed multi-session optimization, output poses (%d) != input poses (%d)", incGraph.size(), poses.size());
}
else
{
UASSERT(uContains(poses, rootId) && uContains(incGraph, rootId));
incGraph.at(rootId) = poses.at(rootId);
return this->optimize(rootId, incGraph, constraints, intermediateGraphes, finalError, iterationsDone);
}
UDEBUG("Failed incremental optimization");
return std::map<int, Transform>();
}
std::map<int, Transform> Optimizer::optimize(
int rootId,
const std::map<int, Transform> & poses,

2
corelib/src/RegistrationVis.cpp
View File

@@ -1595,7 +1595,7 @@ Transform RegistrationVis::computeTransformationImpl(
if((int)allInliers.size() < _minInliers)
{
msg = uFormat("Not enough inliers after bundle adjustment %d/%d (matches=%d) between %d and %d",
(int)allInliers.size(), _minInliers, fromSignature.id(), toSignature.id());
(int)allInliers.size(), _minInliers, (int)allInliers.size()+sbaOutliers.size(), fromSignature.id(), toSignature.id());
transforms[0].setNull();
}
else

29
corelib/src/Rtabmap.cpp
View File

@@ -1073,7 +1073,7 @@ bool Rtabmap::process(
UFATAL("Not supposed to be here...last signature is null?!?");
}
ULOGGER_INFO("Processing signature %d w=%d", signature->id(), signature->getWeight());
ULOGGER_INFO("Processing signature %d w=%d map=%d", signature->id(), signature->getWeight(), signature->mapId());
timeMemoryUpdate = timer.ticks();
ULOGGER_INFO("timeMemoryUpdate=%fs", timeMemoryUpdate);
@@ -2370,12 +2370,14 @@ bool Rtabmap::process(
UINFO("Max optimization linear error = %f m (link %d->%d, var=%f, ratio error/std=%f)", maxLinearError, maxLinearLink->from(), maxLinearLink->to(), maxLinearLink->transVariance(), maxLinearError/sqrt(maxLinearLink->transVariance()));
if(maxLinearErrorRatio > _optimizationMaxError)
{
UWARN("Rejecting all added loop closures (%d) in this "
UWARN("Rejecting all added loop closures (%d, first is %d <-> %d) in this "
"iteration because a wrong loop closure has been "
"detected after graph optimization, resulting in "
"a maximum graph error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). The "
"maximum error ratio parameter \"%s\" is %f of std deviation.",
(int)loopClosureLinksAdded.size(),
loopClosureLinksAdded.front().first,
loopClosureLinksAdded.front().second,
maxLinearErrorRatio,
maxLinearLink->from(),
maxLinearLink->to(),
@@ -2392,12 +2394,14 @@ bool Rtabmap::process(
UINFO("Max optimization angular error = %f deg (link %d->%d, var=%f, ratio error/std=%f)", maxAngularError*180.0f/CV_PI, maxAngularLink->from(), maxAngularLink->to(), maxAngularLink->rotVariance(), maxAngularError/sqrt(maxAngularLink->rotVariance()));
if(maxAngularErrorRatio > _optimizationMaxError)
{
UWARN("Rejecting all added loop closures (%d) in this "
UWARN("Rejecting all added loop closures (%d, first is %d <-> %d) in this "
"iteration because a wrong loop closure has been "
"detected after graph optimization, resulting in "
"a maximum graph error ratio of %f (edge %d->%d, type=%d, abs error=%f deg, stddev=%f). The "
"maximum error ratio parameter \"%s\" is %f of std deviation.",
(int)loopClosureLinksAdded.size(),
loopClosureLinksAdded.front().first,
loopClosureLinksAdded.front().second,
maxAngularErrorRatio,
maxAngularLink->from(),
maxAngularLink->to(),
@@ -2789,7 +2793,7 @@ bool Rtabmap::process(
std::map<int, Signature> signatures;
if(_publishLastSignatureData)
{
UINFO("Adding data %d (rgb/left=%d depth/right=%d)", lastSignatureData.id(), lastSignatureData.sensorData().imageRaw().empty()?0:1, lastSignatureData.sensorData().depthOrRightRaw().empty()?0:1);
UINFO("Adding data %d [%d] (rgb/left=%d depth/right=%d)", lastSignatureData.id(), lastSignatureData.mapId(), lastSignatureData.sensorData().imageRaw().empty()?0:1, lastSignatureData.sensorData().depthOrRightRaw().empty()?0:1);
signatures.insert(std::make_pair(lastSignatureData.id(), lastSignatureData));
}
UDEBUG("");
@@ -2811,6 +2815,11 @@ bool Rtabmap::process(
std::map<int, Transform> groundTruths;
for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{
if(_publishLastSignatureData && lastSignatureData.id() == iter->first)
{
//already added
continue;
}
Transform odomPoseLocal;
int weight = -1;
int mapId = -1;
@@ -3486,26 +3495,26 @@ std::map<int, Transform> Rtabmap::optimizeGraph(
{
optimizedPoses = _graphOptimizer->optimize(fromId, poses, edgeConstraints, covariance, 0, error, iterationsDone);
if(!poses.empty() && optimizedPoses.empty() && guessPoses.empty())
if(!poses.empty() && optimizedPoses.empty())
{
UWARN("Optimization has failed, trying incremental optimization instead, this may take a while (poses=%d, links=%d)...", (int)poses.size(), (int)edgeConstraints.size());
optimizedPoses = _graphOptimizer->optimizeIncremental(fromId, poses, edgeConstraints, 0, error, iterationsDone);
UWARN("Optimization has failed, trying multi-session optimization instead (poses=%d, guess=%d, links=%d)...", (int)poses.size(), (int)guessPoses.size(), (int)edgeConstraints.size());
optimizedPoses = _graphOptimizer->optimizeMultiSession(fromId, poses, edgeConstraints, 0, error, iterationsDone);
if(optimizedPoses.empty())
{
if(!_graphOptimizer->isCovarianceIgnored() || _graphOptimizer->type() != Optimizer::kTypeTORO)
{
UWARN("Incremental optimization also failed. You may try changing parameters to %s=0 and %s=true.",
UWARN("Multi-session optimization also failed. You may try changing parameters to %s=0 and %s=true.",
Parameters::kOptimizerStrategy().c_str(), Parameters::kOptimizerVarianceIgnored().c_str());
}
else
{
UWARN("Incremental optimization also failed.");
UWARN("Multi-session optimization also failed.");
}
}
else
{
UWARN("Incremental optimization succeeded!");
UWARN("Multi-session optimization succeeded!");
}
}
}

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

@@ -1192,7 +1192,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
UASSERT(optimizer.verifyInformationMatrices());
UINFO("g2o optimizing begin (max iterations=%d, robustKernel=%f)", iterations(), robustKernelDelta_);
UDEBUG("g2o optimizing begin (max iterations=%d, robustKernel=%f)", iterations(), robustKernelDelta_);
int it = 0;
UTimer timer;
@@ -1270,7 +1270,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
UDEBUG("outliers=%d outliersCountFar=%d", outliersCount, outliersCountFar);
}
}
UINFO("g2o optimizing end (%d iterations done, error=%f, outliers=%d/%d (delta=%f) time = %f s)", it, optimizer.activeRobustChi2(), outliersCount, (int)edges.size(), robustKernelDelta_, timer.ticks());
UDEBUG("g2o optimizing end (%d iterations done, error=%f, outliers=%d/%d (delta=%f) time = %f s)", it, optimizer.activeRobustChi2(), outliersCount, (int)edges.size(), robustKernelDelta_, timer.ticks());
if(optimizer.activeRobustChi2() > 1000000000000.0)
{

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

@@ -310,7 +310,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
optimizer = new gtsam::LevenbergMarquardtOptimizer(graph, initialEstimate, parameters);
}
UINFO("GTSAM optimizing begin (max iterations=%d, robust=%d)", iterations(), isRobust()?1:0);
UDEBUG("GTSAM optimizing begin (max iterations=%d, robust=%d)", iterations(), isRobust()?1:0);
UTimer timer;
int it = 0;
double lastError = optimizer->error();
@@ -344,7 +344,9 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
}
catch(gtsam::IndeterminantLinearSystemException & e)
{
UWARN("GTSAM exception caught: %s", e.what());
UWARN("GTSAM exception caught: %s\n Graph has %d edges and %d vertices", e.what(),
(int)edgeConstraints.size(),
(int)poses.size());
delete optimizer;
return optimizedPoses;
}
@@ -361,7 +363,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
}
else
{
UINFO("Stop optimizing, not enough improvement (%f < %f)", errorDelta, this->epsilon());
UDEBUG("Stop optimizing, not enough improvement (%f < %f)", errorDelta, this->epsilon());
break;
}
}
@@ -380,7 +382,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
{
*iterationsDone = it;
}
UINFO("GTSAM optimizing end (%d iterations done, error=%f (initial=%f final=%f), time=%f s)",
UDEBUG("GTSAM optimizing end (%d iterations done, error=%f (initial=%f final=%f), time=%f s)",
optimizer->iterations(), optimizer->error(), graph.error(initialEstimate), graph.error(optimizer->values()), timer.ticks());
gtsam::Marginals marginals(graph, optimizer->values());
@@ -406,10 +408,9 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
UTimer t;
gtsam::Marginals marginals(graph, optimizer->values());
gtsam::Matrix info = marginals.marginalCovariance(optimizer->values().rbegin()->key);
UINFO("Computed marginals = %fs (key=%d)", t.ticks(), optimizer->values().rbegin()->key);
if(isSlam2d())
UDEBUG("Computed marginals = %fs (key=%d)", t.ticks(), optimizer->values().rbegin()->key);
if(isSlam2d() && info.cols() == 3 && info.cols() == 3)
{
UASSERT(info.cols() == 3 && info.cols() == 3);
outputCovariance.at<double>(0,0) = info(0,0); // x-x
outputCovariance.at<double>(0,1) = info(0,1); // x-y
outputCovariance.at<double>(0,5) = info(0,2); // x-theta
@@ -420,9 +421,8 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
outputCovariance.at<double>(5,1) = info(2,1); // theta-y
outputCovariance.at<double>(5,5) = info(2,2); // theta-theta
}
else
else if(!isSlam2d() && info.cols() == 6 && info.cols() == 6)
{
UASSERT(info.cols() == 6 && info.cols() == 6);
Eigen::Matrix<double, 6, 6> mgtsam = Eigen::Matrix<double, 6, 6>::Identity();
mgtsam.block(3,3,3,3) = info.block(0,0,3,3); // cov rotation
mgtsam.block(0,0,3,3) = info.block(3,3,3,3); // cov translation
@@ -430,8 +430,18 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
mgtsam.block(3,0,3,3) = info.block(3,0,3,3); // off diagonal
memcpy(outputCovariance.data, mgtsam.data(), outputCovariance.total()*sizeof(double));
}
} catch(std::exception& e) {
cout << e.what() << endl;
else
{
UWARN("GTSAM: Could not compute marginal covariance!");
}
}
catch(gtsam::IndeterminantLinearSystemException & e)
{
UWARN("GTSAM exception caught: %s", e.what());
}
catch(std::exception& e)
{
UWARN("GTSAM exception caught: %s", e.what());
}
delete optimizer;