SlidingWindowIterator.cpp
Go to the documentation of this file.
1 /*
2  * SlidingWindowIterator.cpp
3  *
4  * Created on: Aug 17, 2017
5  * Author: Péter Fankhauser
6  * Institute: ETH Zurich, ANYbotics
7  */
8 
11 
12 #include <iostream>
13 
14 namespace grid_map {
15 
16 SlidingWindowIterator::SlidingWindowIterator(const GridMap& gridMap, const std::string& layer,
17  const EdgeHandling& edgeHandling, const size_t windowSize)
18  : GridMapIterator(gridMap),
19  edgeHandling_(edgeHandling),
20  data_(gridMap[layer])
21 {
22  windowSize_ = windowSize;
23  setup(gridMap);
24 }
25 
27  : GridMapIterator(other),
29  data_(other->data_)
30 {
31  windowSize_ = other->windowSize_;
33 }
34 
35 void SlidingWindowIterator::setWindowLength(const GridMap& gridMap, const double windowLength)
36 {
37  windowSize_ = std::round(windowLength / gridMap.getResolution());
38  if (windowSize_ % 2 != 1) ++windowSize_;
39  setup(gridMap);
40 }
41 
43 {
45  while (!isPastEnd()) {
47  if (dataInsideMap()) break;
48  }
49  } else {
51  }
52  return *this;
53 }
54 
56 {
57  const Index centerIndex(*(*this));
58  const Index windowMargin(Index::Constant(windowMargin_));
59  const Index originalTopLeftIndex(centerIndex - windowMargin);
60  Index topLeftIndex(originalTopLeftIndex);
61  boundIndexToRange(topLeftIndex, size_);
62  Index bottomRightIndex(centerIndex + windowMargin);
63  boundIndexToRange(bottomRightIndex, size_);
64  const Size adjustedWindowSize(bottomRightIndex - topLeftIndex + Size::Ones());
65 
66  switch (edgeHandling_) {
68  case EdgeHandling::CROP:
69  return data_.block(topLeftIndex(0), topLeftIndex(1), adjustedWindowSize(0), adjustedWindowSize(1));
71  case EdgeHandling::MEAN:
72  const Matrix data = data_.block(topLeftIndex(0), topLeftIndex(1), adjustedWindowSize(0), adjustedWindowSize(1));
73  Matrix returnData(windowSize_, windowSize_);
74  if (edgeHandling_ == EdgeHandling::EMPTY) returnData.setConstant(NAN);
75  else if (edgeHandling_ == EdgeHandling::MEAN) returnData.setConstant(data.meanOfFinites());
76  const Index topLeftIndexShift(topLeftIndex - originalTopLeftIndex);
77  returnData.block(topLeftIndexShift(0), topLeftIndexShift(1), adjustedWindowSize(0), adjustedWindowSize(1)) =
78  data_.block(topLeftIndex(0), topLeftIndex(1), adjustedWindowSize(0), adjustedWindowSize(1));
79  return returnData;
80  }
81  return Matrix::Zero(0, 0);
82 }
83 
85 {
86  if (!gridMap.isDefaultStartIndex()) throw std::runtime_error(
87  "SlidingWindowIterator cannot be used with grid maps that don't have a default buffer start index.");
88  if (windowSize_ % 2 == 0) throw std::runtime_error(
89  "SlidingWindowIterator has a wrong window size!");
90  windowMargin_ = (windowSize_ - 1) / 2;
91 
93  if (!dataInsideMap()) {
94  operator++();
95  }
96  }
97 }
98 
100 {
101  const Index centerIndex(*(*this));
102  const Index windowMargin(Index::Constant(windowMargin_));
103  const Index topLeftIndex(centerIndex - windowMargin);
104  const Index bottomRightIndex(centerIndex + windowMargin);
105  return checkIfIndexInRange(topLeftIndex, size_) && checkIfIndexInRange(bottomRightIndex, size_);
106 }
107 
108 } /* namespace grid_map */
bool dataInsideMap() const
Check if data for current index is fully inside map.
const EdgeHandling edgeHandling_
Edge handling method.
Eigen::Array2i Index
Definition: TypeDefs.hpp:22
size_t windowSize_
Size of the sliding window.
size_t windowMargin_
Size of the border of the window around the center cell.
Eigen::Array2i Size
Definition: TypeDefs.hpp:23
Eigen::MatrixXf Matrix
Definition: TypeDefs.hpp:16
SlidingWindowIterator(const GridMap &gridMap, const std::string &layer, const EdgeHandling &edgeHandling=EdgeHandling::CROP, const size_t windowSize=3)
double getResolution() const
Definition: GridMap.cpp:665
SlidingWindowIterator & operator++()
Size size_
Size of the buffer.
bool isDefaultStartIndex() const
Definition: GridMap.cpp:684
void setup(const GridMap &gridMap)
Setup members.
void boundIndexToRange(Index &index, const Size &bufferSize)
bool checkIfIndexInRange(const Index &index, const Size &bufferSize)
void setWindowLength(const GridMap &gridMap, const double windowLength)
virtual GridMapIterator & operator++()


grid_map_core
Author(s): Péter Fankhauser
autogenerated on Tue Jun 25 2019 20:02:08