Program Listing for File occupancy-grid.h

Return to documentation for file (aslam_cv2/aslam_cv_common/include/aslam/common/occupancy-grid.h)

#ifndef ASLAM_FEATURE_TRACKER_OCCUPANCY_GRID_H_
#define ASLAM_FEATURE_TRACKER_OCCUPANCY_GRID_H_

#include <vector>

#include <aslam/common/macros.h>
#include <Eigen/Dense>
#include <glog/logging.h>
#include <opencv2/core/core.hpp>

namespace aslam {
namespace common {

template<typename CoordinatesType = double, typename WeightType = double, typename PointId = int>
struct WeightedKeypoint {
  typedef CoordinatesType coordinate_type;
  typedef WeightType weight_type;
  typedef PointId id_type;

  WeightedKeypoint() {
    LOG(FATAL) << "Only needed to downsize vectors using resize.";
  }
  WeightedKeypoint(CoordinatesType u_rows, CoordinatesType v_cols, WeightType weight, PointId id)
    : u_rows(u_rows), v_cols(v_cols), weight(weight), id(id) {}

  CoordinatesType u_rows, v_cols;
  WeightType weight;
  PointId id;

  inline bool operator<(const WeightedKeypoint& other) const {
    return weight < other.weight;
  }
  inline bool operator>(const WeightedKeypoint& other) const {
    return weight > other.weight;
  }
};

template<typename PointType = WeightedKeypoint<double, double, int>>
class WeightedOccupancyGrid {
 public:
  typedef typename PointType::coordinate_type CoordinatesType;
  typedef typename PointType::weight_type WeightType;
  typedef typename PointType::id_type PointId;

  typedef PointType Point;
  typedef std::vector<PointType> PointList;

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  ASLAM_POINTER_TYPEDEFS(WeightedOccupancyGrid);

 private:
  struct GridCoordinates {
    GridCoordinates(size_t i, size_t j) : i_rows(i), j_cols(j) {}
    size_t i_rows, j_cols;
  };

 public:
  WeightedOccupancyGrid(CoordinatesType max_input_coordinate_rows,
                        CoordinatesType max_input_coordinate_cols,
                        CoordinatesType cell_size_rows,
                        CoordinatesType cell_size_cols);
  void reset();

 private:
  void initializeGrid();

 public:
  void addPointUnconditional(const PointType& point);

  bool addPointOrReplaceWeakestIfCellFull(const PointType& point, size_t max_points_per_cell);

  void addPointOrReplaceWeakestNearestPoints(const PointType& point, CoordinatesType min_distance);

 public:
  size_t getNumPoints() const;
  size_t getAllPointsInGrid(PointList* points) const;

  inline PointList& getGridCell(CoordinatesType u_rows, CoordinatesType v_cols);
  inline const PointList& getGridCell(CoordinatesType u_rows, CoordinatesType v_cols) const;

  inline GridCoordinates getFullestGridCell() const;

  cv::Mat getOccupancyMask(CoordinatesType radius_mask_around_points,
                           size_t max_points_per_cell) const;

 private:
  inline PointList& getGridCell(const GridCoordinates& grid_coordinates);
  inline const PointList& getGridCell(const GridCoordinates& grid_coordinates) const;

 public:
  void setConstantWeightForAllPointsInGrid(WeightType weight);

  size_t removeWeightedPointsFromOverfullCells(size_t max_points_per_cell);

  size_t removeWeightedPointsFromOverfullCell(
      const GridCoordinates& grid_coords, size_t max_points_per_cell);

  void removePointsFromFullestCellsUntilSize(size_t max_total_num_points);

 private:
  GridCoordinates inputToGridCoordinates(CoordinatesType u_rows, CoordinatesType v_cols);
  inline bool isValidInputCoordinate(CoordinatesType u_rows, CoordinatesType v_cols) const;
  inline bool isValidGridCoordinates(const GridCoordinates& grid_coordinates) const;

  const CoordinatesType max_input_coordinate_rows_;
  const CoordinatesType max_input_coordinate_cols_;
  const CoordinatesType cell_size_rows_;
  const CoordinatesType cell_size_cols_;

  size_t num_grid_rows_;
  size_t num_grid_cols_;
  size_t current_num_points_;

  std::vector<std::vector<PointList>> grid_;
};

}  // namespace common
}  // namespace aslam
#include "./occupancy-grid-inl.h"
#endif  // ASLAM_FEATURE_TRACKER_OCCUPANCY_GRID_H_