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

Added SensorData's globalPose and globalPoseCovariance field. Link: added new type PosePrior. g2o: adding edge prior for self-refering edges. Memory: adding self-refering link when SensorData's globalPose is set.

Browse Source
This commit is contained in:
matlabbe
2017-05-22 21:05:34 -04:00
parent 2e906f9b8a
commit e8af8f7792
14 changed files with 320 additions and 240 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/Signature.cpp
+1 -1
View File
@@ -120,7 +120,7 @@ void Signature::addLink(const Link & link)
{
UDEBUG("Add link %d to %d (type=%d var=%f,%f)", link.to(), this->id(), (int)link.type(), link.transVariance(), link.rotVariance());
UASSERT_MSG(link.from() == this->id(), uFormat("%d->%d for signature %d (type=%d)", link.from(), link.to(), this->id(), link.type()).c_str());
UASSERT_MSG(link.to() != this->id(), uFormat("%d->%d for signature %d (type=%d)", link.from(), link.to(), this->id(), link.type()).c_str());
UASSERT_MSG((link.to() != this->id()) || link.type()==Link::kPosePrior, uFormat("%d->%d for signature %d (type=%d)", link.from(), link.to(), this->id(), link.type()).c_str());
std::pair<std::map<int, Link>::iterator, bool> pair = _links.insert(std::make_pair(link.to(), link));
UASSERT_MSG(pair.second, uFormat("Link %d (type=%d) already added to signature %d!", link.to(), link.type(), this->id()).c_str());
_linksModified = true;