i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 09:30:25 +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

12
corelib/src/DBDriver.cpp
View File

@@ -762,7 +762,7 @@ bool DBDriver::getNodeInfo(
return found;
}
void DBDriver::loadLinks(int signatureId, std::map<int, Link> & links, Link::Type type) const
void DBDriver::loadLinks(int signatureId, std::multimap<int, Link> & links, Link::Type type) const
{
bool found = false;
// look in the trash
@@ -782,7 +782,7 @@ void DBDriver::loadLinks(int signatureId, std::map<int, Link> & links, Link::Typ
}
if(type == Link::kLandmark || type == Link::kAllWithLandmarks)
{
uInsert(links, s->getLandmarks());
links.insert(s->getLandmarks().begin(), s->getLandmarks().end());
}
found = true;
}
@@ -1219,11 +1219,11 @@ void DBDriver::generateGraph(
if(otherSignatures.find(*i) == otherSignatures.end())
{
int id = *i;
std::map<int, Link> links;
std::multimap<int, Link> links;
this->loadLinks(id, links);
int weight = 0;
this->getWeight(id, weight);
for(std::map<int, Link>::iterator iter = links.begin(); iter!=links.end(); ++iter)
for(std::multimap<int, Link>::iterator iter = links.begin(); iter!=links.end(); ++iter)
{
int weightNeighbor = 0;
if(otherSignatures.find(iter->first) == otherSignatures.end())
@@ -1281,9 +1281,9 @@ void DBDriver::generateGraph(
if(ids.find(i->first) != ids.end())
{
int id = i->second->id();
const std::map<int, Link> & links = i->second->getLinks();
const std::multimap<int, Link> & links = i->second->getLinks();
int weight = i->second->getWeight();
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)
{
int weightNeighbor = 0;
const Signature * s = uValue(otherSignatures, iter->first, (Signature*)0);

10
corelib/src/DBDriverSqlite3.cpp
View File

@@ -3605,7 +3605,7 @@ void DBDriverSqlite3::loadWordsQuery(const std::set<int> & wordIds, std::list<Vi
void DBDriverSqlite3::loadLinksQuery(
int signatureId,
std::map<int, Link> & links,
std::multimap<int, Link> & links,
Link::Type typeIn) const
{
links.clear();
@@ -4041,8 +4041,8 @@ void DBDriverSqlite3::updateQuery(const std::list<Signature *> & nodes, bool upd
if((*j)->isLinksModified())
{
// Save links
const std::map<int, Link> & links = (*j)->getLinks();
for(std::map<int, Link>::const_iterator i=links.begin(); i!=links.end(); ++i)
const std::multimap<int, Link> & links = (*j)->getLinks();
for(std::multimap<int, Link>::const_iterator i=links.begin(); i!=links.end(); ++i)
{
stepLink(ppStmt, i->second);
}
@@ -4155,8 +4155,8 @@ void DBDriverSqlite3::saveQuery(const std::list<Signature *> & signatures)
for(std::list<Signature *>::const_iterator jter=signatures.begin(); jter!=signatures.end(); ++jter)
{
// Save links
const std::map<int, Link> & links = (*jter)->getLinks();
for(std::map<int, Link>::const_iterator i=links.begin(); i!=links.end(); ++i)
const std::multimap<int, Link> & links = (*jter)->getLinks();
for(std::multimap<int, Link>::const_iterator i=links.begin(); i!=links.end(); ++i)
{
stepLink(ppStmt, i->second);
}

30
corelib/src/DBReader.cpp
View File

@@ -344,7 +344,7 @@ SensorData DBReader::getNextData(CameraInfo * info)
Transform globalPose;
cv::Mat globalPoseCov;
std::map<int, Link> priorLinks;
std::multimap<int, Link> priorLinks;
_dbDriver->loadLinks(*_currentId, priorLinks, Link::kPosePrior);
if( priorLinks.size() &&
!priorLinks.begin()->second.transform().isNull() &&
@@ -355,10 +355,21 @@ SensorData DBReader::getNextData(CameraInfo * info)
globalPoseCov = priorLinks.begin()->second.infMatrix().inv();
}
Transform gravityTransform;
std::multimap<int, Link> gravityLinks;
_dbDriver->loadLinks(*_currentId, gravityLinks, Link::kGravity);
if( gravityLinks.size() &&
!gravityLinks.begin()->second.transform().isNull() &&
gravityLinks.begin()->second.infMatrix().cols == 6 &&
gravityLinks.begin()->second.infMatrix().rows == 6)
{
gravityTransform = gravityLinks.begin()->second.transform();
}
cv::Mat infMatrix = cv::Mat::eye(6,6,CV_64FC1);
if(!_odometryIgnored)
{
std::map<int, Link> links;
std::multimap<int, Link> links;
_dbDriver->loadLinks(*_currentId, links, Link::kNeighbor);
if(links.size() && links.begin()->first < *_currentId)
{
@@ -467,13 +478,24 @@ SensorData DBReader::getNextData(CameraInfo * info)
{
data.setGlobalPose(globalPose, globalPoseCov);
}
if(!gravityTransform.isNull())
{
Eigen::Quaterniond q = gravityTransform.getQuaterniond();
data.setIMU(IMU(
cv::Vec4d(q.x(), q.y(), q.z(), q.w()), cv::Mat::eye(3,3,CV_64FC1),
cv::Vec3d(), cv::Mat(),
cv::Vec3d(), cv::Mat(),
Transform::getIdentity())); // we assume that gravity links are already transformed in base_link
}
UDEBUG("Laser=%d RGB/Left=%d Depth/Right=%d, Grid=%d, UserData=%d",
UDEBUG("Laser=%d RGB/Left=%d Depth/Right=%d, Grid=%d, UserData=%d, GlobalPose=%d, IMU=%d",
data.laserScanRaw().isEmpty()?0:1,
data.imageRaw().empty()?0:1,
data.depthOrRightRaw().empty()?0:1,
data.gridCellSize()==0.0f?0:1,
data.userDataRaw().empty()?0:1);
data.userDataRaw().empty()?0:1,
globalPose.isNull()?0:1,
gravityTransform.isNull()?0:1);
cv::Mat descriptors;
if(!s->getWordsDescriptors().empty())

20
corelib/src/Graph.cpp
View File

@@ -1031,12 +1031,13 @@ std::multimap<int, Link>::const_iterator findLink(
const std::multimap<int, Link> & links,
int from,
int to,
bool checkBothWays)
bool checkBothWays,
Link::Type type)
{
std::multimap<int, Link>::const_iterator iter = links.find(from);
while(iter != links.end() && iter->first == from)
{
if(iter->second.to() == to)
if(iter->second.to() == to && (type==Link::kUndef || type == iter->second.type()))
{
return iter;
}
@@ -1049,7 +1050,7 @@ std::multimap<int, Link>::const_iterator findLink(
iter = links.find(to);
while(iter != links.end() && iter->first == to)
{
if(iter->second.to() == from)
if(iter->second.to() == from && (type==Link::kUndef || type == iter->second.type()))
{
return iter;
}
@@ -1146,7 +1147,14 @@ std::map<int, Link> filterLinks(
std::map<int, Link> output;
for(std::map<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
{
if(iter->second.type() != filteredType)
if(filteredType == Link::kSelfRefLink)
{
if(iter->second.from() != iter->second.to())
{
output.insert(*iter);
}
}
else if(iter->second.type() != filteredType)
{
output.insert(*iter);
}
@@ -1897,7 +1905,7 @@ std::list<std::pair<int, Transform> > computePath(
}
// lookup neighbors
std::map<int, Link> links;
std::multimap<int, Link> links;
if(allLinks.size() == 0)
{
links = memory->getLinks(currentNode->id(), lookInDatabase, true);
@@ -1911,7 +1919,7 @@ std::list<std::pair<int, Transform> > computePath(
links.insert(std::make_pair(iter->second.to(), iter->second));
}
}
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.from() != iter->second.to())
{

247
corelib/src/Memory.cpp
View File

@@ -102,6 +102,7 @@ Memory::Memory(const ParametersMap & parameters) :
_rehearsalMaxAngle(Parameters::defaultRGBDAngularUpdate()),
_rehearsalWeightIgnoredWhileMoving(Parameters::defaultMemRehearsalWeightIgnoredWhileMoving()),
_useOdometryFeatures(Parameters::defaultMemUseOdomFeatures()),
_useOdometryGravity(Parameters::defaultMemUseOdomGravity()),
_createOccupancyGrid(Parameters::defaultRGBDCreateOccupancyGrid()),
_visMaxFeatures(Parameters::defaultVisMaxFeatures()),
_imagesAlreadyRectified(Parameters::defaultRtabmapImagesAlreadyRectified()),
@@ -561,6 +562,7 @@ void Memory::parseParameters(const ParametersMap & parameters)
Parameters::parse(params, Parameters::kRGBDAngularUpdate(), _rehearsalMaxAngle);
Parameters::parse(params, Parameters::kMemRehearsalWeightIgnoredWhileMoving(), _rehearsalWeightIgnoredWhileMoving);
Parameters::parse(params, Parameters::kMemUseOdomFeatures(), _useOdometryFeatures);
Parameters::parse(params, Parameters::kMemUseOdomGravity(), _useOdometryGravity);
Parameters::parse(params, Parameters::kRGBDCreateOccupancyGrid(), _createOccupancyGrid);
Parameters::parse(params, Parameters::kVisMaxFeatures(), _visMaxFeatures);
Parameters::parse(params, Parameters::kRtabmapImagesAlreadyRectified(), _imagesAlreadyRectified);
@@ -1040,9 +1042,9 @@ void Memory::moveSignatureToWMFromSTM(int id, int * reducedTo)
if(_reduceGraph)
{
bool merge = false;
const std::map<int, Link> & links = s->getLinks();
const std::multimap<int, Link> & links = s->getLinks();
std::map<int, Link> neighbors;
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(!merge)
{
@@ -1072,7 +1074,7 @@ void Memory::moveSignatureToWMFromSTM(int id, int * reducedTo)
{
if(s->getLabel().empty())
{
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)
{
merge = true;
Signature * sTo = this->_getSignature(iter->first);
@@ -1101,8 +1103,8 @@ void Memory::moveSignatureToWMFromSTM(int id, int * reducedTo)
}
//remove neighbor links
std::map<int, Link> linksCopy = links;
for(std::map<int, Link>::iterator iter=linksCopy.begin(); iter!=linksCopy.end(); ++iter)
std::multimap<int, Link> linksCopy = links;
for(std::multimap<int, Link>::iterator iter=linksCopy.begin(); iter!=linksCopy.end(); ++iter)
{
if(iter->second.type() == Link::kNeighbor ||
iter->second.type() == Link::kNeighborMerged)
@@ -1146,16 +1148,16 @@ const VWDictionary * Memory::getVWDictionary() const
return _vwd;
}
std::map<int, Link> Memory::getNeighborLinks(
std::multimap<int, Link> Memory::getNeighborLinks(
int signatureId,
bool lookInDatabase) const
{
std::map<int, Link> links;
std::multimap<int, Link> links;
Signature * s = uValue(_signatures, signatureId, (Signature*)0);
if(s)
{
const std::map<int, Link> & allLinks = s->getLinks();
for(std::map<int, Link>::const_iterator iter = allLinks.begin(); iter!=allLinks.end(); ++iter)
const std::multimap<int, Link> & allLinks = s->getLinks();
for(std::multimap<int, Link>::const_iterator iter = allLinks.begin(); iter!=allLinks.end(); ++iter)
{
if(iter->second.type() == Link::kNeighbor ||
iter->second.type() == Link::kNeighborMerged)
@@ -1166,14 +1168,13 @@ std::map<int, Link> Memory::getNeighborLinks(
}
else if(lookInDatabase && _dbDriver)
{
std::map<int, Link> neighbors;
_dbDriver->loadLinks(signatureId, neighbors);
for(std::map<int, Link>::iterator iter=neighbors.begin(); iter!=neighbors.end();)
_dbDriver->loadLinks(signatureId, links);
for(std::multimap<int, Link>::iterator iter=links.begin(); iter!=links.end();)
{
if(iter->second.type() != Link::kNeighbor &&
iter->second.type() != Link::kNeighborMerged)
{
neighbors.erase(iter++);
links.erase(iter++);
}
else
{
@@ -1188,20 +1189,20 @@ std::map<int, Link> Memory::getNeighborLinks(
return links;
}
std::map<int, Link> Memory::getLoopClosureLinks(
std::multimap<int, Link> Memory::getLoopClosureLinks(
int signatureId,
bool lookInDatabase) const
{
const Signature * s = this->getSignature(signatureId);
std::map<int, Link> loopClosures;
std::multimap<int, Link> loopClosures;
if(s)
{
const std::map<int, Link> & allLinks = s->getLinks();
for(std::map<int, Link>::const_iterator iter = allLinks.begin(); iter!=allLinks.end(); ++iter)
const std::multimap<int, Link> & allLinks = s->getLinks();
for(std::multimap<int, Link>::const_iterator iter = allLinks.begin(); iter!=allLinks.end(); ++iter)
{
if(iter->second.type() != Link::kNeighbor &&
iter->second.type() != Link::kNeighborMerged &&
iter->second.type() != Link::kPosePrior &&
iter->second.from() != iter->second.to() &&
iter->second.type() != Link::kUndef)
{
loopClosures.insert(*iter);
@@ -1211,7 +1212,7 @@ std::map<int, Link> Memory::getLoopClosureLinks(
else if(lookInDatabase && _dbDriver)
{
_dbDriver->loadLinks(signatureId, loopClosures);
for(std::map<int, Link>::iterator iter=loopClosures.begin(); iter!=loopClosures.end();)
for(std::multimap<int, Link>::iterator iter=loopClosures.begin(); iter!=loopClosures.end();)
{
if(iter->second.type() == Link::kNeighbor ||
iter->second.type() == Link::kNeighborMerged ||
@@ -1228,12 +1229,12 @@ std::map<int, Link> Memory::getLoopClosureLinks(
return loopClosures;
}
std::map<int, Link> Memory::getLinks(
std::multimap<int, Link> Memory::getLinks(
int signatureId,
bool lookInDatabase,
bool withLandmarks) const
{
std::map<int, Link> links;
std::multimap<int, Link> links;
if(signatureId > 0)
{
Signature * s = uValue(_signatures, signatureId, (Signature*)0);
@@ -1242,7 +1243,7 @@ std::map<int, Link> Memory::getLinks(
links = s->getLinks();
if(withLandmarks)
{
uInsert(links, s->getLandmarks());
links.insert(s->getLandmarks().begin(), s->getLandmarks().end());
}
}
else if(lookInDatabase && _dbDriver)
@@ -1372,9 +1373,9 @@ std::map<int, int> Memory::getNeighborsId(
//UDEBUG("Added %d with margin %d", *jter, m);
// Look up in STM/WM if all ids are here, if not... load them from the database
const Signature * s = this->getSignature(*jter);
std::map<int, Link> tmpLinks;
std::multimap<int, Link> tmpLinks;
std::map<int, Link> tmpLandmarks;
const std::map<int, Link> * links = &tmpLinks;
const std::multimap<int, Link> * links = &tmpLinks;
const std::map<int, Link> * landmarks = &tmpLandmarks;
if(s)
{
@@ -1402,7 +1403,7 @@ std::map<int, int> Memory::getNeighborsId(
_dbDriver->loadLinks(*jter, tmpLinks, ignoreLoopIds?Link::kAllWithoutLandmarks:Link::kAllWithLandmarks);
if(!ignoreLoopIds)
{
for(std::map<int, Link>::iterator kter=tmpLinks.begin(); kter!=tmpLinks.end();)
for(std::multimap<int, Link>::iterator kter=tmpLinks.begin(); kter!=tmpLinks.end();)
{
if(kter->first < 0)
{
@@ -1422,7 +1423,7 @@ std::map<int, int> Memory::getNeighborsId(
}
// links
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( !uContains(ids, iter->first) && ignoredIds.find(iter->first) == ignoredIds.end())
{
@@ -1525,8 +1526,6 @@ std::map<int, float> Memory::getNeighborsIdRadius(
//UDEBUG("Added %d with margin %d", *jter, m);
// Look up in STM/WM if all ids are here, if not... load them from the database
const Signature * s = this->getSignature(*jter);
std::map<int, Link> tmpLinks;
const std::map<int, Link> * links = &tmpLinks;
if(s)
{
const Transform & t = optimizedPoses.at(*jter);
@@ -1537,17 +1536,15 @@ std::map<int, float> Memory::getNeighborsIdRadius(
ids.insert(std::pair<int, float>(*jter,distanceSqrd));
}
links = &s->getLinks();
}
// links
for(std::map<int, Link>::const_iterator iter=links->begin(); iter!=links->end(); ++iter)
{
if(!uContains(ids, iter->first) &&
uContains(optimizedPoses, iter->first) &&
iter->second.type()!=Link::kVirtualClosure)
// links
for(std::multimap<int, Link>::const_iterator iter=s->getLinks().begin(); iter!=s->getLinks().end(); ++iter)
{
nextMargin.insert(iter->first);
if(!uContains(ids, iter->first) &&
uContains(optimizedPoses, iter->first) &&
iter->second.type()!=Link::kVirtualClosure)
{
nextMargin.insert(iter->first);
}
}
}
}
@@ -2316,8 +2313,8 @@ void Memory::moveToTrash(Signature * s, bool keepLinkedToGraph, std::list<int> *
UASSERT_MSG(this->isInSTM(s->id()),
uFormat("Deleting location (%d) outside the "
"STM is not implemented!", s->id()).c_str());
const std::map<int, Link> & links = s->getLinks();
for(std::map<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
const std::multimap<int, Link> & links = s->getLinks();
for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
{
if(iter->second.from() != iter->second.to() && iter->first > 0)
{
@@ -2585,7 +2582,7 @@ void Memory::removeLink(int oldId, int newId)
if(oldS->hasLink(newS->id()) && newS->hasLink(oldS->id()))
{
Link::Type type = oldS->getLinks().at(newS->id()).type();
Link::Type type = oldS->getLinks().find(newS->id())->second.type();
if(type == Link::kGlobalClosure && newS->getWeight() > 0)
{
// adjust the weight
@@ -2607,7 +2604,7 @@ void Memory::removeLink(int oldId, int newId)
{
if(iter->second.type() != Link::kNeighbor &&
iter->second.type() != Link::kNeighborMerged &&
iter->second.type() != Link::kPosePrior &&
iter->second.from() != iter->second.to() &&
iter->first < newS->id())
{
noChildrenAnymore = false;
@@ -2779,7 +2776,7 @@ Transform Memory::computeTransform(
std::multimap<int, cv::KeyPoint> wordsMap;
std::multimap<int, cv::Mat> wordsDescriptorsMap;
const std::map<int, Link> & links = fromS.getLinks();
const std::multimap<int, Link> & links = fromS.getLinks();
{
const std::map<int, cv::Point3f> & words3 = uMultimapToMapUnique(fromS.getWords3());
UDEBUG("fromS.getWords3()=%d uniques=%d", (int)fromS.getWords3().size(), (int)words3.size());
@@ -2795,22 +2792,25 @@ Transform Memory::computeTransform(
}
UDEBUG("words3DMap=%d", (int)words3DMap.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)
{
int id = iter->first;
const Signature * s = this->getSignature(id);
if(s)
if(id != fromS.id())
{
const std::map<int, cv::Point3f> & words3 = uMultimapToMapUnique(s->getWords3());
for(std::map<int, cv::Point3f>::const_iterator jter=words3.begin(); jter!=words3.end(); ++jter)
const Signature * s = this->getSignature(id);
if(s)
{
if( jter->first > 0 &&
util3d::isFinite(jter->second) &&
words3DMap.find(jter->first) == words3DMap.end())
const std::map<int, cv::Point3f> & words3 = uMultimapToMapUnique(s->getWords3());
for(std::map<int, cv::Point3f>::const_iterator jter=words3.begin(); jter!=words3.end(); ++jter)
{
words3DMap.insert(std::make_pair(jter->first, util3d::transformPoint(jter->second, iter->second.transform())));
wordsMap.insert(*s->getWords().find(jter->first));
wordsDescriptorsMap.insert(*s->getWordsDescriptors().find(jter->first));
if( jter->first > 0 &&
util3d::isFinite(jter->second) &&
words3DMap.find(jter->first) == words3DMap.end())
{
words3DMap.insert(std::make_pair(jter->first, util3d::transformPoint(jter->second, iter->second.transform())));
wordsMap.insert(*s->getWords().find(jter->first));
wordsDescriptorsMap.insert(*s->getWordsDescriptors().find(jter->first));
}
}
}
}
@@ -2831,65 +2831,68 @@ Transform Memory::computeTransform(
std::map<int, CameraModel> bundleModels;
std::map<int, std::map<int, FeatureBA> > wordReferences;
std::map<int, Link> links = fromS.getLinks();
std::multimap<int, Link> links = fromS.getLinks();
links.insert(std::make_pair(toS.id(), Link(fromS.id(), toS.id(), Link::kGlobalClosure, transform, info->covariance.inv())));
links.insert(std::make_pair(fromS.id(), Link()));
for(std::map<int, Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
for(std::multimap<int, Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
{
int id = iter->first;
const Signature * s;
if(id == tmpTo.id())
if(id != fromS.id())
{
s = &tmpTo; // reuse matched words
}
else
{
s = this->getSignature(id);
}
if(s)
{
CameraModel model;
if(s->sensorData().cameraModels().size() == 1 && s->sensorData().cameraModels().at(0).isValidForProjection())
const Signature * s;
if(id == tmpTo.id())
{
model = s->sensorData().cameraModels()[0];
}
else if(s->sensorData().stereoCameraModel().isValidForProjection())
{
model = s->sensorData().stereoCameraModel().left();
// Set Tx for stereo BA
model = CameraModel(model.fx(),
model.fy(),
model.cx(),
model.cy(),
model.localTransform(),
-s->sensorData().stereoCameraModel().baseline()*model.fx());
s = &tmpTo; // reuse matched words
}
else
{
UFATAL("no valid camera model to use local bundle adjustment on loop closure!");
s = this->getSignature(id);
}
bundleModels.insert(std::make_pair(id, model));
Transform invLocalTransform = model.localTransform().inverse();
if(iter->second.isValid())
if(s)
{
bundleLinks.insert(std::make_pair(iter->second.from(), iter->second));
bundlePoses.insert(std::make_pair(id, iter->second.transform()));
}
else
{
bundlePoses.insert(std::make_pair(id, Transform::getIdentity()));
}
const std::map<int,cv::KeyPoint> & words = uMultimapToMapUnique(s->getWords());
for(std::map<int, cv::KeyPoint>::const_iterator jter=words.begin(); jter!=words.end(); ++jter)
{
if(points3DMap.find(jter->first)!=points3DMap.end() &&
(id == tmpTo.id() || jter->first > 0))
CameraModel model;
if(s->sensorData().cameraModels().size() == 1 && s->sensorData().cameraModels().at(0).isValidForProjection())
{
std::multimap<int, cv::Point3f>::const_iterator kter = s->getWords3().find(jter->first);
cv::Point3f pt3d = util3d::transformPoint(kter->second, invLocalTransform);
wordReferences.insert(std::make_pair(jter->first, std::map<int, FeatureBA>()));
wordReferences.at(jter->first).insert(std::make_pair(id, FeatureBA(jter->second, pt3d.z)));
model = s->sensorData().cameraModels()[0];
}
else if(s->sensorData().stereoCameraModel().isValidForProjection())
{
model = s->sensorData().stereoCameraModel().left();
// Set Tx for stereo BA
model = CameraModel(model.fx(),
model.fy(),
model.cx(),
model.cy(),
model.localTransform(),
-s->sensorData().stereoCameraModel().baseline()*model.fx());
}
else
{
UFATAL("no valid camera model to use local bundle adjustment on loop closure!");
}
bundleModels.insert(std::make_pair(id, model));
Transform invLocalTransform = model.localTransform().inverse();
if(iter->second.isValid())
{
bundleLinks.insert(std::make_pair(iter->second.from(), iter->second));
bundlePoses.insert(std::make_pair(id, iter->second.transform()));
}
else
{
bundlePoses.insert(std::make_pair(id, Transform::getIdentity()));
}
const std::map<int,cv::KeyPoint> & words = uMultimapToMapUnique(s->getWords());
for(std::map<int, cv::KeyPoint>::const_iterator jter=words.begin(); jter!=words.end(); ++jter)
{
if(points3DMap.find(jter->first)!=points3DMap.end() &&
(id == tmpTo.id() || jter->first > 0))
{
std::multimap<int, cv::Point3f>::const_iterator kter = s->getWords3().find(jter->first);
cv::Point3f pt3d = util3d::transformPoint(kter->second, invLocalTransform);
wordReferences.insert(std::make_pair(jter->first, std::map<int, FeatureBA>()));
wordReferences.at(jter->first).insert(std::make_pair(id, FeatureBA(jter->second, pt3d.z)));
}
}
}
}
@@ -3231,7 +3234,7 @@ void Memory::updateLink(const Link & link, bool updateInDatabase)
{
if(fromS->hasLink(link.to()) && toS->hasLink(link.from()))
{
Link::Type oldType = fromS->getLinks().at(link.to()).type();
Link::Type oldType = fromS->getLinks().find(link.to())->second.type();
fromS->removeLink(link.to());
toS->removeLink(link.from());
@@ -3297,8 +3300,8 @@ void Memory::removeVirtualLinks(int signatureId)
Signature * s = this->_getSignature(signatureId);
if(s)
{
const std::map<int, Link> & links = s->getLinks();
for(std::map<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
const std::multimap<int, Link> & links = s->getLinks();
for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
{
if(iter->second.type() == Link::kVirtualClosure)
{
@@ -3557,15 +3560,15 @@ bool Memory::rehearsalMerge(int oldId, int newId)
if(fullMerge)
{
//remove mutual links
Link newToOldLink = newS->getLinks().at(oldS->id());
Link newToOldLink = newS->getLinks().find(oldS->id())->second;
oldS->removeLink(newId);
newS->removeLink(oldId);
if(_idUpdatedToNewOneRehearsal)
{
// redirect neighbor links
const std::map<int, Link> & links = oldS->getLinks();
for(std::map<int, Link>::const_iterator iter = links.begin(); iter!=links.end(); ++iter)
const std::multimap<int, Link> & links = oldS->getLinks();
for(std::multimap<int, Link>::const_iterator iter = links.begin(); iter!=links.end(); ++iter)
{
if(iter->second.from() != iter->second.to())
{
@@ -4601,7 +4604,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
}
Landmarks landmarks = data.landmarks();
if(_detectMarkers)
if(_detectMarkers && !isIntermediateNode)
{
UDEBUG("Detecting markers...");
if(landmarks.empty())
@@ -5069,9 +5072,9 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
}
}
// prior
if(!isIntermediateNode)
{
// prior
if(!data.globalPose().isNull() && data.globalPoseCovariance().cols==6 && data.globalPoseCovariance().rows==6 && data.globalPoseCovariance().cols==CV_64FC1)
{
s->addLink(Link(s->id(), s->id(), Link::kPosePrior, data.globalPose(), data.globalPoseCovariance().inv()));
@@ -5105,6 +5108,26 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
UERROR("Invalid GPS error value (%f m), must be > 0 m.", data.gps().error());
}
}
// IMU / Gravity constraint
if(_useOdometryGravity && !pose.isNull())
{
s->addLink(Link(s->id(), s->id(), Link::kGravity, pose.rotation()));
UDEBUG("Added gravity constraint from odom pose: %s", pose.rotation().prettyPrint().c_str());
}
else if(!data.imu().localTransform().isNull() &&
(data.imu().orientation()[0] != 0 ||
data.imu().orientation()[1] != 0 ||
data.imu().orientation()[2] != 0 ||
data.imu().orientation()[3] != 0))
{
Transform orientation(0,0,0, data.imu().orientation()[0], data.imu().orientation()[1], data.imu().orientation()[2], data.imu().orientation()[3]);
orientation*=data.imu().localTransform().rotation().inverse();
s->addLink(Link(s->id(), s->id(), Link::kGravity, orientation));
UDEBUG("Added gravity constraint: %s", orientation.prettyPrint().c_str());
}
}
//landmarks
@@ -5404,8 +5427,8 @@ void Memory::getMetricConstraints(
{
if(uContains(poses, *iter))
{
std::map<int, Link> tmpLinks = getLinks(*iter, lookInDatabase, true);
for(std::map<int, Link>::iterator jter=tmpLinks.begin(); jter!=tmpLinks.end(); ++jter)
std::multimap<int, Link> tmpLinks = getLinks(*iter, lookInDatabase, true);
for(std::multimap<int, Link>::iterator jter=tmpLinks.begin(); jter!=tmpLinks.end(); ++jter)
{
if( jter->second.isValid() &&
graph::findLink(links, *iter, jter->first) == links.end() &&
@@ -5424,9 +5447,9 @@ void Memory::getMetricConstraints(
{
// skip to next neighbor, well we assume that bad signatures
// are only linked by max 2 neighbor links.
std::map<int, Link> n = this->getNeighborLinks(s->id(), false);
std::multimap<int, Link> n = this->getNeighborLinks(s->id(), false);
UASSERT(n.size() <= 2);
std::map<int, Link>::iterator uter = n.upper_bound(s->id());
std::multimap<int, Link>::iterator uter = n.upper_bound(s->id());
if(uter != n.end())
{
const Signature * s2 = this->getSignature(uter->first);

128
corelib/src/Rtabmap.cpp
View File

@@ -1172,7 +1172,7 @@ bool Rtabmap::process(
//============================================================
// Minimum displacement required to add to Memory
//============================================================
const std::map<int, Link> & links = signature->getLinks();
const std::multimap<int, Link> & links = signature->getLinks();
if(links.size() && links.begin()->second.type() == Link::kNeighbor)
{
// don't do this if there are intermediate nodes
@@ -1292,9 +1292,9 @@ bool Rtabmap::process(
UASSERT(oldS->hasLink(signature->id()));
UASSERT(uContains(_optimizedPoses, oldId));
statistics_.addStatistic(Statistics::kNeighborLinkRefiningVariance(), oldS->getLinks().at(signature->id()).transVariance());
statistics_.addStatistic(Statistics::kNeighborLinkRefiningVariance(), oldS->getLinks().find(signature->id())->second.transVariance());
newPose = _optimizedPoses.at(oldId) * oldS->getLinks().at(signature->id()).transform();
newPose = _optimizedPoses.at(oldId) * oldS->getLinks().find(signature->id())->second.transform();
_mapCorrection = newPose * signature->getPose().inverse();
if(_mapCorrection.getNormSquared() > 0.001f && _optimizeFromGraphEnd)
{
@@ -1667,7 +1667,7 @@ bool Rtabmap::process(
{
float loopThr = _loopThr;
if((_startNewMapOnLoopClosure || !_memory->isIncremental()) &&
graph::filterLinks(signature->getLinks(), Link::kPosePrior).size() == 0 && // alone in the current map
graph::filterLinks(signature->getLinks(), Link::kSelfRefLink).size() == 0 && // alone in the current map
_memory->getWorkingMem().size()>1 && // should have an old map (beside virtual signature)
(int)_memory->getWorkingMem().size()<=_memory->getMaxStMemSize() &&
_rgbdSlamMode)
@@ -2038,8 +2038,8 @@ bool Rtabmap::process(
{
// If there is a change of direction, better to be retrieving
// ALL nearest signatures than only newest neighbors
const std::map<int, Link> & links = s->getLinks();
for(std::map<int, Link>::const_reverse_iterator jter=links.rbegin();
const std::multimap<int, Link> & links = s->getLinks();
for(std::multimap<int, Link>::const_reverse_iterator jter=links.rbegin();
jter!=links.rend() && retrievalLocalIds.size() < _maxLocalRetrieved;
++jter)
{
@@ -2175,7 +2175,7 @@ bool Rtabmap::process(
// Landmark
//============================================================
int landmarkDetected = 0;
int landmarkDetectedNodeRef = 0;
std::set<int> landmarkDetectedNodesRef;
if(!signature->getLandmarks().empty())
{
for(std::map<int, Link>::const_iterator iter=signature->getLandmarks().begin(); iter!=signature->getLandmarks().end(); ++iter)
@@ -2184,8 +2184,8 @@ bool Rtabmap::process(
_memory->getLandmarksInvertedIndex().find(iter->first)->second.size()>1)
{
landmarkDetected = iter->first;
landmarkDetectedNodeRef = *_memory->getLandmarksInvertedIndex().find(iter->first)->second.begin();
UINFO("Landmark %d observed again! Seen the first time by node %d.", -iter->first, landmarkDetectedNodeRef);
landmarkDetectedNodesRef = _memory->getLandmarksInvertedIndex().find(iter->first)->second;
UINFO("Landmark %d observed again! Seen the first time by node %d.", -iter->first, *landmarkDetectedNodesRef.begin());
break;
}
}
@@ -2536,14 +2536,14 @@ bool Rtabmap::process(
(signature->hasLink(signature->id()) && !_graphOptimizer->priorsIgnored()) || // prior edge
proximityDetectionsInTimeFound>0 ||
landmarkDetected!=0 ||
((_memory->isIncremental() || graph::filterLinks(signature->getLinks(), Link::kPosePrior).size()) && // In localization mode, the new node should be linked
((_memory->isIncremental() || graph::filterLinks(signature->getLinks(), Link::kSelfRefLink).size()) && // In localization mode, the new node should be linked
signaturesRetrieved.size()))) // can be different map of the current one
{
UASSERT(uContains(_optimizedPoses, signature->id()));
//used in localization mode: filter virtual links
std::map<int, Link> localizationLinks = graph::filterLinks(signature->getLinks(), Link::kVirtualClosure);
localizationLinks = graph::filterLinks(localizationLinks, Link::kPosePrior);
std::multimap<int, Link> localizationLinks = graph::filterLinks(signature->getLinks(), Link::kVirtualClosure);
localizationLinks = graph::filterLinks(localizationLinks, Link::kSelfRefLink);
if(landmarkDetected!=0 && !_memory->isIncremental() && _optimizedPoses.find(landmarkDetected)!=_optimizedPoses.end())
{
UASSERT(uContains(signature->getLandmarks(), landmarkDetected));
@@ -2696,20 +2696,30 @@ bool Rtabmap::process(
//For landmarks, use transform against other node looking the landmark
// (because we don't assume that landmarks are aligned with gravity)
int landmarkId = loopId;
const Signature * loopS = _memory->getSignature(landmarkDetectedNodeRef);
UASSERT(!landmarkDetectedNodesRef.empty());
loopId = *landmarkDetectedNodesRef.begin();
const Signature * loopS = _memory->getSignature(loopId);
transform = transform * loopS->getLandmarks().at(landmarkId).transform().inverse();
loopId = landmarkDetectedNodeRef;
UASSERT(_optimizedPoses.find(loopId) != _optimizedPoses.end());
oldPose = _optimizedPoses.at(loopId);
}
float roll,pitch,yaw;
_memory->getSignature(loopId)->getPose().getEulerAngles(roll, pitch, yaw);
Transform targetRotation = signature->getPose().rotation()*transform.rotation();
targetRotation = Transform(0,0,0,roll,pitch,targetRotation.theta());
Transform error = transform.rotation().inverse() * signature->getPose().rotation().inverse() * targetRotation;
transform *= error;
const Signature * loopS = _memory->getSignature(loopId);
UASSERT(loopS !=0);
std::multimap<int, Link>::const_iterator iterGravityLoop = graph::findLink(loopS->getLinks(), loopS->id(), loopS->id(), false, Link::kGravity);
std::multimap<int, Link>::const_iterator iterGravitySign = graph::findLink(signature->getLinks(), signature->id(), signature->id(), false, Link::kGravity);
if(iterGravityLoop!=loopS->getLinks().end() &&
iterGravitySign!=signature->getLinks().end())
{
float roll,pitch,yaw;
iterGravityLoop->second.transform().getEulerAngles(roll, pitch, yaw);
Transform targetRotation = iterGravitySign->second.transform().rotation()*transform.rotation();
targetRotation = Transform(0,0,0,roll,pitch,targetRotation.theta());
Transform error = transform.rotation().inverse() * iterGravitySign->second.transform().rotation().inverse() * targetRotation;
transform *= error;
u = signature->getPose() * transform;
u = signature->getPose() * transform;
}
}
Transform up = u * oldPose.inverse();
for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
@@ -2736,19 +2746,32 @@ bool Rtabmap::process(
//For landmarks, use transform against other node looking the landmark
// (because we don't assume that landmarks are aligned with gravity)
int landmarkId = loopId;
const Signature * loopS = _memory->getSignature(landmarkDetectedNodeRef);
UASSERT(!landmarkDetectedNodesRef.empty());
loopId = *landmarkDetectedNodesRef.begin();
const Signature * loopS = _memory->getSignature(loopId);
transform = transform * loopS->getLandmarks().at(landmarkId).transform().inverse();
loopId = landmarkDetectedNodeRef;
}
float roll,pitch,yaw;
_memory->getSignature(loopId)->getPose().getEulerAngles(roll, pitch, yaw);
Transform targetRotation = signature->getPose().rotation()*transform.rotation();
targetRotation = Transform(0,0,0,roll,pitch,targetRotation.theta());
Transform error = transform.rotation().inverse() * signature->getPose().rotation().inverse() * targetRotation;
transform *= error;
const Signature * loopS = _memory->getSignature(loopId);
UASSERT(loopS !=0);
std::multimap<int, Link>::const_iterator iterGravityLoop = graph::findLink(loopS->getLinks(), loopS->id(), loopS->id(), false, Link::kGravity);
std::multimap<int, Link>::const_iterator iterGravitySign = graph::findLink(signature->getLinks(), signature->id(), signature->id(), false, Link::kGravity);
if(iterGravityLoop!=loopS->getLinks().end() &&
iterGravitySign!=signature->getLinks().end())
{
float roll,pitch,yaw;
iterGravityLoop->second.transform().getEulerAngles(roll, pitch, yaw);
Transform targetRotation = iterGravitySign->second.transform().rotation()*transform.rotation();
targetRotation = Transform(0,0,0,roll,pitch,targetRotation.theta());
Transform error = transform.rotation().inverse() * iterGravitySign->second.transform().rotation().inverse() * targetRotation;
transform *= error;
newPose = _optimizedPoses.at(loopId) * transform.inverse();
newPose = _optimizedPoses.at(loopId) * transform.inverse();
}
else
{
UWARN("Gravity link not found for %d and/or %d, localization won't be corrected with gravity.", loopId, signature->id());
}
}
_optimizedPoses.at(signature->id()) = newPose;
}
@@ -2930,7 +2953,7 @@ bool Rtabmap::process(
}
}
}
if(!hasPrior || _graphOptimizer->priorsIgnored())
if((!hasPrior || _graphOptimizer->priorsIgnored()) && _graphOptimizer->gravitySigma()==0.0f)
{
UERROR("Map correction should be identity when optimizing from the last node. T=%s", _mapCorrection.prettyPrint().c_str());
}
@@ -3010,7 +3033,7 @@ bool Rtabmap::process(
statistics_.addStatistic(Statistics::kLoopOptimization_error(), optimizationError);
statistics_.addStatistic(Statistics::kLoopOptimization_iterations(), optimizationIterations);
statistics_.addStatistic(Statistics::kLoopLandmark_detected(), -landmarkDetected);
statistics_.addStatistic(Statistics::kLoopLandmark_detected_node_ref(), landmarkDetectedNodeRef);
statistics_.addStatistic(Statistics::kLoopLandmark_detected_node_ref(), landmarkDetectedNodesRef.empty()?0:*landmarkDetectedNodesRef.begin());
statistics_.addStatistic(Statistics::kProximityTime_detections(), proximityDetectionsInTimeFound);
statistics_.addStatistic(Statistics::kProximitySpace_detections_added_visually(), proximityDetectionsAddedVisually);
@@ -3024,8 +3047,8 @@ bool Rtabmap::process(
if(_loopClosureHypothesis.first || lastProximitySpaceClosureId)
{
UASSERT(uContains(sLoop->getLinks(), signature->id()));
UINFO("Set loop closure transform = %s", sLoop->getLinks().at(signature->id()).transform().prettyPrint().c_str());
statistics_.setLoopClosureTransform(sLoop->getLinks().at(signature->id()).transform());
UINFO("Set loop closure transform = %s", sLoop->getLinks().find(signature->id())->second.transform().prettyPrint().c_str());
statistics_.setLoopClosureTransform(sLoop->getLinks().find(signature->id())->second.transform());
}
statistics_.setMapCorrection(_mapCorrection);
UINFO("Set map correction = %s", _mapCorrection.prettyPrint().c_str());
@@ -3097,7 +3120,8 @@ bool Rtabmap::process(
Signature lastSignatureData(signature->id());
Transform lastSignatureLocalizedPose;
if(_optimizedPoses.find(signature->id()) != _optimizedPoses.end() && graph::filterLinks(signature->getLinks(), Link::kPosePrior).size())
if(_optimizedPoses.find(signature->id()) != _optimizedPoses.end() &&
graph::filterLinks(signature->getLinks(), Link::kSelfRefLink).size())
{
// only if localized set it
lastSignatureLocalizedPose = _optimizedPoses.at(signature->id());
@@ -3126,7 +3150,7 @@ bool Rtabmap::process(
{
if(_startNewMapOnLoopClosure &&
_memory->isIncremental() && // only in mapping mode
graph::filterLinks(signature->getLinks(), Link::kPosePrior).size() == 0 && // alone in the current map
graph::filterLinks(signature->getLinks(), Link::kSelfRefLink).size() == 0 && // alone in the current map
(landmarkDetected == 0 || rejectedHypothesis) && // if we re not seeing a landmark from a previous map
_memory->getWorkingMem().size()>=2) // The working memory should not be empty (beside virtual signature)
{
@@ -3137,7 +3161,7 @@ bool Rtabmap::process(
}
else if(_startNewMapOnGoodSignature &&
(signature->getLandmarks().empty() && signature->isBadSignature()) &&
graph::filterLinks(signature->getLinks(), Link::kPosePrior).size() == 0) // alone in the current map
graph::filterLinks(signature->getLinks(), Link::kSelfRefLink).size() == 0) // alone in the current map
{
UWARN("Ignoring location %d because a good signature (with enough features or with a landmark detected) is required before starting a new map!",
signature->id());
@@ -3587,9 +3611,9 @@ void Rtabmap::rejectLastLoopClosure()
{
if(_memory && _memory->getStMem().find(getLastLocationId())!=_memory->getStMem().end())
{
std::map<int, Link> links = _memory->getLinks(getLastLocationId(), false);
std::multimap<int, Link> links = _memory->getLinks(getLastLocationId(), false);
bool linksRemoved = false;
for(std::map<int, Link>::iterator iter = links.begin(); iter!=links.end(); ++iter)
for(std::multimap<int, Link>::iterator iter = links.begin(); iter!=links.end(); ++iter)
{
if(iter->second.type() == Link::kGlobalClosure ||
iter->second.type() == Link::kLocalSpaceClosure ||
@@ -3872,8 +3896,8 @@ std::map<int, std::map<int, Transform> > Rtabmap::getPaths(const std::map<int, T
if(!valid)
{
// make sure it has a neighbor added to path
std::map<int, Link> links = _memory->getNeighborLinks(iter->first);
for(std::map<int, Link>::iterator kter=links.begin(); kter!=links.end() && !valid; ++kter)
std::multimap<int, Link> links = _memory->getNeighborLinks(iter->first);
for(std::multimap<int, Link>::iterator kter=links.begin(); kter!=links.end() && !valid; ++kter)
{
valid = path.find(kter->first) != path.end();
}
@@ -3980,12 +4004,6 @@ std::map<int, Transform> Rtabmap::optimizeGraph(
{
for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{
if(iter->first > 0 && _graphOptimizer->gravitySigma() > 0.0f)
{
// add odometry constraints
edgeConstraints.insert(std::make_pair(iter->first, Link(iter->first, iter->first, Link::kPoseOdom, iter->second)));
}
// Apply guess poses (if some)
std::map<int, Transform>::const_iterator foundGuess = guessPoses.find(iter->first);
if(foundGuess!=guessPoses.end())
@@ -4482,18 +4500,6 @@ int Rtabmap::detectMoreLoopClosures(
UASSERT(poses.find(fromId) != poses.end());
UASSERT_MSG(poses.find(from) != poses.end(), uFormat("id=%d poses=%d links=%d", from, (int)poses.size(), (int)links.size()).c_str());
UASSERT_MSG(poses.find(to) != poses.end(), uFormat("id=%d poses=%d links=%d", to, (int)poses.size(), (int)links.size()).c_str());
if(_graphOptimizer->gravitySigma() > 0.0f)
{
for(std::map<int, Transform>::iterator jter=poses.lower_bound(1); jter!=poses.end(); ++jter)
{
std::map<int, Signature>::iterator ster = signatures.find(iter->first);
if(ster != signatures.end() && !ster->second.getPose().isNull())
{
// add odometry constraints
linksIn.insert(std::make_pair(iter->first, Link(iter->first, iter->first, Link::kPoseOdom, ster->second.getPose())));
}
}
}
_graphOptimizer->getConnectedGraph(fromId, poses, linksIn, optimizedPoses, links);
UASSERT(optimizedPoses.find(fromId) != optimizedPoses.end());
UASSERT_MSG(optimizedPoses.find(from) != optimizedPoses.end(), uFormat("id=%d poses=%d links=%d", from, (int)optimizedPoses.size(), (int)links.size()).c_str());
@@ -5090,9 +5096,9 @@ void Rtabmap::updateGoalIndex()
UASSERT(_pathCurrentIndex < _path.size());
const Signature * currentIndexS = _memory->getSignature(_path[_pathCurrentIndex].first);
UASSERT_MSG(currentIndexS != 0, uFormat("_path[%d].first=%d", _pathCurrentIndex, _path[_pathCurrentIndex].first).c_str());
std::map<int, Link> links = currentIndexS->getLinks(); // make a copy
std::multimap<int, Link> links = currentIndexS->getLinks(); // make a copy
bool latestVirtualLinkFound = false;
for(std::map<int, Link>::reverse_iterator iter=links.rbegin(); iter!=links.rend(); ++iter)
for(std::multimap<int, Link>::reverse_iterator iter=links.rbegin(); iter!=links.rend(); ++iter)
{
if(iter->second.type() == Link::kVirtualClosure)
{

16
corelib/src/Signature.cpp
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)
{

6
corelib/src/VWDictionary.cpp
View File

@@ -1173,7 +1173,7 @@ std::vector<int> VWDictionary::findNN(const cv::Mat & queryIn) const
std::map<int, int> mapIndexIdNotIndexed;
std::vector<std::vector<cv::DMatch> > matchesNotIndexed;
if(_notIndexedWords.size())
if(!_notIndexedWords.empty())
{
cv::Mat dataNotIndexed = cv::Mat::zeros(_notIndexedWords.size(), query.cols, query.type());
unsigned int index = 0;
@@ -1198,12 +1198,10 @@ std::vector<int> VWDictionary::findNN(const cv::Mat & queryIn) const
{
descriptor = vw->getDescriptor();
}
UASSERT(vw != 0 && descriptor.cols == query.cols && descriptor.type() == query.type());
vw->getDescriptor().copyTo(dataNotIndexed.row(index));
descriptor.copyTo(dataNotIndexed.row(index));
mapIndexIdNotIndexed.insert(mapIndexIdNotIndexed.end(), std::pair<int,int>(index, vw->id()));
}
// Find nearest neighbor
ULOGGER_DEBUG("Searching in words not indexed...");
cv::BFMatcher matcher(query.type()==CV_8U?cv::NORM_HAMMING:useDistanceL1_?cv::NORM_L1:cv::NORM_L2SQR);

17
corelib/src/odometry/OdometryF2M.cpp
View File

@@ -180,6 +180,11 @@ void OdometryF2M::reset(const Transform & initialPose)
initGravity_ = false;
}
bool OdometryF2M::canProcessIMU() const
{
return sba_ && sba_->gravitySigma() > 0.0f;
}
// return not null transform if odometry is correctly computed
Transform OdometryF2M::computeTransform(
SensorData & data,
@@ -332,14 +337,6 @@ Transform OdometryF2M::computeTransform(
{
double stampDiff = 0.0;
imuT = getClosestIMU(lastFrame_->getStamp(), stampDiff);
if(stampDiff < 0.05)
{
bundleLinks.insert(std::make_pair(lastFrame_->id(), Link(lastFrame_->id(), lastFrame_->id(), Link::kPoseOdom, imuT)));
}
else
{
UWARN("IMUs are set, but we could not find one matching the current frame stamp %f (stampDiff=%f > 0.05)", lastFrame_->getStamp(), stampDiff);
}
}
// local bundle adjustment
@@ -380,7 +377,7 @@ Transform OdometryF2M::computeTransform(
if(!imuT.isNull())
{
bundleLinks.insert(std::make_pair(lastFrame_->id(), Link(lastFrame_->id(), lastFrame_->id(), Link::kPoseOdom, imuT)));
bundleLinks.insert(std::make_pair(lastFrame_->id(), Link(lastFrame_->id(), lastFrame_->id(), Link::kGravity, imuT)));
}
CameraModel model;
@@ -1191,7 +1188,7 @@ Transform OdometryF2M::computeTransform(
if(!imus_.empty())
{
bundleIMUOrientations_.insert(std::make_pair(lastFrame_->id(), Link(lastFrame_->id(), lastFrame_->id(), Link::kPoseOdom, newFramePose)));
bundleIMUOrientations_.insert(std::make_pair(lastFrame_->id(), Link(lastFrame_->id(), lastFrame_->id(), Link::kGravity, newFramePose)));
}
}

10
corelib/src/optimizer/OptimizerG2O.cpp
View File

@@ -530,7 +530,7 @@ std::map<int, Transform> OptimizerG2O::optimize(
}
}
}
else if(!isSlam2d() && gravitySigma() > 0 && iter->second.type() == Link::kPoseOdom && poses.find(iter->first) != poses.end())
else if(!isSlam2d() && gravitySigma() > 0 && iter->second.type() == Link::kGravity && poses.find(iter->first) != poses.end())
{
Eigen::Matrix<double, 6, 1> m;
// Up vector in robot frame
@@ -1403,7 +1403,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
{
#ifndef RTABMAP_ORB_SLAM2
g2o::HyperGraph::Edge * edge = 0;
if(gravitySigma() > 0 && iter->second.type() == Link::kPoseOdom && poses.find(iter->first) != poses.end())
if(gravitySigma() > 0 && iter->second.type() == Link::kGravity && poses.find(iter->first) != poses.end())
{
Eigen::Matrix<double, 6, 1> m;
// Up vector in robot frame
@@ -1932,7 +1932,11 @@ bool OptimizerG2O::saveGraph(
bool isSE2 = true;
bool isSE3 = true;
if (iter->second.type() == Link::kPosePrior)
if (iter->second.type() == Link::kGravity)
{
continue;
}
else if (iter->second.type() == Link::kPosePrior)
{
if (this->priorsIgnored())
{

10
corelib/src/optimizer/OptimizerGTSAM.cpp
View File

@@ -282,7 +282,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
}
}
}
else if(!isSlam2d() && gravitySigma() > 0 && iter->second.type() == Link::kPoseOdom && poses.find(iter->first) != poses.end())
else if(!isSlam2d() && gravitySigma() > 0 && iter->second.type() == Link::kGravity && poses.find(iter->first) != poses.end())
{
Vector3 r = gtsam::Pose3(iter->second.transform().toEigen4d()).rotation().xyz();
gtsam::Unit3 nG = gtsam::Rot3::RzRyRx(r.x(), r.y(), 0).rotate(gtsam::Unit3(0,0,-1));
@@ -376,13 +376,12 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
}
}
}
else
else // id1 != id2
{
#ifdef RTABMAP_VERTIGO
if(this->isRobust() &&
iter->second.type() != Link::kNeighbor &&
iter->second.type() != Link::kNeighborMerged &&
iter->second.type() != Link::kPosePrior)
iter->second.type() != Link::kNeighborMerged)
{
// create new switch variable
// Sunderhauf IROS 2012:
@@ -451,8 +450,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
#ifdef RTABMAP_VERTIGO
if(this->isRobust() &&
iter->second.type() != Link::kNeighbor &&
iter->second.type() != Link::kNeighborMerged &&
iter->second.type() != Link::kPosePrior)
iter->second.type() != Link::kNeighborMerged)
{
// create switchable edge factor
graph.add(vertigo::BetweenFactorSwitchableLinear<gtsam::Pose3>(id1, id2, gtsam::Symbol('s', switchCounter++), gtsam::Pose3(iter->second.transform().toEigen4d()), model));

2
corelib/src/optimizer/OptimizerTORO.cpp
View File

@@ -395,7 +395,7 @@ bool OptimizerTORO::saveGraph(
for(std::multimap<int, Link>::const_iterator iter = edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
{
if (iter->second.type() != Link::kPosePrior)
if (iter->second.type() != Link::kPosePrior && iter->second.type() != Link::kGravity)
{
if (isSlam2d())
{