median_fill_filter_test.cpp
Go to the documentation of this file.
1 #include <gmock/gmock.h>
2 #include <gtest/gtest.h>
3 
4 #include <filters/filter_base.h>
6 
8 
9 using namespace grid_map;
10 using namespace ::testing;
11 
14 
15 #define ASSERT_MATRICES_EQ_WITH_NAN(first, second) assertMatrixesEqualWithNan((first), #first, (second), #second, __LINE__)
16 static void assertMatrixesEqualWithNan(Eigen::Ref<const Eigen::MatrixXf> first, std::string firstName, Eigen::Ref<const Eigen::MatrixXf> second, std::string secondName, int line){
17  ASSERT_EQ(first.rows(), second.rows());
18  ASSERT_EQ(first.cols(), second.cols());
19 
20  bool matricesAreEqual = true;
21  for(size_t row = 0; row < first.rows() && matricesAreEqual; ++row){
22  for(size_t col = 0; col < first.cols() && matricesAreEqual; ++col){
23  bool ifRealThenValid = first.block<1, 1>(row, col).isApprox(second.block<1, 1>(row, col));
24  bool bothNaN = std::isnan(first(row, col)) && std::isnan(second(row, col));
25  if(ifRealThenValid || bothNaN){
26  continue;
27  }else{
28  matricesAreEqual = false;
29  }
30  }
31  }
32 
33  Eigen::IOFormat compactFormat(2, 0, ",", "\n", "[", "]");
34  ASSERT_TRUE(matricesAreEqual) // NO LINT
35  << "L. " << std::to_string(line) << ": Matrices are not equal" // NO LINT
36  << "\n"<<firstName<<"\n" // NO LINT
37  << first.format(compactFormat) // NO LINT
38  << "\n"<<secondName<<"\n" // NO LINT
39  << second.format(compactFormat) << "\n"; // NO LINT
40 
41 }
42 
43 TEST(MedianFillFilter, ConstructFilterTest) { // NOLINT
44  MedianFillFilterT medianFillFilter{};
45  SUCCEED();
46 }
47 
48 TEST(MedianFillFilter, LoadParametersAndUpdateTest) { // NOLINT
49  MedianFillFilterT medianFillFilter{};
50 
51  // Set up the parameters
52  XmlRpc::XmlRpcValue config;
53  config["name"] = "median";
54  config["type"] = "gridMapFilters/MedianFillFilter";
55 
56  XmlRpc::XmlRpcValue params;
57  params["input_layer"] = "elevation";
58  params["output_layer"] = "elevation_filtered";
59  params["fill_hole_radius"] = 0.02;
60  params["filter_existing_values"] = true;
61  params["existing_value_radius"] = 0.02;
62  params["fill_mask_layer"] = "fill_mask";
63  params["debug"] = false;
64 
65  config["params"] = params;
66 
67  medianFillFilter.BASE::configure(config);
68  medianFillFilter.configure();
69 
70  // Set up some test data
71  GridMap filterInput = GridMap({"elevation", "variance"});
72  filterInput.setGeometry(Length(0.4, 0.4), 0.02, Position(1.0, 5.0));
73  filterInput.setFrameId("map");
74 
75  Matrix& elevationLayer = filterInput["elevation"];
76  Matrix& varianceLayer = filterInput["variance"];
77 
78  elevationLayer.setConstant(1);
79  varianceLayer.setConstant(0.05);
80 
81  GridMap filterOutput;
82  medianFillFilter.update(filterInput, filterOutput);
83 
84  ASSERT_THAT(filterOutput.getLayers(),
85  ElementsAre(StrEq("elevation"), StrEq("variance"), StrEq("elevation_filtered"), StrEq("fill_mask")));
86 
87  ASSERT_TRUE(filterInput["elevation"].isApprox(filterOutput["elevation"]))
88  << "Expected output:\n " << filterInput["elevation"] << "\nReceived:\n " << filterOutput["elevation"];
89  ASSERT_TRUE(filterInput["variance"].isApprox(filterOutput["variance"]))
90  << "Expected output:\n " << filterInput["variance"] << "\nReceived:\n " << filterOutput["variance"];
91  ASSERT_TRUE(filterOutput["fill_mask"].isApprox(Matrix::Ones(filterOutput.getSize().x(), filterOutput.getSize().y())))
92  << "Expected output:\n " << Matrix::Ones(filterOutput.getSize().x(), filterOutput.getSize().y()) << "\nReceived:\n "
93  << filterOutput["fill_mask"];
94 
95  // Now we add some NaNs and see if they are filtered out.
96 
97  // add a large unobserved area that should not be filled arbitrarily
98  filterInput["elevation"].bottomRightCorner<5,5>().setConstant(NAN);
99 
100  GridMap noisyFilterInput{filterInput};
101 
102  // Add some holes that should be filled.
103  noisyFilterInput["elevation"](0, 0) = std::numeric_limits<float>::quiet_NaN();
104  noisyFilterInput["elevation"](10, 5) = std::numeric_limits<float>::quiet_NaN();
105  noisyFilterInput["elevation"](2, 4) = std::numeric_limits<float>::quiet_NaN();
106  noisyFilterInput["elevation"](6, 3) = std::numeric_limits<float>::quiet_NaN();
107  noisyFilterInput["elevation"](11, 8) = std::numeric_limits<float>::quiet_NaN();
108  noisyFilterInput["elevation"](3, 15) = std::numeric_limits<float>::quiet_NaN();
109  noisyFilterInput["elevation"](10, 10) = std::numeric_limits<float>::quiet_NaN();
110 
111  // Check if the filter has removed the sparse nans by comparing it with the original input.
112  GridMap noisyFilterOutput;
113  medianFillFilter.update(noisyFilterInput, noisyFilterOutput);
114  ASSERT_MATRICES_EQ_WITH_NAN(filterInput["elevation"], noisyFilterOutput["elevation_filtered"]);
115 }
void setGeometry(const Length &length, const double resolution, const Position &position=Position::Zero())
static void assertMatrixesEqualWithNan(Eigen::Ref< const Eigen::MatrixXf > first, std::string firstName, Eigen::Ref< const Eigen::MatrixXf > second, std::string secondName, int line)
TEST(MedianFillFilter, ConstructFilterTest)
Eigen::MatrixXf Matrix
#define ASSERT_MATRICES_EQ_WITH_NAN(first, second)
const std::vector< std::string > & getLayers() const
Eigen::Vector2d Position
int SUCCEED
void setFrameId(const std::string &frameId)
Eigen::Array2d Length


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