Gmapping（fastSlam）关于插图等

Posted 2020-10-22 cwyblogs

1、获取激光数据并处理

　　如果是第一个激光数据，则初始化粒子，每一个粒子维护一副地图map，机器人的位姿pose，上个时刻的位姿previous，权重weight，权重总和weightSum。

　　定义的粒子结构体：

　　struct Particle_{

　　　　Point2D pose;

　　　　double weight;

　　　　double score;

　　　　Laser laser;

　　};