机器人SLAM算法源代码

PALYER/STAGE 平台下的机器人SLAM算法的源代码

#include “util.h“

static const double PI=3.1415926535 PIx2=6.283185307;
static const double R2D=57.29577951;

//======================Geometry functions==============================

//convert an angle to [02*pi） rad
double angle（double a1）
{
  while（a1>=PIx2） a1-=PIx2;
  while（a1<=-PIx2） a1+=PIx2;
  return a1;
}

//calculate the absolute difference （d） between angles d=[0pi）
double absAngleDiff（double a2 double a1）
{
  a1=angle（a1）;
  a2=angle（a2）;
  double ans=fabs（a2-a1）;
  if（ans>PI） ans=PIx2-ans;
  return ans;
}

//calculate the difference （d） between angles d=[-pipi）
double angleDiff（double a2 double a1）
{
  a1=angle（a1）;
  a2=angle（a2）;
  double ans=angle（a2-a1）;
  if（ans>PI） ans=ans-PIx2;
  return ans;
}

// static double euclDist（Particle &a1 Particle &a2）
// {
//   return sqrt（（a1.est.x-a2.est.x）*（a1.est.x-a2.est.x）+（a1.est.y-a2.est.y）*（a1.est.y-a2.est.y））;
// }

double euclDist（double &x1 double &y1 double &x2 double &y2）
{
  return sqrt（（x1-x2）*（x1-x2）+（y1-y2）*（y1-y2））;
}

//a=sqrt（b^2+c^2-2*b*c*cos（A））
double cosineRule（double b double c double A）
{
  return sqrt（b*b+c*c-2*b*c*cos（A））;
}

//======================General functions==============================
double sign（double x）
{
  if（x>0.0） return 1.0;
  return -1.0;
}



int minDblVec（vector &seq double &minval）
{
  int minidx=0;
  minval=seq[0];
  for（size_t i=1; i    if（seq[i]      minval=seq[i];
      minidx=i;
    }
  }
  return minidx;
}


//======================Probability functions==============================

//generate random normal number
//modifed from float gasdev（long *idum） in 
//1986-92 Numerical Recipes Software
double randn48（void）
{
  float ran1（long *idum）;
  static int iset=0;
  static float gset;
  float facrsqv1v2;
  
  if  （iset == 0） {
    do {
      v1=2.0*drand48（）-1.0;
      v2=2.0*drand48（）-1.0;
      rsq=v1*v1+v2*v2;
    } while （rsq >= 1.0 || rsq == 0.0）;
    fac=sqrt（-2.0*log（rsq）/rsq）;
    gset=v1*fac;
    iset=1;
    return v2*fac;
  } else {
    iset=0;
    return gset;
  }
}

double normalPDF（double x double mu double sigma）
{ 
  const double kNormal=0.39894228; //1/sqrt（2*pi）
  double z=（x-mu）/sigma;
  return kNormal/sqrt（sigma）*exp（-0.5*z*z）;
}

double stdBiNormalPDF（double x1 double x2）
{
 // 1/[2*pi]  const for independent bivariate distribution 
  const double kBiNormal=0.159154943;
  return kBiNormal*exp（-0.5*（x1*x1+x2*x2））;
}

double stdTriNormalPDF（double x1 double x2 double x3）
{
  // 1/[2*sqrt（2）*pi^（3/2）]  const for independent trivariate distribution 
  const double kTriNormal=0.06349363592; 
  return kTriNormal*exp（-0.5*（x1*x1+x2*x2+x3*x3））;
}

double laplacePDF（double x double m double b）
{
  return 0.5/b*exp（-fabs（x-m）/b）;
}

double cauchyPDF（double x double a double b）
{
  double val=（x-a）/b;
  return 1.0/（b*PI*（1.0+val*val））;
}


double randNormal（double m double s）
{
  return randn48（）*s+m;
}

double randLaplace（double m double b）
{
  double D=drand48（）;
  

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     文件        2897  2007-08-31 15:55  mpf_slam final\args.h
     文件        5534  2007-12-20 13:44  mpf_slam final\classes.h
     文件        5545  2005-02-03 22:14  mpf_slam final\gdb_stl_utils
     文件        3917  2007-11-09 13:15  mpf_slam final\gen_util.cpp
     文件       68949  2007-12-18 11:42  mpf_slam final\gmon.out
     文件         264  2008-03-04 12:49  mpf_slam final\input.txt
     文件           0  2008-03-04 13:19  mpf_slam final\Localisation.txt
     文件         706  2007-12-18 11:43  mpf_slam final\Makefile
     文件       12984  2008-03-05 15:45  mpf_slam final\mpfconfig.cpp
     文件       28596  2008-03-05 15:49  mpf_slam final\mpf_cylfeature.cpp
     文件        7386  2007-11-08 12:40  mpf_slam final\particle_util.cpp
     文件     1031036  2008-03-04 13:18  mpf_slam final\playerjoyMPF
     文件       20226  2008-03-04 13:18  mpf_slam final\playerjoyMPF.cc
     文件       20259  2008-03-04 12:50  mpf_slam final\playerjoyMPF.cc~
     文件        5809  2008-03-05 15:50  mpf_slam final\pmpf.cpp
     文件         244  2008-03-05 15:48  mpf_slam final\Readme.txt
     文件        1922  2005-02-03 22:14  mpf_slam final\StlStdContainers.cc
     文件        2335  2007-10-18 11:41  mpf_slam final\util.h
     目录           0  2008-07-29 17:01  mpf_slam final\