Program Listing for File GridMapPclLoader.hpp

↰ Return to documentation for file (include/grid_map_pcl/GridMapPclLoader.hpp)

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

#ifndef GRID_MAP_PCL__GRIDMAPPCLLOADER_HPP_
#define GRID_MAP_PCL__GRIDMAPPCLLOADER_HPP_

#include <pcl/features/normal_3d.h>
#include <pcl/surface/gp3.h>
#include <array>
#include <memory>
#include <vector>
#include <string>
#include "grid_map_core/GridMap.hpp"
#include "grid_map_core/iterators/GridMapIterator.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;

  struct ClusterParameters
  {
    Eigen::Vector3d mean_;
  };

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

  explicit GridMapPclLoader(
    const rclcpp::Logger & node_logger);
  ~GridMapPclLoader();

  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);

private:
  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) const;

  double calculateElevationFromPointsInsideGridMapCell(Pointcloud::ConstPtr cloud) 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_;

  // 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.
  std::unique_ptr<grid_map_pcl::PclLoaderParameters> params_;

  // Logging interface
  rclcpp::Logger node_logger_;

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

}  // namespace grid_map
#endif  // GRID_MAP_PCL__GRIDMAPPCLLOADER_HPP_