i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 01:20:25 +08:00
Code Issues Packages Projects Releases Wiki Activity

Updated version to 0.8.11. Added param RGBD/LocalLoopDetectionPathOdomPosesUsed. DatabaseViewer: export option at a specified framerate. DBReader: option to read database at a rate specified by the stamps saved. Database: Added new column "data2d_max_pts" in Depth table

Browse Source
This commit is contained in:
Mathieu Labbe
2015-05-01 07:30:09 -04:00
parent d09e8f237a
commit abb7eb15ac
29 changed files with 622 additions and 275 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -95,7 +95,7 @@ public:
// Specific queries...
void loadNodeData(std::list<Signature *> & signatures, bool loadMetricData) const;
void getNodeData(int signatureId, cv::Mat & imageCompressed, cv::Mat & depthCompressed, cv::Mat & laserScanCompressed, float & fx, float & fy, float & cx, float & cy, Transform & localTransform) const;
void getNodeData(int signatureId, cv::Mat & imageCompressed, cv::Mat & depthCompressed, cv::Mat & laserScanCompressed, float & fx, float & fy, float & cx, float & cy, Transform & localTransform, int & laserScanMaxPts) const;
void getNodeData(int signatureId, cv::Mat & imageCompressed) const;
bool getNodeInfo(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, std::vector<unsigned char> & userData) const;
void loadLinks(int signatureId, std::map<int, Link> & links, Link::Type type = Link::kUndef) const;
@@ -134,7 +134,7 @@ private:
virtual void loadLinksQuery(int signatureId, std::map<int, Link> & links, Link::Type type = Link::kUndef) const = 0;
virtual void loadNodeDataQuery(std::list<Signature *> & signatures, bool loadMetricData) const = 0;
virtual void getNodeDataQuery(int signatureId, cv::Mat & imageCompressed, cv::Mat & depthCompressed, cv::Mat & laserScanCompressed, float & fx, float & fy, float & cx, float & cy, Transform & localTransform) const = 0;
virtual void getNodeDataQuery(int signatureId, cv::Mat & imageCompressed, cv::Mat & depthCompressed, cv::Mat & laserScanCompressed, float & fx, float & fy, float & cx, float & cy, Transform & localTransform, int & laserScanMaxPts) const = 0;
virtual void getNodeDataQuery(int signatureId, cv::Mat & imageCompressed) const = 0;
virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, std::vector<unsigned char> & userData) const = 0;
virtual void getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren) const = 0;

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

@@ -62,7 +62,7 @@ protected:
private:
std::string _path;
float _frameRate;
float _frameRate; // -1 = use Database stamps, 0 = inf
bool _odometryIgnored;
bool _ignoreGoalDelay;
@@ -70,6 +70,7 @@ private:
UTimer _timer;
std::set<int> _ids;
std::set<int>::iterator _currentId;
double _previousStamp;
};
} /* namespace rtabmap */

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

@@ -165,8 +165,8 @@ public:
bool getBowForce2D() const {return _bowForce2D;}
Transform computeVisualTransform(int oldId, int newId, std::string * rejectedMsg = 0, int * inliers = 0, double * variance = 0) const;
Transform computeVisualTransform(const Signature & oldS, const Signature & newS, std::string * rejectedMsg = 0, int * inliers = 0, double * variance = 0) const;
Transform computeIcpTransform(int oldId, int newId, Transform guess, bool icp3D, std::string * rejectedMsg = 0, int * inliers = 0, double * variance = 0);
Transform computeIcpTransform(const Signature & oldS, const Signature & newS, Transform guess, bool icp3D, std::string * rejectedMsg = 0, int * inliers = 0, double * variance = 0) const;
Transform computeIcpTransform(int oldId, int newId, Transform guess, bool icp3D, std::string * rejectedMsg = 0, int * correspondences = 0, double * variance = 0, float * correspondencesRatio = 0);
Transform computeIcpTransform(const Signature & oldS, const Signature & newS, Transform guess, bool icp3D, std::string * rejectedMsg = 0, int * correspondences = 0, double * variance = 0, float * correspondencesRatio = 0) const;
Transform computeScanMatchingTransform(
int newId,
int oldId,

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

@@ -299,6 +299,7 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(RGBD, LocalLoopDetectionSpace, bool, false, "Detection over locations (in Working Memory or STM) near in space.");
RTABMAP_PARAM(RGBD, LocalLoopDetectionMaxDiffID, int, 50, "Maximum ID difference between the current/last loop closure location and the local loop closure hypotheses. Set 0 to ignore.");
RTABMAP_PARAM(RGBD, LocalLoopDetectionPathFilteringRadius, float, 0.25, "Path filtering radius.");
RTABMAP_PARAM(RGBD, LocalLoopDetectionPathOdomPosesUsed, bool, true, "When comparing to a local path, merge the scan using the odometry poses instead of the ones in the optimized local graph.");
// Graph optimization
RTABMAP_PARAM(RGBD, OptimizeStrategy, int, 0, "Graph optimization strategy: 0=TORO and 1=g2o.");
@@ -373,10 +374,10 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(LccIcp3, PointToPlane, bool, false, "Use point to plane ICP.");
RTABMAP_PARAM(LccIcp3, PointToPlaneNormalNeighbors, int, 20, "Number of neighbors to compute normals for point to plane.");
RTABMAP_PARAM(LccIcp2, MaxCorrespondenceDistance, float, 0.1, "Max distance for point correspondences.");
RTABMAP_PARAM(LccIcp2, MaxCorrespondenceDistance, float, 0.05, "Max distance for point correspondences.");
RTABMAP_PARAM(LccIcp2, Iterations, int, 30, "Max iterations.");
RTABMAP_PARAM(LccIcp2, CorrespondenceRatio, float, 0.7, "Ratio of matching correspondences to accept the transform.");
RTABMAP_PARAM(LccIcp2, VoxelSize, float, 0.05, "Voxel size to be used for ICP computation.");
RTABMAP_PARAM(LccIcp2, CorrespondenceRatio, float, 0.3, "Ratio of matching correspondences to accept the transform.");
RTABMAP_PARAM(LccIcp2, VoxelSize, float, 0.025, "Voxel size to be used for ICP computation.");
// Stereo disparity
RTABMAP_PARAM(Stereo, WinSize, int, 16, "See cv::calcOpticalFlowPyrLK().");

6
corelib/include/rtabmap/core/Rtabmap.h
View File

@@ -149,6 +149,11 @@ private:
bool lookInDatabase,
std::map<int, Transform> & optimizedPoses,
std::multimap<int, Link> * constraints = 0) const;
std::map<int, Transform> optimizeGraph(
int fromId,
const std::vector<int> & ids,
bool lookInDatabase,
std::multimap<int, Link> * constraints = 0) const;
void updateGoalIndex();
bool computePath(int targetNode, const std::map<int, Transform> & nodes, const std::multimap<int, rtabmap::Link> & constraints);
@@ -181,6 +186,7 @@ private:
float _localRadius;
int _localDetectMaxDiffID;
float _localPathFilteringRadius;
bool _localPathOdomPosesUsed;
std::string _databasePath;
bool _optimizeFromGraphEnd;
bool _reextractLoopClosureFeatures;

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

@@ -62,6 +62,7 @@ public:
// Metric constructor + 2d laser scan
SensorData(const cv::Mat & laserScan,
int laserScanMaxPts,
const cv::Mat & image,
const cv::Mat & depthOrRightImage,
float fx,
@@ -95,6 +96,7 @@ public:
cv::Mat rightImage() const {return _depthOrRightImage.type()==CV_8UC1?_depthOrRightImage:cv::Mat();}
const cv::Mat & depthOrRightImage() const {return _depthOrRightImage;}
const cv::Mat & laserScan() const {return _laserScan;}
int laserScanMaxPts() const {return _laserScanMaxPts;}
float fx() const {return _fx;}
float fy() const {return (_depthOrRightImage.type()==CV_8UC1)?0:_fyOrBaseline;}
float cx() const {return _cx;}
@@ -133,6 +135,7 @@ private:
Transform _localTransform;
float _poseRotVariance;
float _poseTransVariance;
int _laserScanMaxPts;
// features
std::vector<cv::KeyPoint> _keypoints;

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

@@ -68,7 +68,8 @@ public:
float fy = 0.0f,
float cx = 0.0f,
float cy = 0.0f,
const Transform & localTransform =Transform::getIdentity());
const Transform & localTransform =Transform::getIdentity(),
int laserScanMaxPts = 0);
virtual ~Signature();
/**
@@ -128,7 +129,7 @@ public:
//metric stuff
void setWords3(const std::multimap<int, pcl::PointXYZ> & words3) {_words3 = words3;}
void setDepthCompressed(const cv::Mat & bytes, float fx, float fy, float cx, float cy);
void setLaserScanCompressed(const cv::Mat & bytes) {_laserScanCompressed = bytes;}
void setLaserScanCompressed(const cv::Mat & bytes, int maxPts = 0) {_laserScanCompressed = bytes; _laserScanMaxPts=maxPts;}
void setLocalTransform(const Transform & t) {_localTransform = t;}
void setPose(const Transform & pose) {_pose = pose;}
const std::multimap<int, pcl::PointXYZ> & getWords3() const {return _words3;}
@@ -146,8 +147,9 @@ public:
const Transform & getLocalTransform() const {return _localTransform;}
void setDepthRaw(const cv::Mat & depth) {_depthRaw = depth;}
const cv::Mat & getDepthRaw() const {return _depthRaw;}
void setLaserScanRaw(const cv::Mat & depth2D) {_laserScanRaw = depth2D;}
void setLaserScanRaw(const cv::Mat & depth2D, int maxPts = 0) {_laserScanRaw = depth2D; _laserScanMaxPts=maxPts;}
const cv::Mat & getLaserScanRaw() const {return _laserScanRaw;}
int getLaserScanMaxPts() const {return _laserScanMaxPts;}
SensorData toSensorData();
void uncompressData();
@@ -183,6 +185,7 @@ private:
Transform _pose;
Transform _localTransform; // camera_link -> base_link
std::multimap<int, pcl::PointXYZ> _words3; // word <id, keypoint>
int _laserScanMaxPts;
cv::Mat _imageRaw; // CV_8UC1 or CV_8UC3
cv::Mat _depthRaw; // depth CV_16UC1 or CV_32FC1, right image CV_8UC1

6
corelib/include/rtabmap/core/Statistics.h
View File

@@ -63,13 +63,17 @@ class RTABMAP_EXP Statistics
RTABMAP_STATS(Loop, VisualInliers,);
RTABMAP_STATS(Loop, Last_id,);
RTABMAP_STATS(LocalLoop, Odom_corrected,);
RTABMAP_STATS(LocalLoop, Time_closures,);
RTABMAP_STATS(LocalLoop, Space_last_closure_id,);
RTABMAP_STATS(LocalLoop, Space_paths,);
RTABMAP_STATS(LocalLoop, Space_closures_added,);
RTABMAP_STATS(LocalLoop, Space_closures_added_icp_only,);
RTABMAP_STATS(OdomCorrection, Accepted,);
RTABMAP_STATS(OdomCorrection, Inliers,);
RTABMAP_STATS(OdomCorrection, Inliers_ratio,);
RTABMAP_STATS(OdomCorrection, Variance,);
RTABMAP_STATS(Memory, Working_memory_size,);
RTABMAP_STATS(Memory, Short_time_memory_size,);
RTABMAP_STATS(Memory, Signatures_removed,);

6
corelib/include/rtabmap/core/util3d.h
View File

@@ -286,7 +286,7 @@ Transform RTABMAP_EXP icp(
int maximumIterations,
bool * hasConverged = 0,
double * variance = 0,
int * inliers = 0);
int * correspondences = 0);
Transform RTABMAP_EXP icpPointToPlane(
const pcl::PointCloud<pcl::PointNormal>::ConstPtr & cloud_source,
@@ -295,7 +295,7 @@ Transform RTABMAP_EXP icpPointToPlane(
int maximumIterations,
bool * hasConverged = 0,
double * variance = 0,
int * inliers = 0);
int * correspondences = 0);
Transform RTABMAP_EXP icp2D(
const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
@@ -304,7 +304,7 @@ Transform RTABMAP_EXP icp2D(
int maximumIterations,
bool * hasConverged = 0,
double * variance = 0,
int * inliers = 0);
int * correspondences = 0);
pcl::PointCloud<pcl::PointNormal>::Ptr RTABMAP_EXP computeNormals(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,