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

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.");

218
corelib/src/Odometry.cpp
View File

@@ -44,7 +44,7 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
_resetCountdown(Parameters::defaultOdomResetCountdown()),
_force2D(Parameters::defaultVisForce2D()),
_holonomic(Parameters::defaultOdomHolonomic()),
_particleFiltering(Parameters::defaultOdomParticleFiltering()),
_filteringStrategy(Parameters::defaultOdomFilteringStrategy()),
_particleSize(Parameters::defaultOdomParticleSize()),
_particleNoiseT(Parameters::defaultOdomParticleNoiseT()),
_particleLambdaT(Parameters::defaultOdomParticleLambdaT()),
@@ -55,6 +55,8 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
_pnpReprojError(Parameters::defaultVisPnPReprojError()),
_pnpFlags(Parameters::defaultVisPnPFlags()),
_varianceFromInliersCount(Parameters::defaultRegVarianceFromInliersCount()),
_kalmanProcessNoise(Parameters::defaultOdomKalmanProcessNoise()),
_kalmanMeasurementNoise(Parameters::defaultOdomKalmanMeasurementNoise()),
_resetCurrentCount(0),
previousStamp_(0),
previousTransform_(Transform::getIdentity()),
@@ -76,7 +78,7 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
Parameters::parse(parameters, Parameters::kVisPnPFlags(), _pnpFlags);
UASSERT(_pnpFlags>=0 && _pnpFlags <=2);
Parameters::parse(parameters, Parameters::kRegVarianceFromInliersCount(), _varianceFromInliersCount);
Parameters::parse(parameters, Parameters::kOdomParticleFiltering(), _particleFiltering);
Parameters::parse(parameters, Parameters::kOdomFilteringStrategy(), _filteringStrategy);
Parameters::parse(parameters, Parameters::kOdomParticleSize(), _particleSize);
Parameters::parse(parameters, Parameters::kOdomParticleNoiseT(), _particleNoiseT);
Parameters::parse(parameters, Parameters::kOdomParticleLambdaT(), _particleLambdaT);
@@ -86,8 +88,11 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
UASSERT(_particleLambdaT>0);
UASSERT(_particleNoiseR>0);
UASSERT(_particleLambdaR>0);
if(_particleFiltering)
Parameters::parse(parameters, Parameters::kOdomKalmanProcessNoise(), _kalmanProcessNoise);
Parameters::parse(parameters, Parameters::kOdomKalmanMeasurementNoise(), _kalmanMeasurementNoise);
if(_filteringStrategy == 2)
{
// Initialize the Particle filters
filters_.resize(6);
for(unsigned int i = 0; i<filters_.size(); ++i)
{
@@ -101,6 +106,10 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
}
}
}
else if(_filteringStrategy == 1)
{
initKalmanFilter();
}
}
Odometry::~Odometry()
@@ -150,6 +159,25 @@ void Odometry::reset(const Transform & initialPose)
filters_[4]->init(pitch);
filters_[5]->init(yaw);
}
if(_filteringStrategy == 1)
{
if(_force2D)
{
kalmanFilter_.statePost.at<float>(0) = x;
kalmanFilter_.statePost.at<float>(1) = y;
kalmanFilter_.statePost.at<float>(6) = yaw;
}
else
{
kalmanFilter_.statePost.at<float>(0) = x;
kalmanFilter_.statePost.at<float>(1) = y;
kalmanFilter_.statePost.at<float>(2) = z;
kalmanFilter_.statePost.at<float>(9) = roll;
kalmanFilter_.statePost.at<float>(10) = pitch;
kalmanFilter_.statePost.at<float>(11) = yaw;
}
}
}
else
{
@@ -176,12 +204,13 @@ Transform Odometry::process(const SensorData & data, OdometryInfo * info)
UTimer time;
Transform t = this->computeTransform(data, info);
double dt = data.stamp() - previousStamp_;
if(info)
{
info->timeEstimation = time.ticks();
info->lost = t.isNull();
info->stamp = data.stamp();
info->interval = data.stamp() - previousStamp_;
info->interval = dt;
info->transform = t;
}
@@ -192,13 +221,27 @@ Transform Odometry::process(const SensorData & data, OdometryInfo * info)
{
_resetCurrentCount = _resetCountdown;
if(_force2D || !_holonomic || filters_.size())
if(_force2D || !_holonomic || filters_.size() || _filteringStrategy==1)
{
float x,y,z, roll,pitch,yaw;
t.getTranslationAndEulerAngles(x, y, z, roll, pitch, yaw);
if(filters_.size())
if(_filteringStrategy == 1)
{
if(_pose.isIdentity())
{
// reset Kalman
initKalmanFilter();
}
else
{
// Kalman filtering
updateKalmanFilter(dt,x,y,z,roll,pitch,yaw);
}
}
else if(filters_.size())
{
// Particle filtering
UASSERT(filters_.size()==6);
if(_pose.isIdentity())
{
@@ -261,7 +304,7 @@ Transform Odometry::process(const SensorData & data, OdometryInfo * info)
x, y, z, roll, pitch, yaw, t.prettyPrint().c_str()).c_str());
t = Transform(x,y,_force2D?0:z, _force2D?0:roll,_force2D?0:pitch,yaw);
if(info && filters_.size())
if(info && _filteringStrategy > 0)
{
info->transformFiltered = t;
}
@@ -296,4 +339,165 @@ Transform Odometry::process(const SensorData & data, OdometryInfo * info)
return Transform();
}
void Odometry::initKalmanFilter()
{
UDEBUG("");
// See OpenCV tutorial: http://docs.opencv.org/master/dc/d2c/tutorial_real_time_pose.html
// See Kalman filter pose/orientation estimation theory: http://campar.in.tum.de/Chair/KalmanFilter
// initialize the Kalman filter
int nStates = 18; // the number of states (x,y,z,x',y',z',x'',y'',z'',roll,pitch,yaw,roll',pitch',yaw',roll'',pitch'',yaw'')
int nMeasurements = 6; // the number of measured states (x,y,z,roll,pitch,yaw)
if(_force2D)
{
nStates = 9; // the number of states (x,y,x',y',x'',y'',yaw,yaw',yaw'')
nMeasurements = 3; // the number of measured states (x,y,z,roll,pitch,yaw)
}
int nInputs = 0; // the number of action control
kalmanFilter_.init(nStates, nMeasurements, nInputs); // init Kalman Filter
cv::setIdentity(kalmanFilter_.processNoiseCov, cv::Scalar::all(_kalmanProcessNoise)); // set process noise
cv::setIdentity(kalmanFilter_.measurementNoiseCov, cv::Scalar::all(_kalmanMeasurementNoise)); // set measurement noise
cv::setIdentity(kalmanFilter_.errorCovPost, cv::Scalar::all(1)); // error covariance
if(_force2D)
{
/* MEASUREMENT MODEL */
// [1 0 0 0 0 0 0 0 0]
// [0 1 0 0 0 0 0 0 0]
// [0 0 0 0 0 0 1 0 0]
kalmanFilter_.measurementMatrix.at<float>(0,0) = 1; // x
kalmanFilter_.measurementMatrix.at<float>(1,1) = 1; // y
kalmanFilter_.measurementMatrix.at<float>(2,6) = 1; // yaw
}
else
{
/* MEASUREMENT MODEL */
// [1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]
// [0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]
// [0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]
// [0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0]
// [0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0]
// [0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0]
kalmanFilter_.measurementMatrix.at<float>(0,0) = 1; // x
kalmanFilter_.measurementMatrix.at<float>(1,1) = 1; // y
kalmanFilter_.measurementMatrix.at<float>(2,2) = 1; // z
kalmanFilter_.measurementMatrix.at<float>(3,9) = 1; // roll
kalmanFilter_.measurementMatrix.at<float>(4,10) = 1; // pitch
kalmanFilter_.measurementMatrix.at<float>(5,11) = 1; // yaw
}
}
void Odometry::updateKalmanFilter(float dt, float & x, float & y, float & z, float & roll, float & pitch, float & yaw)
{
// Set transition matrix with current dt
if(_force2D)
{
// 2D:
// [1 0 dt 0 dt2 0 0 0 0] x
// [0 1 0 dt 0 dt2 0 0 0] y
// [0 0 1 0 dt 0 0 0 0] x'
// [0 0 0 1 0 dt 0 0 0] y'
// [0 0 0 0 1 0 0 0 0] x''
// [0 0 0 0 0 0 0 0 0] y''
// [0 0 0 0 0 0 1 dt dt2] yaw
// [0 0 0 0 0 0 0 1 dt] yaw'
// [0 0 0 0 0 0 0 0 1] yaw''
kalmanFilter_.transitionMatrix.at<float>(0,2) = dt;
kalmanFilter_.transitionMatrix.at<float>(1,3) = dt;
kalmanFilter_.transitionMatrix.at<float>(2,4) = dt;
kalmanFilter_.transitionMatrix.at<float>(3,5) = dt;
kalmanFilter_.transitionMatrix.at<float>(0,4) = 0.5*pow(dt,2);
kalmanFilter_.transitionMatrix.at<float>(1,5) = 0.5*pow(dt,2);
// orientation
kalmanFilter_.transitionMatrix.at<float>(6,7) = dt;
kalmanFilter_.transitionMatrix.at<float>(7,8) = dt;
kalmanFilter_.transitionMatrix.at<float>(6,8) = 0.5*pow(dt,2);
}
else
{
// [1 0 0 dt 0 0 dt2 0 0 0 0 0 0 0 0 0 0 0] x
// [0 1 0 0 dt 0 0 dt2 0 0 0 0 0 0 0 0 0 0] y
// [0 0 1 0 0 dt 0 0 dt2 0 0 0 0 0 0 0 0 0] z
// [0 0 0 1 0 0 dt 0 0 0 0 0 0 0 0 0 0 0] x'
// [0 0 0 0 1 0 0 dt 0 0 0 0 0 0 0 0 0 0] y'
// [0 0 0 0 0 1 0 0 dt 0 0 0 0 0 0 0 0 0] z'
// [0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0] x''
// [0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0] y''
// [0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0] z''
// [0 0 0 0 0 0 0 0 0 1 0 0 dt 0 0 dt2 0 0]
// [0 0 0 0 0 0 0 0 0 0 1 0 0 dt 0 0 dt2 0]
// [0 0 0 0 0 0 0 0 0 0 0 1 0 0 dt 0 0 dt2]
// [0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 dt 0 0]
// [0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 dt 0]
// [0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 dt]
// [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0]
// [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0]
// [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1]
// position
kalmanFilter_.transitionMatrix.at<float>(0,3) = dt;
kalmanFilter_.transitionMatrix.at<float>(1,4) = dt;
kalmanFilter_.transitionMatrix.at<float>(2,5) = dt;
kalmanFilter_.transitionMatrix.at<float>(3,6) = dt;
kalmanFilter_.transitionMatrix.at<float>(4,7) = dt;
kalmanFilter_.transitionMatrix.at<float>(5,8) = dt;
kalmanFilter_.transitionMatrix.at<float>(0,6) = 0.5*pow(dt,2);
kalmanFilter_.transitionMatrix.at<float>(1,7) = 0.5*pow(dt,2);
kalmanFilter_.transitionMatrix.at<float>(2,8) = 0.5*pow(dt,2);
// orientation
kalmanFilter_.transitionMatrix.at<float>(9,12) = dt;
kalmanFilter_.transitionMatrix.at<float>(10,13) = dt;
kalmanFilter_.transitionMatrix.at<float>(11,14) = dt;
kalmanFilter_.transitionMatrix.at<float>(12,15) = dt;
kalmanFilter_.transitionMatrix.at<float>(13,16) = dt;
kalmanFilter_.transitionMatrix.at<float>(14,17) = dt;
kalmanFilter_.transitionMatrix.at<float>(9,15) = 0.5*pow(dt,2);
kalmanFilter_.transitionMatrix.at<float>(10,16) = 0.5*pow(dt,2);
kalmanFilter_.transitionMatrix.at<float>(11,17) = 0.5*pow(dt,2);
}
// Set measurement to predict
cv::Mat measurements;
if(!_force2D)
{
measurements = cv::Mat(6,1,CV_32FC1);
measurements.at<float>(0) = x; // x
measurements.at<float>(1) = y; // y
measurements.at<float>(2) = z; // z
measurements.at<float>(3) = roll; // roll
measurements.at<float>(4) = pitch; // pitch
measurements.at<float>(5) = yaw; // yaw
}
else
{
measurements = cv::Mat(3,1,CV_32FC1);
measurements.at<float>(0) = x; // x
measurements.at<float>(1) = y; // y
measurements.at<float>(5) = yaw; // yaw
}
// First predict, to update the internal statePre variable
UDEBUG("Predict");
cv::Mat prediction = kalmanFilter_.predict();
// The "correct" phase that is going to use the predicted value and our measurement
UDEBUG("Correct");
cv::Mat estimated = kalmanFilter_.correct(measurements);
if(_force2D)
{
x = estimated.at<float>(0);
y = estimated.at<float>(1);
yaw = estimated.at<float>(6);
}
else
{
x = estimated.at<float>(0);
y = estimated.at<float>(1);
z = estimated.at<float>(2);
roll = estimated.at<float>(9);
pitch = estimated.at<float>(10);
yaw = estimated.at<float>(11);
}
}
} /* namespace rtabmap */

4
corelib/src/OdometryOpticalFlow.cpp
View File

@@ -204,7 +204,7 @@ Transform OdometryOpticalFlow::computeTransform(
std::vector<unsigned char> status;
std::vector<float> err;
UDEBUG("cv::calcOpticalFlowPyrLK() begin");
int winSize = (newCorners.size()||!flowGuessFromMotion_)?flowWinSize_:(flowWinSize_*2);
int winSize = flowWinSize_;
cv::calcOpticalFlowPyrLK(
refFrame_,
newLeftFrame,
@@ -213,7 +213,7 @@ Transform OdometryOpticalFlow::computeTransform(
status,
err,
cv::Size(winSize, winSize),
(newCorners.size()||!flowGuessFromMotion_)?flowMaxLevel_:flowMaxLevel_*2,
(newCorners.size()||!flowGuessFromMotion_)?flowMaxLevel_:3,
cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, flowIterations_, flowEps_),
cv::OPTFLOW_LK_GET_MIN_EIGENVALS | (newCorners.size()?cv::OPTFLOW_USE_INITIAL_FLOW:0), 1e-4);
UDEBUG("cv::calcOpticalFlowPyrLK() end");

1
corelib/src/Parameters.cpp
View File

@@ -145,6 +145,7 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
removedParameters_.insert(std::make_pair("RGBD/PoseScanMatching", std::make_pair(true, Parameters::kRGBDNeighborLinkRefining())));
removedParameters_.insert(std::make_pair("Odom/ParticleFiltering", std::make_pair(false, Parameters::kOdomFilteringStrategy())));
removedParameters_.insert(std::make_pair("Odom/FeatureType", std::make_pair(true, Parameters::kVisFeatureType())));
removedParameters_.insert(std::make_pair("Odom/EstimationType", std::make_pair(true, Parameters::kVisEstimationType())));
removedParameters_.insert(std::make_pair("Odom/MaxFeatures", std::make_pair(true, Parameters::kVisMaxFeatures())));