motionmodel.cpp
Go to the documentation of this file.
2 #include <gmapping/utils/stat.h>
3 #include <iostream>
4 
5 #define MotionModelConditioningLinearCovariance 0.01
6 #define MotionModelConditioningAngularCovariance 0.001
7 
8 namespace GMapping {
9 
10 
11 
13 MotionModel::drawFromMotion (const OrientedPoint& p, double linearMove, double angularMove) const{
14  OrientedPoint n(p);
15  double lm=linearMove + fabs( linearMove ) * sampleGaussian( srr ) + fabs( angularMove ) * sampleGaussian( str );
16  double am=angularMove + fabs( linearMove ) * sampleGaussian( srt ) + fabs( angularMove ) * sampleGaussian( stt );
17  n.x+=lm*cos(n.theta+.5*am);
18  n.y+=lm*sin(n.theta+.5*am);
19  n.theta+=am;
20  n.theta=atan2(sin(n.theta), cos(n.theta));
21  return n;
22 }
23 
25 MotionModel::drawFromMotion(const OrientedPoint& p, const OrientedPoint& pnew, const OrientedPoint& pold) const{
26  double sxy=0.3*srr;
28  OrientedPoint noisypoint(delta);
29  noisypoint.x+=sampleGaussian(srr*fabs(delta.x)+str*fabs(delta.theta)+sxy*fabs(delta.y));
30  noisypoint.y+=sampleGaussian(srr*fabs(delta.y)+str*fabs(delta.theta)+sxy*fabs(delta.x));
31  noisypoint.theta+=sampleGaussian(stt*fabs(delta.theta)+srt*sqrt(delta.x*delta.x+delta.y*delta.y));
32  noisypoint.theta=fmod(noisypoint.theta, 2*M_PI);
33  if (noisypoint.theta>M_PI)
34  noisypoint.theta-=2*M_PI;
35  return absoluteSum(p,noisypoint);
36 }
37 
38 
39 /*
40 OrientedPoint
41 MotionModel::drawFromMotion(const OrientedPoint& p, const OrientedPoint& pnew, const OrientedPoint& pold) const{
42 
43  //compute the three stps needed for perfectly matching the two poses if the noise is absent
44 
45  OrientedPoint delta=pnew-pold;
46  double aoffset=atan2(delta.y, delta.x);
47  double alpha1=aoffset-pold.theta;
48  alpha1=atan2(sin(alpha1), cos(alpha1));
49  double rho=sqrt(delta*delta);
50  double alpha2=pnew.theta-aoffset;
51  alpha2=atan2(sin(alpha2), cos(alpha2));
52 
53  OrientedPoint pret=drawFromMotion(p, 0, alpha1);
54  pret=drawFromMotion(pret, rho, 0);
55  pret=drawFromMotion(pret, 0, alpha2);
56  return pret;
57 }
58 */
59 
60 
63  double linearMove=sqrt(delta.x*delta.x+delta.y*delta.y);
64  double angularMove=fabs(delta.x);
65  double s11=srr*srr*linearMove*linearMove;
66  double s22=stt*stt*angularMove*angularMove;
67  double s12=str*angularMove*srt*linearMove;
68  Covariance3 cov;
69  double s=sin(pold.theta),c=cos(pold.theta);
73  cov.xy=s*c*s11;
74  cov.xt=c*s12;
75  cov.yt=s*s12;
76  return cov;
77 }
78 
79 };
80 
const char *const *argv double delta
Definition: gfs2stream.cpp:19
OrientedPoint drawFromMotion(const OrientedPoint &p, double linearMove, double angularMove) const
Definition: motionmodel.cpp:13
double UTILS_EXPORT sampleGaussian(double sigma, unsigned long int S=0)
Definition: stat.cpp:48
#define MotionModelConditioningAngularCovariance
Definition: motionmodel.cpp:6
orientedpoint< T, A > absoluteSum(const orientedpoint< T, A > &p1, const orientedpoint< T, A > &p2)
Definition: point.h:116
orientedpoint< T, A > absoluteDifference(const orientedpoint< T, A > &p1, const orientedpoint< T, A > &p2)
Definition: point.h:107
Covariance3 gaussianApproximation(const OrientedPoint &pnew, const OrientedPoint &pold) const
Definition: motionmodel.cpp:61
unsigned int c
Definition: gfs2stream.cpp:41
#define MotionModelConditioningLinearCovariance
Definition: motionmodel.cpp:5
#define n
Definition: eig3.cpp:11
orientedpoint< double, double > OrientedPoint
Definition: point.h:203


openslam_gmapping
Author(s): Cyrill Stachniss, Udo Frese, Giorgio Grisetti, Wolfram Burgard
autogenerated on Mon Feb 28 2022 22:59:20