0. 前言

最近空闲时间在看点云地图的动态加载，这部分在自动驾驶领域是非常有必要的。由于点云地图的稠密性，导致我们在大场景中没办法一次性加载所有的地图，这就需要我们将地图切分成多个子地图。

1. NDT降维

建立好的点云文件中，有很多点是重合的，需要通过采用合适的downsample_resolution以减小点云文件体积，便于传输和加载，通常降采样后体积可以降到原来的一半以下。同时由于NDT的特性，降采样后并不会影响最终的匹配定位效果。
所以作为大地图我们需要先通过NDT对地图进行预处理，来减少频繁的IO操作并降低内存的占用。

/**初始化pose**/
curr_pose.reset(new geometry_msgs::PoseStamped());
curr_pose->pose.position.x = private_nh.param<float>("init_x", 0.0f);
curr_pose->pose.position.y = private_nh.param<float>("init_y", 0.0f);
curr_pose->pose.position.z = 0.0f;

/**加载全局地图并发布一次**/ // maybe add voxelgrid down sample
full_map.reset(new pcl::PointCloud<pcl::PointXYZI>());
pcl::io::loadPCDFile(globalmap_pcd, *full_map); //full_map指向全局地图

/** 地图下采样 **/
boost::shared_ptr <pcl::VoxelGrid<pcl::PointXYZI>> voxelgrid(new pcl::VoxelGrid<pcl::PointXYZI>());
voxelgrid->setLeafSize(downsample_resolution, downsample_resolution, downsample_resolution);
voxelgrid->setInputCloud(full_map);
pcl::PointCloud<pcl::PointXYZI>::Ptr filtered(new pcl::PointCloud<pcl::PointXYZI>());
voxelgrid->filter(*filtered);
full_map = filtered;

int points_num = 0;
int grid_num =full_map.size();
for (int i = 0; i < grid_num; i++)
{
  if (full_map[i].cloud.points.size() > 0)
  {
    pcl::io::savePCDFileBinary(full_map[i].filename, full_map[i].cloud);
    std::cout << "Wrote " << full_map[i].cloud.points.size() << " points to "
              << full_map[i].filename << "." << std::endl;
    points_num += full_map[i].cloud.points.size();
  }
}
write_csv(grids);
std::cout << "Total points num: " << points_num << " points." << std::endl;

2. PCD地图分割

这里的相关代码作者LitoNeo已经开源。这部分主要思路是去掉z轴，仅对平面x、y方向上的点云按自定义的grid size划分方形网格。在定位的时候根据当前的GPS位置，可实时加载所在区域的网格点云，大量減小內存占用。
点云地图网格划分是离线处理的过程，处理完成后，每一个网格PCD都按一定的格式命名，并且在csv文件中记录，方便后续按文件名加载。

部分csv文件命名:

50_-100_-150.pcd,-100,-150,0,-50,-100,0
50_-50_-150.pcd,-50,-150,0,0,-100,0
50_0_-150.pcd,0,-150,0,50,-100,0
50_50_-150.pcd,50,-150,0,100,-100,0
50_-100_-100.pcd,-100,-100,0,-50,-50,0
50_-50_-100.pcd,-50,-100,0,0,-50,0
50_0_-100.pcd,0,-100,0,50,-50,0
50_50_-100.pcd,50,-100,0,100,-50,0
50_100_-100.pcd,100,-100,0,150,-50,0
50_-100_-50.pcd,-100,-50,0,-50,0,0
50_-50_-50.pcd,-50,-50,0,0,0,0
50_0_-50.pcd,0,-50,0,50,0,0
50_50_-50.pcd,50,-50,0,100,0,0
50_100_-50.pcd,100,-50,0,150,0,0
50_150_-50.pcd,150,-50,0,200,0,0
50_-150_0.pcd,-150,0,0,-100,50,0
50_-100_0.pcd,-100,0,0,-50,50,0

自定义结构体

struct pcd_xyz_grid
{
  std::string filename;　//网格PCD文件名
  std::string name;
  int grid_id;　//网格编号
  int grid_id_x;　//行号
  int grid_id_y;　//列号
  int lower_bound_x;　//x_min
  int lower_bound_y; //y_min
  int upper_bound_x; //x_max
  int upper_bound_y; //y_max
  pcl::PointCloud<pcl::PointXYZ> cloud;
};

具体分割代码实现

/** 加载原始点云地图 **/
PointCloud map;
PointCloud tmp;
for (int i = 0; i < files.size(); i++)
{
    if (pcl::io::loadPCDFile<Point>(files[i], tmp) == -1)
    {
        std::cout << "Failed to load " << files[i] << "." << std::endl;
    }
    map += tmp;
    std::cout << "Finished to load " << files[i] << "." << std::endl;
}
std::cout << "Finished to load all PCDs: " << map.size() << " points."
/** 考虑对整个点云地图文件的点在xy方向的最大最小值，并设定网格划分规则 **/
double min_x = 10000000000.0;
double max_x = -10000000000.0;
double min_y = 10000000000.0;
double max_y = -10000000000.0;

for (PointCloud::const_iterator p = map.begin(); p != map.end(); p++)
{
  if (p->x < min_x)
  {
    min_x = p->x;
  }
  if (p->x > max_x)
  {
    max_x = p->x;
  }
  if (p->y < min_y)
  {
    min_y = p->y;
  }
  if (p->y > max_y)
  {
    max_y = p->y;
  }
}
/** 设定的grid_size对网格进行划分 **/
int div_x = (max_x_b - min_x_b) / grid_size;
int div_y = (max_y_b - min_y_b) / grid_size;
int grid_num = div_x * div_y;

/** 边界网格的xy方向上的坐标 **/
int min_x_b = grid_size * static_cast<int>(floor(min_x / grid_size));
int max_x_b = grid_size * static_cast<int>(floor(max_x / grid_size) + 1);
int min_y_b = grid_size * static_cast<int>(floor(min_y / grid_size));
int max_y_b = grid_size * static_cast<int>(floor(max_y / grid_size) + 1);

/**重新按网格序号组织点云 **/

for (PointCloud::const_iterator p = map.begin(); p != map.end(); p++)
  {
    int idx = static_cast<int>(floor((p->x - static_cast<float>(min_x_b)) / grid_size));
    int idy = static_cast<int>(floor((p->y - static_cast<float>(min_y_b)) / grid_size));
    int id = idy * div_x + idx;

    const Point &tmp = *p;
    grids[id].cloud.points.push_back(tmp);
  }
  
/**一定格式定义文件名并保存 **/
std::vector<pcd_xyz_grid> grids(grid_num);
 for (int y = 0; y < div_y; y++)
 {
   for (int x = 0; x < div_x; x++)
   {
     int id = div_x * y + x;
     grids[id].grid_id = id; //序号
     grids[id].grid_id_x = x; //行号
     grids[id].grid_id_y = y; //列号
     grids[id].lower_bound_x = min_x_b + grid_size * x; //方格的四个顶点
     grids[id].lower_bound_y = min_y_b + grid_size * y;
     grids[id].upper_bound_x = min_x_b + grid_size * (x + 1);
     grids[id].upper_bound_y = min_y_b + grid_size * (y + 1);
     grids[id].filename = OUT_DIR + std::to_string(grid_size) + "_" +
                          std::to_string(grids[id].lower_bound_x) + "_" +
                          std::to_string(grids[id].lower_bound_y) + ".pcd";
     grids[id].name = std::to_string(grid_size) + "_" +
                      std::to_string(grids[id].lower_bound_x) + "_" +
                      std::to_string(grids[id].lower_bound_y) + ".pcd";
   }
 }
int points_num = 0;
for (int i = 0; i < grid_num; i++)
{
  if (grids[i].cloud.points.size() > 0)
  {
    pcl::io::savePCDFileBinary(grids[i].filename, grids[i].cloud);
    std::cout << "Wrote " << grids[i].cloud.points.size() << " points to "
              << grids[i].filename << "." << std::endl;
    points_num += grids[i].cloud.points.size();
  }
}
write_csv(grids);
std::cout << "Total points num: " << points_num << " points." << std::endl;


void write_csv(std::vector<pcd_xyz_grid> &grids)
{
  std::string whole_file_name = OUT_DIR + FILE_NAME;
  std::ofstream ofs(whole_file_name.c_str());
  int grid_num = grids.size();
  for (int i = 0; i < grid_num; i++)
  {
    if (grids[i].cloud.points.size() > 0)
    {
      ofs << grids[i].name
          << "," << grids[i].lower_bound_x
          << "," << grids[i].lower_bound_y
          << "," << 0.0
          << "," << grids[i].upper_bound_x
          << "," << grids[i].upper_bound_y
          << "," << 0.0 << std::endl;
    }
  }
}

3.动态地图加载

/**读网格点云地图，并记录文件名**/
constexpr double MARGIN_UNIT = 100; // meter

gps_tools_.lla_origin_ << origin_latitude, origin_longitude, origin_altitude;//地图起点，后续lla转xyz

if (grid_size == "noupdate") //是否动态更新
    margin = -1;
else if (grid_size == "1x1")
    margin = 0;
else if (grid_size == "3x3")
    margin = MARGIN_UNIT * 1;
else if (grid_size == "5x5")
    margin = MARGIN_UNIT * 2;
else if (grid_size == "7x7")
    margin = MARGIN_UNIT * 3;
else if (grid_size == "9x9")
    margin = MARGIN_UNIT * 4;
else {
    std::cout << "grid_size 有误..." << std::endl;
    return EXIT_FAILURE;
}

std::string front_path;

getAllFiles(area_directory, pcd_paths); //获取pcd文件
if (pcd_paths.size() == 0) {
    return EXIT_FAILURE;
}

int pos = pcd_paths[0].find_last_of('/');//获取路径前缀
std::string path_name(pcd_paths[0].substr(0, pos + 1));
front_path = path_name;

if (margin < 0) {
    can_download = false; //不在线下载
} else {
    can_download = false; //分块更新

    for (const std::string &path : pcd_paths) {
        int pos = path.find_last_of('/');
        std::string file_name(path.substr(pos + 1));
        pcd_names.push_back(file_name);
    }
}

/**校验csv文件中读取到的pcd文件是否存在**/
string arealist_path = "*****";//csv 路径
if (margin < 0) {
    int err = 0;
    publish_pcd(create_pcd(pcd_paths, &err), &err); //不分块
} else {
    std::cout << "can_download... " << std::endl;

    n.param<int>("update_rate", update_rate, DEFAULT_UPDATE_RATE);
    fallback_rate = update_rate * 2; 

    gnss_sub = n.subscribe("pos", 5, publish_gnss_pcd); //有更新
    
    if (can_download) {
        AreaList areas = read_arealist(arealist_path); //读取csv记录的pcd文件

        for (const Area &area : areas) {
            for (const std::string &path : pcd_names) {
                if (path == area.path) {
                    // 将csv记录的并且文件夹中有的pcd文件，放进downloaded_areas中
                    cache_arealist(area, downloaded_areas);
                }
            }
    }
    gnss_time = current_time = ros::Time::now();//当前时间，以gnss为准
}

/**读取csv文件，并查找**/
struct Area {
    std::string path;
    double x_min;
    double y_min;
    double z_min;
    double x_max;
    double y_max;
    double z_max;
};

typedef std::vector<Area> AreaList;   
typedef std::vector<std::vector<std::string>> Tbl;
 
AreaList read_arealist(const std::string &path) {
    Tbl tbl = read_csv(path);　//逐行读取

    AreaList ret;　//用定义的area重新封装
    for (const std::vector<std::string> &cols : tbl) {
        Area area;
        area.path = cols[0];
        area.x_min = std::stod(cols[1]);
        area.y_min = std::stod(cols[2]);
        area.z_min = std::stod(cols[3]);
        area.x_max = std::stod(cols[4]);
        area.y_max = std::stod(cols[5]);
        area.z_max = std::stod(cols[6]);
        ret.push_back(area);
    }
    return ret;
}

Tbl read_csv(const std::string &path) {//逐行读取csv文件
    std::ifstream ifs(path.c_str());
    std::string line;
    Tbl ret;
    while (std::getline(ifs, line)) {
        std::istringstream iss(line);
        std::string col;
        std::vector<std::string> cols;
        while (std::getline(iss, col, ','))
            cols.push_back(col);
        ret.push_back(cols);
    }
    return ret;
}

void cache_arealist(const Area &area, AreaList &areas) {
    for (const Area &a : areas) {//没有的话加入
        if (a.path == area.path)
            return;
    }
    areas.push_back(area);
}

gps位置更新，进行坐标转换

void publish_gnss_pcd(const sensor_msgs::NavSatFixPtr &gps_msg) {
    if (std::isnan(gps_msg->latitude + gps_msg->longitude + gps_msg->altitude)) {
        ROS_INFO("GPS LLA NAN...");
        return;
    }
    if (gps_msg->status.status == 4 || gps_msg->status.status == 5 || gps_msg->status.status == 1 ||
        gps_msg->status.status == 2) {

        ros::Time now = ros::Time::now();//注意更新频率是否符合预定要求
        if (((now - current_time).toSec() * 1000) < fallback_rate)
            return;
        if (((now - gnss_time).toSec() * 1000) < update_rate)
            return;
        gnss_time = now;

        Eigen::Vector3d lla = gps_tools_.GpsMsg2Eigen(*gps_msg);
        Eigen::Vector3d ecef = gps_tools_.LLA2ECEF(lla);
        Eigen::Vector3d enu = gps_tools_.ECEF2ENU(ecef);
        gps_tools_.gps_pos_ = enu;
        gps_pos_ = enu;

        geometry_msgs::Point pose;
        pose.x = gps_pos_(0);
        pose.y = gps_pos_(1);
        pose.z = gps_pos_(2);

        std::cout << "area  lla : " << gps_pos_(0) << ", " << gps_pos_(1) << ", " << gps_pos_(2)<< std::endl;
        publish_pcd(create_pcd(pose));　//pub当前的网格
}

根据位置去查询相应的网格地图

sensor_msgs::PointCloud2 create_pcd(const geometry_msgs::Point &p) {

    sensor_msgs::PointCloud2 pcd, part;
    std::unique_lock<std::mutex> lock(downloaded_areas_mtx);

    for (const Area &area : downloaded_areas) {//遍历一下
        if (is_in_area(p.x, p.y, area, margin)) { //判断当前位置在哪些网格里面
            std::string pcd_name = front_path + area.path;//实际的PCD文件路径

            if (pcd.width == 0)
            pcl::io::loadPCDFile(pcd_name.c_str(), pcd);
            else {
                std::cout << "success load: " << area.path << std::endl;
                pcl::io::loadPCDFile(pcd_name.c_str(), pcd);

                pcd.width += part.width;　//所有符合条件的网格全pub出来
                pcd.row_step += part.row_step;
                pcd.data.insert(pcd.data.end(), part.data.begin(), part.data.end());
            }
        }
    }
    return pcd;
}

bool is_in_area(double x, double y, const Area &area, double m) {
    return ((area.x_min - m) <= x && x <= (area.x_max + m) && (area.y_min - m) <= y && y <= (area.y_max + m));
}

…详情请参照古月居