HDL_Graph_slam骨头记（6）——src/hdl_graph_slam/map_cloud_generator

#include <hdl_graph_slam/map_cloud_generator.hpp>

#include <pcl/octree/octree_search.h>

namespace hdl_graph_slam {

MapCloudGenerator::MapCloudGenerator() {
}

MapCloudGenerator::~MapCloudGenerator() {

}

pcl::PointCloud<MapCloudGenerator::PointT>::Ptr MapCloudGenerator::generate(const std::vector<KeyFrameSnapshot::Ptr>& keyframes, double resolution) const {
  if(keyframes.empty()) {
    std::cerr << "warning: keyframes empty!!" << std::endl;
    return nullptr;
  }

  pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>());
  cloud->reserve(keyframes.front()->cloud->size() * keyframes.size());
  //reserve:
  //Request a change in capacity
  //Requests that the vector capacity be at least enough to contain n elements.
  //If n is greater than the current vector capacity, the function causes the container to reallocate its storage increasing its capacity to n (or greater).
  //In all other cases, the function call does not cause a reallocation and the vector capacity is not affected.
  //This function has no effect on the vector size and cannot alter its elements.


  for(const auto& keyframe : keyframes) {
    Eigen::Matrix4f pose = keyframe->pose.matrix().cast<float>();
    for(const auto& src_pt : keyframe->cloud->points) {
      PointT dst_pt;
      dst_pt.getVector4fMap() = pose * src_pt.getVector4fMap();
      dst_pt.intensity = src_pt.intensity;
      cloud->push_back(dst_pt);
    }
  }

  cloud->width = cloud->size();
  cloud->height = 1;
  cloud->is_dense = false;

  pcl::octree::OctreePointCloud<PointT> octree(resolution);
  octree.setInputCloud(cloud);
  octree.addPointsFromInputCloud();

  pcl::PointCloud<PointT>::Ptr filtered(new pcl::PointCloud<PointT>());
  octree.getOccupiedVoxelCenters(filtered->points);

  filtered->width = filtered->size();
  filtered->height = 1;
  filtered->is_dense = false;

  return filtered;
}

}
/*


PointCloud类包含以下元素：

宽度-指定点云数据集的宽度（以点数为单位）。宽度有两种含义：

它可以为无组织的数据集指定云中的点总数（与下面的点相等）；

它可以指定有组织点云数据集的宽度（行中的点总数）。强制性的。

高度-以点数指定点云数据集的高度。高度有两层含义：

它可以指定有组织的点云数据集的高度（总行数）；

对于无组织的数据集，它设置为1（因此用于检查数据集是否有组织）。强制性的。

points—存储PointT类型的所有点的数据数组。强制性的。

is_dense-指定点中的所有数据是有限的（true），还是可能包含Inf/NaN值（false）。强制性的。

传感器原点-指定传感器采集姿势（原点/平移）。可选。

传感器方向-指定传感器采集姿势（旋转）。可选。

作者

帕特里克·米赫利希，拉杜·B·鲁斯 
 */

HDL_Graph_slam骨头记（6）——src/hdl_graph_slam/map_cloud_generator

原文：https://www.cnblogs.com/chenlinchong/p/11837330.html