资源简介
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\pla
文件 20226 2008-03-04 13:18 mpf_slam final\pla
文件 20259 2008-03-04 12:50 mpf_slam final\pla
文件 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\
相关资源
- 发那科fanuc机器人robot(电气调试启动
- Geometrical methods in robotics
-
Robot fr
amework3.0中文手册 - RoboWare Studio中文版(2018).pdf
- SLAM综述文章
- ORB-SLAM2 论文pdf
- Microsoft_Robotics_Developer_Studio中文教程
- 基于三维激光雷达的自动驾驶车辆高
- ORBSLAM可以用来保存地图和重载地图
- 2.Robotics Modelling Planning and Control
- (Roboguide离线编程及程序导入
- RM2019机甲大师赛官方赠与步兵代码开
- 视觉SLAM十四讲.pdf.zip
- WebRobot1.8
- cartographer-master-19.zip
- RobotStudio 机器人离线仿真建模中文操
- 基于STM32的智能服务型多功能多足机器
- ABB ROBOTSTUDIO
- modern robotics.pdf
- sankyo robot teaching
- Robot Modeling and Control First Edition
- ORB-SLAM2源码中文详解.pdf
- 视觉SLAM综述
- RikiRobot Stm32驱动板开发使用说明
- Robot Programming A Guide to Controlling Auton
- 基于ROS的移动机器人SLAM研究
- Robot Modeling and Control
- ROS Robotics by Example
- State Estimation for Robotics-A Matrix Lie Gro
- State Estimation for Robotics A Matrix Lie Gro
评论
共有 条评论