GMapping的栅格地图的构建

1、代码重写

open_gmapping的代码比较复杂，比较乱． csdn博主 白茶-清欢 对 open_gmapping 与 slam_gmapping 两个包进行了重写，整理了代码使得代码结构更加清晰，同时添加了注释，还增加了激光雷达数据的畸变校正功能． 

参考这个

二、代码阅读

2.1、//如果是第一次接收scan

// 将雷达各个角度的sin与cos值保存下来，以节约计算量

CreateCache(scan_msg);

1.  
   void GMapping::CreateCache(const sensor_msgs::LaserScan::ConstPtr &scan_msg)
2.  
   {
3.  
   a_cos_.clear();
4.  
   a_sin_.clear();
5.  
   double angle;
6.  
    
7.  
   for (unsigned int i = 0; i < scan_msg->ranges.size(); i )
8.  
   {
9.  
   angle = scan_msg->angle_min i * scan_msg->angle_increment;
10.  
    a_cos_.push_back(cos(angle));
11.  
    a_sin_.push_back(sin(angle));
12.  
    }
13.  
    }

2.3 PublishMap()

1.  
   // 计算当前雷达数据对应的栅格地图并发布出去
2.  
   void GMapping::PublishMap(const sensor_msgs::LaserScan::ConstPtr &scan_msg)
3.  
   {
4.  
   // 地图的中点
5.  
   Point center;
6.  
   center.x = (xmin_ xmax_) / 2.0;
7.  
   center.y = (ymin_ ymax_) / 2.0;
8.  
    
9.  
   // ScanMatcherMap为GMapping中存储地图的数据类型
10.  
    //typedef Map<PointAccumulator, HierarchicalArray2D<PointAccumulator>> ScanMatcherMap; //一张地图的类
11.  
    ScanMatcherMap gmapping_map_(center, xmin_, ymin_, xmax_, ymax_, resolution_);
12.  
     
13.  
    // 使用当前雷达数据更新GMapping地图中栅格的值
14.  
    ComputeMap(gmapping_map_, scan_msg);
15.  
     
16.  
    // 将gmapping_map_中的存储的栅格值 赋值到 ros的map中
17.  
    for (int x = 0; x < gmapping_map_.getMapSizeX(); x )
18.  
    {
19.  
    for (int y = 0; y < gmapping_map_.getMapSizeY(); y )
20.  
    {
21.  
    IntPoint p(x, y);
22.  
    // 获取这点栅格的值，只有大于occ_thresh_时才认为是占用
23.  
    double occ = gmapping_map_.cell(p);
24.  
     
25.  
    // 未知
26.  
    if (occ < 0)
27.  
    map_.data[MAP_IDX(map_.info.width, x, y)] = GMAPPING_UNKNOWN;
28.  
    // 占用
29.  
    else if (occ > occ_thresh_) // 默认0.25
30.  
    map_.data[MAP_IDX(map_.info.width, x, y)] = GMAPPING_OCC;
31.  
    // 空闲
32.  
    else
33.  
    map_.data[MAP_IDX(map_.info.width, x, y)] = GMAPPING_FREE;
34.  
    }
35.  
    }
36.  
     
37.  
    // 添加当前的时间戳
38.  
    map_.header.stamp = ros::Time::now();
39.  
    map_.header.frame_id = scan_msg->header.frame_id;
40.  
     
41.  
    // 发布map和map_metadata
42.  
    map_publisher_.publish(map_);
43.  
    map_publisher_metadata_.publish(map_.info);
44.  
    }

ScanMatcherMap

typedef Map<PointAccumulator, HierarchicalArray2D<PointAccumulator>> ScanMatcherMap; //一张地图的类

1.为GMapping中存储地图的数据类型，先声明了一个ScanMatcherMap的对象

2.先声明了一个ScanMatcherMap的对象gmapping_map_，然后通过ComputeMap()为gmapping_map_赋值，然后再将gmapping_map_中存储的值赋值到ros的栅格地图的数据类型中．

gmapping认为ros的栅格地图数据只需要有3个值

• -1 代表栅格状态未知
• 0 代表栅格是空闲的，代表可通过区域
• 100 代表栅格是占用的，代表障碍物，不可通过

3.

 ScanMatcherMap gmapping_map_(center, xmin_, ymin_, xmax_, ymax_, resolution_);

2.4 ComputeMap()

1.头文件中

std::vector<GridLineTraversalLine> line_lists_;

std::vector<Point> hit_lists_;

2.orientedpoint 结构体

1.  
   struct orientedpoint: public point<T>
2.  
   {
3.  
   inline orientedpoint() : point<T>(0,0), theta(0) {};
4.  
   inline orientedpoint(const point<T>& p);
5.  
   inline orientedpoint(T x, T y, A _theta): point<T>(x,y), theta(_theta){}
6.  
   inline void normalize();
7.  
    
8.  
   inline orientedpoint<T,A> rotate(A alpha)
9.  
   {
10.  
    T s=sin(alpha), c=cos(alpha);
11.  
    A a=alpha theta;
12.  
    a=atan2(sin(a),cos(a));
13.  
    return orientedpoint(
14.  
    c*this->x-s*this->y,
15.  
    s*this->x c*this->y,
16.  
    a);
17.  
    }
18.  
    A theta;
19.  
    };

3.

inline IntPoint world2map(double x, double y) const { return world2map(Point(x, y)); }

2.cpp

1.  
   void GMapping::ComputeMap(ScanMatcherMap &map, const sensor_msgs::LaserScan::ConstPtr &scan_msg)
2.  
   {
3.  
   line_lists_.clear();
4.  
   hit_lists_.clear();
5.  
    
6.  
   // lp为地图坐标系下的激光雷达坐标系的位姿
7.  
   OrientedPoint lp(0, 0, 0.0);
8.  
    
9.  
   // 将位姿lp转换成地图坐标系下的位置
10.  
    IntPoint p0 = map.world2map(lp);
11.  
     
12.  
    // 地图的有效区域(地图坐标系)
13.  
    HierarchicalArray2D<PointAccumulator>::PointSet activeArea;
14.  
     
15.  
    // 通过激光雷达的数据，找出地图的有效区域
16.  
    for (unsigned int i = 0; i < scan_msg->ranges.size(); i )
17.  
    {
18.  
    // 排除错误的激光点
19.  
    double d = scan_msg->ranges[i];
20.  
    if (d > max_range_ || d == 0.0 || !std::isfinite(d))
21.  
    continue;
22.  
    if (d > max_use_range_)
23.  
    d = max_use_range_;
24.  
     
25.  
    // p1为激光雷达的数据点在地图坐标系下的坐标
26.  
    Point phit = lp;
27.  
    phit.x = d * a_cos_[i];
28.  
    phit.y = d * a_sin_[i];
29.  
    IntPoint p1 = map.world2map(phit);
30.  
     
31.  
    // 使用bresenham算法来计算 从激光位置到激光点 要经过的栅格的坐标
32.  
    GridLineTraversalLine line;
33.  
    GridLineTraversal::gridLine(p0, p1, &line);
34.  
    // 将line保存起来以备后用
35.  
    line_lists_.push_back(line);
36.  
     
37.  
    // 计算活动区域的大小
38.  
    for (int i = 0; i < line.num_points - 1; i )
39.  
    {
40.  
    activeArea.insert(map.storage().patchIndexes(line.points[i]));
41.  
    }
42.  
    // 如果d<m_usableRange则需要把击中点也算进去 说明这个值是好的。
43.  
    // 同时如果d==max_use_range_ 那么说明这个值只用来进行标记空闲区域　不用来进行标记障碍物
44.  
    if (d < max_use_range_)
45.  
    {
46.  
    IntPoint cp = map.storage().patchIndexes(p1);
47.  
    activeArea.insert(cp);
48.  
    hit_lists_.push_back(phit);
49.  
    }
50.  
    }
51.  
     
52.  
    // 为activeArea分配内存
53.  
    map.storage().setActiveArea(activeArea, true);
54.  
    map.storage().allocActiveArea();
55.  
     
56.  
    // 在map上更新空闲点
57.  
    for (auto line : line_lists_)
58.  
    {
59.  
    // 更新空闲位置
60.  
    for (int k = 0; k < line.num_points - 1; k )
61.  
    {
62.  
    // 未击中，就不记录击中的位置了，所以传入参数Point(0,0)
63.  
    map.cell(line.points[k]).update(false, Point(0, 0));
64.  
    }
65.  
    }
66.  
    // 在map上添加hit点
67.  
    for (auto hit : hit_lists_)
68.  
    {
69.  
    IntPoint p1 = map.world2map(hit);
70.  
    map.cell(p1).update(true, hit);
71.  
    }
72.  
    }

第一部分

之后遍历雷达数据点，使用bresemham画线算法生成从激光位置到激光点的连线在栅格地图中的坐标，并将这些点放入activeArea用于计算区域，同时保存下来以备后用．

再判断雷达数据点也就是 phit ．判断这个点与预先设定的参数 max_use_range_(雷达数据最大使用距离)，如果phit的距离小于max_use_range_则认为是好的数据点，放入hit_lists_中以备后用． 

2.5 bresemham画线算法

gridLine()

判断了计算出的直线坐标点的起始点是否正确，如果不正确，就把直线的所有点顺序反转一下，

1.  
   void GridLineTraversal::gridLine(IntPoint start, IntPoint end, GridLineTraversalLine *line)
2.  
   {
3.  
   int i, j;
4.  
   int half;
5.  
   IntPoint v;
6.  
   gridLineCore(start, end, line);
7.  
   if (start.x != line->points[0].x || start.y != line->points[0].y)
8.  
   {
9.  
   half = line->num_points / 2;
10.  
    for (i = 0, j = line->num_points - 1; i < half; i , j--)
11.  
    {
12.  
    v = line->points[i];
13.  
    line->points[i] = line->points[j];
14.  
    line->points[j] = v;
15.  
    }
16.  
    }
17.  
    }

gridLineCore()

理论理解

 https://www.jianshu.com/p/d63bf63a0e28

1.  
   void GridLineTraversal::gridLineCore(IntPoint start, IntPoint end, GridLineTraversalLine *line)
2.  
   {
3.  
   int dx, dy; // 横纵坐标间距
4.  
   int incr1, incr2; // 增量
5.  
   int d; //
6.  
   int x, y, xend, yend; // 直线增长的首末端点坐标
7.  
   int xdirflag, ydirflag; // 横纵坐标增长方向
8.  
   int cnt = 0; // 直线过点的点的序号
9.  
    
10.  
    dx = abs(end.x - start.x);
11.  
    dy = abs(end.y - start.y);
12.  
     
13.  
    // 斜率绝对值小于等于1的情况，优先增加x
14.  
    if (dy <= dx)
15.  
    {
16.  
    d = 2 * dy - dx; // 初始点P_m0值
17.  
    incr1 = 2 * dy; // 情况（1）
18.  
    incr2 = 2 * (dy - dx); // 情况（2）
19.  
     
20.  
    // 将增长起点设置为横坐标小的点处，将 x的增长方向 设置为 向右侧增长
21.  
    if (start.x > end.x)
22.  
    {
23.  
    // 起点横坐标比终点横坐标大，ydirflag = -1（负号可以理解为增长方向与直线始终点方向相反）
24.  
    x = end.x;
25.  
    y = end.y;
26.  
    ydirflag = (-1);
27.  
    xend = start.x; // 设置增长终点横坐标
28.  
    }
29.  
    else
30.  
    {
31.  
    x = start.x;
32.  
    y = start.y;
33.  
    ydirflag = 1;
34.  
    xend = end.x;
35.  
    }
36.  
     
37.  
    //加入起点坐标
38.  
    line->points.push_back(IntPoint(x, y));
39.  
    cnt ;
40.  
     
41.  
    // 向 右上 方向增长
42.  
    if (((end.y - start.y) * ydirflag) > 0)
43.  
    {
44.  
    while (x < xend)
45.  
    {
46.  
    x ;
47.  
    if (d < 0)
48.  
    {
49.  
    d = incr1;
50.  
    }
51.  
    else
52.  
    {
53.  
    y ;
54.  
    d = incr2; // 纵坐标向正方向增长
55.  
    }
56.  
    line->points.push_back(IntPoint(x, y));
57.  
    cnt ;
58.  
    }
59.  
    }
60.  
    // 向 右下 方向增长
61.  
    else
62.  
    {
63.  
    while (x < xend)
64.  
    {
65.  
    x ;
66.  
    if (d < 0)
67.  
    {
68.  
    d = incr1;
69.  
    }
70.  
    else
71.  
    {
72.  
    y--;
73.  
    d = incr2; // 纵坐标向负方向增长
74.  
    }
75.  
    line->points.push_back(IntPoint(x, y));
76.  
    cnt ;
77.  
    }
78.  
    }
79.  
    }
80.  
    // 斜率绝对值大于1的情况，优先增加y
81.  
    else
82.  
    {
83.  
    // dy > dx，当斜率k的绝对值|k|>1时，在y方向进行单位步进
84.  
    d = 2 * dx - dy;
85.  
    incr1 = 2 * dx;
86.  
    incr2 = 2 * (dx - dy);
87.  
     
88.  
    // 将增长起点设置为纵坐标小的点处，将 y的增长方向 设置为 向上侧增长
89.  
    if (start.y > end.y)
90.  
    {
91.  
    y = end.y; // 取最小的纵坐标作为起点
92.  
    x = end.x;
93.  
    yend = start.y;
94.  
    xdirflag = (-1);
95.  
    }
96.  
    else
97.  
    {
98.  
    y = start.y;
99.  
    x = start.x;
100.  
     yend = end.y;
101.  
     xdirflag = 1;
102.  
     }
103.  
     // 添加起点
104.  
     line->points.push_back(IntPoint(x, y));
105.  
     cnt ;
106.  
     // 向 上右 增长
107.  
     if (((end.x - start.x) * xdirflag) > 0)
108.  
     {
109.  
     while (y < yend)
110.  
     {
111.  
     y ;
112.  
     if (d < 0)
113.  
     {
114.  
     d = incr1;
115.  
     }
116.  
     else
117.  
     {
118.  
     x ;
119.  
     d = incr2; // 横坐标向正方向增长
120.  
     }
121.  
     //添加新的点
122.  
     line->points.push_back(IntPoint(x, y));
123.  
     cnt ;
124.  
     }
125.  
     }
126.  
     // 向 上左 增长
127.  
     else
128.  
     {
129.  
     while (y < yend)
130.  
     {
131.  
     y ;
132.  
     if (d < 0)
133.  
     {
134.  
     d = incr1;
135.  
     }
136.  
     else
137.  
     {
138.  
     x--;
139.  
     d = incr2; //横坐标向负方向增长
140.  
     }
141.  
     line->points.push_back(IntPoint(x, y));
142.  
     // 记录添加所有点的数目
143.  
     cnt ;
144.  
     }
145.  
     }
146.  
     }
147.  
     line->num_points = cnt;
148.  
     }

第二部分

第二部分做了两个工作，一个是更新空闲点，代表可通过区域，另一个是更新hit点，也就是激光雷达数据点在地图中的位置，代表着障碍物．

2.6.2 地图占用值更新的实现

map.h中有个叫做PointAccumulator的结构体，定义了栅格地图的更新方式以及存储值是如何计算的．其代码如下

1.  
   //PointAccumulator表示地图中一个cell（栅格）包括的内容
2.  
   /*
3.  
   acc： 栅格累计被击中位置
4.  
   n： 栅格被击中次数
5.  
   visits：栅格被访问的次数
6.  
   */
7.  
   //PointAccumulator的一个对象，就是一个栅格，gmapping中其他类模板的cell就是这个
8.  
    
9.  
   struct PointAccumulator
10.  
    {
11.  
    //float类型的point
12.  
    typedef point<float> FloatPoint;
13.  
    //构造函数
14.  
    PointAccumulator() : acc(0, 0), n(0), visits(0) {}
15.  
    PointAccumulator(int i) : acc(0, 0), n(0), visits(0) { assert(i == -1); }
16.  
    //计算栅格被击中坐标累计值的平均值
17.  
    inline Point mean() const { return 1. / n * Point(acc.x, acc.y); }
18.  
    //返回该栅格被占用的概率，范围是 -1（没有访问过） 、[0,1]
19.  
    inline operator double() const { return visits ? (double)n * 1 / (double)visits : -1; }
20.  
    //更新该栅格成员变量
21.  
    inline void update(bool value, const Point &p = Point(0, 0));
22.  
    //该栅格被击中的位置累计，最后取累计值的均值
23.  
    FloatPoint acc;
24.  
    //n表示该栅格被击中的次数，visits表示该栅格被访问的次数
25.  
    int n, visits;
26.  
    };
27.  
     
28.  
    //更新该栅格成员变量，value表示该栅格是否被击中，击中n ，未击中仅visits ;
29.  
    void PointAccumulator::update(bool value, const Point &p)
30.  
    {
31.  
    if (value)
32.  
    {
33.  
    acc.x = static_cast<float>(p.x);
34.  
    acc.y = static_cast<float>(p.y);
35.  
    n ;
36.  
    visits = 1;
37.  
    }
38.  
    else
39.  
    visits ;
40.  
    }

可以看到，每个格子都有2个值，一个是visits，一个是n．

更新占用: 在更新被击中的栅格时, 也就是激光点所在的栅格, n与visits都会加1，

更新空闲: 在更新未被击中的栅格时，也就是从激光到激光点之间的栅格，只有visits加1,

通过重载了()，获取获取每个格子的占用值，也可以说成是被占用的概率．占用值的计算公式为 n/visists，如果是没被更新过就返回-1．

由于visits大于等于n，所以占用值的范围是 [0,1]的，只有当这个值在大于0.25（默认参数）时，在赋值到ros地图的时候才赋值为100，才认为是真正的占用了．

举个例子说明一下栅格地图的占用值更新的方式．

例如：当雷达扫到人腿时，会对击中点的栅格更新一次占用，这时在ROS地图下这个点将被表示为障碍物．

如果人腿离开了这个位置，在之后的过程中，有4次在这个栅格更新了空闲，就会将ROS地图中这个栅格的状态更新到空闲，也就是没有障碍物

原因：

第一次更新地图时，这个格子的占用值为 1/1 = 1 > 0.25 ，就会将ROS地图中这个格子设置为100，表示障碍物．

如果之后4次更新地图，这个栅格都被更新空闲了，则这个格子的占用值将变为 1/5 = 0.2 <0.25了，所以就会将ROS地图中对应的栅格设置为0，变成可通过区域了

1.  
   这一步为关键，计算激光点的笛卡尔坐标
2.  
   Point phit = lp;
3.  
   phit.x = d * a_cos_[i];
4.  
   phit.y = d * a_sin_[i];
5.  
   IntPoint p1 = map.world2map(phit);

3.运行

roslaunch lesson4 make_gmapping_map.launch

实时变化的地图，讲扫描的点即使算打中的次数

可以看到，雷达数据是10hz的，map数据大概是2.5hz．

我们只用了一个线程，也就是雷达回调函数的这个线程．处理一次回调函数需要用时0.4秒，而雷达数据的间隔是0.1秒．

也就是说，当我们正在计算地图的时候，会又有4帧雷达数据到达，当回调函数的缓冲区设置为1时，那这4帧雷达数据将只保留最后一个，其他3帧数据将丢失掉．

这还只是80m * 80m范围的地图，构建更大范围地图的时间将更久．所以，很多SLAM都将地图的生成单独开一个线程，以保证不耽误对实时性要求较高的前端里程计部分

1.  
   $ rostopic hz /laser_scan /map
2.  
    
3.  
   topic rate min_delta max_delta std_dev window
4.  
   ===============================================================
5.  
   /laser_scan 9.987 0.09058 0.1108 0.003163 50
6.  
   /map 2.613 0.1914 0.4549 0.06057 50

这篇好文章是转载于：学新通技术网