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 */

350
corelib/src/Odometry.cpp
View File

@@ -65,6 +65,7 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
_resetCountdown(Parameters::defaultOdomResetCountdown()),
_force3DoF(Parameters::defaultRegForce3DoF()),
_holonomic(Parameters::defaultOdomHolonomic()),
guessFromMotion_(Parameters::defaultOdomGuessMotion()),
_filteringStrategy(Parameters::defaultOdomFilteringStrategy()),
_particleSize(Parameters::defaultOdomParticleSize()),
_particleNoiseT(Parameters::defaultOdomParticleNoiseT()),
@@ -76,13 +77,14 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
_kalmanMeasurementNoise(Parameters::defaultOdomKalmanMeasurementNoise()),
_resetCurrentCount(0),
previousStamp_(0),
previousTransform_(Transform::getIdentity()),
previousVelocityTransform_(Transform::getIdentity()),
distanceTravelled_(0)
{
Parameters::parse(parameters, Parameters::kOdomResetCountdown(), _resetCountdown);
Parameters::parse(parameters, Parameters::kRegForce3DoF(), _force3DoF);
Parameters::parse(parameters, Parameters::kOdomHolonomic(), _holonomic);
Parameters::parse(parameters, Parameters::kOdomGuessMotion(), guessFromMotion_);
Parameters::parse(parameters, Parameters::kOdomFillInfoData(), _fillInfoData);
Parameters::parse(parameters, Parameters::kOdomFilteringStrategy(), _filteringStrategy);
Parameters::parse(parameters, Parameters::kOdomParticleSize(), _particleSize);
@@ -99,16 +101,16 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
if(_filteringStrategy == 2)
{
// Initialize the Particle filters
filters_.resize(6);
for(unsigned int i = 0; i<filters_.size(); ++i)
particleFilters_.resize(6);
for(unsigned int i = 0; i<particleFilters_.size(); ++i)
{
if(i<3)
{
filters_[i] = new ParticleFilter(_particleSize, _particleNoiseT, _particleLambdaT);
particleFilters_[i] = new ParticleFilter(_particleSize, _particleNoiseT, _particleLambdaT);
}
else
{
filters_[i] = new ParticleFilter(_particleSize, _particleNoiseR, _particleLambdaR);
particleFilters_[i] = new ParticleFilter(_particleSize, _particleNoiseR, _particleLambdaR);
}
}
}
@@ -120,21 +122,21 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
Odometry::~Odometry()
{
for(unsigned int i=0; i<filters_.size(); ++i)
for(unsigned int i=0; i<particleFilters_.size(); ++i)
{
delete filters_[i];
delete particleFilters_[i];
}
filters_.clear();
particleFilters_.clear();
}
void Odometry::reset(const Transform & initialPose)
{
previousTransform_.setIdentity();
previousVelocityTransform_.setIdentity();
previousGroundTruthPose_.setNull();
_resetCurrentCount = 0;
previousStamp_ = 0;
distanceTravelled_ = 0;
if(_force3DoF || filters_.size())
if(_force3DoF || particleFilters_.size())
{
float x,y,z, roll,pitch,yaw;
initialPose.getTranslationAndEulerAngles(x, y, z, roll, pitch, yaw);
@@ -156,34 +158,20 @@ void Odometry::reset(const Transform & initialPose)
_pose = initialPose;
}
if(filters_.size())
if(particleFilters_.size())
{
UASSERT(filters_.size() == 6);
filters_[0]->init(x);
filters_[1]->init(y);
filters_[2]->init(z);
filters_[3]->init(roll);
filters_[4]->init(pitch);
filters_[5]->init(yaw);
UASSERT(particleFilters_.size() == 6);
particleFilters_[0]->init(x);
particleFilters_[1]->init(y);
particleFilters_[2]->init(z);
particleFilters_[3]->init(roll);
particleFilters_[4]->init(pitch);
particleFilters_[5]->init(yaw);
}
if(_filteringStrategy == 1)
{
if(_force3DoF)
{
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;
}
initKalmanFilter(initialPose);
}
}
else
@@ -208,10 +196,38 @@ Transform Odometry::process(SensorData & data, OdometryInfo * info)
return Transform();
}
UTimer time;
Transform t = this->computeTransform(data, info);
double dt = data.stamp() - previousStamp_;
Transform guess;
if( !previousVelocityTransform_.isNull() &&
!previousVelocityTransform_.isIdentity())
{
if(guessFromMotion_)
{
if(_filteringStrategy == 1)
{
// use Kalman predict transform
float vx,vy,vz, vroll,vpitch,vyaw;
predictKalmanFilter(dt, &vx,&vy,&vz,&vroll,&vpitch,&vyaw);
guess = Transform(vx*dt, vy*dt, vz*dt, vroll*dt, vpitch*dt, vyaw*dt);
}
else
{
float vx,vy,vz, vroll,vpitch,vyaw;
previousVelocityTransform_.getTranslationAndEulerAngles(vx,vy,vz, vroll,vpitch,vyaw);
guess = Transform(vx*dt, vy*dt, vz*dt, vroll*dt, vpitch*dt, vyaw*dt);
}
}
else if(_filteringStrategy == 1)
{
predictKalmanFilter(dt);
}
}
previousVelocityTransform_.setNull();
previousStamp_ = data.stamp();
UTimer time;
Transform t = this->computeTransform(data, guess, info);
if(info)
{
info->timeEstimation = time.ticks();
@@ -230,69 +246,84 @@ Transform Odometry::process(SensorData & data, OdometryInfo * info)
}
}
previousTransform_.setIdentity();
previousStamp_ = data.stamp();
if(!t.isNull())
{
_resetCurrentCount = _resetCountdown;
if(_force3DoF || !_holonomic || filters_.size() || _filteringStrategy==1)
{
float x,y,z, roll,pitch,yaw;
t.getTranslationAndEulerAngles(x, y, z, roll, pitch, yaw);
float vx,vy,vz, vroll,vpitch,vyaw;
t.getTranslationAndEulerAngles(vx,vy,vz, vroll,vpitch,vyaw);
// transform to velocity
if(dt)
{
vx /= dt;
vy /= dt;
vz /= dt;
vroll /= dt;
vpitch /= dt;
vyaw /= dt;
}
if(_force3DoF || !_holonomic || particleFilters_.size() || _filteringStrategy==1)
{
if(_filteringStrategy == 1)
{
if(_pose.isIdentity())
{
// reset Kalman
initKalmanFilter();
if(t.isIdentity())
{
initKalmanFilter();
}
else
{
initKalmanFilter(t, vx,vy,vz,vroll,vpitch,vyaw);
}
}
else
{
// Kalman filtering
updateKalmanFilter(dt,x,y,z,roll,pitch,yaw);
updateKalmanFilter(vx,vy,vz,vroll,vpitch,vyaw);
}
}
else if(filters_.size())
else if(particleFilters_.size())
{
// Particle filtering
UASSERT(filters_.size()==6);
UASSERT(particleFilters_.size()==6);
if(_pose.isIdentity())
{
filters_[0]->init(x);
filters_[1]->init(y);
filters_[2]->init(z);
filters_[3]->init(roll);
filters_[4]->init(pitch);
filters_[5]->init(yaw);
particleFilters_[0]->init(vx);
particleFilters_[1]->init(vy);
particleFilters_[2]->init(vz);
particleFilters_[3]->init(vroll);
particleFilters_[4]->init(vpitch);
particleFilters_[5]->init(vyaw);
}
else
{
x = filters_[0]->filter(x);
y = filters_[1]->filter(y);
yaw = filters_[5]->filter(yaw);
vx = particleFilters_[0]->filter(vx);
vy = particleFilters_[1]->filter(vy);
vyaw = particleFilters_[5]->filter(vyaw);
if(!_holonomic)
{
// arc trajectory around ICR
float tmpY = yaw!=0.0f ? x / tan((CV_PI-yaw)/2.0f) : 0.0f;
if(fabs(tmpY) < fabs(y) || (tmpY<=0 && y >=0) || (tmpY>=0 && y<=0))
float tmpY = vyaw!=0.0f ? vx / tan((CV_PI-vyaw)/2.0f) : 0.0f;
if(fabs(tmpY) < fabs(vy) || (tmpY<=0 && vy >=0) || (tmpY>=0 && vy<=0))
{
y = tmpY;
vy = tmpY;
}
else
{
yaw = (atan(x/y)*2.0f-CV_PI)*-1;
vyaw = (atan(vx/vy)*2.0f-CV_PI)*-1;
}
}
if(!_force3DoF)
{
z = filters_[2]->filter(z);
roll = filters_[3]->filter(roll);
pitch = filters_[4]->filter(pitch);
vz = particleFilters_[2]->filter(vz);
vroll = particleFilters_[3]->filter(vroll);
vpitch = particleFilters_[4]->filter(vpitch);
}
}
@@ -300,30 +331,43 @@ Transform Odometry::process(SensorData & data, OdometryInfo * info)
{
info->timeParticleFiltering = time.ticks();
}
if(_force3DoF)
{
vz = 0.0f;
vroll = 0.0f;
vpitch = 0.0f;
}
}
else if(!_holonomic)
{
// arc trajectory around ICR
float tmpY = yaw!=0.0f ? x / tan((CV_PI-yaw)/2.0f) : 0.0f;
if(fabs(tmpY) < fabs(y) || (tmpY<=0 && y >=0) || (tmpY>=0 && y<=0))
float tmpY = vyaw!=0.0f ? vx / tan((CV_PI-vyaw)/2.0f) : 0.0f;
if(fabs(tmpY) < fabs(vy) || (tmpY<=0 && vy >=0) || (tmpY>=0 && vy<=0))
{
y = tmpY;
vy = tmpY;
}
else
{
yaw = (atan(x/y)*2.0f-CV_PI)*-1;
vyaw = (atan(vx/vy)*2.0f-CV_PI)*-1;
}
if(_force3DoF)
{
vz = 0.0f;
vroll = 0.0f;
vpitch = 0.0f;
}
}
UASSERT_MSG(uIsFinite(x) && uIsFinite(y) && uIsFinite(z) &&
uIsFinite(roll) && uIsFinite(pitch) && uIsFinite(yaw),
uFormat("x=%f y=%f z=%f roll=%f pitch=%f yaw=%f org T=%s",
x, y, z, roll, pitch, yaw, t.prettyPrint().c_str()).c_str());
t = Transform(x,y,_force3DoF?0:z, _force3DoF?0:roll,_force3DoF?0:pitch,yaw);
info->transformFiltered = t;
t = Transform(vx*dt, vy*dt, vz*dt, vroll*dt, vpitch*dt, vyaw*dt);
if(info)
{
info->transformFiltered = t;
}
}
previousTransform_ = t;
previousVelocityTransform_ = Transform(vx, vy, vz, vroll, vpitch, vyaw);
if(info)
{
distanceTravelled_ += t.getNorm();
@@ -347,7 +391,7 @@ Transform Odometry::process(SensorData & data, OdometryInfo * info)
return Transform();
}
void Odometry::initKalmanFilter()
void Odometry::initKalmanFilter(const Transform & initialPose, float vx, float vy, float vz, float vroll, float vpitch, float vyaw)
{
UDEBUG("");
// See OpenCV tutorial: http://docs.opencv.org/master/dc/d2c/tutorial_real_time_pose.html
@@ -355,48 +399,91 @@ void Odometry::initKalmanFilter()
// 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)
int nMeasurements = 6; // the number of measured states (x',y',z',roll',pitch',yaw')
if(_force3DoF)
{
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)
nMeasurements = 3; // the number of measured states (x',y',yaw')
}
int nInputs = 0; // the number of action control
/* From viso2, measurement covariance
* static const boost::array<double, 36> STANDARD_POSE_COVARIANCE =
{ { 0.1, 0, 0, 0, 0, 0,
0, 0.1, 0, 0, 0, 0,
0, 0, 0.1, 0, 0, 0,
0, 0, 0, 0.17, 0, 0,
0, 0, 0, 0, 0.17, 0,
0, 0, 0, 0, 0, 0.17 } };
static const boost::array<double, 36> STANDARD_TWIST_COVARIANCE =
{ { 0.05, 0, 0, 0, 0, 0,
0, 0.05, 0, 0, 0, 0,
0, 0, 0.05, 0, 0, 0,
0, 0, 0, 0.09, 0, 0,
0, 0, 0, 0, 0.09, 0,
0, 0, 0, 0, 0, 0.09 } };
*/
kalmanFilter_.init(nStates, nMeasurements, nInputs); // init Kalman Filter
cv::setIdentity(kalmanFilter_.processNoiseCov, cv::Scalar::all(_kalmanProcessNoise)); // set process noise
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
float x,y,z,roll,pitch,yaw;
initialPose.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
if(_force3DoF)
{
/* 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
/* MEASUREMENT MODEL (velocity) */
// [0 0 1 0 0 0 0 0 0]
// [0 0 0 1 0 0 0 0 0]
// [0 0 0 0 0 0 0 1 0]
kalmanFilter_.measurementMatrix.at<float>(0,2) = 1; // x'
kalmanFilter_.measurementMatrix.at<float>(1,3) = 1; // y'
kalmanFilter_.measurementMatrix.at<float>(2,7) = 1; // yaw'
kalmanFilter_.statePost.at<float>(0) = x;
kalmanFilter_.statePost.at<float>(1) = y;
kalmanFilter_.statePost.at<float>(6) = yaw;
kalmanFilter_.statePost.at<float>(2) = vx;
kalmanFilter_.statePost.at<float>(3) = vy;
kalmanFilter_.statePost.at<float>(7) = vyaw;
}
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
/* MEASUREMENT MODEL (velocity) */
// [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 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]
kalmanFilter_.measurementMatrix.at<float>(0,3) = 1; // x'
kalmanFilter_.measurementMatrix.at<float>(1,4) = 1; // y'
kalmanFilter_.measurementMatrix.at<float>(2,5) = 1; // z'
kalmanFilter_.measurementMatrix.at<float>(3,12) = 1; // roll'
kalmanFilter_.measurementMatrix.at<float>(4,13) = 1; // pitch'
kalmanFilter_.measurementMatrix.at<float>(5,14) = 1; // yaw'
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;
kalmanFilter_.statePost.at<float>(3) = vx;
kalmanFilter_.statePost.at<float>(4) = vy;
kalmanFilter_.statePost.at<float>(5) = vz;
kalmanFilter_.statePost.at<float>(12) = vroll;
kalmanFilter_.statePost.at<float>(13) = vpitch;
kalmanFilter_.statePost.at<float>(14) = vyaw;
}
}
void Odometry::updateKalmanFilter(float dt, float & x, float & y, float & z, float & roll, float & pitch, float & yaw)
void Odometry::predictKalmanFilter(float dt, float * vx, float * vy, float * vz, float * vroll, float * vpitch, float * vyaw)
{
// Set transition matrix with current dt
if(_force3DoF)
@@ -464,48 +551,57 @@ void Odometry::updateKalmanFilter(float dt, float & x, float & y, float & z, flo
kalmanFilter_.transitionMatrix.at<float>(11,17) = 0.5*pow(dt,2);
}
// First predict, to update the internal statePre variable
UDEBUG("Predict");
const cv::Mat & prediction = kalmanFilter_.predict();
if(vx)
*vx = prediction.at<float>(3); // x'
if(vy)
*vy = prediction.at<float>(4); // y'
if(vz)
*vz = _force3DoF?0.0f:prediction.at<float>(5); // z'
if(vroll)
*vroll = _force3DoF?0.0f:prediction.at<float>(12); // roll'
if(vpitch)
*vpitch = _force3DoF?0.0f:prediction.at<float>(13); // pitch'
if(vyaw)
*vyaw = prediction.at<float>(_force3DoF?7:14); // yaw'
}
void Odometry::updateKalmanFilter(float & vx, float & vy, float & vz, float & vroll, float & vpitch, float & vyaw)
{
// Set measurement to predict
cv::Mat measurements;
if(!_force3DoF)
{
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
measurements.at<float>(0) = vx; // x'
measurements.at<float>(1) = vy; // y'
measurements.at<float>(2) = vz; // z'
measurements.at<float>(3) = vroll; // roll'
measurements.at<float>(4) = vpitch; // pitch'
measurements.at<float>(5) = vyaw; // 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
measurements.at<float>(0) = vx; // x'
measurements.at<float>(1) = vy; // y'
measurements.at<float>(2) = vyaw; // 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);
const cv::Mat & estimated = kalmanFilter_.correct(measurements);
if(_force3DoF)
{
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);
}
vx = estimated.at<float>(3); // x'
vy = estimated.at<float>(4); // y'
vz = _force3DoF?0.0f:estimated.at<float>(5); // z'
vroll = _force3DoF?0.0f:estimated.at<float>(12); // roll'
vpitch = _force3DoF?0.0f:estimated.at<float>(13); // pitch'
vyaw = estimated.at<float>(_force3DoF?7:14); // yaw'
}
} /* namespace rtabmap */

6
corelib/src/OdometryF2F.cpp
View File

@@ -37,12 +37,10 @@ namespace rtabmap {
OdometryF2F::OdometryF2F(const ParametersMap & parameters) :
Odometry(parameters),
keyFrameThr_(Parameters::defaultOdomF2FKeyFrameThr()),
guessFromMotion_(Parameters::defaultOdomGuessMotion()),
motionSinceLastKeyFrame_(Transform::getIdentity())
{
registrationPipeline_ = Registration::create(parameters);
Parameters::parse(parameters, Parameters::kOdomF2FKeyFrameThr(), keyFrameThr_);
Parameters::parse(parameters, Parameters::kOdomGuessMotion(), guessFromMotion_);
}
OdometryF2F::~OdometryF2F()
@@ -60,6 +58,7 @@ void OdometryF2F::reset(const Transform & initialPose)
// return not null transform if odometry is correctly computed
Transform OdometryF2F::computeTransform(
SensorData & data,
const Transform & guess,
OdometryInfo * info)
{
UTimer timer;
@@ -84,7 +83,7 @@ Transform OdometryF2F::computeTransform(
output = registrationPipeline_->computeTransformationMod(
refFrame_,
newFrame,
guessFromMotion_&&!this->previousTransform().isNull()?motionSinceLastKeyFrame_*this->previousTransform():Transform(),
!guess.isNull()?motionSinceLastKeyFrame_*guess:Transform(),
&regInfo);
if(info && this->isInfoDataFilled())
@@ -188,6 +187,7 @@ Transform OdometryF2F::computeTransform(
info->inliers = regInfo.inliers;
info->icpInliersRatio = regInfo.icpInliersRatio;
info->matches = regInfo.matches;
info->features = refFrame_.sensorData().keypoints().size();
}
UINFO("Odom update time = %fs lost=%s inliers=%d, ref frame corners=%d, transform accepted=%s",

9
corelib/src/OdometryF2M.cpp
View File

@@ -39,6 +39,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/util3d.h"
#include "rtabmap/utilite/ULogger.h"
#include "rtabmap/utilite/UTimer.h"
#include "rtabmap/utilite/UMath.h"
#include "rtabmap/utilite/UConversion.h"
#include <opencv2/calib3d/calib3d.hpp>
#include <rtabmap/core/OdometryF2M.h>
@@ -167,6 +168,7 @@ void OdometryF2M::reset(const Transform & initialPose)
// return not null transform if odometry is correctly computed
Transform OdometryF2M::computeTransform(
SensorData & data,
const Transform & guess,
OdometryInfo * info)
{
UTimer timer;
@@ -188,9 +190,12 @@ Transform OdometryF2M::computeTransform(
{
if(map_->getWords3().size() && lastFrame_->sensorData().isValid())
{
Transform guess = this->previousTransform().isIdentity()||this->previousTransform().isNull()?Transform():this->getPose()*this->previousTransform();
Signature tmpMap = *map_;
Transform transform = regVis_->computeTransformationMod(tmpMap, *lastFrame_, guess, &regInfo);
Transform transform = regVis_->computeTransformationMod(
tmpMap,
*lastFrame_,
guess.isNull()?Transform():this->getPose()*guess,
&regInfo);
data.setFeatures(lastFrame_->sensorData().keypoints(), lastFrame_->sensorData().descriptors());

12
corelib/src/OdometryMono.cpp
View File

@@ -170,7 +170,7 @@ void OdometryMono::reset(const Transform & initialPose)
keyFramePoses_.clear();
}
Transform OdometryMono::computeTransform(SensorData & data, OdometryInfo * info)
Transform OdometryMono::computeTransform(SensorData & data, const Transform & guess, OdometryInfo * info)
{
Transform output;
@@ -229,14 +229,14 @@ Transform OdometryMono::computeTransform(SensorData & data, OdometryInfo * info)
if((int)newS->getWords().size() > minInliers_)
{
cv::Mat K = cameraModel.K();
Transform guess = (this->getPose() * cameraModel.localTransform()).inverse();
Transform pnpGuess = ((this->getPose() * (guess.isNull()?Transform::getIdentity():guess)) * cameraModel.localTransform()).inverse();
cv::Mat R = (cv::Mat_<double>(3,3) <<
(double)guess.r11(), (double)guess.r12(), (double)guess.r13(),
(double)guess.r21(), (double)guess.r22(), (double)guess.r23(),
(double)guess.r31(), (double)guess.r32(), (double)guess.r33());
(double)pnpGuess.r11(), (double)pnpGuess.r12(), (double)pnpGuess.r13(),
(double)pnpGuess.r21(), (double)pnpGuess.r22(), (double)pnpGuess.r23(),
(double)pnpGuess.r31(), (double)pnpGuess.r32(), (double)pnpGuess.r33());
cv::Mat rvec(1,3, CV_64FC1);
cv::Rodrigues(R, rvec);
cv::Mat tvec = (cv::Mat_<double>(1,3) << (double)guess.x(), (double)guess.y(), (double)guess.z());
cv::Mat tvec = (cv::Mat_<double>(1,3) << (double)pnpGuess.x(), (double)pnpGuess.y(), (double)pnpGuess.z());
std::vector<cv::Point3f> objectPoints;
std::vector<cv::Point2f> imagePoints;

8
corelib/src/Stereo.cpp
View File

@@ -94,8 +94,7 @@ std::vector<cv::Point2f> Stereo::computeCorrespondences(
StereoOpticalFlow::StereoOpticalFlow(const ParametersMap & parameters) :
Stereo(parameters),
epsilon_(Parameters::defaultStereoEps()),
maxSlope_(Parameters::defaultStereoMaxSlope())
epsilon_(Parameters::defaultStereoEps())
{
this->parseParameters(parameters);
}
@@ -104,7 +103,6 @@ void StereoOpticalFlow::parseParameters(const ParametersMap & parameters)
{
Stereo::parseParameters(parameters);
Parameters::parse(parameters, Parameters::kStereoEps(), epsilon_);
Parameters::parse(parameters, Parameters::kStereoMaxSlope(), maxSlope_);
}
@@ -135,9 +133,7 @@ std::vector<cv::Point2f> StereoOpticalFlow::computeCorrespondences(
if(status[i]!=0)
{
float disparity = leftCorners[i].x - rightCorners[i].x;
float slope = fabs((leftCorners[i].y-rightCorners[i].y) / (leftCorners[i].x-rightCorners[i].x));
if(disparity < float(this->minDisparity()) || disparity > float(this->maxDisparity()) ||
(maxSlope_ > 0.0f && fabs(leftCorners[i].y-rightCorners[i].y) > 1.0f && slope > maxSlope_))
if(disparity < float(this->minDisparity()) || disparity > float(this->maxDisparity()))
{
status[i] = 0;
}