i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-10-04 00:57:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

Statistics: added LoopOdom_correction stats for landmark detections.

Browse Source
This commit is contained in:
matlabbe
2021-12-10 21:58:11 -05:00
parent 8cf12c6135
commit cf5e90238b
2 changed files with 43 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/Rtabmap.cpp
+30 -26
View File
@@ -1566,13 +1566,13 @@ bool Rtabmap::process(
} }
if(!_odomCachePoses.empty()) if(!_odomCachePoses.empty())
{ {
_odomCacheConstraints.insert( Link odomLink(_odomCachePoses.rbegin()->first,
std::make_pair(_odomCachePoses.rbegin()->first, signature->id(),
Link(_odomCachePoses.rbegin()->first, Link::kNeighbor,
signature->id(), _odomCachePoses.rbegin()->second.inverse() * signature->getPose(),
Link::kNeighbor, odomCovariance.inv());
_odomCachePoses.rbegin()->second.inverse() * signature->getPose(), _odomCacheConstraints.insert(std::make_pair(_odomCachePoses.rbegin()->first, odomLink));
odomCovariance.inv()))); UDEBUG("Added odom cov = %f %f", odomLink.transVariance(), odomLink.rotVariance());
} }
} }
@@ -2884,10 +2884,10 @@ bool Rtabmap::process(
{ {
poses.insert(*iterPose); poses.insert(*iterPose);
// make the poses in the map fixed // make the poses in the map fixed
constraints.insert(std::make_pair(iterPose->first, Link(iterPose->first, iterPose->first, Link::kPosePrior, iterPose->second, cv::Mat::eye(6,6, CV_64FC1)*100000))); constraints.insert(std::make_pair(iterPose->first, Link(iterPose->first, iterPose->first, Link::kPosePrior, iterPose->second, cv::Mat::eye(6,6, CV_64FC1)*1000000)));
UDEBUG("Constraint %d->%d (type=%s)", iterPose->first, iterPose->first, Link::typeName(Link::kPosePrior).c_str()); UDEBUG("Constraint %d->%d (type=%s)", iterPose->first, iterPose->first, Link::typeName(Link::kPosePrior).c_str());
} }
UDEBUG("Constraint %d->%d (type=%s)", iter->second.from(), iter->second.to(), iter->second.typeName().c_str()); UDEBUG("Constraint %d->%d (type=%s, var = %f %f)", iter->second.from(), iter->second.to(), iter->second.typeName().c_str(), iter->second.transVariance(), iter->second.rotVariance());
} }
for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter) for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{ {
@@ -2930,6 +2930,7 @@ bool Rtabmap::process(
if(maxLinearLink == 0 && maxAngularLink==0) if(maxLinearLink == 0 && maxAngularLink==0)
{ {
UWARN("Could not compute graph errors! Wrong loop closures could be accepted!"); UWARN("Could not compute graph errors! Wrong loop closures could be accepted!");
optPoses = posesOut;
} }
if(maxLinearLink) if(maxLinearLink)
@@ -3374,28 +3375,31 @@ bool Rtabmap::process(
statistics_.addStatistic(Statistics::kLoopMap_id(), (loopId>0 && sLoop)?sLoop->mapId():-1); statistics_.addStatistic(Statistics::kLoopMap_id(), (loopId>0 && sLoop)?sLoop->mapId():-1);
float x,y,z,roll,pitch,yaw; float x,y,z,roll,pitch,yaw;
if(_loopClosureHypothesis.first || lastProximitySpaceClosureId) if(_loopClosureHypothesis.first || lastProximitySpaceClosureId || (!rejectedLandmark && !landmarksDetected.empty()))
{ {
// Loop closure transform if(_loopClosureHypothesis.first || lastProximitySpaceClosureId)
UASSERT(sLoop); {
std::multimap<int, Link>::const_iterator loopIter = sLoop->getLinks().find(signature->id()); // Loop closure transform
UASSERT(loopIter!=sLoop->getLinks().end()); UASSERT(sLoop);
UINFO("Set loop closure transform = %s", loopIter->second.transform().prettyPrint().c_str()); std::multimap<int, Link>::const_iterator loopIter = sLoop->getLinks().find(signature->id());
statistics_.setLoopClosureTransform(loopIter->second.transform()); UASSERT(loopIter!=sLoop->getLinks().end());
UINFO("Set loop closure transform = %s", loopIter->second.transform().prettyPrint().c_str());
statistics_.setLoopClosureTransform(loopIter->second.transform());
statistics_.addStatistic(Statistics::kLoopVisual_words(), sLoop->getWords().size()); statistics_.addStatistic(Statistics::kLoopVisual_words(), sLoop->getWords().size());
// if ground truth exists, compute localization error
if(!sLoop->getGroundTruthPose().isNull() && !signature->getGroundTruthPose().isNull())
{
Transform transformGT = sLoop->getGroundTruthPose().inverse() * signature->getGroundTruthPose();
Transform error = loopIter->second.transform().inverse() * transformGT;
statistics_.addStatistic(Statistics::kGtLocalization_linear_error(), error.getNorm());
statistics_.addStatistic(Statistics::kGtLocalization_angular_error(), error.getAngle(1,0,0)*180/M_PI);
}
}
statistics_.addStatistic(Statistics::kLoopDistance_since_last_loc(), _distanceTravelledSinceLastLocalization); statistics_.addStatistic(Statistics::kLoopDistance_since_last_loc(), _distanceTravelledSinceLastLocalization);
_distanceTravelledSinceLastLocalization = 0.0f; _distanceTravelledSinceLastLocalization = 0.0f;
// if ground truth exists, compute localization error
if(!sLoop->getGroundTruthPose().isNull() && !signature->getGroundTruthPose().isNull())
{
Transform transformGT = sLoop->getGroundTruthPose().inverse() * signature->getGroundTruthPose();
Transform error = loopIter->second.transform().inverse() * transformGT;
statistics_.addStatistic(Statistics::kGtLocalization_linear_error(), error.getNorm());
statistics_.addStatistic(Statistics::kGtLocalization_angular_error(), error.getAngle(1,0,0)*180/M_PI);
}
statistics_.addStatistic(Statistics::kLoopMapToOdom_norm(), _mapCorrection.getNorm()); statistics_.addStatistic(Statistics::kLoopMapToOdom_norm(), _mapCorrection.getNorm());
statistics_.addStatistic(Statistics::kLoopMapToOdom_angle(), _mapCorrection.getAngle()*180.0f/M_PI); statistics_.addStatistic(Statistics::kLoopMapToOdom_angle(), _mapCorrection.getAngle()*180.0f/M_PI);
_mapCorrection.getTranslationAndEulerAngles(x, y, z, roll, pitch, yaw); _mapCorrection.getTranslationAndEulerAngles(x, y, z, roll, pitch, yaw);
corelib/src/optimizer/OptimizerG2O.cpp
+13 -1
View File
@@ -325,7 +325,7 @@ std::map<int, Transform> OptimizerG2O::optimize(
int landmarkVertexOffset = poses.rbegin()->first+1; int landmarkVertexOffset = poses.rbegin()->first+1;
std::map<int, bool> isLandmarkWithRotation; std::map<int, bool> isLandmarkWithRotation;
UDEBUG("fill poses to g2o..."); UDEBUG("fill poses to g2o... (rootId=%d)", rootId);
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter) for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{ {
UASSERT(!iter->second.isNull()); UASSERT(!iter->second.isNull());
@@ -339,6 +339,7 @@ std::map<int, Transform> OptimizerG2O::optimize(
v2->setEstimate(g2o::SE2(iter->second.x(), iter->second.y(), iter->second.theta())); v2->setEstimate(g2o::SE2(iter->second.x(), iter->second.y(), iter->second.theta()));
if(id == rootId) if(id == rootId)
{ {
UDEBUG("Set %d fixed", id);
v2->setFixed(true); v2->setFixed(true);
} }
vertex = v2; vertex = v2;
@@ -361,6 +362,11 @@ std::map<int, Transform> OptimizerG2O::optimize(
{ {
g2o::VertexSE2 * v2 = new g2o::VertexSE2(); g2o::VertexSE2 * v2 = new g2o::VertexSE2();
v2->setEstimate(g2o::SE2(iter->second.x(), iter->second.y(), iter->second.theta())); v2->setEstimate(g2o::SE2(iter->second.x(), iter->second.y(), iter->second.theta()));
if(id == rootId)
{
UDEBUG("Set %d fixed", id);
v2->setFixed(true);
}
vertex = v2; vertex = v2;
isLandmarkWithRotation.insert(std::make_pair(id, true)); isLandmarkWithRotation.insert(std::make_pair(id, true));
id = landmarkVertexOffset - id; id = landmarkVertexOffset - id;
@@ -384,6 +390,7 @@ std::map<int, Transform> OptimizerG2O::optimize(
v3->setEstimate(pose); v3->setEstimate(pose);
if(id == rootId) if(id == rootId)
{ {
UDEBUG("Set %d fixed", id);
v3->setFixed(true); v3->setFixed(true);
} }
vertex = v3; vertex = v3;
@@ -412,6 +419,11 @@ std::map<int, Transform> OptimizerG2O::optimize(
pose = a.linear(); pose = a.linear();
pose.translation() = a.translation(); pose.translation() = a.translation();
v3->setEstimate(pose); v3->setEstimate(pose);
if(id == rootId)
{
UDEBUG("Set %d fixed", id);
v3->setFixed(true);
}
vertex = v3; vertex = v3;
isLandmarkWithRotation.insert(std::make_pair(id, true)); isLandmarkWithRotation.insert(std::make_pair(id, true));
id = landmarkVertexOffset - id; id = landmarkVertexOffset - id;