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

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

4
corelib/include/rtabmap/core/DBDriver.h
View File

@@ -166,7 +166,7 @@ public:
bool getCalibration(int signatureId, std::vector<CameraModel> & models, StereoCameraModel & stereoModel) const;
bool getLaserScanInfo(int signatureId, LaserScan & info) const;
bool getNodeInfo(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector<float> & velocity, GPS & gps, EnvSensors & sensors) const;
void loadLinks(int signatureId, std::map<int, Link> & links, Link::Type type = Link::kUndef) const;
void loadLinks(int signatureId, std::multimap<int, Link> & links, Link::Type type = Link::kUndef) const;
void getWeight(int signatureId, int & weight) const;
void getLastNodeIds(std::set<int> & ids) const;
void getAllNodeIds(std::set<int> & ids, bool ignoreChildren = false, bool ignoreBadSignatures = false) const;
@@ -260,7 +260,7 @@ protected:
virtual void loadLastNodesQuery(std::list<Signature *> & signatures) const = 0;
virtual void loadSignaturesQuery(const std::list<int> & ids, std::list<Signature *> & signatures) const = 0;
virtual void loadWordsQuery(const std::set<int> & wordIds, std::list<VisualWord *> & vws) const = 0;
virtual void loadLinksQuery(int signatureId, std::map<int, Link> & links, Link::Type type = Link::kUndef) const = 0;
virtual void loadLinksQuery(int signatureId, std::multimap<int, Link> & links, Link::Type type = Link::kUndef) const = 0;
virtual void loadNodeDataQuery(std::list<Signature *> & signatures, bool images=true, bool scan=true, bool userData=true, bool occupancyGrid=true) const = 0;
virtual bool getCalibrationQuery(int signatureId, std::vector<CameraModel> & models, StereoCameraModel & stereoModel) const = 0;

2
corelib/include/rtabmap/core/DBDriverSqlite3.h
View File

@@ -130,7 +130,7 @@ protected:
virtual void loadLastNodesQuery(std::list<Signature *> & signatures) const;
virtual void loadSignaturesQuery(const std::list<int> & ids, std::list<Signature *> & signatures) const;
virtual void loadWordsQuery(const std::set<int> & wordIds, std::list<VisualWord *> & vws) const;
virtual void loadLinksQuery(int signatureId, std::map<int, Link> & links, Link::Type type = Link::kUndef) const;
virtual void loadLinksQuery(int signatureId, std::multimap<int, Link> & links, Link::Type type = Link::kUndef) const;
virtual void loadNodeDataQuery(std::list<Signature *> & signatures, bool images=true, bool scan=true, bool userData=true, bool occupancyGrid=true) const;
virtual bool getCalibrationQuery(int signatureId, std::vector<CameraModel> & models, StereoCameraModel & stereoModel) const;

3
corelib/include/rtabmap/core/Graph.h
View File

@@ -142,7 +142,8 @@ std::multimap<int, Link>::const_iterator RTABMAP_EXP findLink(
const std::multimap<int, Link> & links,
int from,
int to,
bool checkBothWays = true);
bool checkBothWays = true,
Link::Type type = Link::kUndef);
std::multimap<int, int>::const_iterator RTABMAP_EXP findLink(
const std::multimap<int, int> & links,
int from,

5
corelib/include/rtabmap/core/Link.h
View File

@@ -47,9 +47,10 @@ public:
kVirtualClosure,
kNeighborMerged,
kPosePrior, // Absolute pose in /world frame, From == To
kLandmark, // Transform /base_link -­­> /landmark, "From" is node observing the landmark "To"
kPoseOdom, // Pose in /odom frame, From == To (mainly used for gravity constraints)
kLandmark, // Transform /base_link -­­> /landmark, "From" is node observing the landmark "To" (landmark is negative id)
kGravity, // Orientation of the base frame accordingly to gravity (From == To)
kEnd,
kSelfRefLink = 97, // Include kPosePrior and kGravity (all links where From=To)
kAllWithLandmarks = 98,
kAllWithoutLandmarks = 99,
kUndef = 99};

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

@@ -147,11 +147,11 @@ public:
const std::map<int, double> & getWorkingMem() const {return _workingMem;}
const std::set<int> & getStMem() const {return _stMem;}
int getMaxStMemSize() const {return _maxStMemSize;}
std::map<int, Link> getNeighborLinks(int signatureId,
std::multimap<int, Link> getNeighborLinks(int signatureId,
bool lookInDatabase = false) const;
std::map<int, Link> getLoopClosureLinks(int signatureId,
std::multimap<int, Link> getLoopClosureLinks(int signatureId,
bool lookInDatabase = false) const;
std::map<int, Link> getLinks(int signatureId, // can be also used to get links from landmarks
std::multimap<int, Link> getLinks(int signatureId, // can be also used to get links from landmarks
bool lookInDatabase = false,
bool withLandmarks = false) const;
std::multimap<int, Link> getAllLinks(bool lookInDatabase, bool ignoreNullLinks = true, bool withLandmarks = false) const;
@@ -312,6 +312,7 @@ private:
float _rehearsalMaxAngle;
bool _rehearsalWeightIgnoredWhileMoving;
bool _useOdometryFeatures;
bool _useOdometryGravity;
bool _createOccupancyGrid;
int _visMaxFeatures;
bool _imagesAlreadyRectified;

5
corelib/include/rtabmap/core/Parameters.h
View File

@@ -220,10 +220,11 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(Mem, ImagePostDecimation, int, 1, "Image decimation (>=1) of saved data in created signatures (after features extraction). Decimation is done from the original image. Negative decimation is done from RGB size instead of depth size (if depth is smaller than RGB, it may be interpolated depending of the decimation value).");
RTABMAP_PARAM(Mem, CompressionParallelized, bool, true, "Compression of sensor data is multi-threaded.");
RTABMAP_PARAM(Mem, LaserScanDownsampleStepSize, int, 1, "If > 1, downsample the laser scans when creating a signature.");
RTABMAP_PARAM(Mem, LaserScanVoxelSize, float, 0.0, uFormat("If > 0 m, voxel filtering is done on laser scans when creating a signature. If the laser scan had normals, they will be removed. To recompute the normals, make sure to use \"%s\" or \"%s\" parameters.", kMemLaserScanNormalK().c_str(), kMemLaserScanNormalRadius().c_str()).c_str());
RTABMAP_PARAM(Mem, LaserScanVoxelSize, float, 0.0, uFormat("If > 0 m, voxel filtering is done on laser scans when creating a signature. If the laser scan had normals, they will be removed. To recompute the normals, make sure to use \"%s\" or \"%s\" parameters.", kMemLaserScanNormalK().c_str(), kMemLaserScanNormalRadius().c_str()));
RTABMAP_PARAM(Mem, LaserScanNormalK, int, 0, "If > 0 and laser scans don't have normals, normals will be computed with K search neighbors when creating a signature.");
RTABMAP_PARAM(Mem, LaserScanNormalRadius, float, 0.0, "If > 0 m and laser scans don't have normals, normals will be computed with radius search neighbors when creating a signature.");
RTABMAP_PARAM(Mem, UseOdomFeatures, bool, true, "Use odometry features.");
RTABMAP_PARAM(Mem, UseOdomFeatures, bool, true, "Use odometry features instead of regenerating them.");
RTABMAP_PARAM(Mem, UseOdomGravity, bool, false, uFormat("Use odometry instead of IMU orientation to add gravity links to new nodes created. We assume that odometry is already aligned with gravity (e.g., we are using a VIO approach). Gravity constraints are used by graph optimization only if \"%s\" is not zero.", kOptimizerGravitySigma().c_str()));
RTABMAP_PARAM(Mem, CovOffDiagIgnored, bool, true, "Ignore off diagonal values of the covariance matrix.");
// KeypointMemory (Keypoint-based)

4
corelib/include/rtabmap/core/Signature.h
View File

@@ -96,7 +96,7 @@ public:
void setSaved(bool saved) {_saved = saved;}
void setModified(bool modified) {_modified = modified; _linksModified = modified;}
const std::map<int, Link> & getLinks() const {return _links;}
const std::multimap<int, Link> & getLinks() const {return _links;}
bool isSaved() const {return _saved;}
bool isModified() const {return _modified || _linksModified;}
bool isLinksModified() const {return _linksModified;}
@@ -143,7 +143,7 @@ private:
int _id;
int _mapId;
double _stamp;
std::map<int, Link> _links; // id, transform
std::multimap<int, Link> _links; // id, transform
std::map<int, Link> _landmarks;
int _weight;
std::string _label;

2
corelib/include/rtabmap/core/odometry/OdometryF2M.h
View File

@@ -50,7 +50,7 @@ public:
virtual void reset(const Transform & initialPose = Transform::getIdentity());
const Signature & getMap() const {return *map_;}
const Signature & getLastFrame() const {return *lastFrame_;}
virtual bool canProcessIMU() const {return true;}
virtual bool canProcessIMU() const;
virtual Odometry::Type getType() {return Odometry::kTypeF2M;}

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())
{