开发者说丨点云地图划分网格并可视化

本章将完成一个点云地图处理的工作，即对点云地图进行网格划分，这样做的好处有：

一、可以根据GPS位置动态加载相应的网格地图，减少内存占用；

二、减小匹配的target点云，以便提升后续定位过程中匹配速度。

本篇主要介绍点云地图的网格划分以及可视化。

下面是由社区开发者-胡想成提供的文章，对点云地图划分网格并可视化进行详细讲解，希望这篇文章能给感兴趣的开发者带来更多帮助。

  以下，ENJOY  

我们的主要方法是去掉z轴，仅对平面x,y方向上的点云按自己定义的grid size划分方格。在定位的时候根据初始的位置，可实时加载当前区域的方格点云，缩小NDT的匹配区域，可以明显提升匹配速度和精度。这个网格划分是离线处理的过程，最后按一定的命名写入文件中，方便后续加载。

这里贴出来本篇的网格划分可视化结果。可视化部分，按顺序加载点云，随机上色，方便查看。

处理好的PCD文件示例如下：

部分CSV文件命名：

 150_-100_-150.pcd,-100,-150,0,-50,-100,0
 250_-50_-150.pcd,-50,-150,0,0,-100,0
 350_0_-150.pcd,0,-150,0,50,-100,0
 450_50_-150.pcd,50,-150,0,100,-100,0
 550_-100_-100.pcd,-100,-100,0,-50,-50,0
 650_-50_-100.pcd,-50,-100,0,0,-50,0
 750_0_-100.pcd,0,-100,0,50,-50,0
 850_50_-100.pcd,50,-100,0,100,-50,0
 950_100_-100.pcd,100,-100,0,150,-50,0
1050_-100_-50.pcd,-100,-50,0,-50,0,0
1150_-50_-50.pcd,-50,-50,0,0,0,0
1250_0_-50.pcd,0,-50,0,50,0,0
1350_50_-50.pcd,50,-50,0,100,0,0
1450_100_-50.pcd,100,-50,0,150,0,0
1550_150_-50.pcd,150,-50,0,200,0,0
1650_-150_0.pcd,-150,0,0,-100,50,0
1750_-100_0.pcd,-100,0,0,-50,50,0

<左右滑动以查看完整代码>

网格划分

首先定义我们重新组织的点云结构,代码如下：

 1struct pcd_xyz_grid
 2{
 3  std::string filename;
 4  std::string name;
 5  int grid_id;
 6  int grid_id_x;
 7  int grid_id_y;
 8  int lower_bound_x;
 9  int lower_bound_y;
10  int upper_bound_x;
11  int upper_bound_y;
12  pcl::PointCloud<:pointxyz> cloud;
13};

<左右滑动以查看完整代码>

其中grid_id就是方格的编号，按照先x后y的方式对所有的方格进行编号，比如x第一行的方格依次编号为0-10，y的第二行即从11开始。

grid_id_x，grid_id_y分别为方格在x，y方向上的编号，即行号和列号。lower_bound_x等四个数为当前方格的四个边界点。could里面是存储的PointXYZ点云，也可以是PointXYZI等，下文的PointCloud均可以自己设定。

加载文件夹内的点云地图PCD文件（可能多个）

 1PointCloud map;
 2  PointCloud tmp;
 3  for (int i = 0; i < files.size(); i++)
 4  {
 5    if (pcl::io::loadPCDFile(files[i], tmp) == -1)
 6    {
 7      std::cout << "Failed to load " << files[i] << "." << std::endl;
 8    }
 9    map += tmp;
10    std::cout << "Finished to load " << files[i] << "." << std::endl;
11  }
12
13  std::cout << "Finished to load all PCDs: " << map.size() << " points."

<左右滑动以查看完整代码>

分别搜索整个点云在xy方向的最大最小值，存到min_x，min_y，max_x， max_y中。

 1double min_x = 10000000000.0;
 2double max_x = -10000000000.0;
 3double min_y = 10000000000.0;
 4double max_y = -10000000000.0;
 5
 6for (PointCloud::const_iterator p = map.begin(); p != map.end(); p++)
 7{
 8  if (p->x < min_x)
 9  {
10    min_x = p->x;
11  }
12  if (p->x > max_x)
13  {
14    max_x = p->x;
15  }
16  if (p->y < min_y)
17  {
18    min_y = p->y;
19  }
20  if (p->y > max_y)
21  {
22    max_y = p->y;
23  }
24}

<左右滑动以查看完整代码>

找到xy方向上网格的边界

1int min_x_b = grid_size * static_cast<int>(floor(min_x / grid_size));
2 int max_x_b = grid_size * static_cast<int>(floor(max_x / grid_size) + 1);
3 int min_y_b = grid_size * static_cast<int>(floor(min_y / grid_size));
4 int max_y_b = grid_size * static_cast<int>(floor(max_y / grid_size) + 1);

<左右滑动以查看完整代码>

计算网格个数

1int div_x = (max_x_b - min_x_b) / grid_size;
2 int div_y = (max_y_b - min_y_b) / grid_size;
3 int grid_num = div_x * div_y;

<左右滑动以查看完整代码>

按一定格式定义文件名。

划分好的PCD文件命名格式为

grid_size_lower_bound_x_lower_bound_y.pcd，比如30_30_0，即grid size为30m，左下角顶点坐标为（30， 0）的方格。

1std::vector grids(grid_num);
2 for (int y = 0; y < div_y; y++)
3 {
4   for (int x = 0; x < div_x; x++)
5   {
6     int id = div_x * y + x;
7     grids[id].grid_id = id; //序号
8     grids[id].grid_id_x = x; //行号
9     grids[id].grid_id_y = y; //列号
10     grids[id].lower_bound_x = min_x_b + grid_size * x; //方格的四个顶点
11     grids[id].lower_bound_y = min_y_b + grid_size * y;
12     grids[id].upper_bound_x = min_x_b + grid_size * (x + 1);
13     grids[id].upper_bound_y = min_y_b + grid_size * (y + 1);
14     grids[id].filename = OUT_DIR + std::to_string(grid_size) + "_" +
15                          std::to_string(grids[id].lower_bound_x) + "_" +
16                          std::to_string(grids[id].lower_bound_y) + ".pcd";
17     grids[id].name = std::to_string(grid_size) + "_" +
18                      std::to_string(grids[id].lower_bound_x) + "_" +
19                      std::to_string(grids[id].lower_bound_y) + ".pcd";
20   }
21 }

<左右滑动以查看完整代码>

重新按网格序号组织点云

1for (PointCloud::const_iterator p = map.begin(); p != map.end(); p++)
2  {
3    int idx = static_cast<int>(floor((p->x - static_cast<float>(min_x_b)) / grid_size));
4    int idy = static_cast<int>(floor((p->y - static_cast<float>(min_y_b)) / grid_size));
5    int id = idy * div_x + idx;
6
7    const Point &tmp = *p;
8    grids[id].cloud.points.push_back(tmp);
9  }

<左右滑动以查看完整代码>

写入PCD文件

 1int points_num = 0;
 2for (int i = 0; i < grid_num; i++)
 3{
 4  if (grids[i].cloud.points.size() > 0)
 5  {
 6    pcl::io::savePCDFileBinary(grids[i].filename, grids[i].cloud);
 7    std::cout << "Wrote " << grids[i].cloud.points.size() << " points to "
 8              << grids[i].filename << "." << std::endl;
 9    points_num += grids[i].cloud.points.size();
10  }
11}
12write_csv(grids);
13std::cout << "Total points num: " << points_num << " points." << std::endl;

<左右滑动以查看完整代码>

同时把每一个网格的相关信息按一定格式存储到CSV文件中，方便查找。每一行存储的信息为PCD文件名-网格的边界，比如30_-90_-120.pcd,-90,-120,0,-60,-90,0。

 1void write_csv(std::vector &grids)
 2{
 3  std::string whole_file_name = OUT_DIR + FILE_NAME;
 4  std::ofstream ofs(whole_file_name.c_str());
 5  int grid_num = grids.size();
 6  for (int i = 0; i < grid_num; i++)
 7  {
 8    if (grids[i].cloud.points.size() > 0)
 9    {
10      ofs << grids[i].name
11          << "," << grids[i].lower_bound_x
12          << "," << grids[i].lower_bound_y
13          << "," << 0.0
14          << "," << grids[i].upper_bound_x
15          << "," << grids[i].upper_bound_y
16          << "," << 0.0 << std::endl;
17    }
18  }
19}
20;

<左右滑动以查看完整代码>

这里我们先用PCL加载所有的网格地图进行可视化，方便查看效果，后续在实际使用过程中，可能需要通过ROS来加载指定序号的网格，代码逻辑非常简单。

网格PCD按文件名称排序

这是一个很有用的工具函数，这里path为网格PCD的文件路径，我们需要将pcd_info.csv文件先移除，只留PCD文件。

 1void getAllFiles(std::string path, std::vector<std::string> &files)
 2{
 3    if (path[path.length() - 1] != '/')
 4        path = path + "/";
 5    DIR *dir;
 6    struct dirent *ptr;
 7    char base[1000];
 8    if ((dir = opendir(path.c_str())) == NULL)
 9    {
10        perror("Open dir error...");
11        std::cout << "Check: " << path << std::endl;
12        exit(1);
13    }
14
15    while ((ptr = readdir(dir)) != NULL)
16    {
17        if (ptr->d_type == 8) // 文件
18        {
19            std::string name = ptr->d_name;
20            int length = name.length();
21            if (name.substr(length - 3, length - 1) == "pcd" || name.substr(length - 3, length - 1) == "PCD")
22            {
23                std::cout << path + name << std::endl;
24                files.push_back(path + name);
25            }
26        }
27    }
28    closedir(dir);
29    std::sort(files.begin(), files.end());
30    return;
31}

<左右滑动以查看完整代码>

加载PCD网格

main函数里面，我们设定了输入目录，遍历读取PCD文件，用pcl_viewer可视化即可。

 1  std::string OUT_DIR = "/home/catalina/ndt_ws/src/smartcar/location/packages/pcl_map_tools/grid_pcd";
 2// Load all PCDs
 3    PointCloud::Ptr map_ptr(new PointCloud);
 4    PointCloud::Ptr tmp_ptr(new PointCloud);
 5    boost::shared_ptr<pcl::visualization::pclvisualizer> viewer(new pcl::visualization::PCLVisualizer("map viewer"));
 6
 7    int num = 0;
 8    int num2 = 255;
 9
10    for (int i = 0; i < grid_files.size(); i++)
11    {
12        if (pcl::io::loadPCDFile(grid_files[i], *tmp_ptr) == -1)
13        {
14            std::cout << "Failed to load " << grid_files[i] << "." << std::endl;
15        }
16
17        num++;
18        pcl::visualization::PointCloudColorHandlerCustom<:pointxyzi> single_color(tmp_ptr, random(255), random(255), random(255)); // green
19        viewer->addPointCloud<:pointxyzi>(tmp_ptr, single_color, "sample cloud"+num);
20        viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1.0, "sample cloud"+num); // 设置点云大小
21
22        *map_ptr += *tmp_ptr;
23        std::cout << "Finished to load " << grid_files[i] << "." << std::endl;
24    }
25
26    std::cout << "map size:" << map_ptr->size()<< "." << std::endl;
27
28//    pcl::visualization::PointCloudColorHandlerGenericField<:pointxyzi> fildColor(map_ptr, "z"); // 按照z字段进行渲染
29    while (!viewer->wasStopped())
30    {
31        viewer->spinOnce(100);
32        boost::this_thread::sleep(boost::posix_time::microseconds(100000));
33    }
</pcl::visualization::pclvisualizer>

<左右滑动以查看完整代码>

根据初始位置动态加载网格点云的代码，后续再结合定位一起来写。

以上是"点云地图划分网格并可视化"的全部内容，更多话题讨论、技术交流可以扫描下方二维码添加『Apollo小哥哥』为好友，进开发者交流群。

* 以上内容为开发者原创，不代表百度官方言论。

内容来自攻城狮の家：

http://xchu.net/2019/10/30/32pcd-divider/，欢迎大家收藏点赞。已获开发者授权，原文地址请戳阅读原文。