openslam_gmapping: motionmodel.cpp Source File

Go to the documentation of this file.
00001 #include <gmapping/gridfastslam/motionmodel.h>
00002 #include <gmapping/utils/stat.h>
00003 #include <iostream>
00004 
00005 #define MotionModelConditioningLinearCovariance 0.01
00006 #define MotionModelConditioningAngularCovariance 0.001
00007 
00008 namespace GMapping {
00009 
00010 
00011 
00012 OrientedPoint 
00013 MotionModel::drawFromMotion (const OrientedPoint& p, double linearMove, double angularMove) const{
00014         OrientedPoint n(p);
00015         double lm=linearMove  + fabs( linearMove ) * sampleGaussian( srr ) + fabs( angularMove ) * sampleGaussian( str );
00016         double am=angularMove + fabs( linearMove ) * sampleGaussian( srt ) + fabs( angularMove ) * sampleGaussian( stt );
00017         n.x+=lm*cos(n.theta+.5*am);
00018         n.y+=lm*sin(n.theta+.5*am);
00019         n.theta+=am;
00020         n.theta=atan2(sin(n.theta), cos(n.theta));
00021         return n;
00022 }
00023 
00024 OrientedPoint 
00025 MotionModel::drawFromMotion(const OrientedPoint& p, const OrientedPoint& pnew, const OrientedPoint& pold) const{
00026         double sxy=0.3*srr;
00027         OrientedPoint delta=absoluteDifference(pnew, pold);
00028         OrientedPoint noisypoint(delta);
00029         noisypoint.x+=sampleGaussian(srr*fabs(delta.x)+str*fabs(delta.theta)+sxy*fabs(delta.y));
00030         noisypoint.y+=sampleGaussian(srr*fabs(delta.y)+str*fabs(delta.theta)+sxy*fabs(delta.x));
00031         noisypoint.theta+=sampleGaussian(stt*fabs(delta.theta)+srt*sqrt(delta.x*delta.x+delta.y*delta.y));
00032         noisypoint.theta=fmod(noisypoint.theta, 2*M_PI);
00033         if (noisypoint.theta>M_PI)
00034                 noisypoint.theta-=2*M_PI;
00035         return absoluteSum(p,noisypoint);
00036 }
00037 
00038 
00039 /*
00040 OrientedPoint 
00041 MotionModel::drawFromMotion(const OrientedPoint& p, const OrientedPoint& pnew, const OrientedPoint& pold) const{
00042         
00043         //compute the three stps needed for perfectly matching the two poses if the noise is absent
00044         
00045         OrientedPoint delta=pnew-pold;
00046         double aoffset=atan2(delta.y, delta.x);
00047         double alpha1=aoffset-pold.theta;
00048         alpha1=atan2(sin(alpha1), cos(alpha1));
00049         double rho=sqrt(delta*delta);
00050         double alpha2=pnew.theta-aoffset;
00051         alpha2=atan2(sin(alpha2), cos(alpha2));
00052         
00053         OrientedPoint pret=drawFromMotion(p, 0, alpha1);
00054         pret=drawFromMotion(pret, rho, 0);
00055         pret=drawFromMotion(pret, 0, alpha2);
00056         return pret;
00057 }
00058 */
00059 
00060 
00061 Covariance3 MotionModel::gaussianApproximation(const OrientedPoint& pnew, const OrientedPoint& pold) const{
00062         OrientedPoint delta=absoluteDifference(pnew,pold);
00063         double linearMove=sqrt(delta.x*delta.x+delta.y*delta.y);
00064         double angularMove=fabs(delta.x);
00065         double s11=srr*srr*linearMove*linearMove;
00066         double s22=stt*stt*angularMove*angularMove;
00067         double s12=str*angularMove*srt*linearMove;
00068         Covariance3 cov;
00069         double s=sin(pold.theta),c=cos(pold.theta);
00070         cov.xx=c*c*s11+MotionModelConditioningLinearCovariance;
00071         cov.yy=s*s*s11+MotionModelConditioningLinearCovariance;
00072         cov.tt=s22+MotionModelConditioningAngularCovariance;
00073         cov.xy=s*c*s11;
00074         cov.xt=c*s12;
00075         cov.yt=s*s12;
00076         return cov;
00077 }
00078 
00079 };
00080