├── src ├── sensor │ ├── odometryreading.cpp │ ├── sensor.cpp │ ├── odometrysensor.cpp │ ├── sensorreading.cpp │ ├── rangesensor.cpp │ └── rangereading.cpp ├── scanmatcher │ ├── smmap.cpp │ └── scanmatcher.cpp ├── motionmodel │ └── motionmodel.cpp ├── particlefilter │ └── particlefilter.cpp ├── utils │ └── stat.cpp └── gridfastslam │ ├── gridslamprocessor_tree.cpp │ └── gridslamprocessor.cpp ├── include ├── grid │ ├── accessstate.h │ ├── array2d.h │ ├── harray2d.h │ └── map.h ├── sensor │ ├── odometrysensor.h │ ├── sensor.h │ ├── sensorreading.h │ ├── odometryreading.h │ ├── rangereading.h │ └── rangesensor.h ├── motionmodel │ └── motionmodel.h ├── utils │ ├── macro_params.h │ ├── autoptr.h │ ├── stat.h │ └── point.h ├── scanmatcher │ ├── smmap.h │ └── scanmatcher.h ├── gridfastslam │ ├── gridslamprocessor.hxx │ └── gridslamprocessor.h └── particlefilter │ └── particlefilter.h └── README.md /src/sensor/odometryreading.cpp: -------------------------------------------------------------------------------- 1 | #include "sensor/odometryreading.h" 2 | 3 | namespace Gmapping { 4 | 5 | OdometryReading::OdometryReading( const OdometrySensor* odo, double time ): SensorReading( odo, time ){ 6 | } 7 | 8 | } // end namespace -------------------------------------------------------------------------------- /src/sensor/sensor.cpp: -------------------------------------------------------------------------------- 1 | #include "sensor/sensor.h" 2 | 3 | namespace Gmapping { 4 | 5 | Sensor::Sensor( const string& name ) { 6 | m_name = name; 7 | } 8 | 9 | Sensor::~Sensor() { 10 | } 11 | 12 | } // end namespace -------------------------------------------------------------------------------- /src/sensor/odometrysensor.cpp: -------------------------------------------------------------------------------- 1 | #include "sensor/odometrysensor.h" 2 | 3 | namespace Gmapping { 4 | 5 | OdometrySensor::OdometrySensor( const std::string& name, bool ideal ): Sensor(name) { 6 | m_ideal = ideal; 7 | } 8 | 9 | } // end namespace -------------------------------------------------------------------------------- /include/grid/accessstate.h: -------------------------------------------------------------------------------- 1 | #ifndef ACCESSTATE_H 2 | #define ACCESSTATE_H 3 | 4 | namespace GMapping { 5 | /** 6 | * AccessibilityState，枚举变量，表示Cell的状态 7 | * Cell，地图中每个单元（栅格）的数据类型 8 | */ 9 | enum AccessibilityState{Outside=0x0, Inside=0x1, Allocated=0x2}; 10 | }; 11 | 12 | #endif -------------------------------------------------------------------------------- /src/sensor/sensorreading.cpp: -------------------------------------------------------------------------------- 1 | #include "sensor/sensorreading.h" 2 | 3 | namespace Gmapping { 4 | 5 | SensorReading::SensorReading( const Sensor* s, double time ) { 6 | m_sensor = s; 7 | m_time = time; 8 | } 9 | 10 | SensorReading::~SensorReading() { 11 | } 12 | 13 | } // end namespace -------------------------------------------------------------------------------- /src/scanmatcher/smmap.cpp: -------------------------------------------------------------------------------- 1 | #include "scanmatcher/smmap.h" 2 | 3 | namespace Gmapping { 4 | 5 | /** 6 | * 利用静态指针unknown_ptr（默认为0）获取一个Cell（栅格） 7 | * 有点像链式结构？？？ 8 | */ 9 | const PointAccumulator& PointAccumulator::Unknown() { 10 | if ( !unknown_ptr ) 11 | unknown_ptr = new PointAccumulator; 12 | return *unknown_ptr; 13 | } 14 | 15 | PointAccumulator* PointAccumulator::unknown_ptr = 0; 16 | 17 | } // end namespace -------------------------------------------------------------------------------- /include/sensor/odometrysensor.h: -------------------------------------------------------------------------------- 1 | #ifndef ODOMETRYSENSOR_H 2 | #define ODOMETRYSENSOR_H 3 | #include "sensor.h" 4 | 5 | namespace Gmapping { 6 | 7 | /** 8 | * OdometrySensor，里程计类，继承自传感器基类 9 | * 数据成员： 10 | * m_ideal，标明是否是理想的里程计 11 | * 函数成员： 12 | * isIdeal()，判断当前里程计是否为理想（误差为0）的传感器 13 | */ 14 | class OdometrySensor: public Sensor { 15 | public: 16 | OdometrySensor( const std::string& name, bool ideal=false ); 17 | 18 | inline bool isIdeal() const { return m_ideal; } 19 | protected: 20 | bool m_ideal; 21 | }; 22 | 23 | } // end namespace 24 | 25 | #endif -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # readGMappingSLAM 2 | 3 | Gmapping SLAM算法源代码阅读，加了一些注释，去掉了一些有的没的。 4 | 5 | GMapping SLAM源码，参考： 6 | 7 | >C++源代码：https://github.com/ros-perception/openslam_gmapping.git 8 | 9 | >ROS Package：https://github.com/ros-perception/slam_gmapping.git 10 | 11 | 相关文章，参考： 12 | 13 | - [GMAPPING SLAM基础、原理与实现（一）：机器人底盘与轮式里程计](https://luolichq.github.io/2019/10/16/gmappingslam-basis-principle-impl-01/) 14 | - [GMAPPING SLAM基础、原理与实现（二）：激光雷达与数据获取](https://luolichq.github.io/2019/10/17/gmappingslam-basis-principle-impl-02/) 15 | - [GMAPPING SLAM基础、原理与实现（三）：移动机器人SLAM数学模型](https://luolichq.github.io/2019/10/17/gmappingslam-basis-principle-impl-03/) 16 | - [GMAPPING SLAM基础、原理与实现（四）：概率学基础知识、贝叶斯滤波器、以及粒子滤波器](https://luolichq.github.io/2019/10/18/gmappingslam-basis-principle-impl-04/) 17 | - [GMAPPING SLAM基础、原理与实现（五）：里程计运动模型更新采样，与激光雷达似然场观测模型](https://luolichq.github.io/2019/10/19/gmappingslam-basis-principle-impl-05/) 18 | -------------------------------------------------------------------------------- /include/sensor/sensor.h: -------------------------------------------------------------------------------- 1 | #ifndef SENSOR_H 2 | #define SENSOR_H 3 | #include 4 | #include 5 | 6 | namespace Gmapping { 7 | 8 | /** 9 | * Sensor，传感器基类，包括里程计，激光雷达等传感器都可由此基类派生 10 | * 数据成员： 11 | * m_name，传感器名字 12 | * 函数成员： 13 | * getName()，获取传感器的名字 14 | * setName(), 设置传感器的名字 15 | */ 16 | class Sensor { 17 | public: 18 | Sensor( const std::string& name="" ); 19 | virtual ~Sensor(); 20 | 21 | // inline 内联函数减少开销，成员函数名后加const表示this指向的对象不能在函数内部被修改 22 | inline std::string getName() const { return m_name; } 23 | inline void setName( const std::string& name ) { m_name = name; } 24 | protected: 25 | std::string m_name; 26 | }; 27 | 28 | // 传感器的map容器类型别名定义 29 | // 车子上有多少传感器都可以存入map容器里面 30 | typedef std::map SensorMap; 31 | 32 | } // end namespace 33 | 34 | 35 | #endif -------------------------------------------------------------------------------- /include/sensor/sensorreading.h: -------------------------------------------------------------------------------- 1 | #ifndef SENSORREADING_H 2 | #define SENSORREADING_H 3 | #include "sensor.h" 4 | 5 | namespace Gmapping { 6 | 7 | /** 8 | * SensorReading，传感器数据读取基类 9 | * SLAM问题不太关心传感器的原理，只要能读取出对应时刻下的数据即可 10 | * 数据成员： 11 | * m_time，传感器读取的时间 12 | * m_sensor，传感器对象 13 | * 函数成员： 14 | * getTime()，获取传感器读数的读取时刻 15 | * setTime()，设置传感器读数的读取时刻 16 | * getSensor()，获取当前传感器对象 17 | */ 18 | class SensorReading { 19 | public: 20 | SensorReading( const Sensor* s=0, double time=0 ); 21 | virtual ~SensorReading(); 22 | 23 | inline double getTime() const { return m_time; } 24 | inline void setTime( double time ) { m_time=time; } 25 | inline Sensor* getSensor() const { return m_sensor; } 26 | protected 27 | double m_time; 28 | const Sensor* m_sensor; 29 | }; 30 | 31 | } // end namespace 32 | 33 | #endif -------------------------------------------------------------------------------- /include/motionmodel/motionmodel.h: -------------------------------------------------------------------------------- 1 | #ifndef MOTIONMODEL_H 2 | #define MOTIONMODEL_H 3 | #include "utils/point.h"、 4 | #include "utils/stat.h" 5 | 6 | namespace Gmapping { 7 | 8 | /** 9 | * MotionModel，里程计运动模型类，主要是运动模型的更新和采样 10 | * 数据成员： 11 | * srr，线性运动造成的线性误差的方差 12 | * str，旋转运动造成的线性误差的方差 13 | * srt，线性运动造成的角度误差的方差 14 | * stt，旋转运动造成的角度误差的方差 15 | * 函数成员： 16 | * drawFromMotion(), 里程计运动模型采样函数（重载，运动直接更新，或运动分解更新） 17 | * 18 | */ 19 | class MotionModel { 20 | public: 21 | double srr, str, srt, stt; 22 | 23 | OrientedPoint drawFromMotion( const OrientedPoint& p, const OrientedPoint& pnew, const OrientedPoint& pold ) const; 24 | OrientedPoint drawFromMotion( const OrientedPoint& p, double linearMove, double angularMove ) const; 25 | Covariance3 gaussianApproximation(const OrientedPoint& pnew, const OrientedPoint& pold) const; 26 | }; 27 | 28 | } // end namespace 29 | 30 | #endif -------------------------------------------------------------------------------- /include/sensor/odometryreading.h: -------------------------------------------------------------------------------- 1 | #ifndef ODOMETRYREADING_H 2 | #define ODOMETRYREADING_H 3 | #include "sensorreading.h" 4 | #include "odometrysensor.h" 5 | #include "utils/point.h" 6 | 7 | namespace Gmapping { 8 | 9 | /** 10 | * OdometryReading，里程计数据读取类，继承自传感器数据读取基类 11 | * 读取一帧里程计信息 12 | * 数据成员： 13 | * m_pose，位姿 14 | * m_speed，速度 15 | * m_acceleration，加速度 16 | * 函数成员： 17 | * getPose()，获取里程计传递过来的位姿 18 | * getSpeed()，获取里程计传递过来的速度 19 | * getAcceleration()，获取里程计传递过来的加速度 20 | * setPose()，设置位姿 21 | * setSpeed()，设置速度 22 | * setAcceleration()，设置加速度 23 | */ 24 | class OdometryReading: public SensorReading { 25 | public: 26 | OdometryReading( const OdometrySensor* odo, double time=0 ); 27 | 28 | inline const OrientedPoint& getPose() const { return m_pose; } 29 | // 成员函数名之前加const，表示返回的对象是一个const对象 30 | inline const OrientedPoint& getSpeed() const { return m_speed; } 31 | inline const OrientedPoint& getAcceleration() const { return m_acceleration; } 32 | inline void setPose( const OrientedPoint& pose ) { m_pose = pose; } 33 | inline void setSpeed( const OrientedPoint& speed ) { m_speed = speed; } 34 | inline void setAcceleration( const OrientedPoint& acceleration ) { m_acceleration = acceleration; } 35 | protected: 36 | OrientedPoint m_pose; 37 | OrientedPoint m_speed; 38 | OrientedPoint m_acceleration; 39 | }; 40 | 41 | } // end namespace 42 | 43 | #endif -------------------------------------------------------------------------------- /include/sensor/rangereading.h: -------------------------------------------------------------------------------- 1 | #ifndef RANGEREADING_H 2 | #define RANGEREADING_H 3 | #include "sensorreading.h" 4 | #include "rangesensor.h" 5 | #include "utils/point.h" 6 | 7 | namespace Gmapping { 8 | 9 | /** 10 | * RangeReading，激光传感器数据读取类，继承自传感器数据读取基类 11 | * 读取激光传感器的 距离数据 与 角度数据 12 | * 数据成员： 13 | * m_pose，机器人位姿（不是激光传感器的位姿） 14 | * m_dists，激光距离数据的 vector 容器，包含每一束激光所测的距离数据 15 | * m_beams，激光束的数量 16 | * m_angles，激光角度数据的 vector 容器，包含每一束激光对应的角度 17 | * 函数成员： 18 | * getPose()，读取这一帧激光数据对应的机器人位姿 19 | * setPose()，设置这一帧激光数据对应的机器人位姿 20 | * getSize()，返回激光束的数量 21 | * rawView()，返回density过滤之后的激光值 22 | * cartesianForm()，转换激光数据到笛卡尔坐标系下 23 | * activeBeams()，指定激光密度，有多少有效的激光 24 | */ 25 | class RangeReading: SensorReading { 26 | public: 27 | unsigned int m_beams; 28 | std::vector m_dists; 29 | std::vector m_angles; 30 | 31 | RangeReading( const RangeSensor* rs, double time=0 ); 32 | RangeReading( unsigned int n_beams, const double* d, const RangeSensor* rs, double time=0 ); 33 | RangeReading( unsigned int n_beams, const double* d, const double* angles, const RangeSensor* rs, double time=0 ); 34 | virtual ~RangeReading(); 35 | 36 | inline const OrientedPoint& getPose() const { return m_pose; } 37 | inline void setPose( const OrientedPoint& pose ) { m_pose = pose; } 38 | inline const unsigned int getSize() const { return m_beams; } 39 | unsigned int rawView( double* v, double density=0. ) const; 40 | std::vector cartesianForm( double maxRange=1e6 ) const; 41 | unsigned int activeBeams( double density=0. ) const; 42 | 43 | protected: 44 | OrientedPoint m_pose; 45 | } 46 | 47 | } // end namespace 48 | 49 | #endif -------------------------------------------------------------------------------- /include/sensor/rangesensor.h: -------------------------------------------------------------------------------- 1 | #ifndef RANGESENSOR_H 2 | #define RANGESENSOR_H 3 | #include 4 | #include "sensor.h" 5 | #include "utils/point.h" 6 | 7 | namespace Gmapping { 8 | 9 | /** 10 | * RangeSensor，激光传感器类，继承自传感器基类 11 | * 数据成员： 12 | * Beam，表示一束激光（又称作一个激光点）的struct结构体 13 | * newFormat，标志位，默认为false（不知道有啥用） 14 | * m_pose，激光传感器的位姿（相对于base_link） 15 | * m_beams，激光数据的vector容器，里面包含 beams_num 束激光（Beam） 16 | * 函数成员： 17 | * getPose()，获取激光传感器的位姿 18 | * beams()，返回所有的激光数据（重载） 19 | * updateBeamsLookup()，计算 m_beams 中每一束激光的sin值和cos值 20 | */ 21 | class RangeSensor: public Sensor { 22 | public: 23 | struct Beam 24 | { 25 | /* 一束激光对应的数据 */ 26 | OrientedPoint pose; // 相对于激光传感器中心的坐标(x,y,theta)，默认为(0,0,angle) 27 | double span; // span=0 表明是一个 0 线激光 28 | double maxRange; // 激光传感器最大测量范围 29 | double s,c; // 该束激光的sin值和cos值 30 | }; 31 | bool newFormat; 32 | 33 | RangeSensor( std::string name ); 34 | RangeSensor( std::string name, unsigned int beams_num, double res, const OrientedPoint& position=OrientedPoint(0,0,0), double span=0, double maxrange=89.0 ); 35 | RangeSensor( std::string name, unsigned int beams_num, double *angles, const OrientedPoint& position=OrientedPoint(0,0,0), double span=0, double maxrange=89.0 ); 36 | 37 | inline OrientedPoint getPose() const { return m_pose; } 38 | inline const std::vector& beams() const { return m_beams; } 39 | inline std::vector& beams() const { return m_beams; } 40 | void updateBeamsLookup(); 41 | protected: 42 | OrientedPoint m_pose; 43 | std::vector m_beams; 44 | }; 45 | 46 | 47 | } // end namespace 48 | 49 | #endif -------------------------------------------------------------------------------- /src/sensor/rangesensor.cpp: -------------------------------------------------------------------------------- 1 | #include "sensor/rangesensor.h" 2 | 3 | namespace Gmapping { 4 | 5 | RangeSensor::RangeSensor( std::string name ): Sensor( name ) { 6 | } 7 | 8 | /** 9 | * 构造函数，给定 res 激光的角度解析度 10 | * beams_num，激光束的数量 11 | * res，激光的角度解析度，用于求解激光角度 12 | */ 13 | RangeSensor::RangeSensor( std::string name, unsigned int beams_num, double res, const OrientedPoint& position, 14 | double span, double maxrange ): Sensor(name), m_pose(position), m_beams(beams_num) 15 | { // m_beams(beams_num)，这是一种构造size指定的vector的初始化方法 16 | double angle = -.5*res*beams_num; 17 | for ( unsigned int i=0; i FloatPoint; 28 | 29 | /**构造函数 */ 30 | PointAccumulator() : acc(0,0), n(0), visits(0) {} 31 | // 只有当i=-1时，返回一个默认的PointAccumulator类型对象 32 | PointAccumulator( int i ) : acc(0,0), n(0), visits(0) { assert(i==-1); } 33 | inline void update( bool value, const Point& p=Point(0,0) ); 34 | inline Point mean() const { return 1./n*Point(acc.x, acc.y); } 35 | inline operator double() const { return visits?(double)n*SIGHT_INC/(double)visits:-1; } 36 | inline void add( const PointAccumulator& p ) { acc=acc+p.acc; n+=p.n; visits+=p.visits; } 37 | inline double entropy() const; 38 | 39 | static const PointAccumulator& Unknown(); 40 | 41 | FloatPoint acc; 42 | int n, visits; 43 | static PointAccumulator* unknown_ptr; 44 | }; 45 | 46 | /** 47 | * 更新某个Cell的状态 48 | * 若被击中，则需要利用当前击中坐标对Cell的acc坐标值进行累加 49 | * 未被击中，则只对Cell的访问次数进行累加 50 | * value，表示是否被击中 51 | * p，表示被击中的坐标（世界坐标系） 52 | */ 53 | void PointAccumulator::update( bool value, const Point& p=Point(0,0) ) { 54 | if ( value ) { 55 | acc.x += static_cast(p.x); 56 | acc.y += static_cast(p.y); 57 | n++; 58 | visits += SIGHT_INC; 59 | } esle { 60 | visits ++; 61 | } 62 | } 63 | 64 | /** 65 | * 求熵运算 66 | * 先以计数总量n除以访问量visits，然后以二值分布的形式计算熵 67 | */ 68 | double PointAccumulator::entropy() { 69 | // 从未被访问过 70 | if ( !visits ) { 71 | return -log(.5); 72 | } 73 | // 一直被击中（击中次数/访问次数=1）或从未被击中 74 | if ( n==visits || n==0 ) { 75 | return 0; 76 | } 77 | // 求解被占用的概率（击中次数/访问次数） 78 | double x = (double)n*SIGHT_INC / (double)visits; 79 | // 以二值分布的形式计算熵 80 | return -(x*log(x) + (1-x)*log(1-x)); 81 | } 82 | 83 | // ScanMatcherMap，Scan-Matching中使用的地图数据类型 84 | // Cell类型为PointAccumulator，Storage存储数据类型为HierarchicalArray2D 85 | typedef Map< PointAccumulator, HierarchicalArray2D > ScanMatcherMap; 86 | } 87 | 88 | #endif -------------------------------------------------------------------------------- /src/motionmodel/motionmodel.cpp: -------------------------------------------------------------------------------- 1 | #include "motionmodel/motionmodel.h" 2 | 3 | namespace Gmapping { 4 | 5 | /** 6 | * 里程计运动模型采样函数，参考 《Probabilistic Robotics》 5.4.2节 7 | * p，之前t-1时刻估计的机器人位姿（世界坐标系下） 8 | * pnew，当前t时刻里程计读数（里程计坐标系下） 9 | * pold，之前t-1时刻里程计读数（里程计坐标系下） 10 | */ 11 | OrientedPoint MotionModel::drawFromMotion( const OrientedPoint& p, const OrientedPoint& pnew, const OrientedPoint& pold ) const { 12 | double sxy = 0.3*srr; 13 | 14 | // 观测值为u_t = ( pnew, pold )，先由观测值计算t-1时刻到t时刻的运动 (pnew - pold) 15 | // 并且由里程计坐标系转换到机器人底盘基准坐标系 16 | OrientedPoint delta = absoluteDifference( pnew, pold ); 17 | 18 | // 求解误差分布，并进行高斯采样（返回误差值） 19 | OrientedPoint noisypoint(delta); 20 | // 最终得到一个带噪声的点（估算的“真值” = 观测值 + 误差采样值） 21 | noisypoint.x += sampleGaussian( srr*fabs(delta.x) + str*fabs(delta.theta) + sxy*fabs(delta.y) ); 22 | noisypoint.y += sampleGaussian( srr*fabs(delta.y) + str*fabs(delta.theta) + sxy*fabs(delta.x) ); 23 | noisypoint.theta += sampleGaussian( srt*sqrt(delta.x*delta.x+delta.y*delta.y) + stt * delta.theta ); 24 | 25 | // 限制运动角度为(-pi, pi) 26 | noisypoint.theta = fmod( noisypoint.theta, 2*M_PI ); 27 | if ( noisypoint.theta > M_PI ) 28 | noisypoint.theta -= 2*M_PI; 29 | 30 | // 运动模型更新，得到当前t时刻估计的机器人位姿 31 | return absoluteSum(p, noisypoint); 32 | } 33 | 34 | /** 35 | * 里程计运动模型采样函数，参考 《Probabilistic Robotics》 5.4.2节 36 | * p，之前t-1时刻估计的机器人位姿（世界坐标系下） 37 | * linearMove，机器人的t-1时刻到t时刻的线性位移（世界坐标系下） 38 | * angularMove，机器人的t-1时刻到t时刻的角度位移（世界坐标系下） 39 | */ 40 | OrientedPoint MotionModel::drawFromMotion( const OrientedPoint& p, double linearMove, double angularMove ) const { 41 | 42 | double lm=linearMove + fabs( linearMove ) * sampleGaussian( srr ) + fabs( angularMove ) * sampleGaussian( str ); 43 | double am=angularMove + fabs( linearMove ) * sampleGaussian( srt ) + fabs( angularMove ) * sampleGaussian( stt ); 44 | n.x+=lm*cos(n.theta+.5*am); 45 | n.y+=lm*sin(n.theta+.5*am); 46 | n.theta+=am; 47 | n.theta=atan2(sin(n.theta), cos(n.theta)); 48 | return n; 49 | 50 | } 51 | 52 | Covariance3 gaussianApproximation(const OrientedPoint& pnew, const OrientedPoint& pold) const { 53 | OrientedPoint delta=absoluteDifference(pnew,pold); 54 | 55 | /*两个位置的线性位移和角度位移*/ 56 | double linearMove=sqrt(delta.x*delta.x+delta.y*delta.y); 57 | double angularMove=fabs(delta.x); 58 | 59 | 60 | double s11=srr*srr*linearMove*linearMove; 61 | double s22=stt*stt*angularMove*angularMove; 62 | double s12=str*angularMove*srt*linearMove; 63 | 64 | Covariance3 cov; 65 | double s=sin(pold.theta),c=cos(pold.theta); 66 | 67 | cov.xx=c*c*s11+MotionModelConditioningLinearCovariance; 68 | cov.yy=s*s*s11+MotionModelConditioningLinearCovariance; 69 | cov.tt=s22+MotionModelConditioningAngularCovariance; 70 | cov.xy=s*c*s11; 71 | cov.xt=c*s12; 72 | cov.yt=s*s12; 73 | 74 | return cov; 75 | } 76 | 77 | } // end namespace -------------------------------------------------------------------------------- /include/utils/autoptr.h: -------------------------------------------------------------------------------- 1 | #ifndef AUTOPTR_H 2 | #define AUTOPTR_H 3 | #include 4 | 5 | namespace GMapping{ 6 | /** 7 | * GMapping是十几年前的项目了，那时C++11的标准还没有出来 8 | * 所以Gmapping自定义了类autoptr实现了智能指针的机制 9 | * 数据成员： 10 | * m_reference，自定义结构体reference类型 11 | * 函数成员： 12 | * =，重载运算符"="，实现智能指针赋值语句 13 | * *，重载运算符"*"，这样就能以类似指针的形式来访问对象了 14 | */ 15 | template 16 | class autoptr{ 17 | protected: 18 | 19 | public: 20 | struct reference{ 21 | X* data; // 指向实际对象的指针 22 | unsigned int shares; // 计数器，记录了使用data所指对象的autoptr数量 23 | }; 24 | inline autoptr(X* p=(X*)(0)); 25 | inline autoptr(const autoptr& ap); 26 | inline autoptr& operator=(const autoptr& ap); 27 | inline ~autoptr(); 28 | inline operator int() const; 29 | inline X& operator*(); 30 | inline const X& operator*() const; 31 | //p 32 | reference * m_reference; 33 | protected: 34 | }; 35 | 36 | /** 37 | * 构造函数，给定指向X类型对象的指针 38 | */ 39 | template 40 | autoptr::autoptr(X* p){ 41 | m_reference=0; 42 | if (p){ 43 | m_reference=new reference; 44 | m_reference->data=p; 45 | m_reference->shares=1; 46 | } 47 | } 48 | 49 | /** 50 | * 构造函数，给定智能指针autoptr类型的对象 51 | * 每当通过拷贝的形式构造新的autoptr，就意味着多了一个autoptr的指针引用目标对象 52 | * 此时需要增加对象的计数shares 53 | */ 54 | template 55 | autoptr::autoptr(const autoptr& ap){ 56 | m_reference=0; 57 | reference* ref=ap.m_reference; 58 | if (ap.m_reference){ 59 | m_reference=ref; 60 | m_reference->shares++; 61 | } 62 | } 63 | 64 | /** 65 | * 重载运算符"="，实现智能指针赋值语句 66 | * ap，智能指针类型右值 67 | */ 68 | template 69 | autoptr& autoptr::operator=(const autoptr& ap){ 70 | reference* ref=ap.m_reference; 71 | // 左值本来就等于右值 72 | if (m_reference==ref){ 73 | return *this; 74 | } 75 | // 左值不为空时，且指向目标对象计数减一（若结果为0，则释放左值指向的资源） 76 | if (m_reference && !(--m_reference->shares)){ 77 | delete m_reference->data; 78 | delete m_reference; 79 | m_reference=0; 80 | } 81 | // 目标对象加一，并将右值赋给左值 82 | if (ref){ 83 | m_reference=ref; 84 | m_reference->shares++; 85 | } 86 | //20050802 nasty changes begin 87 | else 88 | m_reference=0; 89 | //20050802 nasty changes end 90 | return *this; 91 | } 92 | 93 | /** 94 | * 析构函数 95 | * 当一个autoptr对象的生命周期结束的时候，都意味着少了一个指向目标对象的指针，故减小目标对象的计数。 96 | * 当计数值减小到0的时候，说明不再有指针指向目标对象，其生命周期应该结束了。 97 | */ 98 | template 99 | autoptr::~autoptr(){ 100 | if (m_reference && !(--m_reference->shares)){ 101 | delete m_reference->data; 102 | delete m_reference; 103 | m_reference=0; 104 | } 105 | } 106 | 107 | template 108 | autoptr::operator int() const{ 109 | return m_reference && m_reference->shares && m_reference->data; 110 | } 111 | 112 | /** 113 | * 重载运算符"*"，这样就能以类似指针的形式来访问对象了 114 | */ 115 | template 116 | X& autoptr::operator*(){ 117 | assert(m_reference && m_reference->shares && m_reference->data); 118 | return *(m_reference->data); 119 | } 120 | 121 | /** 122 | * 重载运算符"*"，这样就能以类似指针的形式来访问对象了 123 | */ 124 | template 125 | const X& autoptr::operator*() const{ 126 | assert(m_reference && m_reference->shares && m_reference->data); 127 | return *(m_reference->data); 128 | } 129 | 130 | }; 131 | #endif 132 | -------------------------------------------------------------------------------- /include/utils/stat.h: -------------------------------------------------------------------------------- 1 | #ifndef STAT_H 2 | #define STAT_H 3 | #include "point.h" 4 | #include 5 | #include "gvalues.h" 6 | 7 | namespace GMapping { 8 | 9 | /**stupid utility function for drawing particles form a zero mean, sigma variance normal distribution 10 | probably it should not go there*/ 11 | double sampleGaussian(double sigma,unsigned int S=0); 12 | 13 | double evalGaussian(double sigmaSquare, double delta); 14 | double evalLogGaussian(double sigmaSquare, double delta); 15 | int sampleUniformInt(int max); 16 | double sampleUniformDouble(double min, double max); 17 | 18 | struct Covariance3{ 19 | Covariance3 operator + (const Covariance3 & cov) const; 20 | static Covariance3 zero; 21 | double xx, yy, tt, xy, xt, yt; 22 | }; 23 | 24 | struct EigenCovariance3{ 25 | EigenCovariance3(); 26 | EigenCovariance3(const Covariance3& c); 27 | EigenCovariance3 rotate(double angle) const; 28 | OrientedPoint sample() const; 29 | double eval[3]; 30 | double evec[3][3]; 31 | }; 32 | 33 | struct Gaussian3{ 34 | OrientedPoint mean; 35 | EigenCovariance3 covariance; 36 | Covariance3 cov; 37 | double eval(const OrientedPoint& p) const; 38 | void computeFromSamples(const std::vector & poses); 39 | void computeFromSamples(const std::vector & poses, const std::vector& weights ); 40 | }; 41 | 42 | template 43 | Gaussian3 computeGaussianFromSamples(PointIterator& pointBegin, PointIterator& pointEnd, WeightIterator& weightBegin, WeightIterator& weightEnd){ 44 | Gaussian3 gaussian; 45 | OrientedPoint mean=OrientedPoint(0,0,0); 46 | double wcum=0; 47 | double s=0, c=0; 48 | WeightIterator wt=weightBegin; 49 | double *w=new double(); 50 | OrientedPoint *p=new OrientedPoint(); 51 | for (PointIterator pt=pointBegin; pt!=pointEnd; pt++){ 52 | *w=*wt; 53 | *p=*pt; 54 | s+=*w*sin(p->theta); 55 | c+=*w*cos(p->theta); 56 | mean.x+=*w*p->x; 57 | mean.y+=*w*p->y; 58 | wcum+=*w; 59 | wt++; 60 | } 61 | mean.x/=wcum; 62 | mean.y/=wcum; 63 | s/=wcum; 64 | c/=wcum; 65 | mean.theta=atan2(s,c); 66 | 67 | Covariance3 cov=Covariance3::zero; 68 | wt=weightBegin; 69 | for (PointIterator pt=pointBegin; pt!=pointEnd; pt++){ 70 | *w=*wt; 71 | *p=*pt; 72 | OrientedPoint delta=(*p)-mean; 73 | delta.theta=atan2(sin(delta.theta),cos(delta.theta)); 74 | cov.xx+=*w*delta.x*delta.x; 75 | cov.yy+=*w*delta.y*delta.y; 76 | cov.tt+=*w*delta.theta*delta.theta; 77 | cov.xy+=*w*delta.x*delta.y; 78 | cov.yt+=*w*delta.y*delta.theta; 79 | cov.xt+=*w*delta.x*delta.theta; 80 | wt++; 81 | } 82 | cov.xx/=wcum; 83 | cov.yy/=wcum; 84 | cov.tt/=wcum; 85 | cov.xy/=wcum; 86 | cov.yt/=wcum; 87 | cov.xt/=wcum; 88 | EigenCovariance3 ecov(cov); 89 | gaussian.mean=mean; 90 | gaussian.covariance=ecov; 91 | gaussian.cov=cov; 92 | delete w; 93 | delete p; 94 | return gaussian; 95 | } 96 | 97 | template 98 | Gaussian3 computeGaussianFromSamples(PointIterator& pointBegin, PointIterator& pointEnd){ 99 | Gaussian3 gaussian; 100 | OrientedPoint mean=OrientedPoint(0,0,0); 101 | double wcum=1; 102 | double s=0, c=0; 103 | OrientedPoint *p=new OrientedPoint(); 104 | for (PointIterator pt=pointBegin; pt!=pointEnd; pt++){ 105 | *p=*pt; 106 | s+=sin(p->theta); 107 | c+=cos(p->theta); 108 | mean.x+=p->x; 109 | mean.y+=p->y; 110 | wcum+=1.; 111 | } 112 | mean.x/=wcum; 113 | mean.y/=wcum; 114 | s/=wcum; 115 | c/=wcum; 116 | mean.theta=atan2(s,c); 117 | 118 | Covariance3 cov=Covariance3::zero; 119 | for (PointIterator pt=pointBegin; pt!=pointEnd; pt++){ 120 | *p=*pt; 121 | OrientedPoint delta=(*p)-mean; 122 | delta.theta=atan2(sin(delta.theta),cos(delta.theta)); 123 | cov.xx+=delta.x*delta.x; 124 | cov.yy+=delta.y*delta.y; 125 | cov.tt+=delta.theta*delta.theta; 126 | cov.xy+=delta.x*delta.y; 127 | cov.yt+=delta.y*delta.theta; 128 | cov.xt+=delta.x*delta.theta; 129 | } 130 | cov.xx/=wcum; 131 | cov.yy/=wcum; 132 | cov.tt/=wcum; 133 | cov.xy/=wcum; 134 | cov.yt/=wcum; 135 | cov.xt/=wcum; 136 | EigenCovariance3 ecov(cov); 137 | gaussian.mean=mean; 138 | gaussian.covariance=ecov; 139 | gaussian.cov=cov; 140 | delete p; 141 | return gaussian; 142 | } 143 | 144 | 145 | }; //end namespace 146 | #endif 147 | 148 | -------------------------------------------------------------------------------- /src/particlefilter/particlefilter.cpp: -------------------------------------------------------------------------------- 1 | std::vector sistematicResampler::resample(const vector& particles) const{ 2 | Numeric cweight=0; 3 | 4 | //compute the cumulative weights 5 | unsigned int n=0; 6 | for (vector::const_iterator it=particles.begin(); it!=particles.end(); ++it){ 7 | cweight+=it->weight; 8 | n++; 9 | } 10 | 11 | //compute the interval 12 | Numeric interval=cweight/n; 13 | 14 | //compute the initial target weight 15 | Numeric target= 16 | //compute the resampled indexes 17 | 18 | cweight=0; 19 | std::vector indexes(n); 20 | n=0; 21 | unsigned int i=0; 22 | for (vector::const_iterator it=particles.begin(); it!=particles.end(); ++it, ++i){ 23 | cweight+=it->weight; 24 | while(cweight>target){ 25 | indexes[n++]=i; 26 | target+=interval; 27 | } 28 | } 29 | return indexes; 30 | } 31 | 32 | template 33 | std::vector indexResampler::resample(const vector >& weights) const{ 34 | Numeric cweight=0; 35 | 36 | //compute the cumulative weights 37 | unsigned int n=0; 38 | for (vector::const_iterator it=weights.begin(); it!=weights.end(); ++it){ 39 | cweight+=*it; 40 | n++; 41 | } 42 | 43 | //compute the interval 44 | Numeric interval=cweight/n; 45 | 46 | //compute the initial target weight 47 | Numeric target= 48 | //compute the resampled indexes 49 | 50 | cweight=0; 51 | std::vector indexes(n); 52 | n=0; 53 | unsigned int i=0; 54 | for (vector::const_iterator it=weights.begin(); it!=weights.end(); ++it, ++i){ 55 | cweight+=it->weight; 56 | while(cweight>target){ 57 | indexes[n++]=i; 58 | target+=interval; 59 | } 60 | } 61 | return indexes; 62 | } 63 | 64 | /* 65 | The following are patterns for the evolution and the observation classes 66 | The user should implement classes having the specified meaning 67 | template 68 | struct observer{ 69 | Observation& observation 70 | Numeric observe(const class State&) const; 71 | }; 72 | template 73 | struct evolver{ 74 | Input& input; 75 | State& evolve(const State& s); 76 | }; 77 | */ 78 | 79 | template 80 | void evolver::evolve(std::vector& particles) const{ 81 | for (std::vector::const_iterator it=particles.begin(); it!=particles.end(); ++it) 82 | *it=evolutionModel.evolve(*it); 83 | } 84 | 85 | void evolver::evolve(std::vector& dest, const std::vector& src) const{ 86 | dest.clear(); 87 | for (std::vector::const_iterator it=src.begin(); it!=src.end(); ++it) 88 | dest.push_back(evolutionModel.evolve(*it)); 89 | } 90 | 91 | template 92 | struct auxiliaryEvolver{ 93 | typedef particle Particle; 94 | 95 | EvolutionModel evolutionModel; 96 | QualificationModel qualificationModel; 97 | LikelyhoodModel likelyhoodModel; 98 | indexResampler resampler; 99 | 100 | void auxiliaryEvolver::evolve 101 | (std::vector&particles){ 102 | std::vector observationWeights(particles.size()); 103 | unsigned int i=0; 104 | for (std::vector::const_iterator it=particles.begin(); it!=particles.end(); ++it, i++){ 105 | observationWeights[i]=likelyhoodModel.likelyhood(qualificationModel.evolve(*it)); 106 | } 107 | std::vector indexes(indexResampler.resample(observationWeights)); 108 | for (std::vector::const_iterator it=indexes.begin(); it!=indexes.end(); it++){ 109 | Particle & particle=particles[*it]; 110 | particle=evolutionModel.evolve(particle); 111 | particle.weight*=lykelyhoodModel.lykelyhood(particle)/observationWeights[*it]; 112 | } 113 | } 114 | 115 | void auxiliaryEvolver::evolve 116 | (std::vector& dest, const std::vector& src){ 117 | dest.clear(); 118 | std::vector observationWeights(particles.size()); 119 | unsigned int i=0; 120 | for (std::vector::const_iterator it=src.begin(); it!=src.end(); ++it, i++){ 121 | observationWeights[i]=likelyhoodModel.likelyhood(qualificationModel.evolve(*it)); 122 | } 123 | std::vector indexes(indexResampler.resample(observationWeights)); 124 | for (std::vector::const_iterator it=indexes.begin(); it!=indexes.end(); it++){ 125 | Particle & particle=src[*it]; 126 | dest.push_back(evolutionModel.evolve(particle)); 127 | dest.back().weight*=likelyhoodModel.lykelyhood(particle)/observationWeights[*it]; 128 | } 129 | return dest(); 130 | } -------------------------------------------------------------------------------- /src/sensor/rangereading.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "sensor/rangereading.h" 3 | 4 | namespace Gmapping { 5 | 6 | RangeReading::RangeReading( const RangeSensor* rs, double time ): SensorReading( rs, time ) { 7 | } 8 | 9 | /** 10 | * 构造函数，只给定激光距离数据 11 | * n_beams，激光束的数量 12 | * d，激光距离数组 13 | */ 14 | RangeReading::RangeReading( unsigned int n_beams, const double* d, const RangeSensor* rs, 15 | double time ): SensorReading( rs, time ) 16 | { 17 | assert( n_beams == rs->beams().size() ); 18 | m_beams = n_beams; 19 | m_dists.resize(n_beams); 20 | for ( unsigned int i=0; ibeams().size() ); 35 | m_beams = n_beams; 36 | m_dists.resize(n_beams); 37 | m_angles.resize(n_beams); 38 | for ( unsigned int i=0; i(getSensor()); 66 | assert(rs); 67 | Point lp(cos(m_angles[i])*m_dists[i],sin(m_angles[i])*m_dists[i]); 68 | 69 | Point dp=lastPoint-lp; 70 | double distance=sqrt(dp*dp); 71 | /*只有两束激光的距离大于某一个值的之后，才是合法的*/ 72 | if (distance::max(); // 被过滤掉的激光的值，都被赋值为MAX 75 | suppressed++; 76 | } 77 | else 78 | { 79 | lastPoint=lp; 80 | v[i]=m_dists[i]; 81 | } 82 | } 83 | } 84 | // return size(); 85 | return static_cast(m_dists.size()); 86 | } 87 | 88 | unsigned int RangeReading::activeBeams(double density) const { 89 | if (density==0.) 90 | return m_dists.size(); 91 | 92 | int ab=0; 93 | Point lastPoint(0,0); 94 | uint suppressed=0; 95 | for (unsigned int i=0; i < m_dists.size(); i++) 96 | { 97 | const RangeSensor* rs=dynamic_cast(getSensor()); 98 | assert(rs); 99 | Point lp( 100 | cos(rs->beams()[i].pose.theta)*m_dists[i], 101 | sin(rs->beams()[i].pose.theta)*m_dists[i]); 102 | Point dp=lastPoint-lp; 103 | double distance=sqrt(dp*dp); 104 | /*两束激光过于接近 则视为无效*/ 105 | if (distance RangeReading::cartesianForm(double maxRange) const { 123 | const RangeSensor* rangeSensor=dynamic_cast(getSensor()); 124 | assert(rangeSensor && rangeSensor->beams().size()); 125 | // uint m_beams=rangeSensor->beams().size(); 126 | uint m_beams=static_cast(rangeSensor->beams().size()); 127 | std::vector cartesianPoints(m_beams); 128 | double px,py,ps,pc; 129 | px=rangeSensor->getPose().x; 130 | py=rangeSensor->getPose().y; 131 | ps=sin(rangeSensor->getPose().theta); 132 | pc=cos(rangeSensor->getPose().theta); 133 | for (unsigned int i=0; ibeams()[i].s; 137 | const double& c=rangeSensor->beams()[i].c; 138 | if (rho>=maxRange) 139 | { 140 | cartesianPoints[i]=Point(0,0); 141 | } 142 | else 143 | { 144 | Point p=Point(rangeSensor->beams()[i].pose.x+c*rho, rangeSensor->beams()[i].pose.y+s*rho); 145 | cartesianPoints[i].x=px+pc*p.x-ps*p.y; 146 | cartesianPoints[i].y=py+ps*p.x+pc*p.y; 147 | } 148 | } 149 | return cartesianPoints; 150 | } 151 | 152 | } // end namespace -------------------------------------------------------------------------------- /src/scanmatcher/scanmatcher.cpp: -------------------------------------------------------------------------------- 1 | #include "scanmatcher/scanmatcher.h" 2 | 3 | namespace Gmapping { 4 | 5 | using namespace std; 6 | 7 | const double ScanMatcher::nullLikelihood = -.5; 8 | 9 | /** 10 | * 构造函数 11 | */ 12 | ScanMatcher::ScanMatcher() : m_laserPose(0,0,0) { 13 | 14 | m_laserBeams = 0; 15 | //m_laserAngles=0; 16 | 17 | m_optRecursiveIterations=3; 18 | 19 | m_enlargeStep = 10.; 20 | m_fullnessThreshold = 0.1; 21 | 22 | m_angularOdometryReliability = 0.; 23 | m_linearOdometryReliability = 0.; 24 | 25 | m_freeCellRatio = sqrt(2.) 26 | 27 | m_initialBeamsSkip = 0; 28 | 29 | m_activeAreaComputed = false; 30 | 31 | // This are the dafault settings for a grid map of 5 cm 32 | m_llsamplerange = 0.01; 33 | m_llsamplestep = 0.01; 34 | m_lasamplerange = 0.005; 35 | m_lasamplestep = 0.005; 36 | 37 | /* 38 | // This are the dafault settings for a grid map of 10 cm 39 | m_llsamplerange=0.1; 40 | m_llsamplestep=0.1; 41 | m_lasamplerange=0.02; 42 | m_lasamplestep=0.01; 43 | */ 44 | 45 | /* 46 | // This are the dafault settings for a grid map of 20/25 cm 47 | m_llsamplerange=0.2; 48 | m_llsamplestep=0.1; 49 | m_lasamplerange=0.02; 50 | m_lasamplestep=0.01; 51 | m_generateMap=false; 52 | */ 53 | 54 | m_linePoints = new IntPoint[20000]; 55 | } 56 | 57 | /** 58 | * 析构函数 59 | */ 60 | ScanMatcher::~ScanMatcher() { 61 | delete [] m_linePoints; 62 | } 63 | 64 | /** 65 | * 使地图有效区域的计算失效 66 | * 每次调用computeActiveArea()之前，都必须要调用这个函数 67 | */ 68 | void ScanMatcher::invalidateActiveArea() { 69 | m_activeAreaComputed = false; 70 | } 71 | 72 | /** 设置激光雷达参数 */ 73 | void ScanMatcher::setLaserParameters( unsigned int beams, double* angles, const OrientedPoint& lpose ) { 74 | assert(beams < LASER_MAXBEAMS); 75 | m_laserPose = lpose; 76 | m_laserBeams = beams; 77 | //m_laserAngles=new double[beams]; 78 | memcpy( m_laserAngles, angles, sizeof(double)*m_laserBeams ); 79 | } 80 | 81 | /** 设置匹配参数 */ 82 | void ScanMatcher::setMatchingParameters( double urange, double range, 83 | double sigma, int kernsize, double lopt, double aopt, int iterations, 84 | double likelihoodSigma=1, unsigned int likelihoodSkip=0 ) 85 | { 86 | m_usableRange = urange; 87 | m_laserMaxRange = range; 88 | m_gaussianSigma = sigma; 89 | m_kernelSize = kernsize; 90 | m_optLinearDelta = lopt; 91 | m_optAngularDelta = aopt; 92 | m_optRecursiveIterations = iterations; 93 | m_likelihoodSigma = likelihoodSigma; 94 | m_likelihoodSkip = likelihoodSkip; 95 | } 96 | 97 | /** 98 | * 给定scan（激光雷达观测数据）和map（地图，作参考帧），利用似然场观测模型， 99 | * 优化里程计运动模型更新得到的估计位姿，利用该位姿迭代求解得到一个最优位姿 100 | * 这个是Gmapping中主要的Scan-Matching扫描匹配算法 101 | * pnew，优化得到的最优位姿（输出） 102 | * map，地图（参考帧） 103 | * init，当前时刻估计位姿（当前帧） 104 | * readings，激光雷达观测数据 105 | */ 106 | double ScanMatcher::optimize( OrientedPoint& pnew, const ScanMatcherMap& map, const OrientedPoint& init, const double* readings ) const { 107 | double bestScore = -1; 108 | OrientedPoint currentPose = init; 109 | // 利用激光雷达似然场观测模型进行扫描匹配，优化当前位姿 110 | // 即计算出一个初始的优化位姿（不一定是最优的）并返回对应的得分值 111 | double currentScore = score(map, currentPose, readings); 112 | 113 | // 每次迭代的角度增量，和线性位移增量 114 | double adelta = m_optAngularDelta, ldelta =m_optLinearDelta; 115 | // 无效迭代（这一次迭代算出来的得分比上一次要差）的次数 116 | unsigned int refinement = 0; 117 | // 搜索的方向，前，后，左，右，左转，右转 118 | enum Move { Front, Back, Left, Right, TurnLeft, TurnRight, Done }; 119 | // 搜索次数（每个方向算一次，一次不论有效，或无效） 120 | int c_iterations = 0 121 | do { 122 | // 如果这一次(currentScore)算出来比上一次(bestScore)差 123 | // 则有可能是走太多了，要减少搜索步长（减半） 124 | if ( bestScore >= currentScore ) { 125 | refinement++; 126 | adelta *= .5; 127 | ldelta *= .5; 128 | } 129 | bestScore = currentScore; 130 | OrientedPoint bestLocalPose = currentPose; 131 | OrientedPoint localPose = currentPose; 132 | // 将这6个方向都搜索一次，得到这6个方向中最好的一个位姿及其对应得分 133 | Move move = Front; 134 | do { 135 | localPose = currentPose; 136 | switch ( move ) { 137 | case Front: 138 | localPose.x += ldelta; 139 | move = Back; 140 | break; 141 | case Back: 142 | localPose.x -= ldelta; 143 | move = Left; 144 | break; 145 | case Left: 146 | localPose.y -= ldelta; 147 | move = Right; 148 | break; 149 | case Right: 150 | localPose.y += ldelta; 151 | move = TurnLeft; 152 | break; 153 | case TurnLeft: 154 | localPose.theta += adelta; 155 | move = TurnRight; 156 | break; 157 | case TurnRight: 158 | localPose.theta -= adelta; 159 | move = Done; 160 | break; 161 | default:; 162 | } 163 | // 计算当前方向的位姿（角度，线性位移）和原始估计位姿（init）的区别，区别越大增益越小 164 | // 若里程计比较可靠的话，则进行匹配时就需要对离原始估计位姿（init）比较远的位姿施加惩罚 165 | double odo_gain = 1; 166 | if ( m_angularOdometryReliability > 0. ) { 167 | double dth=init.theta-localPose.theta; dth=atan2(sin(dth), cos(dth)); dth*=dth; 168 | odo_gain*=exp(-m_angularOdometryReliability*dth); 169 | } 170 | if ( m_linearOdometryReliability > 0. ) { 171 | double dx=init.x-localPose.x; 172 | double dy=init.y-localPose.y; 173 | double drho=dx*dx+dy*dy; 174 | odo_gain*=exp(-m_linearOdometryReliability*drho); 175 | } 176 | // 计算当前方向的位姿对应的得分 177 | double localScore = odo_gain*score( map, localPose, readings ); 178 | // 若得分更好，则更新 179 | if ( localScore > currentScore ) { 180 | currentScore = localScore; 181 | bestLocalPose = localPose; 182 | } 183 | c_iterations++; 184 | } while( move != Done ); 185 | // 把当前6个方向中最好的位姿设置为最优位姿，若都6个方向无效的话，这个值不会被更新 186 | currentPose = bestLocalPose; 187 | // 这一次迭代得分更好，继续下一次迭代； 188 | // 或这一次迭代更差（无效迭代），修改角度和线性位移增量，继续下一次迭代，直至超出无效迭代次数的最大值 189 | } while ( currentScore > bestScore || refinement < m_optRecursiveIterations ); 190 | // 返回最优位姿及其对应得分 191 | pnew = currentPose; 192 | return bestScore; 193 | } 194 | 195 | 196 | 197 | } // end namespace -------------------------------------------------------------------------------- /include/gridfastslam/gridslamprocessor.hxx: -------------------------------------------------------------------------------- 1 | #ifdef MACOSX 2 | // This is to overcome a possible bug in Apple's GCC. 3 | #define isnan(x) (x==FP_NAN) 4 | #endif 5 | 6 | /** 7 | * 扫描匹配算法实现函数 8 | * 对每个粒子而言，在里程计运动模型采样更新得到的机器人位姿的基础上，通过优化来求解最优估计位姿 9 | * 或者说，通过最近的一次激光雷达数据（观测值），来优化proposal分布 10 | * 除此之外，还会计算优化之后，每个粒子的权重（这里权重用似然表示） 11 | */ 12 | inline void GridSlamProcessor::scanMatch( const double *plainReading ) { 13 | 14 | double sumScore = 0; 15 | for ( ParticleVector::ietrator it=m_particles.begin(); it!=m_particles.end(); it++ ) { 16 | OrientedPoint corrected; 17 | double score, l, s; 18 | // 给定scan（激光雷达观测数据）和map（地图，作参考帧），利用似然场观测模型， 19 | // 优化里程计运动模型更新得到的估计位姿，利用该位姿迭代求解得到一个最优位姿 20 | score = m_matcher.optimize( corrected, it->map, it->pose, plainReading ); 21 | // 如果优化成功，则更新该粒子的位姿为optimize()输出的最优估计位姿 22 | if ( score>m_minimumScore ) { 23 | it->pose = corrected; 24 | } else { 25 | // 如果优化失败，则仍使用之前里程计运动模型采样更新得到机器人位姿 26 | if (m_infoStream) { 27 | m_infoStream << "Scan Matching Failed, using odometry. Likelihood=" << l <map, it->pose, plainReading ); 35 | sumScore += score; 36 | // 一个粒子的权重并不是当前时刻的最优位姿对应的似然值来表示，而是所有时刻的似然值之和来表示 37 | it->weight += l; 38 | it->weightSum += l; 39 | // 计算出来最优的位姿之后，进行地图的扩充 这里不会进行内存分配 40 | // 不进行内存分配的原因是这些粒子进行重采样之后有可能会消失掉，因此在后面进行冲采样的时候统一进行内存分配。 41 | // 理论上来说，这里的操作是没有必要的，因为后面的重采样的时候还会进行一遍 42 | m_matcher.invalidateActiveArea(); 43 | m_matcher.computeActiveArea(m_particles[i].map, m_particles[i].pose, plainReading); 44 | } 45 | if (m_infoStream) 46 | m_infoStream << "Average Scan Matching Score=" << sumScore/m_particles.size() << std::endl; 47 | } 48 | 49 | /** 50 | * 权重归一化处理，同时计算出有效粒子数neff值 51 | */ 52 | inline void GridSlamProcessor::normalize() { 53 | 54 | // normalize the log m_weights 55 | double gain = 1./( m_obsSigmaGain*m_particles.size() ); 56 | // 返回编译器允许的double型数最大值 57 | double lmax = -(std::numeric_limits::max()); 58 | // 求所有粒子中的最大的权重，并将这个最大权重值暂存到lmax中 59 | for ( ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++ ) { 60 | lmax = it->weight>lmax?it->weight:lmax; 61 | } 62 | // cout << "!!!!!!!!!!! maxwaight= "<< lmax << endl; 63 | 64 | // 以最大权重为中心的高斯分布 65 | m_weights.clear(); 66 | double wcum = 0; 67 | m_neff = 0; 68 | for ( std::vector::iterator it=m_particles.begin(); it!=m_particles.end(); it++ ) { 69 | m_weights.push_back( exp( gain*(it->weight - lmax) ) ); 70 | wcum += m_weights.back(); 71 | // cout << "l=" << it->weight<< endl; 72 | } 73 | // 计算有效粒子数 和 归一化权重 74 | m_neff=0; 75 | for ( std::vector::iterator it=m_weights.begin(); it!=m_weights.end(); it++ ) { 76 | *it = *it/wcum; 77 | // 权重 wi = exp( (1/SigmaGain*N)*(wi - wmax) ) / sum( exp( (1/SigmaGain*N)*(wi - wmax) ) ) 78 | double w = *it; 79 | m_neff += w*w; 80 | } 81 | // 有效粒子数 neff = 1./ sum( wi*wi ) 82 | m_neff = 1./m_neff; 83 | } 84 | 85 | /** 86 | * 重采样（会用到 particlefilter.h 文件中的相关数据结构） 87 | * Neff值小于阈值时需要重采样，则所有保留下来的粒子的轨迹都加上一个新的节点，然后进行地图更新。 88 | * 否则不需要重采样，则所有的粒子的轨迹都加上一个新的节点，然后进行地图的更新 89 | */ 90 | inline bool GridSlamProcessor::resample(const double* plainReading, int adaptSize, const RangeReading* reading) { 91 | 92 | bool hasResampled = false; 93 | // 备份旧的粒子对应的轨迹树，即保留树的叶子节点，在增加新节点的时候使用 94 | TNodeVector oldGeneration; 95 | for ( unsigned int i=0; i resampler; 103 | // 保留的粒子会返回下标.里面的下标可能会重复，因为有些粒子会重复采样 104 | m_indexes = resampler.resampleIndexes( m_weights, adaptSize ); 105 | onResampleUpdate(); 106 | 107 | //BEGIN: BUILDING TREE 108 | ParticleVector temp; // 该数组暂存重采样之后的粒子群 109 | unsigned int j = 0; 110 | std::vector deletedParticles; // 要删除的粒子下标 111 | // 枚举每一个要被保留的粒子下标，并且找出要被删除的粒子下标 112 | for ( unsigned int i=0; ireading = reading; 129 | // temp数组暂存重采样之后的粒子群 130 | temp.push_back(p); 131 | temp.back().node = node; 132 | temp.back().previousIndex = m_indexes[i]; 133 | } 134 | // 处理那些重复的下标 135 | while ( jsetWeight(0); // 重采样后，每个粒子的权重都设置为相同的值，这里为0 148 | // 增加了一帧激光数据 因此需要更新地图 149 | m_matcher.invalidateActiveArea(); 150 | m_matcher.registerScan(it->map, it->pose, plainReading); 151 | m_particles.push_back(*it); 152 | } 153 | hasResampled = true; 154 | } else { 155 | // 否则不需要重采样，权值不变。只为轨迹创建一个新的节点 156 | int index = 0; 157 | TNodeVector::iterator node_it = oldGeneration.begin(); 158 | for ( ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++ ) { 159 | //create a new node in the particle tree and add it to the old tree 160 | //BEGIN: BUILDING TREE 161 | TNode* node = 0; 162 | node = new TNode( it->pose, 0.0, *node_it, 0 ); 163 | node->reading = reading; 164 | it->node = node; 165 | //END: BUILDING TREE 166 | m_matcher.invalidateActiveArea(); 167 | m_matcher.registerScan(it->map, it->pose, plainReading); 168 | it->previousIndex = index; 169 | index++; 170 | node_it++; 171 | } 172 | std::cerr << "Done" < 4 | #include 5 | #include 6 | #include "gvalues.h" 7 | 8 | #define DEBUG_STREAM cerr << __PRETTY_FUNCTION__ << ":" //FIXME 9 | 10 | namespace GMapping { 11 | 12 | /*结构体点*/ 13 | template 14 | struct point{ 15 | inline point():x(0),y(0) {} 16 | inline point(T _x, T _y):x(_x),y(_y){} 17 | T x, y; 18 | }; 19 | 20 | /*点的 + 运算符的重载*/ 21 | template 22 | inline point operator+(const point& p1, const point& p2){ 23 | return point(p1.x+p2.x, p1.y+p2.y); 24 | } 25 | 26 | /*点的 - 运算符的重载*/ 27 | template 28 | inline point operator - (const point & p1, const point & p2){ 29 | return point(p1.x-p2.x, p1.y-p2.y); 30 | } 31 | 32 | /*点的 * 运算符的重载 乘以一个数*/ 33 | template 34 | inline point operator * (const point& p, const T& v){ 35 | return point(p.x*v, p.y*v); 36 | } 37 | 38 | /*点的 * 运算符的重载 乘以一个数*/ 39 | template 40 | inline point operator * (const T& v, const point& p){ 41 | return point(p.x*v, p.y*v); 42 | } 43 | 44 | /*点的 * 运算符的重载 point的内积*/ 45 | template 46 | inline T operator * (const point& p1, const point& p2){ 47 | return p1.x*p2.x+p1.y*p2.y; 48 | } 49 | 50 | 51 | /*带方向的点 即除了位置之外，还有角度，可以理解为机器人的位姿*/ 52 | template 53 | struct orientedpoint: public point 54 | { 55 | inline orientedpoint() : point(0,0), theta(0) {}; 56 | inline orientedpoint(const point& p); 57 | inline orientedpoint(T x, T y, A _theta): point(x,y), theta(_theta){} 58 | inline void normalize(); 59 | 60 | inline orientedpoint rotate(A alpha) 61 | { 62 | T s=sin(alpha), c=cos(alpha); 63 | A a=alpha+theta; 64 | a=atan2(sin(a),cos(a)); 65 | return orientedpoint( 66 | c*this->x-s*this->y, 67 | s*this->x+c*this->y, 68 | a); 69 | } 70 | A theta; 71 | }; 72 | 73 | /*角度归一化 即把角度化成-PI~PI*/ 74 | template 75 | void orientedpoint::normalize() 76 | { 77 | if (theta >= -M_PI && theta < M_PI) 78 | return; 79 | 80 | int multiplier = (int)(theta / (2*M_PI)); 81 | theta = theta - multiplier*2*M_PI; 82 | if (theta >= M_PI) 83 | theta -= 2*M_PI; 84 | if (theta < -M_PI) 85 | theta += 2*M_PI; 86 | } 87 | 88 | 89 | /*构造函数*/ 90 | template 91 | orientedpoint::orientedpoint(const point& p){ 92 | this->x=p.x; 93 | this->y=p.y; 94 | this->theta=0.; 95 | } 96 | 97 | 98 | /*位姿的 + 操作符的重载*/ 99 | template 100 | orientedpoint operator+(const orientedpoint& p1, const orientedpoint& p2){ 101 | return orientedpoint(p1.x+p2.x, p1.y+p2.y, p1.theta+p2.theta); 102 | } 103 | 104 | /*位姿的 - 操作符的重载*/ 105 | template 106 | orientedpoint operator - (const orientedpoint & p1, const orientedpoint & p2){ 107 | return orientedpoint(p1.x-p2.x, p1.y-p2.y, p1.theta-p2.theta); 108 | } 109 | 110 | /*位姿的 * 操作符的重载 乘以一个数*/ 111 | template 112 | orientedpoint operator * (const orientedpoint& p, const T& v){ 113 | return orientedpoint(p.x*v, p.y*v, p.theta*v); 114 | } 115 | 116 | /*位姿的 * 操作符的重载 乘以一个数*/ 117 | template 118 | orientedpoint operator * (const T& v, const orientedpoint& p){ 119 | return orientedpoint(p.x*v, p.y*v, p.theta*v); 120 | } 121 | 122 | /* 123 | @desc 两个位姿的差值，算出来P1在以P2为原点的坐标系里面的坐标。 124 | */ 125 | template 126 | orientedpoint absoluteDifference(const orientedpoint& p1,const orientedpoint& p2) 127 | { 128 | orientedpoint delta=p1-p2; 129 | delta.theta=atan2(sin(delta.theta), cos(delta.theta)); // 为什么还要这样算一遍？ 130 | double s=sin(p2.theta), c=cos(p2.theta); 131 | return orientedpoint(c*delta.x+s*delta.y, 132 | -s*delta.x+c*delta.y, delta.theta); 133 | } 134 | 135 | /* 136 | @desc 两个位姿的和 p2表示增量。该函数表示在P1的位姿上，加上一个P2的增量 137 | */ 138 | template 139 | orientedpoint absoluteSum(const orientedpoint& p1,const orientedpoint& p2) 140 | { 141 | double s=sin(p1.theta), c=cos(p1.theta); 142 | return orientedpoint(c*p2.x-s*p2.y, 143 | s*p2.x+c*p2.y, p2.theta) + p1; 144 | } 145 | 146 | /*一个位姿和一个位置的和*/ 147 | template 148 | point absoluteSum(const orientedpoint& p1,const point& p2){ 149 | double s=sin(p1.theta), c=cos(p1.theta); 150 | return point(c*p2.x-s*p2.y, s*p2.x+c*p2.y) + (point) p1; 151 | } 152 | 153 | 154 | /*点的比较函数 一般用于排序*/ 155 | template 156 | struct pointcomparator{ 157 | bool operator ()(const point& a, const point& b) const { 158 | return a.x 164 | struct pointradialcomparator{ 165 | point origin; 166 | bool operator ()(const point& a, const point& b) const { 167 | point delta1=a-origin; 168 | point delta2=b-origin; 169 | return (atan2(delta1.y,delta1.x) 175 | inline point max(const point& p1, const point& p2){ 176 | point p=p1; 177 | p.x=p.x>p2.x?p.x:p2.x; 178 | p.y=p.y>p2.y?p.y:p2.y; 179 | return p; 180 | } 181 | 182 | /*把两个点的最小的X和最小的Y返回*/ 183 | template 184 | inline point min(const point& p1, const point& p2){ 185 | point p=p1; 186 | p.x=p.x 197 | inline point interpolate(const point& p1, const F& t1, const point& p2, const F& t2, const F& t3){ 198 | F gain=(t3-t1)/(t2-t1); 199 | point p=p1+(p2-p1)*gain; 200 | return p; 201 | } 202 | 203 | /* 204 | 两个位姿之间的插值 205 | T1.T2.T3的定义如上个函数 206 | */ 207 | template 208 | inline orientedpoint 209 | interpolate(const orientedpoint& p1, const F& t1, const orientedpoint& p2, const F& t2, const F& t3){ 210 | F gain=(t3-t1)/(t2-t1); 211 | orientedpoint p; 212 | p.x=p1.x+(p2.x-p1.x)*gain; 213 | p.y=p1.y+(p2.y-p1.y)*gain; 214 | double s=sin(p1.theta)+sin(p2.theta)*gain, 215 | c=cos(p1.theta)+cos(p2.theta)*gain; 216 | p.theta=atan2(s,c); 217 | return p; 218 | } 219 | 220 | 221 | /*两个点的欧式距离*/ 222 | template 223 | inline double euclidianDist(const point& p1, const point& p2){ 224 | return hypot(p1.x-p2.x, p1.y-p2.y); 225 | } 226 | 227 | /*两个位姿的欧式距离*/ 228 | template 229 | inline double euclidianDist(const orientedpoint& p1, const orientedpoint& p2){ 230 | return hypot(p1.x-p2.x, p1.y-p2.y); 231 | } 232 | 233 | template 234 | inline double squareDist(const orientedpoint& p1,const orientedpoint& p2) 235 | { 236 | return (p1.x - p2.x)*(p1.x-p2.x) + (p1.y-p2.y)*(p1.y-p2.y); 237 | } 238 | 239 | /*位姿和点的欧式距离*/ 240 | template 241 | inline double euclidianDist(const orientedpoint& p1, const point& p2){ 242 | return hypot(p1.x-p2.x, p1.y-p2.y); 243 | } 244 | 245 | /*点和位姿的欧式距离*/ 246 | template 247 | inline double euclidianDist(const point& p1, const orientedpoint& p2 ){ 248 | return hypot(p1.x-p2.x, p1.y-p2.y); 249 | } 250 | 251 | 252 | /*几个不同类型的重定向指令*/ 253 | typedef point IntPoint; 254 | typedef point Point; 255 | typedef orientedpoint OrientedPoint; 256 | 257 | }; //end namespace 258 | 259 | #endif 260 | -------------------------------------------------------------------------------- /include/grid/array2d.h: -------------------------------------------------------------------------------- 1 | #ifndef ARRAY2D_H 2 | #define ARRAY2D_H 3 | #include 4 | #include "accessstate.h" 5 | #include "assert.h" 6 | #include "utils/point.h" 7 | 8 | // 用于输出调试信息 9 | #ifndef __PRETTY_FUNCTION__ 10 | #define __FUNCDNAME__ 11 | #endif 12 | 13 | namespace Gmapping { 14 | 15 | /** 16 | * Array2D，二维数组类，用作栅格地图存储类型HierarchicalArray2D的基类 17 | * 栅格地图，由世界坐标系中xy坐标值映射得到，在栅格地图对应位置上可以用空闲或者被占用来标记 18 | * 很自然的，我们会想到使用一个矩阵或者说是二维数组的形式来描述地图。 19 | * 20 | * 模板参数： 21 | * Cell，地图中每个单元（栅格）的数据类型 22 | * Gmapping中实例为PointAccumulator类型 23 | * 对于DoubleArray2D来说，实例是double类型 24 | * 25 | * 数据成员： 26 | * m_cells，二维数组指针 27 | * m_xsize，二维数组的大小（栅格地图的长度） 28 | * m_ysize，二维数组的大小（栅格地图的宽度） 29 | * 30 | * 函数成员： 31 | * =，重载运算符，两个Array2D之间赋值 32 | * clear，清除二维数组中所有数据 33 | * resize，重定义二维数组的大小 34 | * isInside，判断给定坐标是否在二维数组内（x:0~m_xsize，y:0~m_ysize，重载函数） 35 | * cell，获取给定xy坐标处的Cell（重载函数） 36 | * cellState，获取给定xy坐标处Cell的状态（重载函数） 37 | * getPatchSize，获取Patch的大小，参考harray2d.hpp文件 38 | * getPatchMagnitude，获取Patch大小的等级，参考harray2d.hpp文件 39 | * getXSize，获取二维数组的长度 40 | * getYSize，获取二维数组的宽度 41 | * cells，获取二维数组 42 | */ 43 | template< class Cell, const bool debug=false > 44 | class Array2D { 45 | public: 46 | Cell** m_cells; 47 | 48 | Array2D( int xsize=0, int ysize=0 ); 49 | Array2D( const Array2D& ) 50 | ~Array2D(); 51 | 52 | Array2D& operator=( const Array2D& ); 53 | 54 | void clear(); 55 | void resize( int xmin, int ymin, int xmax, int ymax ); 56 | inline bool isInside( int x, int y ) const; 57 | inline bool isInside(const IntPoint& p) const { return isInside(p.x, p.y);} 58 | inline const Cell& cell( int x, int y ) const; 59 | inline Cell& cell( int x, int y ); 60 | inline const Cell& cell(const IntPoint& p) const {return cell(p.x,p.y);} 61 | inline Cell& cell(const IntPoint& p) {return cell(p.x,p.y);} 62 | inline AccessibilityState cellState( int x, int y ) const { 63 | return (AccessibilityState) (isInside(x,y)?(Inside|Allocated):Outside); 64 | } 65 | inline AccessibilityState cellState(const IntPoint& p) const { return cellState(p.x, p.y);} 66 | inline int getPatchSize() const { return 0; } // 已经是地图金字塔底层了，无Patch可言 67 | inline int getPatchMagnitude() const { return 0; } 68 | inline int getXSize() const { return m_xsize; } 69 | inline int getYSize() const { return m_ysize; } 70 | inline Cell** cells() { return m_cells; } 71 | 72 | protected: 73 | int m_xsize, m_ysize; 74 | }; 75 | 76 | /** 77 | * 构造函数，给二维数组（m_cells）分配存储空间 78 | */ 79 | template< class Cell, const bool debug > 80 | Array2D::Array2D( int xsize, int ysize ) { 81 | m_xsize = xsize; 82 | m_ysize = ysize; 83 | if ( m_xsize>0 && m_ysize>0 ) { 84 | m_cells = new Cell*[m_xsize]; 85 | for (int i=0; i 1.0 || w==0.0); 45 | 46 | return(sigma * x2 * sqrt(-2.0*log(w)/w)); 47 | } 48 | 49 | double sampleGaussian(double sigma, unsigned int S) { 50 | /* 51 | static gsl_rng * r = NULL; 52 | if(r==NULL) { 53 | gsl_rng_env_setup(); 54 | r = gsl_rng_alloc (gsl_rng_default); 55 | } 56 | */ 57 | if (S!=0) 58 | { 59 | //gsl_rng_set(r, S); 60 | srand(S); 61 | } 62 | if (sigma==0) 63 | return 0; 64 | //return gsl_ran_gaussian (r,sigma); 65 | return pf_ran_gaussian (sigma); 66 | } 67 | #if 0 68 | 69 | double evalGaussian(double sigmaSquare, double delta){ 70 | if (sigmaSquare<=0) 71 | sigmaSquare=1e-4; 72 | return exp(-.5*delta*delta/sigmaSquare)/sqrt(2*M_PI*sigmaSquare); 73 | } 74 | 75 | #endif 76 | double evalLogGaussian(double sigmaSquare, double delta){ 77 | if (sigmaSquare<=0) 78 | sigmaSquare=1e-4; 79 | return -.5*delta*delta/sigmaSquare-.5*log(2*M_PI*sigmaSquare); 80 | } 81 | #if 0 82 | 83 | 84 | Covariance3 Covariance3::zero={0.,0.,0.,0.,0.,0.}; 85 | 86 | Covariance3 Covariance3::operator + (const Covariance3 & cov) const{ 87 | Covariance3 r(*this); 88 | r.xx+=cov.xx; 89 | r.yy+=cov.yy; 90 | r.tt+=cov.tt; 91 | r.xy+=cov.xy; 92 | r.yt+=cov.yt; 93 | r.xt+=cov.xt; 94 | return r; 95 | } 96 | 97 | EigenCovariance3::EigenCovariance3(){} 98 | 99 | EigenCovariance3::EigenCovariance3(const Covariance3& cov){ 100 | static gsl_eigen_symmv_workspace * m_eigenspace=NULL; 101 | static gsl_matrix * m_cmat=NULL; 102 | static gsl_matrix * m_evec=NULL; 103 | static gsl_vector * m_eval=NULL; 104 | static gsl_vector * m_noise=NULL; 105 | static gsl_vector * m_pnoise=NULL; 106 | 107 | if (m_eigenspace==NULL){ 108 | m_eigenspace=gsl_eigen_symmv_alloc(3); 109 | m_cmat=gsl_matrix_alloc(3,3); 110 | m_evec=gsl_matrix_alloc(3,3); 111 | m_eval=gsl_vector_alloc(3); 112 | m_noise=gsl_vector_alloc(3); 113 | m_pnoise=gsl_vector_alloc(3); 114 | } 115 | 116 | gsl_matrix_set(m_cmat,0,0,cov.xx); gsl_matrix_set(m_cmat,0,1,cov.xy); gsl_matrix_set(m_cmat,0,2,cov.xt); 117 | gsl_matrix_set(m_cmat,1,0,cov.xy); gsl_matrix_set(m_cmat,1,1,cov.yy); gsl_matrix_set(m_cmat,1,2,cov.yt); 118 | gsl_matrix_set(m_cmat,2,0,cov.xt); gsl_matrix_set(m_cmat,2,1,cov.yt); gsl_matrix_set(m_cmat,2,2,cov.tt); 119 | gsl_eigen_symmv (m_cmat, m_eval, m_evec, m_eigenspace); 120 | for (int i=0; i<3; i++){ 121 | eval[i]=gsl_vector_get(m_eval,i); 122 | for (int j=0; j<3; j++) 123 | evec[i][j]=gsl_matrix_get(m_evec,i,j); 124 | } 125 | } 126 | 127 | EigenCovariance3 EigenCovariance3::rotate(double angle) const{ 128 | static gsl_matrix * m_rmat=NULL; 129 | static gsl_matrix * m_vmat=NULL; 130 | static gsl_matrix * m_result=NULL; 131 | if (m_rmat==NULL){ 132 | m_rmat=gsl_matrix_alloc(3,3); 133 | m_vmat=gsl_matrix_alloc(3,3); 134 | m_result=gsl_matrix_alloc(3,3); 135 | } 136 | 137 | double c=cos(angle); 138 | double s=sin(angle); 139 | gsl_matrix_set(m_rmat,0,0, c ); gsl_matrix_set(m_rmat,0,1, -s); gsl_matrix_set(m_rmat,0,2, 0.); 140 | gsl_matrix_set(m_rmat,1,0, s ); gsl_matrix_set(m_rmat,1,1, c); gsl_matrix_set(m_rmat,1,2, 0.); 141 | gsl_matrix_set(m_rmat,2,0, 0.); gsl_matrix_set(m_rmat,2,1, 0.); gsl_matrix_set(m_rmat,2,2, 1.); 142 | 143 | for (unsigned int i=0; i<3; i++) 144 | for (unsigned int j=0; j<3; j++) 145 | gsl_matrix_set(m_vmat,i,j,evec[i][j]); 146 | gsl_blas_dgemm (CblasNoTrans, CblasNoTrans, 1., m_rmat, m_vmat, 0., m_result); 147 | EigenCovariance3 ecov(*this); 148 | for (int i=0; i<3; i++){ 149 | for (int j=0; j<3; j++) 150 | ecov.evec[i][j]=gsl_matrix_get(m_result,i,j); 151 | } 152 | return ecov; 153 | } 154 | 155 | OrientedPoint EigenCovariance3::sample() const{ 156 | static gsl_matrix * m_evec=NULL; 157 | static gsl_vector * m_noise=NULL; 158 | static gsl_vector * m_pnoise=NULL; 159 | if (m_evec==NULL){ 160 | m_evec=gsl_matrix_alloc(3,3); 161 | m_noise=gsl_vector_alloc(3); 162 | m_pnoise=gsl_vector_alloc(3); 163 | } 164 | for (int i=0; i<3; i++){ 165 | for (int j=0; j<3; j++) 166 | gsl_matrix_set(m_evec,i,j, evec[i][j]); 167 | } 168 | for (int i=0; i<3; i++){ 169 | double v=sampleGaussian(sqrt(eval[i])); 170 | if(isnan(v)) 171 | v=0; 172 | gsl_vector_set(m_pnoise,i, v); 173 | } 174 | gsl_blas_dgemv (CblasNoTrans, 1., m_evec, m_pnoise, 0, m_noise); 175 | OrientedPoint ret(gsl_vector_get(m_noise,0),gsl_vector_get(m_noise,1),gsl_vector_get(m_noise,2)); 176 | ret.theta=atan2(sin(ret.theta), cos(ret.theta)); 177 | return ret; 178 | } 179 | 180 | #endif 181 | 182 | double Gaussian3::eval(const OrientedPoint& p) const 183 | { 184 | OrientedPoint q=p-mean; 185 | q.theta=atan2(sin(p.theta-mean.theta),cos(p.theta-mean.theta)); 186 | double v1,v2,v3; 187 | v1 = covariance.evec[0][0]*q.x+covariance.evec[1][0]*q.y+covariance.evec[2][0]*q.theta; 188 | v2 = covariance.evec[0][1]*q.x+covariance.evec[1][1]*q.y+covariance.evec[2][1]*q.theta; 189 | v3 = covariance.evec[0][2]*q.x+covariance.evec[1][2]*q.y+covariance.evec[2][2]*q.theta; 190 | return evalLogGaussian(covariance.eval[0], v1)+evalLogGaussian(covariance.eval[1], v2)+evalLogGaussian(covariance.eval[2], v3); 191 | } 192 | 193 | #if 0 194 | void Gaussian3::computeFromSamples(const std::vector & poses, const std::vector& weights ){ 195 | OrientedPoint mean=OrientedPoint(0,0,0); 196 | double wcum=0; 197 | double s=0, c=0; 198 | std::vector::const_iterator w=weights.begin(); 199 | for (std::vector::const_iterator p=poses.begin(); p!=poses.end(); p++){ 200 | s+=*w*sin(p->theta); 201 | c+=*w*cos(p->theta); 202 | mean.x+=*w*p->x; 203 | mean.y+=*w*p->y; 204 | wcum+=*w; 205 | w++; 206 | } 207 | mean.x/=wcum; 208 | mean.y/=wcum; 209 | s/=wcum; 210 | c/=wcum; 211 | mean.theta=atan2(s,c); 212 | 213 | Covariance3 cov=Covariance3::zero; 214 | w=weights.begin(); 215 | for (std::vector::const_iterator p=poses.begin(); p!=poses.end(); p++){ 216 | OrientedPoint delta=(*p)-mean; 217 | delta.theta=atan2(sin(delta.theta),cos(delta.theta)); 218 | cov.xx+=*w*delta.x*delta.x; 219 | cov.yy+=*w*delta.y*delta.y; 220 | cov.tt+=*w*delta.theta*delta.theta; 221 | cov.xy+=*w*delta.x*delta.y; 222 | cov.yt+=*w*delta.y*delta.theta; 223 | cov.xt+=*w*delta.x*delta.theta; 224 | w++; 225 | } 226 | cov.xx/=wcum; 227 | cov.yy/=wcum; 228 | cov.tt/=wcum; 229 | cov.xy/=wcum; 230 | cov.yt/=wcum; 231 | cov.xt/=wcum; 232 | EigenCovariance3 ecov(cov); 233 | this->mean=mean; 234 | this->covariance=ecov; 235 | this->cov=cov; 236 | } 237 | 238 | void Gaussian3::computeFromSamples(const std::vector & poses){ 239 | OrientedPoint mean=OrientedPoint(0,0,0); 240 | double wcum=1; 241 | double s=0, c=0; 242 | for (std::vector::const_iterator p=poses.begin(); p!=poses.end(); p++){ 243 | s+=sin(p->theta); 244 | c+=cos(p->theta); 245 | mean.x+=p->x; 246 | mean.y+=p->y; 247 | wcum+=1.; 248 | } 249 | mean.x/=wcum; 250 | mean.y/=wcum; 251 | s/=wcum; 252 | c/=wcum; 253 | mean.theta=atan2(s,c); 254 | 255 | Covariance3 cov=Covariance3::zero; 256 | for (std::vector::const_iterator p=poses.begin(); p!=poses.end(); p++){ 257 | OrientedPoint delta=(*p)-mean; 258 | delta.theta=atan2(sin(delta.theta),cos(delta.theta)); 259 | cov.xx+=delta.x*delta.x; 260 | cov.yy+=delta.y*delta.y; 261 | cov.tt+=delta.theta*delta.theta; 262 | cov.xy+=delta.x*delta.y; 263 | cov.yt+=delta.y*delta.theta; 264 | cov.xt+=delta.x*delta.theta; 265 | } 266 | cov.xx/=wcum; 267 | cov.yy/=wcum; 268 | cov.tt/=wcum; 269 | cov.xy/=wcum; 270 | cov.yt/=wcum; 271 | cov.xt/=wcum; 272 | EigenCovariance3 ecov(cov); 273 | this->mean=mean; 274 | this->covariance=ecov; 275 | this->cov=cov; 276 | } 277 | #endif 278 | 279 | }// end namespace 280 | 281 | -------------------------------------------------------------------------------- /src/gridfastslam/gridslamprocessor_tree.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include "utils/stat.h" 8 | #include "gridfastslam/gridslamprocessor.h" 9 | 10 | /** 11 | * 构造函数 12 | * TNode，树的结点 结构体，一个树储存了一整条轨迹，一个节点表示这条轨迹中的其中一个点 13 | * 对于树的一个节点而言，无指向子节点的指针，只有指向parent的指针 14 | */ 15 | GridSlamProcessor::TNode::TNode(const OrientedPoint& p, double w, TNode* n, unsigned int c) { 16 | pose = p; 17 | weight = w; 18 | childs=c; 19 | parent=n; 20 | reading=0; 21 | gweight=0; 22 | 23 | // 如果父节点存在，则父节点的孩子数+1 24 | if ( n ) { 25 | n->childs++; 26 | } 27 | 28 | flag=0; 29 | accWeight=0; 30 | } 31 | 32 | /** 33 | * 析构函数 34 | */ 35 | GridSlamProcessor::TNode::~TNode() { 36 | if ( parent && (--parent->childs)<=0 ) { 37 | delete parent; 38 | } 39 | assert(!childs); 40 | } 41 | 42 | //BEGIN State Save/Restore 43 | /** 44 | * 得到所有粒子的轨迹的起点(得到了轨迹的起点，就可以得到一整条轨迹) 45 | * 起点，就是每个粒子（结构体中）对应的那个node，它没有孩子节点，只有父节点 46 | */ 47 | GridSlamProcessor::TNodeVector GridSlamProcessor::getTrajectories() const 48 | { 49 | TNodeVector v; 50 | TNodeMultimap parentCache; 51 | TNodeDeque border; 52 | 53 | // 将每个粒子对应的那颗轨迹树中所有节点，置flag值为false 54 | for (ParticleVector::const_iterator it=m_particles.begin(); it!=m_particles.end(); it++) 55 | { 56 | TNode* node=it->node; 57 | while(node) 58 | { 59 | node->flag=false; 60 | node=node->parent; 61 | } 62 | } 63 | 64 | // 将每个粒子（结构体中）对应的那个node存储到数组v中 65 | // 并将其父节点存储到队列border中 66 | // 并将(node->parent, node)暂存到parentCache中 67 | for (ParticleVector::const_iterator it=m_particles.begin(); it!=m_particles.end(); it++) 68 | { 69 | TNode* newnode=new TNode(* (it->node) ); 70 | 71 | v.push_back(newnode); 72 | assert(newnode->childs==0); 73 | if (newnode->parent) 74 | { 75 | parentCache.insert(make_pair(newnode->parent, newnode)); 76 | //cerr << __PRETTY_FUNCTION__ << ": node " << newnode->parent << " flag=" << newnode->parent->flag<< endl; 77 | if (! newnode->parent->flag) 78 | { 79 | //cerr << __PRETTY_FUNCTION__ << ": node " << newnode->parent << " flag=" << newnode->parent->flag<< endl; 80 | newnode->parent->flag=true; 81 | border.push_back(newnode->parent); 82 | } 83 | } 84 | } 85 | 86 | //cerr << __PRETTY_FUNCTION__ << ": border.size(INITIAL)=" << border.size() << endl; 87 | //cerr << __PRETTY_FUNCTION__ << ": parentCache.size()=" << parentCache.size() << endl; 88 | while (! border.empty()) 89 | { 90 | //cerr << __PRETTY_FUNCTION__ << ": border.size(PREPROCESS)=" << border.size() << endl; 91 | //cerr << __PRETTY_FUNCTION__ << ": parentCache.size(PREPROCESS)=" << parentCache.size() << endl; 92 | const TNode* node=border.front(); 93 | //cerr << __PRETTY_FUNCTION__ << ": node " << node << endl; 94 | border.pop_front(); 95 | if (! node) 96 | continue; 97 | 98 | TNode* newnode=new TNode(*node); 99 | node->flag=false; 100 | 101 | //update the parent of all of the referring childs 102 | pair p=parentCache.equal_range(node); 103 | double childs=0; 104 | for (TNodeMultimap::iterator it=p.first; it!=p.second; it++) 105 | { 106 | assert(it->second->parent==it->first); 107 | (it->second)->parent=newnode; 108 | //cerr << "PS(" << it->first << ", "<< it->second << ")"; 109 | childs++; 110 | } 111 | ////cerr << endl; 112 | parentCache.erase(p.first, p.second); 113 | //cerr << __PRETTY_FUNCTION__ << ": parentCache.size(POSTERASE)=" << parentCache.size() << endl; 114 | assert(childs==newnode->childs); 115 | 116 | //unmark the node 117 | if ( node->parent ) 118 | { 119 | parentCache.insert(make_pair(node->parent, newnode)); 120 | if(! node->parent->flag) 121 | { 122 | border.push_back(node->parent); 123 | node->parent->flag=true; 124 | } 125 | } 126 | //insert the parent in the cache 127 | } 128 | //cerr << __PRETTY_FUNCTION__ << " : checking cloned trajectories" << endl; 129 | for (unsigned int i=0; iparent; 136 | } 137 | //cerr << endl; 138 | } 139 | // 返回存储有，每个粒子（结构体中）对应的那个node，的数组v 140 | return v; 141 | } 142 | 143 | /** 144 | * 集成所有的scan（这个函数没有被调用过。） 145 | */ 146 | void GridSlamProcessor::integrateScanSequence(GridSlamProcessor::TNode* node) 147 | { 148 | // reverse the list 149 | // 整条取反 因为路径的存储是按照时间顺序排列的。 150 | // 我们只能得到最近时刻的节点，然后反推出来整条路径 151 | // 但是如果我们想要集成所有的scan的话，我们必须从时间最早的节点开始。所以需要把整条路径反过来。 152 | TNode* aux=node; 153 | TNode* reversed=0; 154 | double count=0; 155 | while(aux!=0) 156 | { 157 | TNode * newnode=new TNode(*aux); 158 | newnode->parent=reversed; 159 | reversed=newnode; 160 | aux=aux->parent; 161 | count++; 162 | } 163 | 164 | //attach the path to each particle and compute the map; 165 | if (m_infoStream ) 166 | m_infoStream << "Restoring State Nodes=" <pose; 178 | first=false; 179 | oldWeight=aux->weight; 180 | } 181 | 182 | OrientedPoint dp=aux->pose-oldPose; 183 | double dw=aux->weight-oldWeight; 184 | oldPose=aux->pose; 185 | 186 | 187 | double * plainReading = new double[m_beams]; 188 | for(unsigned int i=0; ireading->m_dists[i]; 190 | 191 | for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++) 192 | { 193 | //compute the position relative to the path; 194 | double s=sin(oldPose.theta-it->pose.theta), 195 | c=cos(oldPose.theta-it->pose.theta); 196 | 197 | it->pose.x+=c*dp.x-s*dp.y; 198 | it->pose.y+=s*dp.x+c*dp.y; 199 | it->pose.theta+=dp.theta; 200 | it->pose.theta=atan2(sin(it->pose.theta), cos(it->pose.theta)); 201 | 202 | //register the scan 203 | // m_matcher.invalidateActiveArea(); 204 | m_matcher.computeActiveArea(it->map, it->pose, plainReading); 205 | it->weight+=dw; 206 | it->weightSum+=dw; 207 | 208 | // this should not work, since it->weight is not the correct weight! 209 | // it->node=new TNode(it->pose, it->weight, it->node); 210 | it->node=new TNode(it->pose, 0.0, it->node); 211 | //update the weight 212 | } 213 | 214 | delete [] plainReading; 215 | aux=aux->parent; 216 | } 217 | 218 | //destroy the path 219 | aux=reversed; 220 | while (reversed) 221 | { 222 | aux=reversed; 223 | reversed=reversed->parent; 224 | delete aux; 225 | } 226 | } 227 | 228 | //END State Save/Restore 229 | 230 | /** 231 | * 更新权重 232 | */ 233 | void GridSlamProcessor::updateTreeWeights(bool weightsAlreadyNormalized) { 234 | if ( !weightsAlreadyNormalized ) { 235 | // 权重归一化处理，同时计算出有效粒子数neff值 236 | normalize(); 237 | } 238 | resetTree(); 239 | propagateWeights(); 240 | } 241 | 242 | /** 243 | * 把每个粒子对应的轨迹树中各个节点的 accWeight 清零 244 | * accWeight，之前所有节点对应粒子的权重之和（该节点的子节点对应粒子的权重之和） 245 | */ 246 | void GridSlamProcessor::resetTree() { 247 | // don't calls this function directly, use updateTreeWeights(..) ! 248 | for ( ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++ ) { 249 | TNode* n = it->node; 250 | while ( n ) { 251 | n->accWeight = 0; 252 | n->visitCounter=0; 253 | n = n->parent; 254 | } 255 | } 256 | } 257 | 258 | /** 259 | * 被下面的propagateWeights()函数调用，最终返回根节点的累计权重accWeight 260 | * 通过递归方法求解了一条路径树上根节点的累计权重accWeight 261 | * n，当前树的一个节点 262 | * weight，表示当前节点n的所有子节点的累计权重 263 | */ 264 | double propagateWeight( GridSlamProcessor::TNode* n, double weight ) { 265 | // 最终会递归到根节点（根节点没有父节点，直接返回根节点的累计权重accWeight） 266 | if ( !n ) { 267 | return weight; 268 | } 269 | 270 | double w = 0; 271 | n->visitCounter++; 272 | // 更新节点n的accWeight值 273 | n->accWeight+=weight; 274 | 275 | if ( n->visitCounter == n->childs ) { 276 | w = propagateWeight(n->parent, n->accWeight); 277 | } 278 | assert( n->visitCounter <= n->childs ); 279 | return w; 280 | } 281 | 282 | /** 283 | * propagateWeights，更新权重，被updateTreeWeights()函数调用 284 | */ 285 | double GridSlamProcessor::propagateWeights() { 286 | // don't calls this function directly, use updateTreeWeights(..) ! 287 | // all nodes must be resetted to zero and weights normalized 288 | // the accumulated weight of the root 289 | 290 | double lastNodeWeight = 0; 291 | double aw = 0; 292 | 293 | // m_weights 数组中存储的是每一个粒子对应的轨迹树的叶子节点权重值 294 | std::vector::iterator w = m_weights.begin(); 295 | for ( ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++ ) { 296 | double weight = *w; 297 | // 所有粒子对应轨迹树的叶子节点，求它们的权重之和 298 | aw += weight; 299 | // 每一个粒子的路径都是从叶子节点开始的，得到了叶子节点，就得到了路径 300 | TNode* n = it->node; 301 | // 叶子节点的子节点累计权重就等于自己的权重 因为它没有子节点 302 | n->accWeight = weight; 303 | // propagateWeight()通过递归方法求解了一条路径树上根节点的累计权重accWeight 304 | // lastNodeWeight则是求所有根节点的累计权重之和 305 | lastNodeWeight += propagateWeight(n->parent, n->accWeight) 306 | 307 | w++; 308 | } 309 | if (fabs(aw-1.0) > 0.0001 || fabs(lastNodeWeight-1.0) > 0.0001) 310 | { 311 | cerr << "ERROR: "; 312 | cerr << "root->accWeight=" << lastNodeWeight << " sum_leaf_weights=" << aw << endl; 313 | assert(0); 314 | } 315 | return lastNodeWeight; 316 | } 317 | 318 | 319 | 320 | 321 | -------------------------------------------------------------------------------- /include/particlefilter/particlefilter.h: -------------------------------------------------------------------------------- 1 | #ifndef PARTICLEFILTER_H 2 | #define PARTICLEFILTER_H 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include "" 10 | 11 | 12 | /** 13 | the particle class has to be convertible into numeric data type; 14 | That means that a particle must define the Numeric conversion operator; 15 | operator Numeric() const. 16 | that returns the weight, and the method 17 | setWeight(Numeric) 18 | that sets the weight. 19 | */ 20 | 21 | typedef std::pair UIntPair; 22 | 23 | template 24 | double toNormalForm(OutputIterator& out, const Iterator & begin, const Iterator & end){ 25 | //determine the maximum 26 | //double lmax=-MAXDOUBLE; 27 | double lmax=-DBL_MAX; 28 | for (Iterator it=begin; it!=end; it++){ 29 | lmax=lmax>((double)(*it))? lmax: (double)(*it); 30 | } 31 | //convert to raw form 32 | for (Iterator it=begin; it!=end; it++){ 33 | *out=exp((double)(*it)-lmax); 34 | out++; 35 | } 36 | return lmax; 37 | } 38 | 39 | template 40 | void toLogForm(OutputIterator& out, const Iterator & begin, const Iterator & end, Numeric lmax){ 41 | //determine the maximum 42 | for (Iterator it=begin; it!=end; it++){ 43 | *out=log((Numeric)(*it))-lmax; 44 | out++; 45 | } 46 | return lmax; 47 | } 48 | 49 | template 50 | void resample(std::vector& indexes, const WeightVector& weights, unsigned int nparticles=0){ 51 | double cweight=0; 52 | 53 | //compute the cumulative weights 54 | unsigned int n=0; 55 | for (typename WeightVector::const_iterator it=weights.begin(); it!=weights.end(); ++it){ 56 | cweight+=(double)*it; 57 | n++; 58 | } 59 | 60 | if (nparticles>0) 61 | n=nparticles; 62 | 63 | //compute the interval 64 | double interval=cweight/n; 65 | 66 | //compute the initial target weight 67 | double target=interval*::drand48(); 68 | //compute the resampled indexes 69 | 70 | cweight=0; 71 | indexes.resize(n); 72 | 73 | n=0; 74 | unsigned int i=0; 75 | for (typename WeightVector::const_iterator it=weights.begin(); it!=weights.end(); ++it, ++i){ 76 | cweight+=(double)* it; 77 | while(cweight>target){ 78 | indexes[n++]=i; 79 | target+=interval; 80 | } 81 | } 82 | } 83 | 84 | template 85 | void repeatIndexes(Vector& dest, const std::vector& indexes, const Vector& particles){ 86 | assert(indexes.size()==particles.size()); 87 | dest.resize(particles.size()); 88 | unsigned int i=0; 89 | for (std::vector::const_iterator it=indexes.begin(); it!=indexes.end(); ++it){ 90 | dest[i]=particles[*it]; 91 | i++; 92 | } 93 | } 94 | 95 | 96 | template 97 | double neff(const Iterator& begin, const Iterator& end){ 98 | double sum=0; 99 | for (Iterator it=begin; it!=end; ++it){ 100 | sum+=*it; 101 | } 102 | double cum=0; 103 | for (Iterator it=begin; it!=end; ++it){ 104 | double w=*it/sum; 105 | cum+=w*w; 106 | } 107 | return 1./cum; 108 | } 109 | 110 | template 111 | void normalize(const Iterator& begin, const Iterator& end){ 112 | double sum=0; 113 | for (Iterator it=begin; it!=end; ++it){ 114 | sum+=*it; 115 | } 116 | for (Iterator it=begin; it!=end; ++it){ 117 | *it=*it/sum; 118 | } 119 | } 120 | 121 | template 122 | void rle(OutputIterator& out, const Iterator & begin, const Iterator & end){ 123 | unsigned int current=0; 124 | unsigned int count=0; 125 | for (Iterator it=begin; it!=end; it++){ 126 | if (it==begin){ 127 | current=*it; 128 | count=1; 129 | continue; 130 | } 131 | if (((uint)*it) ==current) 132 | count++; 133 | if (((uint)*it)!=current){ 134 | *out=std::make_pair(current,count); 135 | out++; 136 | current=*it; 137 | count=1; 138 | } 139 | } 140 | if (count>0) 141 | *out=std::make_pair(current,count); 142 | out++; 143 | } 144 | 145 | //BEGIN legacy 146 | template 147 | struct uniform_resampler{ 148 | std::vector resampleIndexes(const std::vector & particles, int nparticles=0) const; 149 | std::vector resample(const std::vector & particles, int nparticles=0) const; 150 | Numeric neff(const std::vector & particles) const; 151 | }; 152 | 153 | /*Implementation of the above stuff*/ 154 | template 155 | std::vector uniform_resampler:: resampleIndexes(const std::vector& particles, int nparticles) const{ 156 | Numeric cweight=0; 157 | 158 | //compute the cumulative weights 159 | unsigned int n=0; 160 | for (typename std::vector::const_iterator it=particles.begin(); it!=particles.end(); ++it){ 161 | cweight+=(Numeric)*it; 162 | n++; 163 | } 164 | 165 | if (nparticles>0) 166 | n=nparticles; 167 | 168 | //compute the interval 169 | Numeric interval=cweight/n; 170 | 171 | //compute the initial target weight 172 | Numeric target=interval*::drand48(); 173 | //compute the resampled indexes 174 | 175 | cweight=0; 176 | std::vector indexes(n); 177 | n=0; 178 | unsigned int i=0; 179 | for (typename std::vector::const_iterator it=particles.begin(); it!=particles.end(); ++it, ++i){ 180 | cweight+=(Numeric)* it; 181 | while(cweight>target){ 182 | indexes[n++]=i; 183 | target+=interval; 184 | } 185 | } 186 | return indexes; 187 | } 188 | 189 | template 190 | std::vector uniform_resampler::resample 191 | (const typename std::vector& particles, int nparticles) const{ 192 | Numeric cweight=0; 193 | 194 | //compute the cumulative weights 195 | unsigned int n=0; 196 | for (typename std::vector::const_iterator it=particles.begin(); it!=particles.end(); ++it){ 197 | cweight+=(Numeric)*it; 198 | n++; 199 | } 200 | 201 | if (nparticles>0) 202 | n=nparticles; 203 | 204 | //weight of the particles after resampling 205 | double uw=1./n; 206 | 207 | //compute the interval 208 | Numeric interval=cweight/n; 209 | 210 | //compute the initial target weight 211 | Numeric target=interval*::drand48(); 212 | //compute the resampled indexes 213 | 214 | cweight=0; 215 | std::vector resampled; 216 | n=0; 217 | unsigned int i=0; 218 | for (typename std::vector::const_iterator it=particles.begin(); it!=particles.end(); ++it, ++i){ 219 | cweight+=(Numeric)*it; 220 | while(cweight>target){ 221 | resampled.push_back(*it); 222 | resampled.back().setWeight(uw); 223 | target+=interval; 224 | } 225 | } 226 | return resampled; 227 | } 228 | 229 | template 230 | Numeric uniform_resampler::neff(const std::vector & particles) const{ 231 | double cum=0; 232 | double sum=0; 233 | for (typename std::vector::const_iterator it=particles.begin(); it!=particles.end(); ++it){ 234 | Numeric w=(Numeric)*it; 235 | cum+=w*w; 236 | sum+=w; 237 | } 238 | return sum*sum/cum; 239 | } 240 | 241 | 242 | /* 243 | The following are patterns for the evolution and the observation classes 244 | The user should implement classes having the specified meaning 245 | template 246 | struct observer{ 247 | Observation& observation 248 | Numeric observe(const class State&) const; 249 | }; 250 | template 251 | struct evolver{ 252 | Input& input; 253 | State& evolve(const State& s); 254 | }; 255 | */ 256 | 257 | 258 | template 259 | struct evolver{ 260 | EvolutionModel evolutionModel; 261 | void evolve(std::vector& particles); 262 | void evolve(std::vector& dest, const std::vector& src); 263 | }; 264 | 265 | template 266 | void evolver::evolve(std::vector& particles){ 267 | for (typename std::vector::iterator it=particles.begin(); it!=particles.end(); ++it){ 268 | *it=evolutionModel.evolve(*it); 269 | } 270 | } 271 | 272 | template 273 | void evolver::evolve(std::vector& dest, const std::vector& src){ 274 | dest.clear(); 275 | for (typename std::vector::const_iterator it=src.begin(); it!=src.end(); ++it) 276 | dest.push_back(evolutionModel.evolve(*it)); 277 | } 278 | 279 | 280 | template 281 | struct auxiliary_evolver{ 282 | EvolutionModel evolutionModel; 283 | QualificationModel qualificationModel; 284 | LikelyhoodModel likelyhoodModel; 285 | void evolve(std::vector& particles); 286 | void evolve(std::vector& dest, const std::vector& src); 287 | }; 288 | 289 | template 290 | void auxiliary_evolver::evolve 291 | (std::vector&particles){ 292 | std::vector observationWeights(particles.size()); 293 | unsigned int i=0; 294 | for (typename std::vector::const_iterator it=particles.begin(); it!=particles.end(); ++it, i++){ 295 | observationWeights[i]=likelyhoodModel.likelyhood(qualificationModel.evolve(*it)); 296 | } 297 | uniform_resampler resampler; 298 | std::vector indexes(resampler.resampleIndexes(observationWeights)); 299 | for (typename std::vector::const_iterator it=indexes.begin(); it!=indexes.end(); it++){ 300 | Particle & particle=particles[*it]; 301 | particle=evolutionModel.evolve(particle); 302 | particle.setWeight(likelyhoodModel.likelyhood(particle)/observationWeights[*it]); 303 | } 304 | } 305 | 306 | template 307 | void auxiliary_evolver::evolve 308 | (std::vector& dest, const std::vector& src){ 309 | dest.clear(); 310 | std::vector observationWeights(src.size()); 311 | unsigned int i=0; 312 | for (typename std::vector::const_iterator it=src.begin(); it!=src.end(); ++it, i++){ 313 | observationWeights[i]=likelyhoodModel.likelyhood(qualificationModel.evolve(*it)); 314 | } 315 | uniform_resampler resampler; 316 | std::vector indexes(resampler.resampleIndexes(observationWeights)); 317 | for (typename std::vector::const_iterator it=indexes.begin(); it!=indexes.end(); it++){ 318 | Particle & particle=src[*it]; 319 | dest.push_back(evolutionModel.evolve(particle)); 320 | dest.back().weight*=likelyhoodModel.likelyhood(particle)/observationWeights[*it]; 321 | } 322 | } 323 | //END legacy 324 | 325 | #endif -------------------------------------------------------------------------------- /include/grid/harray2d.h: -------------------------------------------------------------------------------- 1 | #ifndef HARRAY2D_H 2 | #define HARRAY2D_H 3 | #include 4 | #include "array2d.h" 5 | #include "utils/autoptr.h" 6 | #include "utils/point.h" 7 | 8 | namespace Gmapping { 9 | 10 | /** 11 | * HierarchicalArray2D，地图金字塔类，继承自二维数组Array2D类 12 | * 一种类似金字塔的形式组织地图内存（降低内存消耗），只是这个金字塔只有两层 13 | * 它本身是一个Array2D，其存储单元是一个指向Array2D对象的智能指针autoptr 14 | * 15 | * 两层地图金字塔：第一层的元素为Patch，第二层的元素为Cell； 16 | * 首先把HierarchicalArray2D的元素为Patch(Array2D)，然后Patch的元素为Cell。 17 | * 一个Patch就是一个地图块，即一个(2^m_patchMagnitude, 2^m_patchMagnitude)大小的Array2D的指针 18 | * 一个Patch的实际大小：m_patchSize = 1< 40 | class HierarchicalArray2D: public Array2D< autoptr< Array2D > > { 41 | 42 | public: 43 | typedef std::set< point, pointcomparator > PointSet; 44 | HierarchicalArray2D( int xsize, int ysize, int patchMagnitude=5 ); 45 | HierarchicalArray2D( const HierarchicalArray2D& hg ); // 没看懂 46 | virtual ~HierarchicalArray2D(){} 47 | 48 | HierarchicalArray2D& operator = ( const HierarchicalArray2D& hg ); 49 | 50 | void resize( int ixmin, int iymin, int ixmax, int iymax ); // 没看懂 51 | inline int getPatchSize() const { return m_patchMagnitude; } 52 | inline int getPatchMagnitude() const { return m_patchMagnitude; } 53 | 54 | inline patchIndexes( int x, int y ) const; 55 | inline patchIndexes( const IntPoint& p ) const { return patchIndexes(p.x, p.y); } 56 | 57 | inline bool isAllocated( int x, int y ) const; 58 | inline bool isAllocated( const IntPoint& p ) const { return isAllocated(p.x, p.y); } 59 | 60 | inline AccessibilityState cellState( int x, int y ) const; 61 | inline AccessibilityState cellState( const IntPoint& p ) const { return cellState(p.x, p.y); } 62 | 63 | inline const Cell& cell( int x, int y ) const; 64 | inline Cell& cell( int x, int y ); 65 | inline const Cell& cell( const IntPoint& p ) const { return cell(p.x, p.y); } 66 | inline Cell& cell( const IntPoint& p ) { return cell(p.x, p.y); } 67 | 68 | inline void setActiveArea( const PointSet&, bool patchCoords=false ); 69 | const PointSets& getActiveArea() const { return m_activeArea; } 70 | inline void allocActiveArea(); 71 | 72 | protected: 73 | virtual Array2D* createPatch( const IntPoint& p ) const; 74 | PointSet m_activeArea; 75 | int m_patchMagnitude; 76 | int m_patchSize; 77 | 78 | }; 79 | 80 | /** 81 | * 构造函数，构建地图金字塔 82 | * 在HierarchicalArray2D中把地图分为若干个Patch，其拆分的粒度由m_patchMagnitude来描述 83 | * 一个原始是(xsize, ysize)大小的Array2D数组，拆分为xsize/(2^patchMagnitude) * ysize/(2^patchMagnitude)个Patch 84 | * 这里只是构建了(xsize>>patchMagnitude, ysize>>patchMagnitude)大小的Array2D数组 85 | * 每个元素为指向Patch的智能指针，即autoptr< Array2D >类型的指针，指向一个Patch元素 86 | * 一个Patch即为(2^patchMagnitude, 2^patchMagnitude)大小的Array2D数组，每个元素为Cell 87 | * 但这里并没有初始化一个Patch数组，而只是初始化了patchMagnitude供构建Patch数组使用 88 | */ 89 | template< class Cell > 90 | HierarchicalArray2D::HierarchicalArray2D( int xsize, int ysize, int patchMagnitude ) 91 | : Array2D< autoptr< Array2D > >::Array2D((xsize>>patchMagnitude), (ysize>>patchMagnitude)) 92 | { 93 | // 94 | m_patchMagnitude = patchMagnitude; 95 | m_patchSize = 1< 102 | HierarchicalArray2D::HierarchicalArray2D( const HierarchicalArray2D& hg ) 103 | :Array2D< autoptr< Array2D > >::Array2D( (hg.m_xsize>>hg.m_patchMagnitude), (hg.m_ysize>>hg.m_patchMagnitude) ) // added by cyrill: if you have a resize error, check this again 104 | { 105 | // 这两步应该和 hg.m_xsize>>hg.m_patchMagnitude，hg.m_ysize>>hg.m_patchMagnitude冲突了吧？ 106 | // 个人认为只保留 this->m_xsize = hg.m_xsize; this->m_ysize = hg.m_ysize; 是正确的。 107 | // 因为参考下面重载运算符"="，可以发现this->m_xsize = hg.m_xsize，而不是this->m_xsize = hg.m_xsize >> hg.m_patchMagnitude 108 | this->m_xsize = hg.m_xsize; 109 | this->m_ysize = hg.m_ysize; 110 | this->m_cells = new autoptr< Array2D >* [this->m_xsize]; 111 | for ( int x=0; xm_xsize; x++ ) { 112 | this->m_cells[x] = new autoptr< Array2D >[this->m_ysize]; 113 | for ( int y=0; ym_ysize; y++ ) { 114 | this->m_cells[x][y] = hg.m_cells[x][y]; 115 | } 116 | } 117 | this->m_patchMagnitude = hg.m_patchMagnitude; 118 | this->m_patchSize = hg.m_patchSize; 119 | } 120 | 121 | /** 122 | * 重载运算符"="，实现两个地图的赋值运算 123 | * 如果复制的两个地图的大小不一样 则需要把目前的地图删除，然后重新复制为新的地图 124 | */ 125 | template< class Cell > 126 | HierarchicalArray2D& HierarchicalArray2D::operator = ( const HierarchicalArray2D& hg ) { 127 | // 删除当前地图，并且重新分配内存 128 | if ( this->m_xsize!=hg.m_xsize || this->m_ysize!=hg.m_ysize ) { 129 | for ( int i=0; im_xsize; i++ ) { 130 | delete [] this->m_cells[i]; 131 | } 132 | delete [] this->m_celss; 133 | 134 | this->m_xsize = hg.m_xsize; 135 | this->m_ysize = hg.m_ysize; 136 | this->m_cells = new autoptr< Array2D >* [this->m_xsize]; 137 | for ( int i=0; im_xsize; i++ ) 138 | this->m_cells[i] = new autoptr< Array2D >[this->m_ysize]; 139 | } 140 | // 赋值为新的地图 141 | for ( int x=0; xm_xsize; x++ ) { 142 | for ( int y=0; ym_ysize; y++ ) { 143 | this->m_cells[x][y] = hg.m_cells[x][y]; 144 | } 145 | } 146 | 147 | m_activeArea.clear(); 148 | m_patchMagnitude=hg.m_patchMagnitude; 149 | m_patchSize=hg.m_patchSize; 150 | return *this; 151 | } 152 | 153 | /** 154 | * 重新扩充地图的大小 155 | */ 156 | template< class Cell > 157 | void HierarchicalArray2D::resize( int xmin, int ymin, int xmax, int ymax ) { 158 | int xsize=xmax-xmin; 159 | int ysize=ymax-ymin; 160 | //分配一个xsize*ysize大小的patch的内存 161 | autoptr< Array2D > ** newcells=new autoptr< Array2D > *[xsize]; 162 | for (int x=0; x >[ysize]; 165 | for (int y=0; y >(0); 168 | } 169 | } 170 | 171 | //把原来地图中存在的数据 拷贝到新分配的地图中 这样就完成了地图扩充 172 | int dx= xmin < 0 ? 0 : xmin; 173 | int dy= ymin < 0 ? 0 : ymin; 174 | int Dx=xmaxm_xsize?xmax:this->m_xsize; 175 | int Dy=ymaxm_ysize?ymax:this->m_ysize; 176 | 177 | for (int x=dx; xm_cells[x][y]; 182 | } 183 | delete [] this->m_cells[x]; 184 | } 185 | delete [] this->m_cells; 186 | this->m_cells=newcells; 187 | this->m_xsize=xsize; 188 | this->m_ysize=ysize; 189 | } 190 | 191 | /** 192 | * 把原始xy坐标，转换为对应的Patch的坐标 193 | * 相当于原始坐标/m_patchMagnitude 194 | */ 195 | template< class Cell > 196 | inline HierarchicalArray2D::patchIndexes( int x, int y ) const { 197 | if ( x>=0 && y>=0 ) 198 | return IntPoint( x>>m_patchMagnitude, y>>m_patchMagnitude); 199 | return IntPoint(-1, -1); 200 | } 201 | 202 | /** 203 | * 判断xy坐标处的Cell是否被分配了内存 204 | * 要访问某个具体的cell，首先转换到patch坐标系，再从对应的patch中访问对应的cell元素 205 | */ 206 | template< class Cell > 207 | inline bool HierarchicalArray2D::isAllocated( int x, int y ) const { 208 | IntPoint c = patchIndexes(x, y); 209 | autoptr< Array2D >& ptr = this->m_cells[c.x][c.y]; 210 | // 实际上是Patch是否被分配了内存，Patch被分配了内存，元素Cell自然也拥有内存 211 | return (ptr != 0); 212 | } 213 | 214 | /** 215 | * 创建一个Patch 216 | * 并没有初始化一个Patch数组，但初始化了patchMagnitude供这里构建Patch数组使用 217 | * 一个patch表示一个Array2D数组，大小为2^m_patchMagnitude * 2^m_patchMagnitude 218 | */ 219 | template< class Cell > 220 | Array2D* HierarchicalArray2D::createPatch( const IntPoint& p ) const { 221 | // 这里形参 p 都没用到 222 | return new Array2D(1<isInside( patchIndexes(x, y) ) ) { 231 | if ( isAllocated(x, y) ) 232 | return (AccessibilityState)((int)Inside|(int)Allocated); 233 | else 234 | return Inside; 235 | } 236 | return Outside; 237 | } 238 | 239 | /** 240 | * 获取给定xy坐标处的Cell（已分配内存了的） 241 | * 要访问某个具体的cell，首先转换到patch坐标系，再从对应的patch中访问对应的cell元素 242 | */ 243 | template< class Cell > 244 | inline const Cell& HierarchicalArray2D::cell( int x, int y ) const { 245 | assert( isAllocated(x, y) ); 246 | IntPoint c = patchIndexes(x, y); 247 | const autoptr< Array2D >& ptr = this->m_cells[c.x][c.y]; 248 | return (*ptr).cell(IntPoint(x-(c.x< 256 | inline Cell& HierarchicalArray2D::cell( int x, int y ) { 257 | IntPoint c = patchIndexes(x, y); 258 | assert(this->isInside(c.x, c.y)); 259 | // 若没有对cell所在的Patch分配内存 260 | if ( !this->m_cells[c.x][c.y] ) { 261 | Array2D* patch = createPatch(IntPoint(x, y)); 262 | this->m_cells[c.x][c.y] = autoptr< Array2D >(patch); 263 | } 264 | autoptr< Array2D >& ptr = this->m_cells[c.x][c.y] 265 | return (*ptr).cell(IntPoint(x-(c.x< 276 | inline void HierarchicalArray2D::setActiveArea( const typename HierarchicalArray2D::PointSet& aa, bool patchCoords ) { 277 | m_activeArea.clear(); 278 | // 这里只是把点存储到对应的集合m_activeArea 279 | // 并没有为对应的Patch进行内存的分配，真正的内存分配在后面的allocActiveArea()函数 280 | for ( PointSet::const_iterator it=aa.begin(); it!=aa.end(); ++it ) { 281 | IntPoint p; 282 | if ( patchCoords ) 283 | p = *it; 284 | else 285 | p = patchIndexes(*it); 286 | m_activeArea.insert(p); 287 | } 288 | } 289 | 290 | /** 291 | * 给地图的有效区域分配内存 292 | * 293 | */ 294 | template< class Cell > 295 | inline void HierarchicalArray2D::allocActiveArea() { 296 | for ( PointSet::const_iterator it=m_activeArea.begin(); it!=m_activeArea.end(); ++it ) { 297 | const autoptr< Array2D >& ptr = this->m_cells[it->x][it->y]; 298 | Array2D* patch = 0; 299 | if ( !ptr ) { 300 | // 如果对应的patch没有被分配内存 则进行内存分配 301 | patch = createPatch(*it); 302 | } else { 303 | //如果已经分配了还是赋原值 304 | patch = new Array2D(*ptr); 305 | } 306 | this->m_cells[it->x][it->y] = autoptr< Array2D >(patch); 307 | } 308 | } 309 | 310 | } // end namespace 311 | 312 | #endif -------------------------------------------------------------------------------- /include/grid/map.h: -------------------------------------------------------------------------------- 1 | #ifndef MAP_H 2 | #define MAP_H 3 | #include 4 | #include "harray2d.hpp" 5 | #include "array2d.h" 6 | #include "accessstate.hpp" 7 | #include "utils/point.h" 8 | 9 | namespace Gmapping { 10 | 11 | typedef Array2D DoubleArray2D; 12 | 13 | /** 14 | * Map，栅格地图类（地图），在连续的物理世界和离散的栅格地图之间建立一个映射关系。 15 | * 在GMapping中每个粒子都维护了一个地图，并最终采用了最优粒子的地图。 16 | * 它们使用类GMapping::ScanMatcherMap来描述地图，参考smmap.hpp文件： 17 | * typedef Map > ScanMatcherMap; 18 | * 其中地图单元（栅格）类型为PointAccumulator， 存储结构为HierachicalArray2D 19 | * 20 | * 模板参数： 21 | * Cell，地图中每个单元（栅格）的数据类型 22 | * Storage，地图的存储结构类型 23 | * 24 | * 数据成员： 25 | * m_center，地图中心的坐标（世界坐标系下） 26 | * m_worldSizeX，世界坐标系X轴大小（物理世界是一个连续的坐标空间，需要使用double类型） 27 | * m_worldSizeY，世界坐标系Y轴大小 28 | * m_delta，地图的分辨率，即世界坐标系到地图坐标系的缩放比例关系（worldSize=mapSize*delta） 29 | * m_mapSizeX，地图长度（栅格地图的表示则是离散的，所以使用int类型） 30 | * m_mapSizeY，地图宽度 31 | * m_sizeX2，地图长度的一半 32 | * m_sizeY2，地图宽度的一半 33 | * m_storage，存储地图数据的对象，HierachicalArray2D类型 34 | * m_unknown，静态成员变量，返回一个默认的Cell类型对象（PointAccumulator类型默认输出的栅格 35 | * 36 | * 函数成员： 37 | * resize，重新设置地图大小 38 | * grow，扩展地图大小 39 | * world2map，世界坐标系转换到地图坐标系（重载函数） 40 | * map2world，地图坐标系转换到世界坐标系（重载函数） 41 | * getCenter，获取地图中心点的坐标 42 | * getworldSizeX，获取世界坐标系X轴大小 43 | * getworldSizeY，获取世界坐标系Y轴大小 44 | * getMapSizeX，获取地图长度 45 | * getMapSizeY，获取地图宽度 46 | * getDelta，获取地图分辨率 47 | * getMapResolution，获取地图分辨率 48 | * getResolution，获取地图分辨率 49 | * getSize，获取世界坐标系的X轴，Y轴的起点与终点（xmin, ymin, xmax, ymax） 50 | * cell，获取给定坐标处的Cell（重载函数） 51 | * isInside，给定一个点的坐标，判断是否在地图里面（重载函数） 52 | * storage，获取存储地图数据的对象，即m_storage（重载函数） 53 | * toDoubleArray，依据当前的Map对象构造DoubleArray（没有使用过） 54 | * toDoubleMap，依据当前的Map对象构造DoubleMap（没有使用过） 55 | */ 56 | template< class Cell, class Storage, const bool isClass=true > 57 | class Map { 58 | public: 59 | static const Cell m_unknown; 60 | 61 | Map( int mapSizeX, int mapSizeY, double delta ); 62 | Map( const Point& center, double worldSizeX, double worldSizeY, double delta ); 63 | Map( const Point& center, double xmin, double ymin, double xmax, double ymax, double delta ); 64 | void resize( double xmin, double ymin, double xmax, double ymax ); 65 | void grow( double xmin, double ymin, double xmax, double ymax ); 66 | 67 | inline IntPoint world2map( const Point& p ) const; 68 | inline Point map2world( const IntPoint& p ) const; 69 | inline IntPoint world2map( double x, double y ) const { return world2map( Point(x,y) ); } 70 | inline Point map2world( int x, int y ) const { return map2world( Inpoint(x,y) ); } 71 | 72 | inline Point getCenter() const { return m_cenetr; } 73 | inline double getWorldSizeX() const { return m_worldSizeX; } 74 | inline double getWorldSizeY() const { return m_worldSizeY; } 75 | inline int getMapSizeX() const { return m_mapSizeX; } 76 | inline int getMapSizeY() const { return m_mapSizeY; } 77 | inline double getDelta() const { return m_delta; } 78 | inline double getMapResolution() const { return m_delta; } 79 | inline double getResolution() const { return m_delta; } 80 | inline void getSize( double& xmin, double& ymin, double& xmax, double& ymax ) const { 81 | Point min=map2world( 0, 0 ), max = map2world( IntPoint(m_mapSizeX-1, m_mapSizeY-1) ); 82 | xmin = min.x, ymin = min.y, xmax = max.x, ymax = max.y; 83 | } 84 | 85 | inline Cell& cell( const IntPoint& p ); 86 | inline Cell& cell( int x, int y ) { 87 | return cell( IntPoint(x, y) ); 88 | } 89 | inline const Cell& cell( const IntPoint& p ) const; 90 | inline const Cell& cell( int x, int y ) const { 91 | return cell( IntPoint(x, y) ); 92 | } 93 | inline Cell& cell( const Point& p ); 94 | inline Cell& cell( double x, double y ) { 95 | return cell( Point(x, y) ); 96 | } 97 | inline const Cell& cell( const Point& p ) const; 98 | inline const Cell& cell( double x, double y ) const { 99 | return cell( Point(x, y) ); 100 | } 101 | 102 | inline bool isInside( int x, int y ) const { 103 | return m_storage.cellState(IntPoint(x, y))&Inside; 104 | } 105 | inline bool isInside( const IntPoint& p ) const { 106 | return m_storage.cellState(p)&Inside; 107 | } 108 | inline bool isInside( double x, double y ) const { 109 | return m_storage.cellState(world2map(x, y))&Inside; 110 | } 111 | inline bool isInside( const Point& p ) const { 112 | return m_storage.cellState(world2map(p))&Inside; 113 | } 114 | 115 | inline Storage& storage() { return m_storage; } 116 | inline const Storage& storage() const { return m_storage; } 117 | 118 | DoubleArray2D* toDoubleArray() cosnt; // 没有使用过 119 | Map< double, DoubleArray2D, false >* toDoubleMap() const; // 没有使用过 120 | 121 | 122 | 123 | protected: 124 | Point m_center; 125 | double m_worldSizeX, m_worldSizeY, m_delta; 126 | int m_mapSizeX, m_mapSizeY; 127 | int m_sizeX2, m_sizeY2; 128 | Storage m_storage; 129 | }; 130 | 131 | // 浮点数地图 132 | typedef Map< double, DoubleArray2D, false > DoubleMap; 133 | 134 | /** 135 | * m_unknown，静态成员变量初始化，返回一个默认的Cell类型对象（PointAccumulator类型默认输出的栅格） 136 | */ 137 | template< class Cell, class Storage, const bool isClass > 138 | const Cell Map::m_unknown = Cell(-1); 139 | 140 | /** 141 | * 构造函数，给定地图的大小，分辨率（worldSize=mapSize*delta） 142 | */ 143 | template< class Cell, class Storage, const bool isClass > 144 | Map::Map( int mapSizeX, int mapSizeY, double delta ) 145 | : m_storage( mapSizeX, mapSizeY ) 146 | { 147 | // 这里是否有初始化m_mapSizeX，m_mapSizeY？？？ 148 | // 我认为没有，应该添加： 149 | // m_mapSizeX = mapSizeX; 150 | // m_mapSizeY = mapSizeY; 151 | m_worldSizeX = mapSizeX * delta; 152 | m_worldSizeY = mapSizeY * delta; 153 | m_delta = delta; 154 | m_center = Point( 0.5*m_worldSizeX, 0.5*m_worldSizeY ); 155 | m_sizeX2 = m_mapSizeX >> 1; 156 | m_sizeY2 = m_mapSizeY >> 1; 157 | } 158 | 159 | /** 160 | * 构造函数，给定世界坐标系的大小，地图的分辨率（worldSize=mapSize*delta） 161 | */ 162 | template< class Cell, class Storage, const bool isClass > 163 | Map::Map( const Point& center, 164 | double worldSizeX, double worldSizeY, double delta ) 165 | : m_storage( (int)ceil(worldSizeX/delta), (int)ceil(worldSizeY/delta) ) 166 | { 167 | m_center = center; 168 | m_worldSizeX = worldSizeX; 169 | m_worldSizeY = worldSizeY; 170 | m_delta = delta; 171 | // 这里得注意，m_mapSizeX是原始地图的大小 172 | // 划分地图金字塔后，得到元素为Patch指针的二维数组大小（地图金字塔Patch数组层）： 173 | // m_storage.getXSize() = m_mapSizeX >> m_storage.m_patchMagnitude 174 | m_mapSizeX = m_storage.getXSize() << m_storage.getPatchSize(); 175 | m_mapSizeY = m_storage.getYSize() << m_storage.getPatchSize(); 176 | m_sizeX2 = m_mapSizeX >> 1; 177 | m_sizeY2 = m_mapSizeY >> 1; 178 | } 179 | 180 | /** 181 | * 构造函数，给定世界坐标系的大小，地图的分辨率（worldSize=mapSize*delta） 182 | * worldSizeX = xmax-xmin 183 | * worldSizeY = ymax-ymin 184 | */ 185 | template< class Cell, class Storage, const bool isClass > 186 | Map::Map( const Point& center, double xmin, double ymin, 187 | double xmax, double ymax, double delta ) 188 | : m_storage( (int)ceil((xmax-xmin)/delta), (int)ceil((ymax-ymin)/delta) ) 189 | { 190 | m_center = center; 191 | m_worldSizeX = xmax-xmin; 192 | m_worldSizeY = ymax-ymin; 193 | m_delta = delta; 194 | m_mapSizeX = m_storage.getXSize() << m_storage.getPatchSize(); 195 | m_mapSizeY = m_storage.getYSize() << m_storage.getPatchSize(); 196 | m_sizeX2 = (int)round((m_center.x - xmin)/m_delta); 197 | m_sizeY2 = (int)round((m_center.y - ymin)/m_delta); 198 | } 199 | 200 | /** 201 | * 重新设置地图大小 202 | * worldSizeX = xmax-xmin 203 | * worldSizeY = ymax-ymin 204 | */ 205 | template< class Cell, class Storage, const bool isClass > 206 | void Map::resize( double xmin, double ymin, double xmax, double ymax ) { 207 | // 将尺寸由世界坐标系转换到地图坐标系 208 | IntPoint imin=world2map(xmin, ymin); 209 | IntPoint imax=world2map(xmax, ymax); 210 | // 获取元素为Patch指针的二维数组大小（地图金字塔Patch数组层） 211 | int pxmin, pymin, pxmax, pymax; 212 | pxmin=(int)floor((float)imin.x/(1< 233 | void Map::grow(double xmin, double ymin, double xmax, double ymax) 234 | { 235 | // 将尺寸由世界坐标系转换到地图坐标系 236 | IntPoint imin=world2map(xmin, ymin); 237 | IntPoint imax=world2map(xmax, ymax); 238 | //尺寸更大，才需要扩展 239 | if (isInside(imin) && isInside(imax)) 240 | return; 241 | imin=min(imin, IntPoint(0,0)); 242 | imax=max(imax, IntPoint(m_mapSizeX-1,m_mapSizeY-1)); 243 | // 获取元素为Patch指针的二维数组大小（地图金字塔Patch数组层） 244 | int pxmin, pymin, pxmax, pymax; 245 | pxmin=(int)floor((float)imin.x/(1< 265 | IntPoint Map::world2map( const Point& p ) const { 266 | return InPoint( (int)round((p.x-m_center.x)/m_delta)+m_sizeX2, (int)round((p.y-m_center.y)/m_delta)+m_sizeY2 ); 267 | } 268 | 269 | /** 270 | * 地图坐标系转换到世界坐标系 271 | * p，地图坐标系中一个点的坐标 272 | */ 273 | template< class Cell, class Storage, const bool isClass > 274 | Point Map::map2world( const IntPoint& p ) const { 275 | return Point( (p.x-m_sizeX2)*m_delta, (p.y-m_sizeY2)*m_delta ) + m_center; 276 | } 277 | 278 | /** 279 | * 获取给定坐标处的Cell 280 | * p，地图坐标系下某个点的坐标 281 | */ 282 | template< class Cell, class Storage, const bool isClass > 283 | Cell& Map::cell( const IntPoint& p ) { 284 | AccessibilityState s = m_storage.cellState(p); 285 | // 如果点p不在地图内 286 | if ( !(s&Inside) ) 287 | assert(0); 288 | return m_storage.cell(p); 289 | } 290 | 291 | template< class Cell, class Storage, const bool isClass > 292 | const Cell& Map::cell( const IntPoint& p ) const { 293 | AccessibilityState s=m_storage.cellState(p); 294 | // 如果点p对应的栅格已经分配内存，则返回对应的Cell 295 | if (s&Allocated) 296 | return m_storage.cell(p); 297 | // 否则返回一个默认的Cell 298 | return m_unknown; 299 | } 300 | 301 | /** 302 | * 获取给定坐标处的Cell 303 | * p，世界坐标系下某个点的坐标 304 | */ 305 | template< class Cell, class Storage, const bool isClass > 306 | Cell& Map::cell( const Point& p ) { 307 | IntPoint ip = world2map(p); 308 | AccessibilityState s = m_storage.cellState(ip); 309 | // 如果点p不在地图内 310 | if ( !(s&Inside) ) 311 | assert(0); 312 | return m_storage.cell(ip); 313 | } 314 | 315 | template< class Cell, class Storage, const bool isClass > 316 | const Cell& Map::cell( const Point& p ) const { 317 | IntPoint ip = world2map(p); 318 | AccessibilityState s=m_storage.cellState(ip); 319 | // 如果点p对应的栅格已经分配内存，则返回对应的Cell 320 | if (s&Allocated) 321 | return m_storage.cell(ip); 322 | // 否则返回一个默认的Cell 323 | return m_unknown; 324 | } 325 | 326 | //FIXME check why the last line of the map is corrupted. 327 | template < class Cell, class Storage, const bool isClass > 328 | DoubleArray2D* Map::toDoubleArray() const 329 | { 330 | DoubleArray2D* darr=new DoubleArray2D(getMapSizeX()-1, getMapSizeY()-1); 331 | for(int x=0; xcell(p)=cell(p); 336 | } 337 | return darr; 338 | } 339 | 340 | 341 | template < class Cell, class Storage, const bool isClass > 342 | Map* Map::toDoubleMap() const 343 | { 344 | //FIXME size the map so that m_center will be setted accordingly 345 | Point pmin=map2world(IntPoint(0,0)); 346 | Point pmax=map2world(getMapSizeX()-1,getMapSizeY()-1); 347 | Point center=(pmax+pmin)*0.5; 348 | Map* plainMap=new Map(center, (pmax-pmin).x, (pmax-pmin).y, getDelta()); 349 | for(int x=0; xcell(p)=cell(p); 354 | } 355 | return plainMap; 356 | } 357 | 358 | } // end namespace 359 | 360 | #endif -------------------------------------------------------------------------------- /include/gridfastslam/gridslamprocessor.h: -------------------------------------------------------------------------------- 1 | #ifndef GRIDSLAMPROCESSOR_H 2 | #define GRIDSLAMPROCESSOR_H 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include "utils/macro_params.h" 9 | #include "utils/point.h" 10 | #include "scanmatcher/scanmatcher.h" 11 | #include "motionmodel/motionmodel.h" 12 | #include "sensor/odometryreading.h" 13 | #include "sensor/rangesensor.h" 14 | #include "sensor/rangereading.h" 15 | #include "particlefilter/particlefilter.h" 16 | 17 | namespace GMapping { 18 | 19 | /** 20 | * GridSlamProcessor，GridFastSLAM算法实现类，实现了一个RBPF，且每个粒子都拥有自己的地图和机器人位姿。 21 | * 22 | * 算法流程： 23 | * 获取里程计下机器人位姿（两时刻测量的位姿差值为控制输入），对每一个粒子的上一时刻最优估计位姿进行运动模型来更新采样 24 | * 运动模型更新采样得到的位姿作为初始值，读取当前时刻激光雷达数据，利用观测模型进行Scan-Matching（扫描匹配） 25 | * （Scan-Matching实际是在观测模型的基础上，根据地图来进一步优化位姿，得到最优的粒子群，即proposal分布对应的粒子群） 26 | * 权重处理，权重更新，重采样 27 | * 数据成员： 28 | * TNode，树的结点（结构体），一个树储存了一整条轨迹，一个节点表示这条轨迹中的其中一个点 29 | * Particle，粒子滤波器中的粒子（结构体），每个粒子有对应的地图、位姿、权重、轨迹 30 | * m_matcher，ScanMatcher类，扫描匹配类对象，用于Scan-Matching算法实现 31 | * 32 | * m_motionModel，MotionModel类，里程计运动模型类对象，用于运动模型更新采样算法的实现 33 | * m_beams，一帧激光数据的激光束数量 34 | * last_update_time_，上一次处理激光数据的时刻 35 | * period_，用于判断两次处理激光数据时刻的间隔 36 | * m_particles，vector数组，存储Particle类型的粒子 37 | * m_indexes，vector数组，重采样之后，剩余的（或被保留的）粒子的下标（这个下标的是会重复的） 38 | * m_weights，vector数组，所有粒子当前的权重 39 | * m_count，判断processScan()函数的调用次数（是否是第0次调用） 40 | * m_readingCount，每成功调用processScan()函数一次，该函数内部会是其处理激光数据的次数+1 41 | * m_lastPartPose，将当前时刻里程计下机器人位姿赋给它，为下一次迭代做准备 42 | * m_odoPose，上一时刻里程计下机器人位姿 43 | * m_pose 44 | * m_linearDistance，统计机器人在进行激光雷达更新之前 走了多远的距离 以及　平移了多少的角度（累计值） 45 | * m_angularDistance，统计机器人在进行激光雷达更新之前 走了多远的距离 以及　平移了多少的角度（累计值） 46 | * 47 | * m_xmin，地图的尺寸（double类型，worldSize） 48 | * m_xmax，地图的尺寸 49 | * m_ymin，地图的尺寸 50 | * m_ymax，地图的尺寸 51 | * m_delta，地图的分辨率，即世界坐标系到地图坐标系的缩放比例关系（worldSize=mapSize*delta） 52 | * 53 | * m_neff，自适应重采样中的Neff值（有效粒子数个数） 54 | * m_resampleThreshold，自适应重采样中Neff应判断的阈值 55 | * m_minimumScore，在scanMatch()函数中评估扫描匹配结果，若score大于该值则优化成功，可更新最优位姿 56 | * m_regScore 57 | * m_critScore 58 | * m_maxMove 59 | * m_linearThresholdDistance，每当机器人走过一段距离后，处理一次激光数据 60 | * m_angularThresholdDistance ，每当机器人转动一段角度后再，处理一次激光数据 61 | * m_obsSigmaGain，权重归一化时使用的平滑因子 62 | * 63 | * 函数成员： 64 | * clone，对当前GridSlamProcessor类对象进行深拷贝 65 | * setSensorMap，设置传感器地图 66 | * init，GridFastSLAM初始化，主要用于初始化各个粒子的一些信息 67 | * setMatchingParameters，设置扫描匹配的参数，主要是调用ScanMatcher::setMatchingParameters()函数 68 | * setMotionModelParameters，设置里程计运动模型的噪声参数 69 | * setUpdateDistances，设置机器人更新滤波器的阈值（机器人每走过一段距离会对激光数据进行处理） 70 | * setUpdatePeriod，设置机器人更新滤波器的阈值（机器人每走过一段距离会对激光数据进行处理） 71 | * 72 | * processTruePos，处理真实位姿，仿真，当里程计为理想传感器（误差为0）时使用 73 | * processScan，处理一帧激光雷达数据，核心算法函数 74 | * 75 | * getParticles，获取m_particles数组（存储Particle类型的粒子） 76 | * getIndexes，获取m_indexes数组（存储重采样之后，剩余的粒子的下标） 77 | * getBestParticleIndex，获取粒子群中 累计权重最大 的粒子的下标 78 | * scanMatch，扫描匹配算法实现函数 79 | * normalize，权重归一化处理，同时计算出有效粒子数neff值 80 | * resample，重采样 81 | * 82 | * getTrajectories，得到所有粒子的轨迹的起点(得到了轨迹的起点，就可以得到一整条轨迹) 83 | * integrateScanSequence，集成所有的scan（这个函数没有被调用过。） 84 | * updateTreeWeights，更新权重（调用了normalize()函数进行权重归一化处理，同时计算出有效粒子数neff值） 85 | * resetTree，把每个粒子对应的轨迹树中各个节点的 accWeight 清零 86 | * propagateWeights，更新权重，被updateTreeWeights()函数调用 87 | */ 88 | class GridSlamProcessor { 89 | public: 90 | /** 91 | * TNode，树的结点 结构体，一个树储存了一整条轨迹，一个节点表示这条轨迹中的其中一个点 92 | * 每个粒子对应一个树，从该树最近的一个node，遍历该树，可以得到该粒子对应的整条轨迹 93 | * 94 | * 数据成员： 95 | * pose，该节点对应的机器人位姿 96 | * weight，该节点对应粒子的权重 97 | * accWeight，之前所有节点对应粒子的权重之和（该节点的子节点对应粒子的权重之和） 98 | * parent，指向父节点的指针，每个粒子的路径都是记录最近的一个点，然后通过parent指针遍历整条路径 99 | * reading，该节点激光雷达的读数 100 | * childs，该节点的子节点的数量 101 | */ 102 | struct TNode { 103 | OrientedPoint pose; 104 | double weight; 105 | double accWeight; 106 | double gweight; 107 | TNode* parent; // 无指向子节点的指针，只有指向parent的指针 108 | const RangeReading* reading; 109 | unsigned int childs; 110 | mutable unsigned int visitCounter; // counter in visiting the node (internally used) 111 | mutable bool flag; // visit flag (internally used) 112 | 113 | TNode(const OrientedPoint& pose, double weight, TNode* parent=0, unsigned int childs=0); 114 | ~TNode(); 115 | }; 116 | /** 117 | * Particle，粒子滤波器中的粒子结构体，每个粒子有对应的地图、位姿、权重、轨迹 118 | * 数据成员： 119 | * map，地图 120 | * running_scans，存储最近的N帧激光雷达的数据，用来生成临时地图，从而进行CSM 121 | * pose，位姿 122 | * previousPose，前一时刻机器人位姿，用于计算里程计位移 123 | * weight，权重 124 | * weightSum，累计权重（该粒子在各个时刻的权重之和，轨迹上的各个节点的权重之和） 125 | * gweight， 126 | * previousIndex，上一个粒子的下标 127 | * node，该粒子对应的整条轨迹（树），存储的是粒子的最近的一个节点（树中没有孩子的那个节点） 128 | * （每个粒子对应一个树，从该树最近的一个node，遍历该树，可以得到该粒子对应的整条轨迹） 129 | */ 130 | struct Particle { 131 | ScanMatcherMap map; 132 | std::vector running_scans; 133 | OrientedPoint pose; 134 | OrientedPoint previousPose; 135 | double weight; 136 | double weightSum; 137 | double gweight; 138 | int previousIndex; 139 | TNode* node; 140 | 141 | Particle(const ScanMatcherMap& map); 142 | inline operator double() const { return weight; } 143 | inline operator OrientedPoint() const { return pose; } 144 | inline void setWeight( double w ) { weight = w }; 145 | }; 146 | typedef std::vector TNodeVector; 147 | typedef std::deque TNodeDeque; 148 | typedef std::vector ParticleVector; 149 | ScanMatcher m_matcher; 150 | 151 | GridSlamProcessor(); 152 | GridSlamProcessor(std::ostream& infoStr); 153 | GridSlamProcessor* clone() const; 154 | virtual ~GridSlamProcessor(); 155 | 156 | // methods for accessing the parameters 157 | void setSensorMap(const SensorMap& smap); 158 | void init(unsigned int size, double xmin, double ymin, double xmax, double ymax, double delta, 159 | OrientedPoint initialPose=OrientedPoint(0,0,0)); 160 | void setMatchingParameters(double urange, double range, double sigma, int kernsize, double lopt, double aopt, 161 | int iterations, double likelihoodSigma=1, double likelihoodGain=1, unsigned int likelihoodSkip=0); 162 | void setMotionModelParameters(double srr, double srt, double str, double stt); 163 | void setUpdateDistances(double linear, double angular, double resampleThreshold); 164 | void setUpdatePeriod(double p) {period_=p;} 165 | 166 | 167 | // the "core" algorithm 核心算法 168 | void processTruePos( const OdometryReading& odometry ); 169 | bool processScan( const RangeReading& reading, int adaptParticles=0 ); 170 | 171 | // callbacks 回调函数，什么都没做 172 | virtual void onOdometryUpdate(); 173 | virtual void onResampleUpdate(); 174 | virtual void onScanmatchUpdate(); 175 | 176 | int getBestParticleIndex() const; 177 | inline ParticleVector& getParticles() const { return m_particles; } 178 | inline std::vector& getIndexes() const { return m_indexes; } 179 | /**the stream used for writing the output of the algorithm*/ 180 | std::ofstream& outputStream() { return m_outputStream; }; 181 | /**the stream used for writing the info/debug messages*/ 182 | std::ostream& infoStream( return m_infoStream; ); 183 | 184 | // gridslamprocessor_tree.cpp 中定义 185 | TNodeVector getTrajectories() const; 186 | void integrateScanSequence(TNode* node); 187 | 188 | // 获取扫描匹配类对象m_matcher的属性 189 | /**the maxrange of the laser to consider */ 190 | MEMBER_PARAM_SET_GET(m_matcher, double, laserMaxRange, protected, public, public); 191 | /**the maximum usable range of the laser. A beam is cropped to this value. [scanmatcher]*/ 192 | MEMBER_PARAM_SET_GET(m_matcher, double, usableRange, protected, public, public); 193 | /**The sigma used by the greedy endpoint matching. [scanmatcher]*/ 194 | MEMBER_PARAM_SET_GET(m_matcher,double, gaussianSigma, protected, public, public); 195 | /**The sigma of a beam used for likelihood computation [scanmatcher]*/ 196 | MEMBER_PARAM_SET_GET(m_matcher,double, likelihoodSigma, protected, public, public); 197 | /**The kernel in which to look for a correspondence[scanmatcher]*/ 198 | MEMBER_PARAM_SET_GET(m_matcher, int, kernelSize, protected, public, public); 199 | /**The optimization step in rotation [scanmatcher]*/ 200 | MEMBER_PARAM_SET_GET(m_matcher, double, optAngularDelta, protected, public, public); 201 | /**The optimization step in translation [scanmatcher]*/ 202 | MEMBER_PARAM_SET_GET(m_matcher, double, optLinearDelta, protected, public, public); 203 | /**The number of iterations of the scanmatcher [scanmatcher]*/ 204 | MEMBER_PARAM_SET_GET(m_matcher, unsigned int, optRecursiveIterations, protected, public, public); 205 | /**the beams to skip for computing the likelihood (consider a beam every likelihoodSkip) [scanmatcher]*/ 206 | MEMBER_PARAM_SET_GET(m_matcher, unsigned int, likelihoodSkip, protected, public, public); 207 | /**translational sampling range for the likelihood [scanmatcher]*/ 208 | MEMBER_PARAM_SET_GET(m_matcher, double, llsamplerange, protected, public, public); 209 | /**angular sampling range for the likelihood [scanmatcher]*/ 210 | MEMBER_PARAM_SET_GET(m_matcher, double, lasamplerange, protected, public, public); 211 | /**translational sampling range for the likelihood [scanmatcher]*/ 212 | MEMBER_PARAM_SET_GET(m_matcher, double, llsamplestep, protected, public, public); 213 | /**angular sampling step for the likelihood [scanmatcher]*/ 214 | MEMBER_PARAM_SET_GET(m_matcher, double, lasamplestep, protected, public, public); 215 | /**generate an accupancy grid map [scanmatcher]*/ 216 | MEMBER_PARAM_SET_GET(m_matcher, bool, generateMap, protected, public, public); 217 | /**enlarge the map when the robot goes out of the boundaries [scanmatcher]*/ 218 | MEMBER_PARAM_SET_GET(m_matcher, bool, enlargeStep, protected, public, public); 219 | /**pose of the laser wrt the robot [scanmatcher]*/ 220 | MEMBER_PARAM_SET_GET(m_matcher, OrientedPoint, laserPose, protected, public, public); 221 | 222 | // 获取里程计运动模型类对象m_motionModel的属性 223 | /**odometry error in translation as a function of translation (rho/rho) [motionmodel]*/ 224 | STRUCT_PARAM_SET_GET(m_motionModel, double, srr, protected, public, public); 225 | /**odometry error in translation as a function of rotation (rho/theta) [motionmodel]*/ 226 | STRUCT_PARAM_SET_GET(m_motionModel, double, srt, protected, public, public); 227 | /**odometry error in rotation as a function of translation (theta/rho) [motionmodel]*/ 228 | STRUCT_PARAM_SET_GET(m_motionModel, double, str, protected, public, public); 229 | /**odometry error in rotation as a function of rotation (theta/theta) [motionmodel]*/ 230 | STRUCT_PARAM_SET_GET(m_motionModel, double, stt, protected, public, public); 231 | 232 | protected: 233 | MotionModel m_motionModel; 234 | unsigned int m_beams; 235 | double last_update_time_; 236 | double period_; 237 | ParticleVector m_particles; 238 | std::vector m_indexes; 239 | std::vector m_weights; 240 | int m_count, m_readingCount; 241 | OrientedPoint m_lastPartPose; 242 | OrientedPoint m_odoPose; 243 | OrientedPoint m_pose; 244 | double m_linearDistance, m_angularDistance; 245 | 246 | // Copy constructor 拷贝构造函数 247 | GridSlamProcessor(const GridSlamProcessor& gsp); 248 | 249 | //processing parameters (size of the map) 250 | PARAM_GET(double, xmin, protected, public); 251 | PARAM_GET(double, ymin, protected, public); 252 | PARAM_GET(double, xmax, protected, public); 253 | PARAM_GET(double, ymax, protected, public); 254 | //processing parameters (resolution of the map) 255 | PARAM_GET(double, delta, protected, public); 256 | PARAM_GET(double, neff, protected, public); 257 | /**this sets the neff based resampling threshold*/ 258 | PARAM_SET_GET(double, resampleThreshold, protected, public, public); 259 | /**minimum score for considering the outcome of the scanmatching good*/ 260 | PARAM_SET_GET(double, minimumScore, protected, public, public); 261 | //registration score (if a scan score is above this threshold it is registered in the map) 262 | PARAM_SET_GET(double, regScore, protected, public, public); 263 | //registration score (if a scan score is below this threshold a scan matching failure is reported) 264 | PARAM_SET_GET(double, critScore, protected, public, public); 265 | //registration score maximum move allowed between consecutive scans 266 | PARAM_SET_GET(double, maxMove, protected, public, public); 267 | //process a scan each time the robot translates of linearThresholdDistance 268 | PARAM_SET_GET(double, linearThresholdDistance, protected, public, public); 269 | //process a scan each time the robot rotates more than angularThresholdDistance 270 | PARAM_SET_GET(double, angularThresholdDistance, protected, public, public); 271 | //smoothing factor for the likelihood 272 | PARAM_SET_GET(double, obsSigmaGain, protected, public, public); 273 | //stream in which to write the gfs file 274 | std::ofstream m_outputStream; 275 | // stream in which to write the messages 276 | std::ostream& m_infoStream; 277 | 278 | private: 279 | // gridslamprocessor.hxx 中定义 280 | inline void scanMatch( const double *plainReading ); 281 | inline void normalize(); 282 | inline bool resample( const double* plainReading, int adaptParticles, const RangeReading* rr=0 ); 283 | 284 | // gridslamprocessor_tree.cpp 中定义 285 | void updateTreeWeights(bool weightsAlreadyNormalized = false); 286 | void resetTree(); 287 | double propagateWeights(); 288 | }; 289 | 290 | typedef std::multimap TNodeMultimap; 291 | 292 | // scanMatch normalize resample 在此文件中定义 293 | #include "gridslamprocessor.hxx" 294 | 295 | } // end namespace 296 | 297 | #endif -------------------------------------------------------------------------------- /include/scanmatcher/scanmatcher.h: -------------------------------------------------------------------------------- 1 | #ifndef SCANMATCHER_H 2 | #define SCANMATCHER_H 3 | #include "utils/macro_params.h" 4 | #include "utils/point.h" 5 | #include "utils/stat.h" 6 | #include "smmap.h" 7 | #include "sensor/rangereading.h" 8 | 9 | #define LASER_MAXBEAMS 2048 10 | 11 | namespace Gmapping { 12 | 13 | /** 14 | * ScanMatcher，扫描匹配类 15 | * 数据成员： 16 | * m_laserPose，激光的位置，默认为(0,0,0)，也可由setLaserParameters函数成员设置 17 | * m_laserBeams，激光束的数量，默认为0，也可由setLaserParameters函数成员设置 18 | * m_laserAngles，激光束的夹角数据，也可由setLaserParameters函数成员设置 19 | * 20 | * m_laserMaxRange，激光的最大测距范围，由setMatchingParameters函数成员设置 21 | * m_usableRange，（观测时，使用时）激光的最大测距范围（Zmax），由setMatchingParameters函数成员设置 22 | * m_gaussianSigma，计算socre时的方差，在score函数成员中被使用，由setMatchingParameters函数成员设置 23 | * m_likelihoodSigma，计算似然时的方差，在likelihoodAndScore函数成员中被使用，由setMatchingParameters函数成员设置 24 | * m_kernelSize，主要用在计算score时搜索框的大小，由setMatchingParameters函数成员设置 25 | * m_optAngularDelta，优化时的角度增量，由setMatchingParameters函数成员设置 26 | * m_optLinearDelta，优化时的长度增量，由setMatchingParameters函数成员设置 27 | * m_optRecursiveIterations，优化时的迭代次数，由setMatchingParameters函数成员设置 28 | * m_likelihoodSkip，计算似然时，每隔m_likelihoodSkip个激光束，就跳过一个激光束，由setMatchingParameters函数成员设置 29 | * 30 | * m_llsamplerange，likelihood函数成员使用，不同解析度（如5cm/10cm/20cm/25cm）的栅格地图该参数值不一样 31 | * m_llsamplestep，likelihood函数成员使用，不同解析度（如5cm/10cm/20cm/25cm）的栅格地图该参数值不一样 32 | * m_lasamplerange，likelihood函数成员使用，不同解析度（如5cm/10cm/20cm/25cm）的栅格地图该参数值不一样 33 | * m_lasamplestep，likelihood函数成员使用，不同解析度（如5cm/10cm/20cm/25cm）的栅格地图该参数值不一样 34 | * m_generateMap，是否需要生成地图（true，会为了空闲区域分配内存；fasle，不会） 35 | * 36 | * m_enlargeStep，地图进行拓展的大小，默认为10 37 | * m_fullnessThreshold，被认为是占用的阈值，默认为0.1 38 | * m_angularOdometryReliability，里程计的角度可靠性，默认为0.0 39 | * m_linearOdometryReliability，里程计的长度可靠性，默认为0.0 40 | * m_freeCellRatio，free和occupany的阈值，默认为sqrt(2.0)（离激光点的空闲距离，即沿激光束方向离激光点这么远距离的栅格一定是空闲的。） 41 | * m_initialBeamsSkip，去掉初始的几个激光束的数量，默认为0 42 | * m_activeAreaComputed，地图有效区域是否被计算过，默认为false 43 | * nullLikelihood，空似然值（激光雷达未击中，即观测模型没有输出），默认为-0.5，用作标志位 44 | * m_linePoints，激光束起点到终点的路径点数组，只分配一次内存给该数组 45 | * 函数成员： 46 | * icpOptimize，icp方法求解最优位姿 47 | * optimize，似然场观测模型求解最优位姿（重载函数） 48 | * registerScan，已知位置的覆盖栅格地图算法（使用的模型为Counting Model） 49 | * setLaserParameters，设置激光参数，包括激光束，夹角值和激光位置 50 | * setMatchingParameters，设置Scan-Matching扫描匹配参数 51 | * invalidateActiveArea，使地图有效区域的计算失效，即设置m_activeAreaComputed为false 52 | * computeActiveArea，计算有效区域，通过激光雷达的数据计算出来哪个地图栅格应该要被更新了 53 | * icpStep，icpOptimize中的一步，这个函数目前在gmapping中没有被使用（不完善，还需修改） 54 | * score，根据地图、机器人位置、激光雷达数据，计算出一个得分：原理为likelihood_field_range_finder_model 55 | * likelihoodAndScore，根据地图、机器人位置、激光雷达数据，同时计算出一个得分和似然：原理为likelihood_field_range_finder_model 56 | * likelihood，计算某一个机器人位姿的似然值（重载函数） 57 | * laserAngles，获取激光束的夹角数据 58 | * laserBeams，获取激光束的数量 59 | */ 60 | class ScanMatcher { 61 | public: 62 | typedef Covariance3 CovarianceMatrix; 63 | // allocate this large array only once 64 | IntPoint* m_linePoints; 65 | static const double nullLikelihood; 66 | 67 | ScanMatcher(); 68 | ~ScanMatcher(); 69 | 70 | double icpOptimize( OrientedPoint& pnew, const ScanMatcherMap& map, const OrientedPoint& p, const double* readings ) const; 71 | double optimize( OrientedPoint& pnew, const ScanMatcherMap& map, const OrientedPoint& init, const double* readings ) const; // 已写 72 | double optimize( OrientedPoint& mean, CovarianceMatrix& cov, const ScanMatcherMap& map, const OrientedPoint& init, const double* readings ) const; 73 | double registerScan( ScanMatcherMap& map, const OrientedPoint& p, const double* readings ); 74 | void setLaserParameters( unsigned int beams, double* angles, const OrientedPoint& lpose );// 已写 75 | void setMatchingParameters( double urange, double range, double sigma, int kernsize, double lopt, double aopt, int iterations, double likelihoodSigma=1, unsigned int likelihoodSkip=0 );// 已写 76 | void invalidateActiveArea();// 已写 77 | void computeActiveArea( ScanMatcherMap& map, const OrientedPoint& p, const double* readings ); 78 | inline double icpStep( OrientedPoint& pret, const ScanMatcherMap& map, const OrientedPoint& p, const double* readings ) const; 79 | inline double score( const ScanMatcherMap& map, const OrientedPoint& p, const double* readings ) const;// 已写 80 | inline unsigned int likelihoodAndScore( double& s, double& l, const ScanMatcherMap& map, const OrientedPoint& p, const double* readings ) const;// 已写 81 | double likelihood( double& lmax, OrientedPoint& mean, CovarianceMatrix& cov, const ScanMatcherMap& map, const OrientedPoint& p, const double* readings ); 82 | double likelihood( double& _lmax, OrientedPoint& _mean, CovarianceMatrix& _cov, const ScanMatcherMap& map, const OrientedPoint& p, Gaussian3& odometry, const double* readings, double gain=180. ); 83 | inline const double* laserAngles() const { return m_laserAngles; } 84 | inline unsigned int laserBeams() const { return m_laserBeams; } 85 | 86 | protected: 87 | bool m_activeAreaComputed; 88 | unsigned int m_laserBeams; 89 | double m_laserAngles; 90 | 91 | PARAM_SET_GET(OrientedPoint, laserPose, protected, public, public) 92 | PARAM_SET_GET(double, laserMaxRange, protected, public, public) 93 | PARAM_SET_GET(double, usableRange, protected, public, public) 94 | PARAM_SET_GET(double, gaussianSigma, protected, public, public) 95 | PARAM_SET_GET(double, likelihoodSigma, protected, public, public) 96 | PARAM_SET_GET(int, kernelSize, protected, public, public) 97 | PARAM_SET_GET(double, optAngularDelta, protected, public, public) 98 | PARAM_SET_GET(double, optLinearDelta, protected, public, public) 99 | PARAM_SET_GET(unsigned int, optRecursiveIterations, protected, public, public) 100 | PARAM_SET_GET(unsigned int, likelihoodSkip, protected, public, public) 101 | PARAM_SET_GET(double, llsamplerange, protected, public, public) 102 | PARAM_SET_GET(double, llsamplestep, protected, public, public) 103 | PARAM_SET_GET(double, lasamplerange, protected, public, public) 104 | PARAM_SET_GET(double, lasamplestep, protected, public, public) 105 | PARAM_SET_GET(bool, generateMap, protected, public, public) 106 | PARAM_SET_GET(double, enlargeStep, protected, public, public) 107 | PARAM_SET_GET(double, fullnessThreshold, protected, public, public) 108 | PARAM_SET_GET(double, angularOdometryReliability, protected, public, public) 109 | PARAM_SET_GET(double, linearOdometryReliability, protected, public, public) 110 | PARAM_SET_GET(double, freeCellRatio, protected, public, public) 111 | PARAM_SET_GET(unsigned int, initialBeamsSkip, protected, public, public) 112 | }; 113 | 114 | /** 115 | * 给定当前时刻机器人的估计位姿，利用激光雷达似然场模型与观测值 116 | * 去计算观测值与地图中可能击中障碍物的匹配程度 117 | * 相当于评价当前这个估计位姿是否准确，越准确得分越高 118 | * 原理为likelihood_field_range_finder_model 119 | * map，地图 120 | * p，当前t时刻估计的机器人位姿（世界坐标系） 121 | * readings，存储激光雷达读数的数组（观测值，一帧激光数据） 122 | */ 123 | inline double ScanMatcher::score( const ScanMatcherMap& map, const OrientedPoint& p, const double* readings ) const { 124 | // 初始化score=0 125 | double s=0; 126 | // 将激光束的夹角数组，跳过m_initialBeamSkip个元素处的指针赋给指针变量 angle 127 | const double* angle = m_laserAngles + m_initialBeamSkip; 128 | 129 | // m_laserPose 表示 激光雷达坐标系 在 机器人底盘基准坐标系 中的坐标 130 | // 将激光雷达坐标系转换到世界坐标系 131 | OrientedPoint lp = p; 132 | lp.x += cos(p.theta) * m_laserPose.x - sin(p.theta) * m_laserPose.y; 133 | lp.y += sin(p.theta) * m_laserPose.x + cos(p.theta) * m_laserPose.y; 134 | lp.theta += m_laserPose.theta; 135 | 136 | unsigned int skip = 0; 137 | // 初始化地图中占用和空闲的阈值，并缩放到世界坐标系中 138 | double freeDelta = map.getDelta()*m_freeCellRatio; 139 | 140 | // 遍历一帧激光数据中的所有激光束 141 | for ( const double* r=readings+m_initialBeamSkip; r m_likehoodSkip?0:skip; 147 | if ( skip || *r > m_usableRange || *r == 0.0 ) 148 | continue; 149 | 150 | // 计算当前激光束击中点（观测值）在世界坐标系下的坐标，再转换到地图坐标系下 151 | Point phit = lp; 152 | phit.x += *r * cos( lp.theta + *angle ); 153 | phit.y += *r * sin( lp.theta + *angle ); 154 | IntPoint iphit = map.world2map(phit); 155 | 156 | // 该激光束经过的最后一个空闲点在世界坐标系下的坐标 157 | // 原理：沿着激光束方向，击中点（观测值）的前面一个点必定是没有障碍物的 158 | Point pfree = lp; 159 | pfree.x += (*r - freeDelta) * cos( lp.theta + *angle ); 160 | pfree.y += (*r - freeDelta) * sin( lp.theta + *angle ); 161 | pfree = pfree - phit; // 两者距离 162 | IntPoint ipfree = map.world2map(pfree); 163 | 164 | bool found = false; 165 | Point bestMu(0., 0.); 166 | // 遍历地图中 m_kernelSize*m_kernelSize 大小的窗口 167 | // 搜索地图中当前激光束击中点附近距离最近的点（这个点表示一个障碍物） 168 | for ( int xx=-m_kernelSize; xx<=m_kernelSize; xx++ ) 169 | for ( int yy=-m_kernelSize; yy<=m_kernelSize; yy++ ) { 170 | // 击中点（观测值）和空闲点各自偏移(xx, yy) 171 | IntPoint pr = iphit + IntPoint(xx, yy); 172 | IntPoint pf = pr + ipfree; 173 | // 得到各自对应的Cell 174 | const PointAccumulator& cell = map.cell(pr); 175 | const PointAccumulator& fcell = map.cell(pf); 176 | // PointAccumulator类中运算符 double 重载 177 | // 即(double)cell返回的是该cell被占用的概率 178 | if ( ((double)cell) > m_fullnessThreshold && ((double)fcell) < m_fullnessThreshold ) { 179 | // 如果地图中点pr对应的cell被占用，而pf对应的cell没有被占用，则说明找到了一个合法的点 180 | Point mu = phit - cell.mean(); 181 | if ( !found ) { 182 | bestMu = mu; 183 | found = true; 184 | } else { 185 | bestMu = (mu*mu) < (bestMu*bestMu)?mu:bestMu; 186 | } 187 | } 188 | } 189 | // 计算socre，公式exp(-d^2 / sigma))，sigma表示方差 190 | if ( found ) { 191 | s += exp(-1.0/m_gaussianSigma*bestMu*bestMu); 192 | } 193 | } 194 | return s; 195 | } 196 | 197 | /** 198 | * 根据地图、机器人位置、激光雷达数据，同时计算出一个得分和似然：原理为likelihood_field_range_finder_model 199 | * 输出计算出来的得分（score），以及似然（粒子的权重，p(z|x,m)） 200 | * s，得分（score） 201 | * l，似然（粒子的权重，p(z|x,m)） 202 | * map，地图 203 | * p，当前t时刻估计的机器人位姿（世界坐标系） 204 | * readings，存储激光雷达读数的数组（观测值，一帧激光数据） 205 | */ 206 | inline unsigned int likelihoodAndScore( double& s, double& l, 207 | const ScanMatcherMap& map, const OrientedPoint& p, const double* readings ) const 208 | { 209 | // 初始化s，l 210 | s = 0; 211 | l = 0; 212 | // 将激光束的夹角数组，跳过m_initialBeamSkip个元素处的指针赋给指针变量 angle 213 | const double* angle = m_laserAngles + m_initialBeamSkip; 214 | // m_laserPose 表示 激光雷达坐标系 在 机器人底盘基准坐标系 中的坐标 215 | // 将激光雷达系转换到世界坐标系 216 | OrientedPoint lp = p; 217 | lp.x += cos(p.theta) * m_laserPose.x - sin(p.theta) * m_laserPose.y; 218 | lp.y += cos(p.theta) * m_laserPose.y + sin(p.theta) * m_laserPose.x; 219 | lp.theta += m_laserPose.theta; 220 | 221 | // 如果没有击中的时候（没有观测到时），似然值设置为 nullLikelihood = -0.5 222 | double noHit = nullLikelihood / m_likelihoodSigma; 223 | 224 | 225 | unsigned int skip = 0; 226 | unsigned int c = 0; 227 | // 初始化地图中占用和空闲的阈值，并缩放到世界坐标系中 228 | double freeDleta = map.getDelta() * m_freeCellRatio; 229 | // 遍历一帧激光数据中的所有激光束（跳过了开始的m_initialBeamSkip个激光束） 230 | for ( const double* r = readings + m_initialBeamSkip; r m_likelihoodSkip?0: skip; 235 | if ( *r > m_usableRange ) continue; 236 | if ( skip ) continue; 237 | 238 | // 计算当前激光束击中点（观测值）在世界坐标系下的坐标，再转换到地图坐标系下 239 | Point phit = lp; 240 | phit.x += *r * cos(lp.theta + *angle); 241 | phit.y += *r * sin(lp.theta + *angle); 242 | IntPoint iphit = map.world2map(phit); 243 | 244 | // 该激光束经过的最后一个空闲点在世界坐标系下的坐标 245 | // 原理：沿着激光束方向，击中点（观测值）的前面一个点必定是没有障碍物的 246 | Point pfree = lp; 247 | pfree.x += (*r - freeDleta) * cos(lp.theta + *angle); 248 | pfree.y += (*r - freeDleta) * sin(lp.theta + *angle); 249 | pfree = pfree - phit; // 两者距离 250 | IntPoint ipfree = map.world2map(pfree); 251 | 252 | bool found = false; 253 | Point bestMu(0., 0.); 254 | // 遍历地图中 m_kernelSize*m_kernelSize 大小的窗口 255 | // 搜索地图中当前激光束击中点附近距离最近的点（这个点表示一个障碍物） 256 | for (int xx=-m_kernelSize; xx<=m_kernelSize; xx++) 257 | for (int yy=-m_kernelSize; yy<=m_kernelSize; yy++) { 258 | // 击中点（观测值）和空闲点各自偏移(xx, yy) 259 | IntPoint pr = iphit + IntPoint(xx, yy); 260 | intPoint pf = pr + ipfree; 261 | // 得到各自对应的Cell 262 | const PointAccumulator& cell = map.cell(pr); 263 | const PointAccumulator& cell = map.cell(pr); 264 | // PointAccumulator类中运算符 double 重载 265 | // 即(double)cell返回的是该cell被占用的概率 266 | if ( ( (double)cell )>m_fullnessThreshold && ( (double)fcell ) 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include "utils/stat.h" 9 | #include "gridfastslam/gridslamprocessor.h" 10 | #include "utils/point.h" 11 | 12 | namespace GMapping { 13 | 14 | // 若机器人在走了m_distanceThresholdCheck这么远的距离都没有进行激光雷达的更新，则需要进行报警 15 | const double m_distanceThresholdCheck = 5; 16 | 17 | using namespace std; 18 | 19 | /** 20 | * 构造函数 21 | */ 22 | GridSlamProcessor::GridSlamProcessor() : m_infoStream(cout) { 23 | period_ = 5.0; 24 | m_obsSigmaGain=1; 25 | m_resampleThreshold=0.5; 26 | m_minimumScore=0.; 27 | } 28 | 29 | GridSlamProcessor(std::ostream& infoS) : m_infoStream(infoS){ 30 | period_ = 5.0; 31 | m_obsSigmaGain=1; 32 | m_resampleThreshold=0.5; 33 | m_minimumScore=0.; 34 | } 35 | 36 | /** 37 | * 拷贝构造函数 38 | * gsp，被拷贝的GridSlamProcessor类对象 39 | */ 40 | GridSlamProcessor::GridSlamProcessor(const GridSlamProcessor& gsp) { 41 | period_ = 5.0; 42 | 43 | m_obsSigmaGain = gsp.m_obsSigmaGain; 44 | m_resampleThreshold = gsp.m_resampleThreshold; 45 | m_minimumScore=gsp.m_minimumScore; 46 | 47 | m_beams=gsp.m_beams; 48 | m_indexes=gsp.m_indexes; 49 | m_motionModel=gsp.m_motionModel; 50 | m_resampleThreshold=gsp.m_resampleThreshold; 51 | m_matcher=gsp.m_matcher; 52 | 53 | m_count=gsp.m_count; 54 | m_readingCount=gsp.m_readingCount; 55 | m_lastPartPose=gsp.m_lastPartPose; 56 | m_pose=gsp.m_pose; 57 | m_odoPose=gsp.m_odoPose; 58 | m_linearDistance=gsp.m_linearDistance; 59 | m_angularDistance=gsp.m_angularDistance; 60 | m_neff=gsp.m_neff; 61 | 62 | cerr << "FILTER COPY CONSTRUCTOR" << endl; 63 | cerr << "m_odoPose=" << m_odoPose.x << " " < >::reference* const, int> PointerMap; 104 | PointerMap pmap; 105 | for (ParticleVector::const_iterator it=m_particles.begin(); it!=m_particles.end(); it++){ 106 | const ScanMatcherMap& m1(it->map); 107 | const HierarchicalArray2D& h1(m1.storage()); 108 | for (int x=0; x >& a1(h1.m_cells[x][y]); 111 | if (a1.m_reference){ 112 | PointerMap::iterator f=pmap.find(a1.m_reference); 113 | if (f==pmap.end()) 114 | pmap.insert(make_pair(a1.m_reference, 1)); 115 | else 116 | f->second++; 117 | } 118 | } 119 | } 120 | } 121 | cerr << __PRETTY_FUNCTION__ << ": Number of allocated chunks" << pmap.size() << endl; 122 | for(PointerMap::const_iterator it=pmap.begin(); it!=pmap.end(); it++) 123 | assert(it->first->shares==(unsigned int)it->second); 124 | 125 | cerr << __PRETTY_FUNCTION__ << ": SUCCESS, the error is somewhere else" << endl; 126 | # endif 127 | GridSlamProcessor* cloned = new GridSlamProcessor(*this); 128 | # ifdef MAP_CONSISTENCY_CHECK 129 | cerr << __PRETTY_FUNCTION__ << ": trajectories end" << endl; 130 | cerr << __PRETTY_FUNCTION__ << ": performing afterclone_fit_test" << endl; 131 | ParticleVector::const_iterator jt=cloned->m_particles.begin(); 132 | for (ParticleVector::const_iterator it=m_particles.begin(); it!=m_particles.end(); it++){ 133 | const ScanMatcherMap& m1(it->map); 134 | const ScanMatcherMap& m2(jt->map); 135 | const HierarchicalArray2D& h1(m1.storage()); 136 | const HierarchicalArray2D& h2(m2.storage()); 137 | jt++; 138 | for (int x=0; x >& a1(h1.m_cells[x][y]); 141 | const autoptr< Array2D >& a2(h2.m_cells[x][y]); 142 | assert(a1.m_reference==a2.m_reference); 143 | assert((!a1.m_reference) || !(a1.m_reference->shares%2)); 144 | } 145 | } 146 | } 147 | cerr << __PRETTY_FUNCTION__ << ": SUCCESS, the error is somewhere else" << endl; 148 | # endif 149 | 150 | return cloned; 151 | } 152 | 153 | /** 154 | * 析构函数 155 | */ 156 | GridSlamProcessor::~GridSlamProcessor() { 157 | cerr << __PRETTY_FUNCTION__ << ": Start" << endl; 158 | cerr << __PRETTY_FUNCTION__ << ": Deleting tree" << endl; 159 | for (std::vector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){ 160 | #ifdef TREE_CONSISTENCY_CHECK 161 | TNode* node=it->node; 162 | while(node) 163 | node=node->parent; 164 | cerr << "@" << endl; 165 | #endif 166 | if (it->node) 167 | delete it->node; 168 | //cout << "l=" << it->weight<< endl; 169 | } 170 | # ifdef MAP_CONSISTENCY_CHECK 171 | cerr << __PRETTY_FUNCTION__ << ": performing predestruction_fit_test" << endl; 172 | typedef std::map >::reference* const, int> PointerMap; 173 | PointerMap pmap; 174 | for (ParticleVector::const_iterator it=m_particles.begin(); it!=m_particles.end(); it++){ 175 | const ScanMatcherMap& m1(it->map); 176 | const HierarchicalArray2D& h1(m1.storage()); 177 | for (int x=0; x >& a1(h1.m_cells[x][y]); 180 | if (a1.m_reference){ 181 | PointerMap::iterator f=pmap.find(a1.m_reference); 182 | if (f==pmap.end()) 183 | pmap.insert(make_pair(a1.m_reference, 1)); 184 | else 185 | f->second++; 186 | } 187 | } 188 | } 189 | } 190 | cerr << __PRETTY_FUNCTION__ << ": Number of allocated chunks" << pmap.size() << endl; 191 | for(PointerMap::const_iterator it=pmap.begin(); it!=pmap.end(); it++) 192 | assert(it->first->shares>=(unsigned int)it->second); 193 | cerr << __PRETTY_FUNCTION__ << ": SUCCESS, the error is somewhere else" << endl; 194 | # endif 195 | } 196 | 197 | /** 198 | * 设置传感器地图 199 | */ 200 | void GridSlamProcessor::setSensorMap(const SensorMap& smap) { 201 | // 初始化一个迭代器const_iterator，指向键名为"FLASER"的Sensor对象 202 | SensorMap::const_iterator laser_it = smap.find( std::string("FLASER") ); 203 | if ( laser_it == smap.end()) { 204 | cerr << "Attempting to load the new carmen log format" << endl; 205 | laser_it=smap.find(std::string("ROBOTLASER1")); 206 | assert(laser_it!=smap.end()); 207 | } 208 | // 将laser_it指向的Sensor对象取出 209 | const RangeSensor* rangeSensor=dynamic_cast((laser_it->second)); 210 | assert(rangeSensor && rangeSensor->beams().size()); 211 | // 根据取出的Sensor对象，设置 m_beams 等扫描匹配需要的激光参数 212 | m_beams=static_cast(rangeSensor->beams().size()); 213 | double* angles=new double[rangeSensor->beams().size()]; 214 | for ( unsigned int i=0; ibeams()[i].pose.theta; 216 | } 217 | m_matcher.setLaserParameters(m_beams, angles, rangeSensor->getPose()); 218 | delete [] angles; 219 | } 220 | 221 | /** 222 | * GridFastSLAM初始化，主要用于初始化各个粒子的一些信息 223 | */ 224 | void GridSlamProcessor::init(unsigned int size, double xmin, double ymin, double xmax, double ymax, 225 | double delta, OrientedPoint initialPose) 226 | { 227 | //设置地图大小和分辨率 228 | m_xmin=xmin; 229 | m_ymin=ymin; 230 | m_xmax=xmax; 231 | m_ymax=ymax; 232 | m_delta=delta; 233 | // 每个粒子初始化 234 | m_particles.clear(); 235 | TNode* node = new TNode( initialPose, 0, 0, 0 ); 236 | // 粒子对应的地图初始化 237 | // 高分辨率地图由自己指定，低分辨率地图固定为0.1m 238 | ScanMatcherMap lmap( Point( Point(xmin+xmax, ymin+ymax)*.5, xmax-xmin, ymax-ymin, delta ) ); 239 | ScanMatcherMap lowMap(Point(xmin+xmax,ymin+ymax)*0.5,xmax-xmin,ymax-ymin,0.1); 240 | for ( unsigned int i=0; i(o.getSensor()); 308 | if (os && os->isIdeal() && m_outputStream){ 309 | m_outputStream << setiosflags(ios::fixed) << setprecision(3); 310 | m_outputStream << "SIMULATOR_POS " << o.getPose().x << " " << o.getPose().y << " " ; 311 | m_outputStream << setiosflags(ios::fixed) << setprecision(6) << o.getPose().theta << " " << o.getTime() << endl; 312 | } 313 | } 314 | 315 | /** 316 | * 处理一帧激光雷达数据，核心算法函数 317 | */ 318 | bool GridSlamProcessor::processScan( const RangeReading& reading, int adaptParticles ) { 319 | /**retireve the position from the reading, and compute the odometry*/ 320 | // 获取当前时刻里程计下机器人的位姿 321 | OrientedPoint relPose = reading.getPose(); 322 | if ( !m_count ) { 323 | // 如果processScan()是第0次调用，则所有位姿都是一样的 324 | m_lastPartPose = m_odoPose = relPose; 325 | } 326 | 327 | // 利用里程计运动模型对每个粒子进行运动学采样更新，求解proposal分布 328 | int tmp_size = m_particles.size(); 329 | for ( int i=0; im_distanceThresholdCheck){ 348 | cerr << "***********************************************************************" << endl; 349 | cerr << "********** Error: m_distanceThresholdCheck overridden!!!! *************" << endl; 350 | cerr << "m_distanceThresholdCheck=" << m_distanceThresholdCheck << endl; 351 | cerr << "Old Odometry Pose= " << m_odoPose.x << " " << m_odoPose.y 352 | << " " <=m_linearThresholdDistance 369 | || m_angularDistance>=m_angularThresholdDistance 370 | || (period_ >= 0.0 && (reading.getTime() - last_update_time_) > period_) ) 371 | { 372 | // this is for converting the reading in a scan-matcher feedable form 373 | // 复制一帧来自ROS的激光数据，转换为scan-matching需要的格式 374 | assert( reading.getSize()==m_beams ); 375 | double* plainReading = new double[m_beams]; 376 | for ( unsigned int i=0; i(reading.getSensor()), 382 | reading.getTime() ); 383 | // 如果不是第一帧数据 384 | if ( m_count>0 ) { 385 | 386 | // 扫描匹配，利用最近的一次观测来提高proposal分布 387 | // 利用proposal分布和激光雷达数据来确定各个粒子的权重 388 | scanMatching(plainReading); 389 | onScanmatchUpdate(); 390 | // 更新各个粒子对应的轨迹树上的累计权重 391 | // （调用了normalize()函数进行权重归一化处理，同时计算出有效粒子数neff值） 392 | updateTreeWeights(false); 393 | // 对最终的proposal对应的粒子群进行（自适应）重采样 394 | // 根据neff的大小来进行重采样 不但进行了重采样，也对地图进行更新 395 | resample( plainReading, adaptParticles, reading_copy ); 396 | } else { 397 | // 如果是第一帧数据，则可以直接计算activeArea。因为这个时候，对机器人的位置是非常确定的，就是(0,0,0)。 398 | for ( ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++ ) { 399 | m_matcher.invalidateActiveArea(); 400 | m_matcher.computeActiveArea(it->map, it->pose, plainReading); 401 | m_matcher.registerScan(it->map, it->pose, plainReading); 402 | TNode* node = new TNode(it->pose, 0., it->node, 0); 403 | node->reading = reading_copy; 404 | it->node = node; 405 | } 406 | } 407 | // 进行重采样之后，粒子的权重又会发生变化，因此需要再次更新粒子轨迹的累计权重 408 | // （调用了normalize()函数进行权重归一化处理，同时计算出有效粒子数neff值） 409 | updateTreeWeights(false); 410 | 411 | delete [] plainReading; 412 | m_lastPartPose=m_odoPose; // update the past pose for the next iteration 413 | // m_linearDistance 和 m_angularDistance 每次激光雷达数据更新之后会清零 414 | m_linearDistance=0; 415 | m_angularDistance=0; 416 | m_count++; 417 | processed = true; 418 | // keep ready for the next step 419 | for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++) { 420 | it->previousPose=it->pose; 421 | } 422 | } 423 | m_readingCount++; 424 | return processed; 425 | } 426 | 427 | /** 428 | * 获取粒子群中 累计权重最大 的粒子的下标 429 | */ 430 | int GridSlamProcessor::getBestParticleIndex() const { 431 | unsigned int bi = 0; 432 | // 返回编译器允许的double型数最大值 433 | double bw = -( std::numeric_limits::max() ); 434 | for ( unsigned int i=0; i