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

Memory: Added parameter Mem/UseOdomGravity, adding kGravity links when creating a node if IMU is present or Mem/UseOdomGravity is set. Fixed OpenCV4 related build errors on stereo fisheye rectification code. Signature: changed links from map to multimap to support having multiple self references (prior, gravity constraints...). DBReader: publish IMU orientation if a gravity link is detected.

Browse Source
This commit is contained in:
matlabbe
2019-05-31 15:36:35 -04:00
parent 71f7515775
commit e887d462ce
28 changed files with 1264 additions and 344 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/Signature.cpp
+8 -8
View File
@@ -120,9 +120,9 @@ 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()) || 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());
UASSERT_MSG((link.to() != this->id()) || link.type()==Link::kPosePrior || link.type()==Link::kGravity, uFormat("%d->%d for signature %d (type=%d)", link.from(), link.to(), this->id(), link.type()).c_str());
UASSERT_MSG(link.to() == this->id() || _links.find(link.to()) == _links.end(), uFormat("Link %d (type=%d) already added to signature %d!", link.to(), link.type(), this->id()).c_str());
_links.insert(std::make_pair(link.to(), link));
_linksModified = true;
}
@@ -133,11 +133,11 @@ bool Signature::hasLink(int idTo) const
void Signature::changeLinkIds(int idFrom, int idTo)
{
std::map<int, Link>::iterator iter = _links.find(idFrom);
if(iter != _links.end())
std::multimap<int, Link>::iterator iter = _links.find(idFrom);
while(iter != _links.end() && iter->first == idFrom)
{
Link link = iter->second;
_links.erase(iter);
_links.erase(iter++);
link.setTo(idTo);
_links.insert(std::make_pair(idTo, link));
_linksModified = true;
@@ -164,7 +164,7 @@ void Signature::removeLink(int idTo)
void Signature::removeVirtualLinks()
{
for(std::map<int, Link>::iterator iter=_links.begin(); iter!=_links.end();)
for(std::multimap<int, Link>::iterator iter=_links.begin(); iter!=_links.end();)
{
if(iter->second.type() == Link::kVirtualClosure)
{
@@ -276,7 +276,7 @@ cv::Mat Signature::getPoseCovariance() const
cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1);
if(_links.size())
{
for(std::map<int, Link>::const_iterator iter = _links.begin(); iter!=_links.end(); ++iter)
for(std::multimap<int, Link>::const_iterator iter = _links.begin(); iter!=_links.end(); ++iter)
{
if(iter->second.kNeighbor)
{