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

0.17.2: compute marginals (covariance) on graph optimization

Browse Source
This commit is contained in:
matlabbe
2018-05-30 15:35:08 -04:00
parent 055cccd151
commit 1914275fa8
13 changed files with 155 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files

19
corelib/src/Optimizer.cpp
View File

@@ -328,10 +328,29 @@ std::map<int, Transform> Optimizer::optimizeIncremental(
return std::map<int, Transform>();
}
std::map<int, Transform> Optimizer::optimize(
int rootId,
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & edgeConstraints,
std::list<std::map<int, Transform> > * intermediateGraphes,
double * finalError,
int * iterationsDone)
{
cv::Mat covariance;
return optimize(rootId,
poses,
edgeConstraints,
covariance,
intermediateGraphes,
finalError,
iterationsDone);
}
std::map<int, Transform> Optimizer::optimize(
int rootId,
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & constraints,
cv::Mat & outputCovariance,
std::list<std::map<int, Transform> > * intermediateGraphes,
double * finalError,
int * iterationsDone)

43
corelib/src/OptimizerG2O.cpp
View File

@@ -164,6 +164,7 @@ std::map<int, Transform> OptimizerG2O::optimize(
int rootId,
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & edgeConstraints,
cv::Mat & outputCovariance,
std::list<std::map<int, Transform> > * intermediateGraphes,
double * finalError,
int * iterationsDone)
@@ -659,6 +660,31 @@ std::map<int, Transform> OptimizerG2O::optimize(
UERROR("Vertex %d not found!?", iter->first);
}
}
g2o::VertexSE2* v = (g2o::VertexSE2*)optimizer.vertex(poses.rbegin()->first);
if(v)
{
UTimer t;
g2o::SparseBlockMatrix<g2o::MatrixXD> spinv;
optimizer.computeMarginals(spinv, v);
UINFO("Computed marginals = %fs (cols=%d rows=%d, v=%d id=%d)", t.ticks(), spinv.cols(), spinv.rows(), v->hessianIndex(), poses.rbegin()->first);
g2o::SparseBlockMatrix<g2o::MatrixXD>::SparseMatrixBlock * block = spinv.blockCols()[v->hessianIndex()].begin()->second;
UASSERT(block && block->cols() == 3 && block->cols() == 3);
outputCovariance = cv::Mat::eye(6,6,CV_64FC1);
outputCovariance.at<double>(0,0) = (*block)(0,0); // x-x
outputCovariance.at<double>(0,1) = (*block)(0,1); // x-y
outputCovariance.at<double>(0,5) = (*block)(0,2); // x-theta
outputCovariance.at<double>(1,0) = (*block)(1,0); // y-x
outputCovariance.at<double>(1,1) = (*block)(1,1); // y-y
outputCovariance.at<double>(1,5) = (*block)(1,2); // y-theta
outputCovariance.at<double>(5,0) = (*block)(2,0); // theta-x
outputCovariance.at<double>(5,1) = (*block)(2,1); // theta-y
outputCovariance.at<double>(5,5) = (*block)(2,2); // theta-theta
}
else
{
UERROR("Vertex %d not found!? Cannot compute marginals...", poses.rbegin()->first);
}
}
else
{
@@ -676,6 +702,23 @@ std::map<int, Transform> OptimizerG2O::optimize(
UERROR("Vertex %d not found!?", iter->first);
}
}
g2o::VertexSE3* v = (g2o::VertexSE3*)optimizer.vertex(poses.rbegin()->first);
if(v)
{
UTimer t;
g2o::SparseBlockMatrix<g2o::MatrixXD> spinv;
optimizer.computeMarginals(spinv, v);
UINFO("Computed marginals = %fs (cols=%d rows=%d, v=%d id=%d)", t.ticks(), spinv.cols(), spinv.rows(), v->hessianIndex(), poses.rbegin()->first);
g2o::SparseBlockMatrix<g2o::MatrixXD>::SparseMatrixBlock * block = spinv.blockCols()[v->hessianIndex()].begin()->second;
UASSERT(block && block->cols() == 6 && block->cols() == 6);
outputCovariance = cv::Mat(6,6,CV_64FC1);
memcpy(outputCovariance.data, block->data(), outputCovariance.total()*sizeof(double));
}
else
{
UERROR("Vertex %d not found!? Cannot compute marginals...", poses.rbegin()->first);
}
}
}
else if(poses.size() == 1 || iterations() <= 0)

38
corelib/src/OptimizerGTSAM.cpp
View File

@@ -79,6 +79,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
int rootId,
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & edgeConstraints,
cv::Mat & outputCovariance,
std::list<std::map<int, Transform> > * intermediateGraphes,
double * finalError,
int * iterationsDone)
@@ -381,6 +382,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
UINFO("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());
for(gtsam::Values::const_iterator iter=optimizer->values().begin(); iter!=optimizer->values().end(); ++iter)
{
if(iter->value.dim() > 1)
@@ -397,6 +399,42 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
}
}
}
// compute marginals
try {
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())
{
UASSERT(info.cols() == 3 && info.cols() == 3);
outputCovariance = cv::Mat::eye(6,6,CV_64FC1);
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
outputCovariance.at<double>(1,0) = info(1,0); // y-x
outputCovariance.at<double>(1,1) = info(1,1); // y-y
outputCovariance.at<double>(1,5) = info(1,2); // y-theta
outputCovariance.at<double>(5,0) = info(2,0); // theta-x
outputCovariance.at<double>(5,1) = info(2,1); // theta-y
outputCovariance.at<double>(5,5) = info(2,2); // theta-theta
}
else
{
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
mgtsam.block(0,3,3,3) = info.block(0,3,3,3); // off diagonal
mgtsam.block(3,0,3,3) = info.block(3,0,3,3); // off diagonal
outputCovariance = cv::Mat(6,6,CV_64FC1);
memcpy(outputCovariance.data, mgtsam.data(), outputCovariance.total()*sizeof(double));
}
} catch(std::exception& e) {
cout << e.what() << endl;
}
delete optimizer;
}
else if(poses.size() == 1 || iterations() <= 0)

4
corelib/src/OptimizerTORO.cpp
View File

@@ -55,6 +55,7 @@ std::map<int, Transform> OptimizerTORO::optimize(
int rootId,
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & edgeConstraints,
cv::Mat & outputCovariance,
std::list<std::map<int, Transform> > * intermediateGraphes, // contains poses after tree init to last one before the end
double * finalError,
int * iterationsDone)
@@ -312,6 +313,9 @@ std::map<int, Transform> OptimizerTORO::optimize(
optimizedPoses.insert(std::pair<int, Transform>(iter->first, newPose));
}
}
// TORO doesn't compute marginals...
outputCovariance = cv::Mat::eye(6,6,CV_64FC1);
}
else if(poses.size() == 1 || iterations() <= 0)
{

28
corelib/src/Rtabmap.cpp
View File

@@ -797,7 +797,8 @@ void Rtabmap::exportPoses(const std::string & path, bool optimized, bool global,
if(optimized)
{
this->optimizeCurrentMap(_memory->getLastWorkingSignature()->id(), global, poses, &constraints);
cv::Mat covariance;
this->optimizeCurrentMap(_memory->getLastWorkingSignature()->id(), global, poses, covariance, &constraints);
}
else
{
@@ -846,7 +847,8 @@ void Rtabmap::resetMemory()
_memory->init(_databasePath, true, _parameters, true);
if(_memory->getLastWorkingSignature())
{
optimizeCurrentMap(_memory->getLastWorkingSignature()->id(), false, _optimizedPoses, &_constraints);
cv::Mat covariance;
optimizeCurrentMap(_memory->getLastWorkingSignature()->id(), false, _optimizedPoses, covariance, &_constraints);
}
if(_bayesFilter)
{
@@ -2116,7 +2118,8 @@ bool Rtabmap::process(
if(_proximityRawPosesUsed)
{
//optimize the path's poses locally
path = optimizeGraph(nearestId, uKeysSet(path), std::map<int, Transform>(), false);
cv::Mat covariance;
path = optimizeGraph(nearestId, uKeysSet(path), std::map<int, Transform>(), false, covariance);
// transform local poses in optimized graph referential
UASSERT(uContains(path, nearestId));
Transform t = _optimizedPoses.at(nearestId) * path.at(nearestId).inverse();
@@ -2234,6 +2237,7 @@ bool Rtabmap::process(
float maxLinearErrorRatio = 0.0f;
double optimizationError = 0.0;
int optimizationIterations = 0;
cv::Mat localizationCovariance;
if(_rgbdSlamMode &&
(_loopClosureHypothesis.first>0 ||
lastProximitySpaceClosureId>0 || // can be different map of the current one
@@ -2282,6 +2286,7 @@ bool Rtabmap::process(
{
_optimizedPoses.at(signature->id()) = _optimizedPoses.at(localizationLinks.begin()->first) * localizationLinks.begin()->second.transform().inverse();
}
localizationCovariance = localizationLinks.begin()->second.infMatrix().inv();
}
else
{
@@ -2306,7 +2311,8 @@ bool Rtabmap::process(
}
std::multimap<int, Link> constraints;
optimizeCurrentMap(signature->id(), false, poses, &constraints, &optimizationError, &optimizationIterations);
cv::Mat covariance;
optimizeCurrentMap(signature->id(), false, poses, covariance, &constraints, &optimizationError, &optimizationIterations);
// Check added loop closures have broken the graph
// (in case of wrong loop closures).
@@ -2389,6 +2395,7 @@ bool Rtabmap::process(
UINFO("Updated local map (old size=%d, new size=%d)", (int)_optimizedPoses.size(), (int)poses.size());
_optimizedPoses = poses;
_constraints = constraints;
localizationCovariance = covariance;
}
}
@@ -2479,6 +2486,7 @@ bool Rtabmap::process(
}
statistics_.setMapCorrection(_mapCorrection);
UINFO("Set map correction = %s", _mapCorrection.prettyPrint().c_str());
statistics_.setLocalizationCovariance(localizationCovariance);
// timings...
statistics_.addStatistic(Statistics::kTimingMemory_update(), timeMemoryUpdate*1000);
@@ -3221,6 +3229,7 @@ void Rtabmap::optimizeCurrentMap(
int id,
bool lookInDatabase,
std::map<int, Transform> & optimizedPoses,
cv::Mat & covariance,
std::multimap<int, Link> * constraints,
double * error,
int * iterationsDone) const
@@ -3237,7 +3246,7 @@ void Rtabmap::optimizeCurrentMap(
}
UINFO("get %d ids time %f s", (int)ids.size(), timer.ticks());
std::map<int, Transform> poses = Rtabmap::optimizeGraph(id, uKeysSet(ids), optimizedPoses, lookInDatabase, constraints, error, iterationsDone);
std::map<int, Transform> poses = Rtabmap::optimizeGraph(id, uKeysSet(ids), optimizedPoses, lookInDatabase, covariance, constraints, error, iterationsDone);
UINFO("optimize time %f s", timer.ticks());
if(poses.size())
@@ -3267,6 +3276,7 @@ std::map<int, Transform> Rtabmap::optimizeGraph(
const std::set<int> & ids,
const std::map<int, Transform> & guessPoses,
bool lookInDatabase,
cv::Mat & covariance,
std::multimap<int, Link> * constraints,
double * error,
int * iterationsDone) const
@@ -3348,7 +3358,7 @@ std::map<int, Transform> Rtabmap::optimizeGraph(
}
else
{
optimizedPoses = _graphOptimizer->optimize(fromId, poses, edgeConstraints, 0, error, iterationsDone);
optimizedPoses = _graphOptimizer->optimize(fromId, poses, edgeConstraints, covariance, 0, error, iterationsDone);
if(!poses.empty() && optimizedPoses.empty() && guessPoses.empty())
{
@@ -3522,7 +3532,8 @@ void Rtabmap::get3DMap(
if(optimized)
{
poses = _optimizedPoses; // guess
this->optimizeCurrentMap(_memory->getLastWorkingSignature()->id(), global, poses, &constraints);
cv::Mat covariance;
this->optimizeCurrentMap(_memory->getLastWorkingSignature()->id(), global, poses, covariance, &constraints);
}
else
{
@@ -3609,7 +3620,8 @@ void Rtabmap::getGraph(
if(optimized)
{
poses = _optimizedPoses; // guess
this->optimizeCurrentMap(_memory->getLastWorkingSignature()->id(), global, poses, &constraints);
cv::Mat covariance;
this->optimizeCurrentMap(_memory->getLastWorkingSignature()->id(), global, poses, covariance, &constraints);
}
else
{