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

increased version to 0.8.4: Database Link's "variance" field split into "rot_variance" and "trans_variance". Added Parameter LccIcp/HighTransitionalVariance

Browse Source
This commit is contained in:
Mathieu Labbe
2015-02-24 16:06:02 -05:00
parent f514cfa5fe
commit eb96fe1249
24 changed files with 317 additions and 267 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -57,7 +57,7 @@ public:
CameraEvent(const cv::Mat & rgb, const cv::Mat & depth, float fx, float fy, float cx, float cy, const Transform & localTransform, int id) :
UEvent(kCodeImageDepth),
data_(rgb, depth, fx, fy, cx, cy, localTransform, Transform(), 1.0f, id)
data_(rgb, depth, fx, fy, cx, cy, localTransform, Transform(), 1.0f, 1.0f, id)
{
}

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

@@ -40,15 +40,17 @@ public:
from_(0),
to_(0),
type_(kUndef),
variance_(1.0f)
rotVariance_(1.0f),
transVariance_(1.0f)
{
}
Link(int from, int to, Type type, const Transform & transform, float variance) :
Link(int from, int to, Type type, const Transform & transform, float rotVariance, float transVariance) :
from_(from),
to_(to),
transform_(transform),
type_(type),
variance_(variance)
rotVariance_(rotVariance),
transVariance_(transVariance)
{
}
@@ -58,20 +60,22 @@ public:
int to() const {return to_;}
const Transform & transform() const {return transform_;}
Type type() const {return type_;}
float variance() const {return variance_;}
float rotVariance() const {return rotVariance_;}
float transVariance() const {return transVariance_;}
void setFrom(int from) {from_ = from;}
void setTo(int to) {to_ = to;}
void setTransform(const Transform & transform) {transform_ = transform;}
void setType(Type type) {type_ = type;}
void setVariance(float variance) {variance_ = variance;}
void setVariance(float rotVariance, float transVariance) {rotVariance_ = rotVariance; transVariance_ = transVariance;}
private:
int from_;
int to_;
Transform transform_;
Type type_;
float variance_;
float rotVariance_;
float transVariance_;
};
}

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

@@ -80,8 +80,8 @@ public:
std::list<int> cleanup(const std::list<int> & ignoredIds = std::list<int>());
void emptyTrash();
void joinTrashThread();
bool addLink(int to, int from, const Transform & transform, Link::Type type, float variance);
void updateLink(int fromId, int toId, const Transform & transform, float variance);
bool addLink(int to, int from, const Transform & transform, Link::Type type, float rotVariance, float transVariance);
void updateLink(int fromId, int toId, const Transform & transform, float rotVariance, float transVariance);
void removeAllVirtualLinks();
std::map<int, int> getNeighborsId(int signatureId,
int margin,
@@ -167,7 +167,7 @@ public:
private:
void preUpdate();
void addSignatureToStm(Signature * signature, float odomVariance);
void addSignatureToStm(Signature * signature, float poseRotVariance, float poseTransVariance);
void clear();
void moveToTrash(Signature * s, bool saveToDatabase = true, std::list<int> * deletedWords = 0);

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

@@ -332,6 +332,7 @@ class RTABMAP_EXP Parameters
// Loop closure constraint
RTABMAP_PARAM(LccIcp, Type, int, 0, "0=No ICP, 1=ICP 3D, 2=ICP 2D");
RTABMAP_PARAM(LccIcp, MaxDistance, float, 0.2, "Maximum ICP correction distance accepted (m).");
RTABMAP_PARAM(LccIcp, HighTransitionalVariance, bool, true, "Set high transitional variance on odometry pose correction and on local loop closure in space. Particularly useful when the robot is moving in hallways with short-range laser rangefinder.");
RTABMAP_PARAM(LccBow, MinInliers, int, 20, "Minimum visual word correspondences to compute geometry transform.");
RTABMAP_PARAM(LccBow, InlierDistance, float, 0.02, "Maximum distance for visual word correspondences.");
@@ -349,14 +350,14 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(LccIcp3, VoxelSize, float, 0.01, "Voxel size to be used for ICP computation.");
RTABMAP_PARAM(LccIcp3, Samples, int, 0, "Random samples to be used for ICP computation. Not used if voxelSize is set.");
RTABMAP_PARAM(LccIcp3, MaxCorrespondenceDistance, float, 0.05, "ICP 3D: Max distance for point correspondences.");
RTABMAP_PARAM(LccIcp3, Iterations, int, 30, "ICP 3D: Max iterations.");
RTABMAP_PARAM(LccIcp3, CorrespondenceRatio, float, 0.7, "ICP 3D: Ratio of matching correspondences to accept the transform.");
RTABMAP_PARAM(LccIcp3, PointToPlane, bool, false, "ICP 3D: Use point to plane ICP.");
RTABMAP_PARAM(LccIcp3, PointToPlaneNormalNeighbors, int, 20, "ICP 3D: Number of neighbors to compute normals for point to plane.");
RTABMAP_PARAM(LccIcp3, Iterations, int, 30, "Max iterations.");
RTABMAP_PARAM(LccIcp3, CorrespondenceRatio, float, 0.7, "Ratio of matching correspondences to accept the transform.");
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, "ICP 2D: Max distance for point correspondences.");
RTABMAP_PARAM(LccIcp2, Iterations, int, 30, "ICP 2D: Max iterations.");
RTABMAP_PARAM(LccIcp2, CorrespondenceRatio, float, 0.7, "ICP 2D: Ratio of matching correspondences to accept the transform.");
RTABMAP_PARAM(LccIcp2, MaxCorrespondenceDistance, float, 0.1, "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.005, "Voxel size to be used for ICP computation.");
// Stereo disparity

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

@@ -183,6 +183,7 @@ private:
bool _startNewMapOnLoopClosure;
float _goalReachedRadius; // meters
bool _planWithNearNodesLinked;
bool _icpHighTransVariance;
std::pair<int, float> _loopClosureHypothesis;
std::pair<int, float> _highestHypothesis;

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

@@ -110,7 +110,8 @@ private:
Rtabmap * _rtabmap;
bool _paused;
Transform lastPose_;
float _variance;
float _rotVariance;
float _transVariance;
};
} /* namespace rtabmap */

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

@@ -54,8 +54,9 @@ public:
float cy,
const Transform & localTransform,
const Transform & pose,
float poseVariance,
int id = 0);
float poseRotVariance,
float poseTransVariance,
int id);
// Metric constructor + 2d laser scan
SensorData(const cv::Mat & laserScan,
@@ -67,8 +68,9 @@ public:
float cy,
const Transform & localTransform,
const Transform & pose,
float poseVariance,
int id = 0);
float poseRotVariance,
float poseTransVariance,
int id);
virtual ~SensorData() {}
@@ -82,7 +84,7 @@ public:
void setId(int id) {_id = id;}
bool isMetric() const {return !_depthOrRightImage.empty() || _fx != 0.0f || _fyOrBaseline != 0.0f || !_pose.isNull();}
void setPose(const Transform & pose, float variance) {_pose = pose; _poseVariance=variance;}
void setPose(const Transform & pose, float rotVariance, float transVariance) {_pose = pose; _poseRotVariance=rotVariance; _poseTransVariance = transVariance;}
cv::Mat depth() const {return (_depthOrRightImage.type()==CV_32FC1 || _depthOrRightImage.type()==CV_16UC1)?_depthOrRightImage:cv::Mat();}
cv::Mat rightImage() const {return _depthOrRightImage.type()==CV_8UC1?_depthOrRightImage:cv::Mat();}
const cv::Mat & depthOrRightImage() const {return _depthOrRightImage;}
@@ -95,7 +97,8 @@ public:
float fyOrBaseline() const {return _fyOrBaseline;}
const Transform & pose() const {return _pose;}
const Transform & localTransform() const {return _localTransform;}
float poseVariance() const {return _poseVariance;}
float poseRotVariance() const {return _poseRotVariance;}
float poseTransVariance() const {return _poseTransVariance;}
void setFeatures(const std::vector<cv::KeyPoint> & keypoints, const cv::Mat & descriptors)
{
@@ -118,7 +121,8 @@ private:
float _cy;
Transform _pose;
Transform _localTransform;
float _poseVariance;
float _poseRotVariance;
float _poseTransVariance;
// features
std::vector<cv::KeyPoint> _keypoints;