19 #include <opencv2/core.hpp> 20 #include <opencv2/core/eigen.hpp> 21 #include <opencv2/imgproc.hpp> 36 ROS_ERROR(
"Median filter did not find parameter fill_hole_radius.");
41 ROS_ERROR(
"fill_hole_radius must be greater than zero.");
48 ROS_INFO(
"Median filter did not find parameter filter_existing_values. Not filtering existing values.");
55 ROS_ERROR(
"Median filter did not find parameter `input_layer`.");
61 ROS_ERROR(
"Median filter did not find parameter existing_value_radius.");
66 ROS_ERROR(
"existing_value_radius must be greater than zero.");
76 ROS_ERROR(
"Median filter did not find parameter `output_layer`.");
83 ROS_ERROR(
"Median filter did not find parameter `fill_mask_layer`.");
90 ROS_INFO(
"Median filter did not find parameter debug. Disabling debug output.");
97 ROS_ERROR(
"Median filter did not find parameter `debug_infill_mask_layer`.");
106 template <
typename T>
114 mapOut.convertToDefaultStartIndex();
121 Eigen::MatrixXf shouldFill;
122 if (std::find(mapOut.getLayers().begin(), mapOut.getLayers().end(),
fillMaskLayer_) == mapOut.getLayers().end()) {
128 const size_t radiusInPixels{
static_cast<size_t>(
fillHoleRadius_ / mapIn.getResolution())};
129 const size_t existingValueRadiusInPixels{
static_cast<size_t>(
existingValueRadius_ / mapIn.getResolution())};
131 unsigned int numNans{0u};
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) {
139 outputValue =
getMedian(inputMap, index, radiusInPixels, bufferSize);
142 outputValue =
getMedian(inputMap, index, existingValueRadiusInPixels, bufferSize);
144 outputValue = inputValue;
147 ROS_DEBUG_STREAM(
"Median fill filter " << this->
getName() <<
" observed " << numNans <<
" Nans in input layer!");
150 mapOut.setBasicLayers({});
154 template <
typename T>
165 const auto& neighbourhood{inputMap.block(topLeftIndex(0), topLeftIndex(1), neighbourPatchSize(0), neighbourPatchSize(1))};
167 const size_t cols{
static_cast<size_t>(neighbourhood.cols())};
169 std::vector<float> cellValues;
170 cellValues.reserve(neighbourhood.rows() * neighbourhood.cols());
172 for (Eigen::Index 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)) {
177 cellValues.emplace_back(cellValue);
182 if (cellValues.empty()) {
183 return std::numeric_limits<float>::quiet_NaN();
185 std::nth_element(cellValues.begin(), cellValues.begin() + cellValues.size() / 2, cellValues.end());
186 return cellValues[cellValues.size() / 2];
190 template <
typename T>
192 Eigen::MatrixXf shouldFill;
194 using MaskMatrix = Eigen::Matrix<bool, Eigen::Dynamic, Eigen::Dynamic>;
195 const MaskMatrix isValid{inputMap.array().unaryExpr([&](
float v) {
return std::isfinite(v); })};
199 cv::eigen2cv(isValid, isValidCV);
200 cv::Mat_<bool> isValidOutlierRemoved{
cleanedMask(isValidCV)};
201 cv::Mat shouldFillCV{
fillHoles(isValidOutlierRemoved, 4)};
209 cv::cv2eigen(shouldFillCV, shouldFill);
215 template <
typename T>
217 auto element{cv::getStructuringElement(cv::MORPH_RECT, cv::Size(3, 3), cv::Point(1, 1))};
219 cv::Mat_<bool> cleanedInputMask(inputMask.size(), inputMask.type());
222 cv::dilate(inputMask, cleanedInputMask, element);
223 cv::erode(cleanedInputMask, cleanedInputMask, element);
224 cv::erode(inputMask, cleanedInputMask, element);
225 cv::dilate(cleanedInputMask, cleanedInputMask, element);
227 return cleanedInputMask;
230 template <
typename T>
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());
235 cv::dilate(isValidMask, holesFilledMask, element);
236 for (
size_t iteration = 1; iteration < numDilationClosingIterations; iteration++) {
237 cv::dilate(holesFilledMask, holesFilledMask, element);
239 for (
size_t iteration = 0; iteration < numDilationClosingIterations; iteration++) {
240 cv::erode(holesFilledMask, holesFilledMask, element);
243 return holesFilledMask;
246 template <
typename T>
248 Eigen::MatrixXf tmpEigenMatrix;
249 cv::cv2eigen(cvLayer, tmpEigenMatrix);
250 gridMap.add(layerName, tmpEigenMatrix);
const std::string & getName()
#define ROS_DEBUG_STREAM(args)
void boundIndexToRange(Index &index, const Size &bufferSize)
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)