add slam_gmapping

2025-06-06 16:15:07 +08:00
parent 9187b9fb85
commit a7b75c31cb
141 changed files with 15992 additions and 0 deletions
@@ -0,0 +1,80 @@
+#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;
+}
+
+};
+