1 #include <gmock/gmock.h> 2 #include <gtest/gtest.h> 10 using namespace ::testing;
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());
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){
28 matricesAreEqual =
false;
33 Eigen::IOFormat compactFormat(2, 0,
",",
"\n",
"[",
"]");
34 ASSERT_TRUE(matricesAreEqual)
35 <<
"L. " << std::to_string(line) <<
": Matrices are not equal" 36 <<
"\n"<<firstName<<
"\n" 37 << first.format(compactFormat)
38 <<
"\n"<<secondName<<
"\n" 39 << second.format(compactFormat) <<
"\n";
53 config[
"name"] =
"median";
54 config[
"type"] =
"gridMapFilters/MedianFillFilter";
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;
65 config[
"params"] = params;
67 medianFillFilter.BASE::configure(config);
68 medianFillFilter.configure();
75 Matrix& elevationLayer = filterInput[
"elevation"];
76 Matrix& varianceLayer = filterInput[
"variance"];
78 elevationLayer.setConstant(1);
79 varianceLayer.setConstant(0.05);
82 medianFillFilter.update(filterInput, filterOutput);
85 ElementsAre(StrEq(
"elevation"), StrEq(
"variance"), StrEq(
"elevation_filtered"), StrEq(
"fill_mask")));
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"];
98 filterInput[
"elevation"].bottomRightCorner<5,5>().setConstant(NAN);
100 GridMap noisyFilterInput{filterInput};
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();
113 medianFillFilter.update(noisyFilterInput, noisyFilterOutput);
void setGeometry(const Length &length, const double resolution, const Position &position=Position::Zero())
const std::vector< std::string > & getLayers() const
void setFrameId(const std::string &frameId)