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

Updated how odometry guess is done (adapt to time between timestamps) and Kalman filtering (using velocity)

Browse Source
This commit is contained in:
matlabbe
2016-02-27 18:30:00 -05:00
parent a8d1b91083
commit 6d06b66496
16 changed files with 311 additions and 253 deletions
Download Patch File Download Diff File Expand all files Collapse all files

13
corelib/include/rtabmap/core/Odometry.h
View File

@@ -60,18 +60,19 @@ public:
//getters
const Transform & getPose() const {return _pose;}
bool isInfoDataFilled() const {return _fillInfoData;}
const Transform & previousTransform() const {return previousTransform_;}
private:
virtual Transform computeTransform(SensorData & data, OdometryInfo * info = 0) = 0;
virtual Transform computeTransform(SensorData & data, const Transform & guess = Transform(), OdometryInfo * info = 0) = 0;
void initKalmanFilter();
void updateKalmanFilter(float dt, float & x, float & y, float & z, float & roll, float & pitch, float & yaw);
void initKalmanFilter(const Transform & initialPose = Transform::getIdentity(), float vx=0.0f, float vy=0.0f, float vz=0.0f, float vroll=0.0f, float vpitch=0.0f, float vyaw=0.0f);
void predictKalmanFilter(float dt, float * vx=0, float * vy=0, float * vz=0, float * vroll=0, float * vpitch=0, float * vyaw=0);
void updateKalmanFilter(float & vx, float & vy, float & vz, float & vroll, float & vpitch, float & vyaw);
private:
int _resetCountdown;
bool _force3DoF;
bool _holonomic;
bool guessFromMotion_;
int _filteringStrategy;
int _particleSize;
float _particleNoiseT;
@@ -84,11 +85,11 @@ private:
Transform _pose;
int _resetCurrentCount;
double previousStamp_;
Transform previousTransform_;
Transform previousVelocityTransform_;
Transform previousGroundTruthPose_;
float distanceTravelled_;
std::vector<ParticleFilter *> filters_;
std::vector<ParticleFilter *> particleFilters_;
cv::KalmanFilter kalmanFilter_;
protected:

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

@@ -46,12 +46,11 @@ public:
const Signature & getRefFrame() const {return refFrame_;}
private:
virtual Transform computeTransform(SensorData & image, OdometryInfo * info = 0);
virtual Transform computeTransform(SensorData & image, const Transform & guess = Transform(), OdometryInfo * info = 0);
private:
//Parameters:
int keyFrameThr_;
bool guessFromMotion_;
Registration * registrationPipeline_;
Signature refFrame_;

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

@@ -46,7 +46,7 @@ public:
const Signature & getLastFrame() const {return *lastFrame_;}
private:
virtual Transform computeTransform(SensorData & data, OdometryInfo * info = 0);
virtual Transform computeTransform(SensorData & data, const Transform & guess = Transform(), OdometryInfo * info = 0);
private:
//Parameters

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

@@ -43,7 +43,7 @@ public:
virtual void reset(const Transform & initialPose);
private:
virtual Transform computeTransform(SensorData & data, OdometryInfo * info = 0);
virtual Transform computeTransform(SensorData & data, const Transform & guess = Transform(), OdometryInfo * info = 0);
private:
//Parameters:
int flowWinSize_;

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

@@ -362,7 +362,7 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(OdomMono, MaxVariance, float, 0.01, "Maximum variance to add new points to local map.");
// Odometry Optical Flow
RTABMAP_PARAM(OdomF2F, KeyFrameThr, int, 100, "Create a new keyframe when the number of inliers drops under this threshold. Setting the value to 0 means that a keyframe is created for each processed frame.");
RTABMAP_PARAM(OdomF2F, KeyFrameThr, int, 500, "Create a new keyframe when the number of inliers drops under this threshold. Setting the value to 0 means that a keyframe is created for each processed frame.");
// Common registration parameters
RTABMAP_PARAM(Reg, VarianceFromInliersCount, bool, false, "Set variance as the inverse of the number of inliers. Otherwise, the variance is computed as the average 3D position error of the inliers.");
@@ -420,7 +420,6 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(Stereo, OpticalFlow, bool, true, "Use optical flow to find stereo correspondences, otherwise a simple block matching approach is used.");
RTABMAP_PARAM(Stereo, SSD, bool, true, "[Stereo/OpticalFlow = false] Use Sum of Squared Differences (SSD) window, otherwise Sum of Absolute Differences (SAD) window is used.");
RTABMAP_PARAM(Stereo, Eps, double, 0.01, "[Stereo/OpticalFlow = true] Epsilon stop criterion.");
RTABMAP_PARAM(Stereo, MaxSlope, float, 0.1, "[Stereo/OpticalFlow = true] The maximum slope for each stereo pairs.");
RTABMAP_PARAM(StereoBM, BlockSize, int, 15, "See cv::StereoBM");
RTABMAP_PARAM(StereoBM, MinDisparity, int, 0, "See cv::StereoBM");

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

@@ -80,11 +80,9 @@ public:
std::vector<unsigned char> & status) const;
float epsilon() const {return epsilon_;}
float maxSlope() const {return maxSlope_;}
private:
float epsilon_;
float maxSlope_;
};
} /* namespace rtabmap */