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

Odometry: Added Kalman Filtering option

Browse Source
This commit is contained in:
matlabbe
2015-12-14 16:50:17 -05:00
parent 82d76f3e99
commit f5c062448d
7 changed files with 366 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -70,6 +70,9 @@ public:
private:
virtual Transform computeTransform(const SensorData & image, OdometryInfo * info = 0) = 0;
void initKalmanFilter();
void updateKalmanFilter(float dt, float & x, float & y, float & z, float & roll, float & pitch, float & yaw);
private:
std::string _roiRatios;
int _minInliers;
@@ -80,7 +83,7 @@ private:
int _resetCountdown;
bool _force2D;
bool _holonomic;
bool _particleFiltering;
int _filteringStrategy;
int _particleSize;
float _particleNoiseT;
float _particleLambdaT;
@@ -91,6 +94,8 @@ private:
double _pnpReprojError;
int _pnpFlags;
bool _varianceFromInliersCount;
float _kalmanProcessNoise;
float _kalmanMeasurementNoise;
Transform _pose;
int _resetCurrentCount;
double previousStamp_;
@@ -98,6 +103,7 @@ private:
float distanceTravelled_;
std::vector<ParticleFilter *> filters_;
cv::KalmanFilter kalmanFilter_;
protected:
Odometry(const rtabmap::ParametersMap & parameters);

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

@@ -330,12 +330,14 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(Odom, Holonomic, bool, true, "If the robot is holonomic (strafing commands can be issued). If not, y value will be estimated from x and yaw values (y=x*tan(yaw)).");
RTABMAP_PARAM(Odom, FillInfoData, bool, true, "Fill info with data (inliers/outliers features).");
RTABMAP_PARAM(Odom, ImageBufferSize, unsigned int, 1, "Data buffer size (0 min inf).");
RTABMAP_PARAM(Odom, ParticleFiltering, bool, false, "Particle filtering to smooth the odometry trajectory.");
RTABMAP_PARAM(Odom, FilteringStrategy, int, 0, "0=No filtering 1=Kalman filtering 2=Particle filtering");
RTABMAP_PARAM(Odom, ParticleSize, unsigned int, 400, "Number of particles of the filter.");
RTABMAP_PARAM(Odom, ParticleNoiseT, float, 0.002, "Noise (m) of translation components (x,y,z).");
RTABMAP_PARAM(Odom, ParticleLambdaT, float, 100, "Lambda of translation components (x,y,z).");
RTABMAP_PARAM(Odom, ParticleNoiseR, float, 0.002, "Noise (rad) of rotational components (roll,pitch,yaw).");
RTABMAP_PARAM(Odom, ParticleLambdaR, float, 100, "Lambda of rotational components (roll,pitch,yaw).");
RTABMAP_PARAM(Odom, KalmanProcessNoise, float, 0.001, "Process noise covariance value.");
RTABMAP_PARAM(Odom, KalmanMeasurementNoise, float, 0.01, "Process measurement covariance value.");
// Odometry Bag-of-words
RTABMAP_PARAM(OdomBow, LocalHistorySize, int, 1000, "Local history size: If > 0 (example 5000), the odometry will maintain a local map of X maximum words.");