GridMapPclLoader.hpp

Go to the documentation of this file.

/*
 * GridMapPclLoader.hpp
 *
 *  Created on: Aug 26, 2019
 *      Author: Edo Jelavic
 *      Institute: ETH Zurich, Robotic Systems Lab
 */

#pragma once

#include <array>
#include <memory>
#include <string>

#include <grid_map_core/GridMap.hpp>

#include "grid_map_pcl/PclLoaderParameters.hpp"
#include "grid_map_pcl/PointcloudProcessor.hpp"

namespace grid_map {

namespace grid_map_pcl_test {
class GridMapPclLoaderTest_CalculateElevation_Test;
}
/*
 * Computes elevation of a grid map from a raw point cloud.
 * The algorithm divides the point cloud into cells (in x and y direction)
 * that are the same size as the grid map cells. Then it looks for clusters
 * of points in each of those cells and computes their mean. Mean of a cluster
 * with minimal z value out of all clusters inside a cell is taken to
 * be the elevation in the grid map. This allows the algorithm to also work
 * indoors. All the calculations are performed in the point cloud frame.
 */

class GridMapPclLoader {
  friend class grid_map_pcl_test::GridMapPclLoaderTest_CalculateElevation_Test;

 public:
  using Point = ::pcl::PointXYZ;
  using Pointcloud = ::pcl::PointCloud<Point>;

  GridMapPclLoader() = default;
  ~GridMapPclLoader() = default;

  void loadCloudFromPcdFile(const std::string& filename);

  void setInputCloud(Pointcloud::ConstPtr inputCloud);

  void preProcessInputCloud();

  void initializeGridMapGeometryFromInputCloud();

  void addLayerFromInputCloud(const std::string& layer);

  const grid_map::GridMap& getGridMap() const;

  void savePointCloudAsPcdFile(const std::string& filename) const;

  void loadParameters(const std::string& filename);

  void setParameters(const grid_map_pcl::PclLoaderParameters::Parameters parameters);

  const grid_map_pcl::PclLoaderParameters::Parameters& getParameters() const { return params_.get(); }

  grid_map::GridMap& getWorkingGridMap() { return workingGridMap_; }

  const std::vector<std::vector<std::vector<float>>>& getClusterHeightsWithingGridMapCell() const {
    return clusterHeightsWithingGridMapCell_;
  }

 protected:
  void setWorkingCloud(Pointcloud::ConstPtr workingCloud);

  void setRawInputCloud(Pointcloud::ConstPtr rawInputCloud);

  // processing the grid map

  Pointcloud::Ptr getPointcloudInsideGridMapCellBorder(const grid_map::Index& index) const;

  void processGridMapCell(const unsigned int linearGridMapIndex, grid_map::Matrix* gridMapData);

  void calculateElevationFromPointsInsideGridMapCell(Pointcloud::ConstPtr cloud, std::vector<float>& heights) const;

  void preprocessGridMapCells();

  void allocateSpaceForCloudsInsideCells();

  void dispatchWorkingCloudToGridMapCells();

  // Matrix of point clouds. Each point cloud has only points that fall within a grid map cell.
  std::vector<std::vector<Pointcloud::Ptr>> pointcloudWithinGridMapCell_;

  // Matrix of cluster-height-vectors. Dimensions: x, y, cluster_index.
  std::vector<std::vector<std::vector<float>>> clusterHeightsWithingGridMapCell_;

  // Point cloud that pcl filters have been applied to (it can change).
  Pointcloud::Ptr workingCloud_;

  // Copy of the raw point cloud. This cloud will not be changed by pcl filters.
  Pointcloud::Ptr rawInputCloud_;

  // Grid map computed from working point cloud.
  grid_map::GridMap workingGridMap_;

  // Parameters for the algorithm. Also includes parameters for the pcl filters.
  grid_map_pcl::PclLoaderParameters params_;

  // Class that handles point cloud processing
  grid_map_pcl::PointcloudProcessor pointcloudProcessor_;
};

}  // namespace grid_map