lio-sam中点云地图保存

最新推荐文章于 2024-09-11 18:52:43 发布

FourierTransformer

最新推荐文章于 2024-09-11 18:52:43 发布

阅读量2.9k

点赞数

分类专栏：点云文章标签：自动驾驶算法

本文链接：https://blog.csdn.net/qfpkzheng/article/details/120718031

版权

点云专栏收录该内容

9 篇文章 0 订阅

订阅专栏

在 src/save_map.srv 中有如下的定义

float32 resolution                                                                                                                                              
string destination
---
bool success

注意 --- 下面指的是feedback，

接下来看save_map这个服务是如何写的

首先是创建 save_map这个 server

  srvSaveMap  = nh.advertiseService("lio_sam/save_map", &mapOptimization::saveMapService, this);

接下来主要就是看saveMapService这个 callback函数


  bool saveMapService(lio_sam::save_mapRequest& req, lio_sam::save_mapResponse& res) 
    {
      string saveMapDirectory;
 
      cout << "****************************************************" << endl;
      cout << "Saving map to pcd files ..." << endl;
      //判断参数中是否传入了  destination.
      if(req.destination.empty()) saveMapDirectory = std::getenv("HOME") + savePCDDirectory;
      else saveMapDirectory = std::getenv("HOME") + req.destination;
      cout << "Save destination: " << saveMapDirectory << endl;
      //上面这几句刚好解了为何生成的地面是在home下面放着的
      // create directory and remove old files;
      // 删除老的文件,代码里面执行终命令
      int unused = system((std::string("exec rm -r ") + saveMapDirectory).c_str());
      unused = system((std::string("mkdir -p ") + saveMapDirectory).c_str());
      // save key frame transformations
      // 存储关键桢的轨迹和pose信息.
      pcl::io::savePCDFileBinary(saveMapDirectory + "/trajectory.pcd", *cloudKeyPoses3D);
      pcl::io::savePCDFileBinary(saveMapDirectory + "/transformations.pcd", *cloudKeyPoses6D);
      // extract global point cloud map
      // global的点去其实就是每桢的信息根据pose拼接起来的.
      pcl::PointCloud<PointType>::Ptr globalCornerCloud(new pcl::PointCloud<PointType>());
      pcl::PointCloud<PointType>::Ptr globalCornerCloudDS(new pcl::PointCloud<PointType>());  //进行下采样之后的，因为数据可能会比较大.
      pcl::PointCloud<PointType>::Ptr globalSurfCloud(new pcl::PointCloud<PointType>());
      pcl::PointCloud<PointType>::Ptr globalSurfCloudDS(new pcl::PointCloud<PointType>());  //下采样后的.
      pcl::PointCloud<PointType>::Ptr globalMapCloud(new pcl::PointCloud<PointType>());
 
      //点去拼接
      for (int i = 0; i < (int)cloudKeyPoses3D->size(); i++) {
          //这里的变换函数实际上就是 P_w = T*P_c, 一个坐标系的转换。即从lidar的ego坐标系到以出发点为原点的世界坐标系.
          *globalCornerCloud += *transformPointCloud(cornerCloudKeyFrames[i],  &cloudKeyPoses6D->points[i]);
          *globalSurfCloud   += *transformPointCloud(surfCloudKeyFrames[i],    &cloudKeyPoses6D->points[i]);
          cout << "\r" << std::flush << "Processing feature cloud " << i << " of " << cloudKeyPoses6D->size() << " ...";
      }    
 
        //判断是否传入了resolution这个参数，这个参数是用来进行下采样的，即体素的size大小。
      if(req.resolution != 0)
      {    
        cout << "\n\nSave resolution: " << req.resolution << endl;
 
        // down-sample and save corner cloud
        downSizeFilterCorner.setInputCloud(globalCornerCloud);
        downSizeFilterCorner.setLeafSize(req.resolution, req.resolution, req.resolution);
        downSizeFilterCorner.filter(*globalCornerCloudDS);
        pcl::io::savePCDFileBinary(saveMapDirectory + "/CornerMap.pcd", *globalCornerCloudDS);
        // down-sample and save surf cloud
        downSizeFilterSurf.setInputCloud(globalSurfCloud);
        downSizeFilterSurf.setLeafSize(req.resolution, req.resolution, req.resolution);
        downSizeFilterSurf.filter(*globalSurfCloudDS);
        pcl::io::savePCDFileBinary(saveMapDirectory + "/SurfMap.pcd", *globalSurfCloudDS);
      }    
      else 
      {    
        // save corner cloud
        pcl::io::savePCDFileBinary(saveMapDirectory + "/CornerMap.pcd", *globalCornerCloud);
        // save surf cloud
        pcl::io::savePCDFileBinary(saveMapDirectory + "/SurfMap.pcd", *globalSurfCloud);
      }    
 
      // save global point cloud map
      // 因此从这里来看，其实global的地图，就是global的corner与global的surface的拼接.
      *globalMapCloud += *globalCornerCloud;
      *globalMapCloud += *globalSurfCloud;
 
      int ret = pcl::io::savePCDFileBinary(saveMapDirectory + "/GlobalMap.pcd", *globalMapCloud);
      res.success = ret == 0;
 
      downSizeFilterCorner.setLeafSize(mappingCornerLeafSize, mappingCornerLeafSize, mappingCornerLeafSize);
      downSizeFilterSurf.setLeafSize(mappingSurfLeafSize, mappingSurfLeafSize, mappingSurfLeafSize);
 
      cout << "****************************************************" << endl;
      cout << "Saving map to pcd files completed\n" << endl;
 
      return true;
    }