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

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;}