1 #include <gmock/gmock.h> 2 #include <gtest/gtest.h> 12 using namespace ::testing;
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());
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){
30 matricesAreEqual =
false;
35 Eigen::IOFormat compactFormat(2, 0,
",",
"\n",
"[",
"]");
36 ASSERT_TRUE(matricesAreEqual)
37 <<
"L. " << std::to_string(line) <<
": Matrices are not equal" 38 <<
"\n"<<firstName<<
"\n" 39 << first.format(compactFormat)
40 <<
"\n"<<secondName<<
"\n" 41 << second.format(compactFormat) <<
"\n";
50 config[
"name"] =
"threshold_filter";
51 config[
"type"] =
"gridMapFilters/ThresholdFilter";
54 params[
"condition_layer"] =
"standard_deviation";
55 params[
"output_layer"] =
"standard_deviation_filtered";
56 params[
"upper_threshold"] = 0.05;
57 params[
"set_to"] = NAN;
59 config[
"params"] = params;
61 thresholdFilter.BASE::configure(config);
62 thresholdFilter.configure();
67 GridMap filterInput =
GridMap({
"standard_deviation",
"standard_deviation_filtered"});
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);
78 Matrix& outputLayer = filterInput[
"standard_deviation_filtered"];
79 outputLayer.setConstant(1.0);
82 Matrix outputLayerExpected{outputLayer.rows(), outputLayer.cols()};
83 outputLayerExpected.setConstant(1.0
f);
84 outputLayerExpected.topLeftCorner<3, 3>().setConstant(NAN);
85 outputLayerExpected.bottomRightCorner<5, 5>().setConstant(NAN);
89 thresholdFilter.update(filterInput, filterOutput);
92 ASSERT_THAT(filterOutput.
getLayers(), ElementsAre(StrEq(
"standard_deviation"), StrEq(
"standard_deviation_filtered")));
void setGeometry(const Length &length, const double resolution, const Position &position=Position::Zero())
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
void setFrameId(const std::string &frameId)
#define ASSERT_MATRICES_EQ_WITH_NAN(first, second)