MedianFillFilter.cpp
Go to the documentation of this file.
1 /*
2  * MedianFillFilter.cpp
3  *
4  * Created on: September 7, 2020
5  * Author: Magnus Gärtner
6  * Institute: ETH Zurich, ANYbotics
7  */
8 
10 
11 // Wrap as ROS Filter
13 
14 #include <cmath>
15 
16 // Grid Map
18 
19 #include <opencv2/core.hpp>
20 #include <opencv2/core/eigen.hpp>
21 #include <opencv2/imgproc.hpp>
22 
23 using namespace filters;
24 
25 namespace grid_map {
26 
27 template <typename T>
28 MedianFillFilter<T>::MedianFillFilter() : fillHoleRadius_(0.05), filterExistingValues_(false) {}
29 
30 template <typename T>
32 
33 template <typename T>
35  if (!FilterBase<T>::getParam(std::string("fill_hole_radius"), fillHoleRadius_)) {
36  ROS_ERROR("Median filter did not find parameter fill_hole_radius.");
37  return false;
38  }
39 
40  if (fillHoleRadius_ < 0.0) {
41  ROS_ERROR("fill_hole_radius must be greater than zero.");
42  return false;
43  }
44 
45  ROS_DEBUG("fill_hole_radius = %f.", fillHoleRadius_);
46 
47  if (!FilterBase<T>::getParam(std::string("filter_existing_values"), filterExistingValues_)) {
48  ROS_INFO("Median filter did not find parameter filter_existing_values. Not filtering existing values.");
49  filterExistingValues_ = false;
50  }
51 
52  ROS_DEBUG("Filter_existing_values = %s.", filterExistingValues_ ? "true" : "false");
53 
54  if (!FilterBase<T>::getParam(std::string("input_layer"), inputLayer_)) {
55  ROS_ERROR("Median filter did not find parameter `input_layer`.");
56  return false;
57  }
58 
60  if (!FilterBase<T>::getParam(std::string("existing_value_radius"), existingValueRadius_)) {
61  ROS_ERROR("Median filter did not find parameter existing_value_radius.");
62  return false;
63  }
64 
65  if (existingValueRadius_ < 0.0) {
66  ROS_ERROR("existing_value_radius must be greater than zero.");
67  return false;
68  }
69 
70  ROS_DEBUG("existing_value_radius = %f.", existingValueRadius_);
71  }
72 
73  ROS_DEBUG("Median input layer is = %s.", inputLayer_.c_str());
74 
75  if (!FilterBase<T>::getParam(std::string("output_layer"), outputLayer_)) {
76  ROS_ERROR("Median filter did not find parameter `output_layer`.");
77  return false;
78  }
79 
80  ROS_DEBUG("Median output layer = %s.", outputLayer_.c_str());
81 
82  if (!FilterBase<T>::getParam(std::string("fill_mask_layer"), fillMaskLayer_)) {
83  ROS_ERROR("Median filter did not find parameter `fill_mask_layer`.");
84  return false;
85  }
86 
87  ROS_DEBUG("Median fill mask layer = %s.", fillMaskLayer_.c_str());
88 
89  if (!FilterBase<T>::getParam(std::string("debug"), debug_)) {
90  ROS_INFO("Median filter did not find parameter debug. Disabling debug output.");
91  debug_ = false;
92  }
93 
94  ROS_DEBUG("Debug mode= %s.", debug_ ? "true" : "false");
95 
96  if (debug_ && !FilterBase<T>::getParam(std::string("debug_infill_mask_layer"), debugInfillMaskLayer_)) {
97  ROS_ERROR("Median filter did not find parameter `debug_infill_mask_layer`.");
98  return false;
99  }
100 
101  ROS_DEBUG("Median debug infill mask layer = %s.", debugInfillMaskLayer_.c_str());
102 
103  return true;
104 }
105 
106 template <typename T>
107 bool MedianFillFilter<T>::update(const T& mapIn, T& mapOut) {
108  // Copy input map and add new layer to it.
109  mapOut = mapIn;
110  if (!mapOut.exists(outputLayer_)) {
111  mapOut.add(outputLayer_);
112  }
113 
114  mapOut.convertToDefaultStartIndex();
115 
116  // Avoid hash map lookups afterwards. I.e, get data matrices as references.
117  grid_map::Matrix inputMap{mapOut[inputLayer_]}; // copy by value to do filtering first.
118  grid_map::Matrix& outputMap{mapOut[outputLayer_]};
119 
120  // Check if mask is already computed from a previous iteration.
121  Eigen::MatrixXf shouldFill;
122  if (std::find(mapOut.getLayers().begin(), mapOut.getLayers().end(), fillMaskLayer_) == mapOut.getLayers().end()) {
123  shouldFill = computeAndAddFillMask(inputMap, mapOut);
124  } else { // The mask already exists, retrieve it from a previous iteration.
125  shouldFill = mapOut[fillMaskLayer_];
126  }
127 
128  const size_t radiusInPixels{static_cast<size_t>(fillHoleRadius_ / mapIn.getResolution())};
129  const size_t existingValueRadiusInPixels{static_cast<size_t>(existingValueRadius_ / mapIn.getResolution())};
130  const grid_map::Index& bufferSize{mapOut.getSize()};
131  unsigned int numNans{0u};
132  // Iterate through the entire GridMap and update NaN values with the median.
133  for (GridMapIterator iterator(mapOut); !iterator.isPastEnd(); ++iterator) {
134  const Index index(*iterator);
135  const auto& inputValue{inputMap(index(0), index(1))};
136  const float& shouldFillThisCell{shouldFill(index(0), index(1))};
137  auto& outputValue{outputMap(index(0), index(1))};
138  if (!std::isfinite(inputValue) && shouldFillThisCell) { // Fill the NaN input value with the median.
139  outputValue = getMedian(inputMap, index, radiusInPixels, bufferSize);
140  numNans++;
141  } else if (filterExistingValues_ && shouldFillThisCell) { // Value is already finite. Optionally add some filtering.
142  outputValue = getMedian(inputMap, index, existingValueRadiusInPixels, bufferSize);
143  } else { // Dont do any filtering, just take the input value.
144  outputValue = inputValue;
145  }
146  }
147  ROS_DEBUG_STREAM("Median fill filter " << this->getName() << " observed " << numNans << " Nans in input layer!");
148  // By removing all basic layers the selected layer will always be visualized, otherwise isValid will also check for the basic layers
149  // and hide infilled values where the corresponding basic layers are still NAN.
150  mapOut.setBasicLayers({});
151  return true;
152 }
153 
154 template <typename T>
155 float MedianFillFilter<T>::getMedian(Eigen::Ref<const grid_map::Matrix> inputMap, const grid_map::Index& centerIndex,
156  const size_t radiusInPixels, const grid_map::Size bufferSize) {
157  // Bound the median window to the GridMap boundaries. Calculate the neighbour patch.
158  grid_map::Index topLeftIndex{centerIndex - grid_map::Index(radiusInPixels, radiusInPixels)};
159  grid_map::Index bottomRightIndex{centerIndex + grid_map::Index(radiusInPixels, radiusInPixels)};
160  grid_map::boundIndexToRange(topLeftIndex, bufferSize);
161  grid_map::boundIndexToRange(bottomRightIndex, bufferSize);
162  const grid_map::Index neighbourPatchSize{bottomRightIndex - topLeftIndex + grid_map::Index{1, 1}};
163 
164  // Extract local neighbourhood.
165  const auto& neighbourhood{inputMap.block(topLeftIndex(0), topLeftIndex(1), neighbourPatchSize(0), neighbourPatchSize(1))};
166 
167  const size_t cols{static_cast<size_t>(neighbourhood.cols())};
168 
169  std::vector<float> cellValues;
170  cellValues.reserve(neighbourhood.rows() * neighbourhood.cols());
171 
172  for (size_t row = 0; row < neighbourhood.rows(); ++row) {
173  const auto& currentRow{neighbourhood.row(row)};
174  for (size_t col = 0; col < cols; ++col) {
175  const float& cellValue{currentRow[col]};
176  if (std::isfinite(cellValue)) { // Calculate the median of the finite neighbours.
177  cellValues.emplace_back(cellValue);
178  }
179  }
180  }
181 
182  if (cellValues.empty()) {
183  return std::numeric_limits<float>::quiet_NaN();
184  } else { // Compute the median of the finite values in the neighbourhood.
185  std::nth_element(cellValues.begin(), cellValues.begin() + cellValues.size() / 2, cellValues.end());
186  return cellValues[cellValues.size() / 2];
187  }
188 }
189 
190 template <typename T>
191 Eigen::MatrixXf MedianFillFilter<T>::computeAndAddFillMask(const Eigen::MatrixXf& inputMap, T& mapOut) {
192  Eigen::MatrixXf shouldFill;
193  // Precompute mask of valid height values
194  using MaskMatrix = Eigen::Matrix<bool, Eigen::Dynamic, Eigen::Dynamic>;
195  const MaskMatrix isValid{inputMap.array().unaryExpr([&](float v) { return std::isfinite(v); })};
196 
197  // Remove sparse valid values and fill holes.
198  cv::Mat isValidCV;
199  cv::eigen2cv(isValid, isValidCV);
200  cv::Mat_<bool> isValidOutlierRemoved{cleanedMask(isValidCV)};
201  cv::Mat shouldFillCV{fillHoles(isValidOutlierRemoved, 4)};
202 
203  // Outlier removed mask to eigen.
204  if (debug_) {
205  addCvMatAsLayer(mapOut, isValidOutlierRemoved, debugInfillMaskLayer_);
206  }
207 
208  // Convert to eigen and add to the map.
209  cv::cv2eigen(shouldFillCV, shouldFill);
210  mapOut.add(fillMaskLayer_, shouldFill);
211 
212  return shouldFill;
213 }
214 
215 template <typename T>
216 cv::Mat_<bool> MedianFillFilter<T>::cleanedMask(const cv::Mat_<bool>& inputMask) {
217  auto element{cv::getStructuringElement(cv::MORPH_RECT, cv::Size(3, 3), cv::Point(1, 1))};
218 
219  cv::Mat_<bool> cleanedInputMask(inputMask.size(), inputMask.type());
220 
221  // Erode then dilate to remove sparse points
222  cv::dilate(inputMask, cleanedInputMask, element);
223  cv::erode(cleanedInputMask, cleanedInputMask, element);
224  cv::erode(inputMask, cleanedInputMask, element);
225  cv::dilate(cleanedInputMask, cleanedInputMask, element);
226 
227  return cleanedInputMask;
228 }
229 
230 template <typename T>
231 cv::Mat_<bool> MedianFillFilter<T>::fillHoles(const cv::Mat_<bool>& isValidMask, const size_t numDilationClosingIterations) {
232  auto element{cv::getStructuringElement(cv::MORPH_RECT, cv::Size(3, 3), cv::Point(1, 1))};
233  cv::Mat_<bool> holesFilledMask(isValidMask.size(), isValidMask.type());
234  // Remove holes in the mask by morphological closing.
235  cv::dilate(isValidMask, holesFilledMask, element);
236  for (size_t iteration = 1; iteration < numDilationClosingIterations; iteration++) {
237  cv::dilate(holesFilledMask, holesFilledMask, element);
238  }
239  for (size_t iteration = 0; iteration < numDilationClosingIterations; iteration++) {
240  cv::erode(holesFilledMask, holesFilledMask, element);
241  }
242 
243  return holesFilledMask;
244 }
245 
246 template <typename T>
247 void MedianFillFilter<T>::addCvMatAsLayer(T& gridMap, const cv::Mat& cvLayer, const std::string& layerName) {
248  Eigen::MatrixXf tmpEigenMatrix;
249  cv::cv2eigen(cvLayer, tmpEigenMatrix);
250  gridMap.add(layerName, tmpEigenMatrix);
251 }
252 
253 } // namespace grid_map
254 
255 // Explicitly define the specialization for GridMap to have the filter implementation available for testing.
257 // Export the filter.
Eigen::Array2i Index
float getMedian(Eigen::Ref< const grid_map::Matrix > inputMap, const grid_map::Index &centerIndex, const size_t radiusInPixels, const grid_map::Size bufferSize)
virtual bool update(const T &mapIn, T &mapOut)
Eigen::Array2i Size
double fillHoleRadius_
Median filtering radius of NaN values in the input.
Eigen::MatrixXf Matrix
const std::string & getName()
std::string inputLayer_
Input layer name.
void addCvMatAsLayer(T &gridMap, const cv::Mat &cvLayer, const std::string &layerName)
Eigen::MatrixXf computeAndAddFillMask(const Eigen::MatrixXf &inputMap, T &mapOut)
cv::Mat_< bool > fillHoles(const cv::Mat_< bool > &isValidMask, const size_t numDilationClosingIterations)
std::string debugInfillMaskLayer_
Layer used to visualize the intermediate, sparse outlier removed fill mask.
#define ROS_INFO(...)
cv::Mat_< bool > cleanedMask(const cv::Mat_< bool > &inputMask)
#define ROS_DEBUG_STREAM(args)
std::string outputLayer_
Output layer name.
bool debug_
If set, the filtered grid_map is augmented with additional debug layers.
void boundIndexToRange(Index &index, const Size &bufferSize)
bool filterExistingValues_
Flag indicating whether to also filter finite values.
std::string fillMaskLayer_
Layer containing indicating which areas to fill, will be computed if not present. ...
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
#define ROS_ERROR(...)
double existingValueRadius_
Median filtering radius for existing values in the input.
#define ROS_DEBUG(...)


grid_map_filters
Author(s): Péter Fankhauser , Martin Wermelinger
autogenerated on Fri Feb 19 2021 03:15:18