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

Rtabmap: avoid optimizing for only gravity if odometry is already aligned with gravity

Browse Source
This commit is contained in:
matlabbe
2020-02-26 13:40:03 -05:00
parent 80bee99bfc
commit 5daf4e5b2b
2 changed files with 6 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
corelib/include/rtabmap/core/Memory.h
View File

@@ -218,6 +218,7 @@ public:
const Feature2D * getFeature2D() const {return _feature2D;} const Feature2D * getFeature2D() const {return _feature2D;}
bool isGraphReduced() const {return _reduceGraph;} bool isGraphReduced() const {return _reduceGraph;}
const std::vector<double> & getOdomMaxInf() const {return _odomMaxInf;} const std::vector<double> & getOdomMaxInf() const {return _odomMaxInf;}
bool isOdomGravityUsed() const {return _useOdometryGravity;}
void dumpMemoryTree(const char * fileNameTree) const; void dumpMemoryTree(const char * fileNameTree) const;
virtual void dumpMemory(std::string directory) const; virtual void dumpMemory(std::string directory) const;

5
corelib/src/Rtabmap.cpp
View File

@@ -1275,8 +1275,11 @@ bool Rtabmap::process(
{ {
UINFO("Odometry refining rejected: %s", info.rejectedMsg.c_str()); UINFO("Odometry refining rejected: %s", info.rejectedMsg.c_str());
if(!info.covariance.empty() && info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(0,0) != 1.0 && info.covariance.at<double>(5,5) > 0.0 && info.covariance.at<double>(5,5) != 1.0) if(!info.covariance.empty() && info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(0,0) != 1.0 && info.covariance.at<double>(5,5) > 0.0 && info.covariance.at<double>(5,5) != 1.0)
{
if(ULogger::level() <= ULogger::kInfo)
{ {
std::cout << info.covariance << std::endl; std::cout << info.covariance << std::endl;
}
_memory->updateLink(Link(oldId, signature->id(), signature->getLinks().begin()->second.type(), guess, (info.covariance*100.0).inv())); _memory->updateLink(Link(oldId, signature->id(), signature->getLinks().begin()->second.type(), guess, (info.covariance*100.0).inv()));
} }
} }
@@ -2562,7 +2565,7 @@ bool Rtabmap::process(
lastProximitySpaceClosureId>0 || // can be different map of the current one lastProximitySpaceClosureId>0 || // can be different map of the current one
statistics_.reducedIds().size() || statistics_.reducedIds().size() ||
(signature->hasLink(signature->id(), Link::kPosePrior) && !_graphOptimizer->priorsIgnored()) || // prior edge (signature->hasLink(signature->id(), Link::kPosePrior) && !_graphOptimizer->priorsIgnored()) || // prior edge
(signature->hasLink(signature->id(), Link::kGravity) && _graphOptimizer->gravitySigma()>0.0f) || // gravity edge (signature->hasLink(signature->id(), Link::kGravity) && _graphOptimizer->gravitySigma()>0.0f && !_memory->isOdomGravityUsed()) || // gravity edge
proximityDetectionsInTimeFound>0 || proximityDetectionsInTimeFound>0 ||
landmarkDetected!=0 || landmarkDetected!=0 ||
signaturesRetrieved.size()) // can be different map of the current one signaturesRetrieved.size()) // can be different map of the current one