栅格地图中自由区域之Bresenham算法及个人搜索算法对比

最新推荐文章于 2024-04-26 21:21:49 发布

年少的梦007

最新推荐文章于 2024-04-26 21:21:49 发布

阅读量1.1k

点赞数

分类专栏： slam

本文链接：https://blog.csdn.net/weixin_41518014/article/details/118142187

版权

slam 专栏收录该内容

10 篇文章 1 订阅

订阅专栏

栅格地图中的区域可以分为三种Occupied被占用（有障碍）， Free自由区域（无障碍），

Unknown Space未知区域。

假设图1，中黄色代表机器人，显眼的绿色代表激光雷达，红色线代表一条激光线束，黑色栅格A代表被激光雷达击中的障碍物。

1.Occupied被占用，可以理解为是障碍物上的点，高于地面，对机器人运动过程有影响的一些栅格，具体是计算后概率大的那些栅格。图1中为黑色栅格A；

2.Free自由区域（无障碍）为图中灰色的栅格，此区域内激光雷达线直接穿过，本文主要是算这些栅格点的位置；

3.Unknown Space未知区域，图1中白色的栅格，激光雷达未扫描到区域，具体什么情况未知。

栅格的概率计算可根据参考文献2中基于二值贝叶斯的占用栅格占有概率的计算

这里重点研究自由区域的栅格计算方法，通过激光雷达给的数据，计算处自由区域的栅格的位置，最常用的方法就是Bresenham直线算法，具体可看参考文献

个人方法及代码，采用高数书中积分的方式查找这些点起个名字，积分搜索 integral search（IS），话不多说，主要方法解释：

核心思想：因为采用多线激光雷达，为降低计算量，将点云分为扇形区域，并取最远的点作为一个扇形区域的代表点；所有的激光雷达线束都是线段，单增或单减，利用这个性质积分，分4个区域积分，在分析时k值太大，计算不准，采用y方向积分。


#include "ros/ros.h"
#include "sensor_msgs/LaserScan.h"
#include "nav_msgs/OccupancyGrid.h"
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/radius_outlier_removal.h>
#include <math.h>
#include <pcl/kdtree/kdtree_flann.h>  //kdtree近邻搜索
#include <pcl/surface/convex_hull.h>
class GridCircle//以车体坐标系原点为中心，将360度区域划分为720份
{
public:
     int q;//记录哪一个区域
     pcl::PointCloud<pcl::PointXYZI>::Ptr q_cloud {new pcl::PointCloud<pcl::PointXYZI>}; 
     pcl::PointIndices::Ptr q_inliers {new pcl::PointIndices};//栅格内点云索引的指针

};
static int circle_num=1440;
float circle_angle = 360.0/circle_num;

nav_msgs::OccupancyGrid local_grid_map;
ros::Publisher grid_map_pub;
//构建栅格地图

pcl::PointCloud<pcl::PointXYZI>::Ptr noGroundCloud(new pcl::PointCloud<pcl::PointXYZI>);
void no_ground_lidar_subCallback(const sensor_msgs::PointCloud2ConstPtr& input_pointcloud2)
{
   pcl::fromROSMsg (*input_pointcloud2, *noGroundCloud);
}




void ground_lidar_subCallback(const sensor_msgs::PointCloud2ConstPtr& input_pointcloud2)
{
  pcl::PointCloud<pcl::PointXYZI>::Ptr GroundCloud(new pcl::PointCloud<pcl::PointXYZI>);
  pcl::fromROSMsg (*input_pointcloud2, *GroundCloud);

  //构建栅格地图

  //未知区域表示为-1，地面表示为20，障碍物表示为50；
  local_grid_map.header.frame_id="velodyne";
  local_grid_map.info.origin.position.x = -20;
  local_grid_map.info.origin.position.y = -12;
  local_grid_map.info.origin.position.z = 0.0;
  local_grid_map.info.origin.orientation.x = 0.0;
  local_grid_map.info.origin.orientation.y = 0.0;
  local_grid_map.info.origin.orientation.z = 0.0;
  local_grid_map.info.origin.orientation.w = 1.0;

  local_grid_map.header.stamp = ros::Time::now(); 
  local_grid_map.info.resolution = 0.2;         // float32
  local_grid_map.info.width = 200;           // uint32
  local_grid_map.info.height= 120;           // uint32
  local_grid_map.data.resize(local_grid_map.info.width * local_grid_map.info.height); 
  int p[local_grid_map.info.width*local_grid_map.info.height]={1};   // [0,100]
  for (size_t i = 0; i<local_grid_map.info.width*local_grid_map.info.height;i++)
  {
    p[i] = -1;
  }

  pcl::PointCloud<pcl::PointXYZI>::Ptr GroundCloudFiltered(new pcl::PointCloud<pcl::PointXYZI>);
  pcl::RadiusOutlierRemoval<pcl::PointXYZI> outrem;// 创建滤波器    
  outrem.setInputCloud(GroundCloud);              //设置输入点云
  //搜索半径设为0.8，在此半径内点必须要有至少1个邻居时，此点才会被保留
  outrem.setRadiusSearch(4);              //设置在0.8半径的范围内找邻近点
  outrem.setMinNeighborsInRadius(10);        //设置查询点的邻近点集数小于1的删除
  outrem.filter(*GroundCloudFiltered);   //执行条件滤波，存储结果到cloud_filtered

  int column_i = local_grid_map.info.width;//设置栅格的行
  int row_i = local_grid_map.info.height;//设置栅格的列
  float grid_size = local_grid_map.info.resolution;
  /*
  for(size_t m = 0; m < GroundCloudFiltered->size(); m++)//cloud_sourround设置为预处理后的点云
      {
          for(int i_g= -column_i/2; i_g<column_i/2; i_g++)
          {
            if (((GroundCloudFiltered->points[m].x>i_g*grid_size)&&(GroundCloudFiltered->points[m].x<i_g*grid_size+grid_size)))//如果x在某范围内
            {
                for(int j_g = -row_i/2; j_g < row_i/2; j_g++)
                {
                  if((GroundCloudFiltered->points[m].y>j_g*grid_size)&&(GroundCloudFiltered->points[m].y<j_g*grid_size+grid_size))
                  {
                      //grid_inliner是pointindices类型的指针,pointindices类型的内容，包括header和索引向量indices
                      //grid[i+column/2][j+row/2].grid_inliers->indices.push_back(m);    
                      //grid[i+column/2][j+row/2].grid_inliers->header=cloud_sourround->header;
                      //grid[i+column/2][j+row/2].grid_cloud->points.push_back(cloud_sourround->points[m]);
                      p[i_g+column_i/2+(j_g+row_i/2)*column_i] =100;

                  }
                }
            }
          }
      }
  */

//为了减小计算量取最远点
    GridCircle GridCircle[circle_num];//边界点
    for(size_t g_i = 0; g_i < GroundCloudFiltered->size(); g_i++ )
    {
        int q = floor((atan2(GroundCloudFiltered->points[g_i].y,GroundCloudFiltered->points[g_i].x)+M_PI)/(M_PI*circle_angle/180));//floor是取整数
        GridCircle[q].q_cloud->points.push_back(GroundCloudFiltered->points[g_i]);
    }


//提取circle_num个区域最近的点
    pcl::PointCloud<pcl::PointXYZI>::Ptr boundaryPoints (new pcl::PointCloud<pcl::PointXYZI>); //储存边界点
    for(size_t u_i = 0; u_i < circle_num; u_i++)
    { 
        if(GridCircle[u_i].q_cloud->points.size() > 0)
        {
           float dis = 0;
           for(size_t j = 0; j< GridCircle[u_i].q_cloud->points.size(); j++) 
            {
              
              float dis_1 = sqrt(GridCircle[u_i].q_cloud->points[j].x*GridCircle[u_i].q_cloud->points[j].x + 
                       GridCircle[u_i].q_cloud->points[j].y*GridCircle[u_i].q_cloud->points[j].y);
              boundaryPoints->points.push_back(GridCircle[u_i].q_cloud->points[j]);
              if( dis_1 > dis)
              {
                dis = dis_1;  
                boundaryPoints->points.pop_back();
                boundaryPoints->points.push_back(GridCircle[u_i].q_cloud->points[j]);
              }

            }    
        }   
    }
  /*
  pcl::PointCloud<pcl::PointXYZI>::Ptr boundaryPoints (new pcl::PointCloud<pcl::PointXYZI>); //储存边界点
  pcl::ConvexHull<pcl::PointXYZI> hull;                  
  hull.setInputCloud(GroundCloudFiltered);                   
  hull.setDimension(3);
  std::vector<pcl::Vertices> polygons;                 
  //pcl::PointCloud<pcl::PointXYZI>::Ptr surface_hull(new pcl::PointCloud<pcl::PointXYZI>);
  hull.reconstruct(*boundaryPoints, polygons);
*/

 float res = local_grid_map.info.resolution;

 for(size_t m = 0; m < boundaryPoints->size(); m++)
 {
    
    //假设某一个点的作为x_start,y_start; x_end,y_end ,栅格的分辨率为 res,采用积分的方法扫描
    
    float x_start = 0;
    float y_start = 0;

    float x_end = boundaryPoints->points[m].x;
    float y_end = boundaryPoints->points[m].y;
   
    float x_shift = -local_grid_map.info.origin.position.x; //为方便计算将所有的点放到第一象限内
    float y_shift = -local_grid_map.info.origin.position.y;

    //所有的位置进行偏移让栅格的原点和坐标的原点相重合

    x_start = x_start + x_shift;
    y_start = y_start + y_shift;
    x_end = x_end + x_shift;
    y_end = y_end + y_shift;
     
    if(x_start != x_end)
    {
        //将x值小的点作为起点
      if(x_start > x_end)
      {
        float t_x = x_start;
        float t_y = y_start;
        x_start = x_end;
        y_start = y_end;
        x_end = t_x;
        y_end = t_y;
      }

      //计算斜率  y = Kx+b
      float line_k = (y_end - y_start)/(x_end - x_start);
      float line_b = y_shift - line_k*x_shift;//所有的直线都经过激光雷达的原点，
     
      //计算初始栅格的位置，栅格坐标定定义为 grid_x_start , grid_y_start , grid_x_end ,grid_y_end 
      int grid_x_start = floor(x_start/res);
      int grid_y_start = floor(y_start/res);
      int grid_x_end = floor(x_end/res);
      int grid_y_end = floor(y_end/res); 

      //std::cout<< " 1 "<< grid_x_start<< "  "<< grid_y_start<<std::endl;
      //std::cout<< " 2 "<< grid_x_end<< "  "<< grid_y_end<<std::endl; 
      //std::cout<<GroundCloudFiltered->size()<<std::endl;
      p[grid_x_start+(grid_y_start)* local_grid_map.info.width] = 1;
      p[grid_x_end+grid_y_end* local_grid_map.info.width] = 1;
      
      //对所有的坐标进行偏移，x+20,y+12,按照斜率将值分为正和负两个
      if((line_k <= 1)&&(line_k >= -1))
      {
        for(int i = grid_x_start; i<= grid_x_end; i++)//计算x上的栅格的范围，进行遍历，Ceil不大于自变量的最大整数
        { 
          
          
          //分四个区域
          if(line_k>=0 )//
          {
            int j_value = floor((line_k*(i*res)+ line_b)/res);
            int j_high_value = ceil((line_k*((i+1)*res)+ line_b)/res);
            //if((line_k*(i*res)+ line_b)<0) 
            //std::cout<<i*res<<line_k*(i*res)+ line_b<<std::endl; 
            
            for(int j = j_value ;j<= j_high_value ;j=j+1 )//计算y方向上的遍历
            {    
              if(!((i+j*local_grid_map.info.width)>24000 || (i+j*local_grid_map.info.width)<0) )
              {
                  p[i+j*local_grid_map.info.width] = 1;
              }     
            }
          }
      
          if(line_k<0)// 
          {

            int j_value = ceil((line_k*(i*res)+ line_b)/res);
            int j_high_value = floor((line_k*((i+1)*res)+ line_b)/res);

          // std::cout << j_value<<" "<< j_high_value <<std::endl;
            for(int j = j_value;j>=j_high_value;j=j-1 )//计算y方向上的遍历
              { 
                if(!((i+j*local_grid_map.info.width)>24000 || (i+j*local_grid_map.info.width)<0) )
              {
                  p[i+j*local_grid_map.info.width] = 1;
              }
              
              } 
          }

        }
      }
      else
      {
        if(line_k>=0 )//
        {
          for(int i = grid_y_start; i<= grid_y_end ; i++)
          {
              int j_value = floor(((i*res - line_b)/line_k)/res);
              int j_high_value = ceil((((i+1)*res - line_b)/line_k)/res);
              //if((line_k*(i*res)+ line_b)<0) 
              //std::cout<<i*res<<line_k*(i*res)+ line_b<<std::endl; 
              for(int j = j_value ;j<= j_high_value ;j=j+1 )//计算y方向上的遍历
              {    
                if(!((j+i*local_grid_map.info.width)>24000 || (j+i*local_grid_map.info.width)<0) )
                {
                    p[j+i*local_grid_map.info.width] = 1;
                }     
              }
          }
        }
        
        if(line_k<0)// 
          {
            for(int i = grid_y_end; i<= grid_y_start; i++)
            {
              int j_value = ceil(((i*res - line_b)/line_k)/res);
              int j_high_value = floor((((i+1)*res - line_b)/line_k)/res);
          // std::cout << j_value<<" "<< j_high_value <<std::endl;
             for(int j = j_value;j>=j_high_value;j=j-1 )//计算x方向上的遍历
              { 
                if(!((j+i*local_grid_map.info.width)>24000 || (j+i*local_grid_map.info.width)<0) )
                {
                    p[j+i*local_grid_map.info.width] = 1;
                }     
              
              } 
            }
          }
      
      }
              
    }
  
 }


 if(noGroundCloud->size()>0)
 {
  for(size_t m = 0; m < noGroundCloud->size(); m++)
  {
      
      //假设某一个点的作为x_start,y_start; x_end,y_end ,栅格的分辨率为 res,采用积分的方法扫描
      
      float x_start = 0;
      float y_start = 0;

      float x_end = noGroundCloud->points[m].x;
      float y_end = noGroundCloud->points[m].y;
    
      float x_shift = -local_grid_map.info.origin.position.x; //为方便计算将所有的点放到第一象限内
      float y_shift = -local_grid_map.info.origin.position.y;

      //所有的位置进行偏移让栅格的原点和坐标的原点相重合

      x_start = x_start + x_shift;
      y_start = y_start + y_shift;
      x_end = x_end + x_shift;
      y_end = y_end + y_shift;
      
      if(x_start != x_end)
      {
          //将x值小的点作为起点
        if(x_start > x_end)
        {
          float t_x = x_start;
          float t_y = y_start;
          x_start = x_end;
          y_start = y_end;
          x_end = t_x;
          y_end = t_y;
        }

        //计算斜率  y = Kx+b
        float line_k = (y_end - y_start)/(x_end - x_start);
        float line_b = y_shift - line_k*x_shift;//所有的直线都经过激光雷达的原点，
      
        //计算初始栅格的位置，栅格坐标定定义为 grid_x_start , grid_y_start , grid_x_end ,grid_y_end 
        int grid_x_start = floor(x_start/res);
        int grid_y_start = floor(y_start/res);
        int grid_x_end = floor(x_end/res);
        int grid_y_end = floor(y_end/res); 

        //std::cout<< " 1 "<< grid_x_start<< "  "<< grid_y_start<<std::endl;
        //std::cout<< " 2 "<< grid_x_end<< "  "<< grid_y_end<<std::endl; 
        //std::cout<<GroundCloudFiltered->size()<<std::endl;
        p[grid_x_start+(grid_y_start)* local_grid_map.info.width] = 100;
        p[grid_x_end+grid_y_end* local_grid_map.info.width] = 100;

      }
  }
 }

  std::vector<signed char> a(p, p+24000);//复制[begin,end)区间内另一个数组的元素到vector中
  local_grid_map.data = a;
  grid_map_pub.publish(local_grid_map);

//直线栅格化，为找出空闲的区域


}


int main(int argc, char * argv[]) {

  ros::init(argc, argv, "gridMap");

  ros::NodeHandle nh;
  ros::NodeHandle nh_private("~");

  grid_map_pub = nh.advertise<nav_msgs::OccupancyGrid>("/gridMap", 1);
  ros::Subscriber ground_sub = nh.subscribe("/sourround_points", 1, ground_lidar_subCallback);
  ros::Subscriber no_ground_sub = nh.subscribe("/no_ground_points", 1, no_ground_lidar_subCallback);

  ros::spin ();
}

-------------------------------------------------读书破万卷---------------------------------------------------

参考文献

1.ROS导航之地图costmap_2d与bresenham算法

2.Occupancy grid mapping 占有格栅地图构建

年少的梦007

关注

0
点赞
踩
9

收藏

觉得还不错? 一键收藏
0
评论
栅格地图中自由区域之Bresenham算法及个人搜索算法对比

栅格地图中的区域可以分为三种Occupied被占用（有障碍）， Free自由区域（无障碍），Unknown Space未知区域。栅格地图的假设图1，中黄色代表机器人，显眼的绿色代表激光雷达，红色线代表一条激光线束，黑色栅格A代表被激光雷达击中的障碍物。1.Occupied被占用，可以理解为是障碍物上的点，高于地面，对机器人运动过程有影响的一些栅格，具体是计算后概率大的那些栅格。图1中为黑色栅格A；2.Free自由区域（无障碍）为图中灰色的栅格，此区域内激光雷达线直接穿过，本文主要是算这.
复制链接

扫一扫

专栏目录