threshold_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 #include <Eigen/Core>
10 
11 using namespace grid_map;
12 using namespace ::testing;
13 
16 
17 #define ASSERT_MATRICES_EQ_WITH_NAN(first, second) assertMatrixesEqualWithNan((first), #first, (second), #second, __LINE__)
18 static void assertMatrixesEqualWithNan(Eigen::Ref<const Eigen::MatrixXf> first, std::string firstName, Eigen::Ref<const Eigen::MatrixXf> second, std::string secondName, int line){
19  ASSERT_EQ(first.rows(), second.rows());
20  ASSERT_EQ(first.cols(), second.cols());
21 
22  bool matricesAreEqual = true;
23  for(size_t row = 0; row < first.rows() && matricesAreEqual; ++row){
24  for(size_t col = 0; col < first.cols() && matricesAreEqual; ++col){
25  bool ifRealThenValid = first.block<1, 1>(row, col).isApprox(second.block<1, 1>(row, col));
26  bool bothNaN = std::isnan(first(row, col)) && std::isnan(second(row, col));
27  if(ifRealThenValid || bothNaN){
28  continue;
29  }else{
30  matricesAreEqual = false;
31  }
32  }
33  }
34 
35  Eigen::IOFormat compactFormat(2, 0, ",", "\n", "[", "]");
36  ASSERT_TRUE(matricesAreEqual) // NO LINT
37  << "L. " << std::to_string(line) << ": Matrices are not equal" // NO LINT
38  << "\n"<<firstName<<"\n" // NO LINT
39  << first.format(compactFormat) // NO LINT
40  << "\n"<<secondName<<"\n" // NO LINT
41  << second.format(compactFormat) << "\n"; // NO LINT
42 
43 }
44 
45 TEST(ThresholdFilter, LoadParametersAndUpdateTest) { // NOLINT
46  ThresholdFilterT thresholdFilter{};
47 
48  // Set up the parameters
49  XmlRpc::XmlRpcValue config;
50  config["name"] = "threshold_filter";
51  config["type"] = "gridMapFilters/ThresholdFilter";
52 
53  XmlRpc::XmlRpcValue params;
54  params["condition_layer"] = "standard_deviation";
55  params["output_layer"] = "standard_deviation_filtered";
56  params["upper_threshold"] = 0.05;
57  params["set_to"] = NAN;
58 
59  config["params"] = params;
60 
61  thresholdFilter.BASE::configure(config);
62  thresholdFilter.configure();
63 
64  // Set up some test data
65 
66  // General grid map geometry.
67  GridMap filterInput = GridMap({"standard_deviation", "standard_deviation_filtered"});
68  filterInput.setGeometry(Length(0.4, 0.4), 0.02, Position(1.0, 5.0));
69  filterInput.setFrameId("map");
70 
71  // Set the condition layer to be generally within the threshold except two corners.
72  Matrix& conditionLayer = filterInput["standard_deviation"];
73  conditionLayer.setConstant(0.03);
74  conditionLayer.bottomRightCorner<5, 5>().setConstant(0.06);
75  conditionLayer.topLeftCorner<3, 3>().setConstant(NAN);
76 
77  // Initialize the output layer with ones.
78  Matrix& outputLayer = filterInput["standard_deviation_filtered"];
79  outputLayer.setConstant(1.0);
80 
81  // Setup the expected output, eg all values unchanged except the two corners set to nan.
82  Matrix outputLayerExpected{outputLayer.rows(), outputLayer.cols()};
83  outputLayerExpected.setConstant(1.0f);
84  outputLayerExpected.topLeftCorner<3, 3>().setConstant(NAN);
85  outputLayerExpected.bottomRightCorner<5, 5>().setConstant(NAN);
86 
87  // Run the filter.
88  GridMap filterOutput;
89  thresholdFilter.update(filterInput, filterOutput);
90 
91  // Check the layers existent in the output and the expected outputs of them.
92  ASSERT_THAT(filterOutput.getLayers(), ElementsAre(StrEq("standard_deviation"), StrEq("standard_deviation_filtered")));
93  ASSERT_MATRICES_EQ_WITH_NAN(conditionLayer, filterOutput["standard_deviation"]);
94  ASSERT_MATRICES_EQ_WITH_NAN(filterOutput["standard_deviation_filtered"], outputLayerExpected);
95 }
void setGeometry(const Length &length, const double resolution, const Position &position=Position::Zero())
f
Eigen::MatrixXf Matrix
TEST(ThresholdFilter, LoadParametersAndUpdateTest)
static void assertMatrixesEqualWithNan(Eigen::Ref< const Eigen::MatrixXf > first, std::string firstName, Eigen::Ref< const Eigen::MatrixXf > second, std::string secondName, int line)
const std::vector< std::string > & getLayers() const
Eigen::Vector2d Position
void setFrameId(const std::string &frameId)
#define ASSERT_MATRICES_EQ_WITH_NAN(first, second)
Eigen::Array2d Length


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