X-lanni
81 lines
2.6 KiB
C++
81 lines
2.6 KiB
C++
#include <gmapping/gridfastslam/motionmodel.h>
|
|
#include <gmapping/utils/stat.h>
|
|
#include <iostream>
|
|
|
|
#define MotionModelConditioningLinearCovariance 0.01
|
|
#define MotionModelConditioningAngularCovariance 0.001
|
|
|
|
namespace GMapping {
|
|
|
|
|
|
|
|
OrientedPoint
|
|
MotionModel::drawFromMotion (const OrientedPoint& p, double linearMove, double angularMove) const{
|
|
OrientedPoint n(p);
|
|
double lm=linearMove + fabs( linearMove ) * sampleGaussian( srr ) + fabs( angularMove ) * sampleGaussian( str );
|
|
double am=angularMove + fabs( linearMove ) * sampleGaussian( srt ) + fabs( angularMove ) * sampleGaussian( stt );
|
|
n.x+=lm*cos(n.theta+.5*am);
|
|
n.y+=lm*sin(n.theta+.5*am);
|
|
n.theta+=am;
|
|
n.theta=atan2(sin(n.theta), cos(n.theta));
|
|
return n;
|
|
}
|
|
|
|
OrientedPoint
|
|
MotionModel::drawFromMotion(const OrientedPoint& p, const OrientedPoint& pnew, const OrientedPoint& pold) const{
|
|
double sxy=0.3*srr;
|
|
OrientedPoint delta=absoluteDifference(pnew, pold);
|
|
OrientedPoint noisypoint(delta);
|
|
noisypoint.x+=sampleGaussian(srr*fabs(delta.x)+str*fabs(delta.theta)+sxy*fabs(delta.y));
|
|
noisypoint.y+=sampleGaussian(srr*fabs(delta.y)+str*fabs(delta.theta)+sxy*fabs(delta.x));
|
|
noisypoint.theta+=sampleGaussian(stt*fabs(delta.theta)+srt*sqrt(delta.x*delta.x+delta.y*delta.y));
|
|
noisypoint.theta=fmod(noisypoint.theta, 2*M_PI);
|
|
if (noisypoint.theta>M_PI)
|
|
noisypoint.theta-=2*M_PI;
|
|
return absoluteSum(p,noisypoint);
|
|
}
|
|
|
|
|
|
/*
|
|
OrientedPoint
|
|
MotionModel::drawFromMotion(const OrientedPoint& p, const OrientedPoint& pnew, const OrientedPoint& pold) const{
|
|
|
|
//compute the three stps needed for perfectly matching the two poses if the noise is absent
|
|
|
|
OrientedPoint delta=pnew-pold;
|
|
double aoffset=atan2(delta.y, delta.x);
|
|
double alpha1=aoffset-pold.theta;
|
|
alpha1=atan2(sin(alpha1), cos(alpha1));
|
|
double rho=sqrt(delta*delta);
|
|
double alpha2=pnew.theta-aoffset;
|
|
alpha2=atan2(sin(alpha2), cos(alpha2));
|
|
|
|
OrientedPoint pret=drawFromMotion(p, 0, alpha1);
|
|
pret=drawFromMotion(pret, rho, 0);
|
|
pret=drawFromMotion(pret, 0, alpha2);
|
|
return pret;
|
|
}
|
|
*/
|
|
|
|
|
|
Covariance3 MotionModel::gaussianApproximation(const OrientedPoint& pnew, const OrientedPoint& pold) const{
|
|
OrientedPoint delta=absoluteDifference(pnew,pold);
|
|
double linearMove=sqrt(delta.x*delta.x+delta.y*delta.y);
|
|
double angularMove=fabs(delta.x);
|
|
double s11=srr*srr*linearMove*linearMove;
|
|
double s22=stt*stt*angularMove*angularMove;
|
|
double s12=str*angularMove*srt*linearMove;
|
|
Covariance3 cov;
|
|
double s=sin(pold.theta),c=cos(pold.theta);
|
|
cov.xx=c*c*s11+MotionModelConditioningLinearCovariance;
|
|
cov.yy=s*s*s11+MotionModelConditioningLinearCovariance;
|
|
cov.tt=s22+MotionModelConditioningAngularCovariance;
|
|
cov.xy=s*c*s11;
|
|
cov.xt=c*s12;
|
|
cov.yt=s*s12;
|
|
return cov;
|
|
}
|
|
|
|
};
|
|
|