SLAM-基于Gmapping的栅格地图

基于Gmapping的栅格地图创建

回调函数：

// 回调函数 进行数据处理
void GMapping::ScanCallback(const sensor_msgs::LaserScan::ConstPtr &scan_msg)
{
    static ros::Time last_map_update(0, 0); //存储上一次地图更新的时间

    if (!got_first_scan_) //找第一帧的scan
    {
        // 将雷达各个角度的sin与cos值保存下来，以节约计算量
        CreateCache(scan_msg);
        got_first_scan_ = true; //改变第一帧的标志位
    }

    start_time_ = std::chrono::steady_clock::now();

    // 计算当前雷达数据对应的栅格地图并发布出去
    PublishMap(scan_msg);

    end_time_ = std::chrono::steady_clock::now();
    time_used_ = std::chrono::duration_cast<std::chrono::duration<double>>(end_time_ - start_time_);
    std::cout << "\n转换一次地图用时: " << time_used_.count() << " 秒。" << std::endl;
}

 PublishMap()

通过computemap计算Gmapping中栅格的值

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

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

• #define GMAPPING_UNKNOWN (-1)
  #define GMAPPING_FREE (0)
  #define GMAPPING_OCC (100)
  
  // 计算当前雷达数据对应的栅格地图并发布出去
  void GMapping::PublishMap(const sensor_msgs::LaserScan::ConstPtr &scan_msg)
  {
      // 地图的中点
      Point center;
      center.x = (xmin_ + xmax_) / 2.0;
      center.y = (ymin_ + ymax_) / 2.0;
  
      // ScanMatcherMap为GMapping中存储地图的数据类型
      ScanMatcherMap gmapping_map_(center, xmin_, ymin_, xmax_, ymax_, resolution_);
      
      // 使用当前雷达数据更新GMapping地图中栅格的值
      ComputeMap(gmapping_map_, scan_msg);
  
      // 将gmapping_map_中的存储的栅格值 赋值到 ros的map中
      for (int x = 0; x < gmapping_map_.getMapSizeX(); x++)
      {
          for (int y = 0; y < gmapping_map_.getMapSizeY(); y++)
          {
              IntPoint p(x, y);
              // 获取这点栅格的值，只有大于occ_thresh_时才认为是占用
              double occ = gmapping_map_.cell(p); 
  
              // 未知
              if (occ < 0)
                  map_.data[MAP_IDX(map_.info.width, x, y)] = GMAPPING_UNKNOWN;
              // 占用
              else if (occ > occ_thresh_) // 默认0.25
                  map_.data[MAP_IDX(map_.info.width, x, y)] = GMAPPING_OCC;
              // 空闲
              else
                  map_.data[MAP_IDX(map_.info.width, x, y)] = GMAPPING_FREE;
          }
      }
  
      // 添加当前的时间戳
      map_.header.stamp = ros::Time::now();
      map_.header.frame_id = scan_msg->header.frame_id;
  
      // 发布map和map_metadata
      map_publisher_.publish(map_);
      map_publisher_metadata_.publish(map_.info);
  }
  

  ComputeMap()

• // 使用当前雷达数据更新GMapping地图中栅格的值
  void GMapping::ComputeMap(ScanMatcherMap &map, const sensor_msgs::LaserScan::ConstPtr &scan_msg)
  {
      line_lists_.clear();
      hit_lists_.clear();
      
      // lp为地图坐标系下的激光雷达坐标系的位姿
      OrientedPoint lp(0, 0, 0.0);
      // 将位姿lp转换成地图坐标系下的位置
      IntPoint p0 = map.world2map(lp);
      
  	// 第一部分
      // 地图的有效区域(地图坐标系)
      HierarchicalArray2D<PointAccumulator>::PointSet activeArea;
      // 通过激光雷达的数据，找出地图的有效区域
      for (unsigned int i = 0; i < scan_msg->ranges.size(); i++)
      {
          // 排除错误的激光点
          double d = scan_msg->ranges[i];
          if (d > max_range_ || d == 0.0 || !std::isfinite(d))
              continue;
          if (d > max_use_range_)
              d = max_use_range_;
  
          // p1为激光雷达的数据点在地图坐标系下的坐标
          Point phit = lp;
          phit.x += d * a_cos_[i];
          phit.y += d * a_sin_[i];
          IntPoint p1 = map.world2map(phit);
  
          // 使用bresenham算法来计算 从激光位置到激光点 要经过的栅格的坐标
          GridLineTraversalLine line;
          GridLineTraversal::gridLine(p0, p1, &line);
          // 将line保存起来以备后用
          line_lists_.push_back(line);
  
          // 计算活动区域的大小
          for (int i = 0; i < line.num_points - 1; i++)
          {
              activeArea.insert(map.storage().patchIndexes(line.points[i]));
          }
          // 如果d<m_usableRange则需要把击中点也算进去 说明这个值是好的。
          // 同时如果d==max_use_range_那么说明这个值只用来进行标记空闲区域　不用来进行标记障碍物
          if (d < max_use_range_)
          {
              IntPoint cp = map.storage().patchIndexes(p1);
              activeArea.insert(cp);
              hit_lists_.push_back(phit);
          }
      }
  
      // 为activeArea分配内存
      map.storage().setActiveArea(activeArea, true);
      map.storage().allocActiveArea();
  
  	// 第二部分
      // 在map上更新空闲点
      for (auto line : line_lists_)
      {
          // 更新空闲位置
          for (int k = 0; k < line.num_points - 1; k++)
          {
              // 未击中，就不记录击中的位置了，所以传入参数Point(0,0)
              map.cell(line.points[k]).update(false, Point(0, 0));
          }
      }
      // 在map上添加hit点
      for (auto hit : hit_lists_)
      {
          IntPoint p1 = map.world2map(hit);
          map.cell(p1).update(true, hit);
      }
  }
  

gridLine()

void GridLineTraversal::gridLine(IntPoint start, IntPoint end, GridLineTraversalLine *line)
{
    int i, j;
    int half;
    IntPoint v;
    gridLineCore(start, end, line);
    if (start.x != line->points[0].x || start.y != line->points[0].y)
    {
        half = line->num_points / 2;
        for (i = 0, j = line->num_points - 1; i < half; i++, j--)
        {
            v = line->points[i];
            line->points[i] = line->points[j];
            line->points[j] = v;
        }
    }
}

 gridLineCore()

void GridLineTraversal::gridLineCore(IntPoint start, IntPoint end, GridLineTraversalLine *line)
{
    int dx, dy;             // 横纵坐标间距
    int incr1, incr2;       // 增量
    int d;                  // 
    int x, y, xend, yend;   // 直线增长的首末端点坐标
    int xdirflag, ydirflag; // 横纵坐标增长方向
    int cnt = 0;            // 直线过点的点的序号

    dx = abs(end.x - start.x);
    dy = abs(end.y - start.y);

    // 斜率绝对值小于等于1的情况，优先增加x
    if (dy <= dx)
    {
        d = 2 * dy - dx;       // 初始点P_m0值
        incr1 = 2 * dy;        // 情况（1）
        incr2 = 2 * (dy - dx); // 情况（2）

        // 将增长起点设置为横坐标小的点处，将 x的增长方向 设置为 向右侧增长
        if (start.x > end.x)
        {
            // 起点横坐标比终点横坐标大，ydirflag = -1（负号可以理解为增长方向与直线始终点方向相反）
            x = end.x; 
            y = end.y;
            ydirflag = (-1);
            xend = start.x; // 设置增长终点横坐标
        }
        else
        {
            x = start.x;
            y = start.y;
            ydirflag = 1;
            xend = end.x;
        }

        //加入起点坐标
        line->points.push_back(IntPoint(x, y));
        cnt++;

        // 向 右上 方向增长
        if (((end.y - start.y) * ydirflag) > 0)
        {
            while (x < xend)
            {
                x++;
                if (d < 0)
                {
                    d += incr1;
                }
                else
                {
                    y++;
                    d += incr2; // 纵坐标向正方向增长
                }
                line->points.push_back(IntPoint(x, y));
                cnt++;
            }
        }
        // 向 右下 方向增长
        else
        {
            while (x < xend)
            {
                x++;
                if (d < 0)
                {
                    d += incr1;
                }
                else
                {
                    y--;
                    d += incr2; // 纵坐标向负方向增长
                }
                line->points.push_back(IntPoint(x, y));
                cnt++;
            }
        }
    }
    // 斜率绝对值大于1的情况，优先增加y
    else
    {
        // dy > dx，当斜率k的绝对值|k|>1时，在y方向进行单位步进
        d = 2 * dx - dy; 
        incr1 = 2 * dx;  
        incr2 = 2 * (dx - dy);

        // 将增长起点设置为纵坐标小的点处，将 y的增长方向 设置为 向上侧增长
        if (start.y > end.y)
        {
            y = end.y; // 取最小的纵坐标作为起点
            x = end.x;
            yend = start.y;
            xdirflag = (-1); 
        }
        else
        {
            y = start.y;
            x = start.x;
            yend = end.y;
            xdirflag = 1;
        }
        // 添加起点
        line->points.push_back(IntPoint(x, y));
        cnt++;
        // 向 上右 增长
        if (((end.x - start.x) * xdirflag) > 0)
        {
            while (y < yend)
            {
                y++;
                if (d < 0)
                {
                    d += incr1;
                }
                else
                {
                    x++;
                    d += incr2; // 横坐标向正方向增长
                }
                //添加新的点
                line->points.push_back(IntPoint(x, y));
                cnt++;
            }
        }
        // 向 上左 增长
        else
        {
            while (y < yend)
            {
                y++;
                if (d < 0)
                {
                    d += incr1;
                }
                else
                {
                    x--;
                    d += incr2; //横坐标向负方向增长
                }
                line->points.push_back(IntPoint(x, y));
                // 记录添加所有点的数目
                cnt++;
            }
        }
    }
    line->num_points = cnt;
}

GMapping的地图相关的文件有3个，array.h，harray2d.h，map.h

地图占用值更新的实现

//PointAccumulator表示地图中一个cell（栅格）包括的内容
/*
acc：   栅格累计被击中位置
n：     栅格被击中次数
visits：栅格被访问的次数
*/
//PointAccumulator的一个对象，就是一个栅格，gmapping中其他类模板的cell就是这个

struct PointAccumulator
{
    //float类型的point
    typedef point<float> FloatPoint;
    //构造函数
    PointAccumulator() : acc(0, 0), n(0), visits(0) {}
    PointAccumulator(int i) : acc(0, 0), n(0), visits(0) { assert(i == -1); }
    //计算栅格被击中坐标累计值的平均值
    inline Point mean() const { return 1. / n * Point(acc.x, acc.y); }
    //返回该栅格被占用的概率，范围是 -1（没有访问过） 、[0,1]
    inline operator double() const { return visits ? (double)n * 1 / (double)visits : -1; }
    //更新该栅格成员变量
    inline void update(bool value, const Point &p = Point(0, 0));
    //该栅格被击中的位置累计，最后取累计值的均值
    FloatPoint acc;
    //n表示该栅格被击中的次数，visits表示该栅格被访问的次数
    int n, visits;
};

//更新该栅格成员变量，value表示该栅格是否被击中，击中n++，未击中仅visits++;
void PointAccumulator::update(bool value, const Point &p)
{
    if (value)
    {
        acc.x += static_cast<float>(p.x);
        acc.y += static_cast<float>(p.y);
        n++;
        visits += 1;
    }
    else
        visits++;
}