开发者说丨网格地图的动态加载

在《开发者说丨点云地图划分网格并可视化》的分享中，详细讲解了点云地图的网格划分以及可视化。

本章主要讲解点云地图的动态加载原理及实现过程，最后说明动态加载需要注意的一些问题。

下面是由社区开发者—胡想成提供的文章，对网格地图的动态加载进行详细讲解，希望这篇文章能给感兴趣的开发者带来更多帮助。

  以下，ENJOY  

要想实现在定位过程中动态加载相应的网格点云，需要接收当前的GPS位置，确定当前位置的网格点云，考虑当前位置在网格的边界时，我们可以预定义一个margin距离，在当前网格周围margin范围内的网格点云均为需要加载的区域。基本的原理如下例、图所示。

举例：当前的网格size为100米，当前汽车的XY位置为(340，210)，首先可以确定汽车当前位置所在网格左下角坐标为(300，200)，这样就可以很轻松得根据PCD文件名加载当前的网格点云。之后根据margin大小，以当前网格的四个顶点坐标为基准，向四周扩展margin范围，将此范围内的所有网格地图加载出来即可。如图，最多加载9块，要加载的区域是x_min-margin，x_max+margin，y_min-margin，y_max+margin。

由于这部分的工程基本上没什么难度，将主要在代码注释里面加以说明。

 1constexpr double MARGIN_UNIT = 100; // meter
 2
 3gps_tools_.lla_origin_ << origin_latitude, origin_longitude, origin_altitude;//地图起点，后续lla转xyz
 4
 5if (grid_size == "noupdate") //是否动态更新
 6    margin = -1;
 7else if (grid_size == "1x1")
 8    margin = 0;
 9else if (grid_size == "3x3")
10    margin = MARGIN_UNIT * 1;
11else if (grid_size == "5x5")
12    margin = MARGIN_UNIT * 2;
13else if (grid_size == "7x7")
14    margin = MARGIN_UNIT * 3;
15else if (grid_size == "9x9")
16    margin = MARGIN_UNIT * 4;
17else {
18    std::cout << "grid_size 有误..." << std::endl;
19    return EXIT_FAILURE;
20}

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

读网格点云地图，并记录文件名。

 1std::string front_path;
 2
 3getAllFiles(area_directory, pcd_paths); //获取pcd文件
 4if (pcd_paths.size() == 0) {
 5    return EXIT_FAILURE;
 6}
 7
 8int pos = pcd_paths[0].find_last_of('/');//获取路径前缀
 9std::string path_name(pcd_paths[0].substr(0, pos + 1));
10front_path = path_name;
11
12if (margin < 0) {
13    can_download = false; //不在线下载
14} else {
15    can_download = false; //分块更新
16
17    for (const std::string &path : pcd_paths) {
18        int pos = path.find_last_of('/');
19        std::string file_name(path.substr(pos + 1));
20        pcd_names.push_back(file_name);
21    }
22}

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

pub和sub。

1ins_pub = n.advertise<:odometry>("/gps_odom", 5, false);
2pcd_pub = n.advertise<:pointcloud2>("localmap", 5, true);
3stat_pub = n.advertise<:bool>("pmap_stat", 1, true);
4
5stat_msg.data = false;
6stat_pub.publish(stat_msg);

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

校验一下csv文件中读取到的pcd文件是否存在。

 1if (margin < 0) {
 2    int err = 0;
 3    publish_pcd(create_pcd(pcd_paths, &err), &err); //不分块
 4} else {
 5    std::cout << "can_download... " << std::endl;
 6
 7    n.param<int>("points_map_loader/update_rate", update_rate, DEFAULT_UPDATE_RATE);
 8    fallback_rate = update_rate * 2; 
 9
10    gnss_sub = n.subscribe("/novatel718d/pos", 5, publish_gnss_pcd); //有更新
11
12    if (can_download) {
13        AreaList areas = read_arealist(arealist_path); //读取csv记录的pcd文件
14
15        for (const Area &area : areas) {
16            for (const std::string &path : pcd_names) {
17                if (path == area.path) {
18                    // 将csv记录的并且文件夹中有的pcd文件，放进downloaded_areas中
19                    cache_arealist(area, downloaded_areas);
20                }
21            }
22    }
23    gnss_time = current_time = ros::Time::now();//当前时间，以gnss为准
24}

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

读取csv文件，并查找。

 1struct Area {
 2    std::string path;
 3    double x_min;
 4    double y_min;
 5    double z_min;
 6    double x_max;
 7    double y_max;
 8    double z_max;
 9};
10
11typedef std::vector AreaList;   
12typedef std::vector<std::vector<:string>> Tbl;
13
14AreaList read_arealist(const std::string &path) {
15    Tbl tbl = read_csv(path);　//逐行读取
16
17    AreaList ret;　//用定义的area重新封装
18    for (const std::vector<:string> &cols : tbl) {
19        Area area;
20        area.path = cols[0];
21        area.x_min = std::stod(cols[1]);
22        area.y_min = std::stod(cols[2]);
23        area.z_min = std::stod(cols[3]);
24        area.x_max = std::stod(cols[4]);
25        area.y_max = std::stod(cols[5]);
26        area.z_max = std::stod(cols[6]);
27        ret.push_back(area);
28    }
29    return ret;
30}
31
32Tbl read_csv(const std::string &path) {//逐行读取csv文件
33    std::ifstream ifs(path.c_str());
34    std::string line;
35    Tbl ret;
36    while (std::getline(ifs, line)) {
37        std::istringstream iss(line);
38        std::string col;
39        std::vector<:string> cols;
40        while (std::getline(iss, col, ','))
41            cols.push_back(col);
42        ret.push_back(cols);
43    }
44    return ret;
45}
46
47void cache_arealist(const Area &area, AreaList &areas) {
48    for (const Area &a : areas) {//没有的话加入
49        if (a.path == area.path)
50            return;
51    }
52    areas.push_back(area);
53}

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

最后是根据GPS位置更新，进行坐标转换。

 1void publish_gnss_pcd(const sensor_msgs::NavSatFixPtr &gps_msg) {
 2    if (std::isnan(gps_msg->latitude + gps_msg->longitude + gps_msg->altitude)) {
 3        ROS_INFO("GPS LLA NAN...");
 4        return;
 5    }
 6    if (gps_msg->status.status == 4 || gps_msg->status.status == 5 || gps_msg->status.status == 1 ||
 7        gps_msg->status.status == 2) {
 8
 9        ros::Time now = ros::Time::now();//注意更新频率是否符合预定要求
10        if (((now - current_time).toSec() * 1000) < fallback_rate)
11            return;
12        if (((now - gnss_time).toSec() * 1000) < update_rate)
13            return;
14        gnss_time = now;
15
16        Eigen::Vector3d lla = gps_tools_.GpsMsg2Eigen(*gps_msg);
17        Eigen::Vector3d ecef = gps_tools_.LLA2ECEF(lla);
18        Eigen::Vector3d enu = gps_tools_.ECEF2ENU(ecef);
19        gps_tools_.gps_pos_ = enu;
20        gps_pos_ = enu;
21
22        geometry_msgs::Point pose;
23        pose.x = gps_pos_(0);
24        pose.y = gps_pos_(1);
25        pose.z = gps_pos_(2);
26
27        std::cout << "area  lla : " << gps_pos_(0) << ", " << gps_pos_(1) << ", " << gps_pos_(2)<< std::endl;
28        publish_pcd(create_pcd(pose));　//pub当前的网格
29}

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

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

 1sensor_msgs::PointCloud2 create_pcd(const geometry_msgs::Point &p) {
 2
 3    sensor_msgs::PointCloud2 pcd, part;
 4    std::unique_lock<std::mutex> lock(downloaded_areas_mtx);
 5
 6    for (const Area &area : downloaded_areas) {//遍历一下
 7        if (is_in_area(p.x, p.y, area, margin)) { //判断当前位置在哪些网格里面
 8            std::string pcd_name = front_path + area.path;//实际的PCD文件路径
 9
10            if (pcd.width == 0)
11            pcl::io::loadPCDFile(pcd_name.c_str(), pcd);
12            else {
13                std::cout << "success load: " << area.path << std::endl;
14                pcl::io::loadPCDFile(pcd_name.c_str(), pcd);
15
16                pcd.width += part.width;　//所有符合条件的网格全pub出来
17                pcd.row_step += part.row_step;
18                pcd.data.insert(pcd.data.end(), part.data.begin(), part.data.end());
19            }
20        }
21    }
22    return pcd;
23}

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

判断当前位置是否在此Area里面。

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

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

这里我们根据连续的GPS位置计算一个实时的yaw角，来大致观察车辆的运动轨迹。

 1//   pub gps odom
 2nav_msgs::Odometry odom;
 3odom.header.stamp = gnss_time;
 4odom.header.frame_id = "map";
 5
 6odom.pose.pose.position.x = gps_pos_(0);
 7odom.pose.pose.position.y = gps_pos_(1);
 8odom.pose.pose.position.z = gps_pos_(2);
 9
10double distance = sqrt(pow(odom.pose.pose.position.y - _prev_pose.pose.position.y, 2) + pow(odom.pose.pose.position.x - _prev_pose.pose.position.x, 2));
11if (distance > 0.2) {
12    //返回值是此点与远点连线与x轴正方向的夹角
13    yaw = atan2(odom.pose.pose.position.y - _prev_pose.pose.position.y,
14    odom.pose.pose.position.x - _prev_pose.pose.position.x);
15    _quat = tf::createQuaternionMsgFromYaw(yaw);
16    _prev_pose = odom.pose;
17}
18odom.pose.pose.orientation = _quat;
19
20odom.child_frame_id = "base_link";
21odom.twist.twist.linear.x = 0.0;
22odom.twist.twist.linear.y = 0.0;
23odom.twist.twist.angular.z = 0.0;
24
25ins_pub.publish(odom);
26} else {
27    ROS_INFO("no rtk, map stop update...");
28}

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

最后看一下打印出来的log，当前车辆的LLA位置为(-174.828，-1210.92，9.50905)，加载的网格地图为300_-300_-1200.pcd，由于我这里设定的网格大小为300米，margin为100米，所以只需要加载一个网格。

动态加载的主要作用是大地图时减少内存空间，拿IO换内存。

比如10km*10km级别的地图，size为1km，一共100块网格，每次加载一块网格的话，地图占用内存就会减少到原来的1/100。

特别需要注意，如果网格size过小，划分网格太多，频繁的加载网格，IO以及内存分配会消耗大量的资源，反而会拖慢整个定位的速度，最终得不偿失。所以我们的网格size一般是1km级别的，对大地图作用明显，小地图就没有动态加载的必要了。

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

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

内容来自攻城狮の家：

http://xchu.net/2020/01/20/39dynamic_map_loader/，欢迎大家收藏点赞。已获开发者授权，原文地址请戳阅读原文。