00001
00002
00003
00004
00005
00006
00007
00008
00009 #include "grid_map_core/grid_map_core.hpp"
00010
00011
00012 #include <gtest/gtest.h>
00013
00014
00015 #include <Eigen/Core>
00016
00017 using namespace std;
00018 using namespace Eigen;
00019
00020 TEST(EigenMatrixBaseAddons, numberOfFinites)
00021 {
00022 Eigen::Matrix3f matrix(Eigen::Matrix3f::Ones());
00023 matrix(0, 0) = NAN;
00024 matrix(1, 0) = NAN;
00025 EXPECT_EQ(7, matrix.numberOfFinites());
00026
00027 Matrix<double, 13, 10> matrix2;
00028 matrix2.setOnes();
00029 EXPECT_EQ(matrix2.rows() * matrix2.cols(), matrix2.numberOfFinites());
00030
00031 Matrix<double, 13, 10> matrix3;
00032 matrix3.setConstant(NAN);
00033 matrix3.col(3).setConstant(0.0);
00034 EXPECT_EQ(matrix3.rows(), matrix3.numberOfFinites());
00035 }
00036
00037 TEST(EigenMatrixBaseAddons, sumOfFinites)
00038 {
00039 Matrix<double, 7, 18> matrix;
00040 matrix.setRandom();
00041 EXPECT_NEAR(matrix.sum(), matrix.sumOfFinites(), 1e-10);
00042 double finiteSum = matrix.sum() - matrix(0, 0) - matrix(1, 2) - matrix(3, 6) - matrix(6, 12);
00043 matrix(0, 0) = NAN;
00044 matrix(1, 2) = NAN;
00045 matrix(3, 6) = NAN;
00046 matrix(6, 12) = NAN;
00047 EXPECT_NEAR(finiteSum, matrix.sumOfFinites(), 1e-10);
00048 matrix.setConstant(NAN);
00049 EXPECT_TRUE(std::isnan(matrix.sumOfFinites()));
00050 matrix(5, 7) = 1.0;
00051 EXPECT_NEAR(1.0, matrix.sumOfFinites(), 1e-10);
00052 }
00053
00054 TEST(EigenMatrixBaseAddons, meanOfFinites)
00055 {
00056 Eigen::Matrix3f matrix(Eigen::Matrix3f::Ones());
00057 matrix(0, 0) = NAN;
00058 matrix(1, 1) = NAN;
00059 EXPECT_DOUBLE_EQ(1.0, matrix.meanOfFinites());
00060
00061 Matrix<double, 13, 10> matrix2;
00062 matrix2.setRandom();
00063 EXPECT_NEAR(matrix2.mean(), matrix2.meanOfFinites(), 1e-10);
00064 }
00065
00066 TEST(EigenMatrixBaseAddons, minCoeffOfFinites)
00067 {
00068 Matrix<double, 7, 18> matrix;
00069 matrix.setRandom();
00070 double min = matrix.minCoeff();
00071 EXPECT_NEAR(min, matrix.minCoeffOfFinites(), 1e-10);
00072
00073 int i, j;
00074 matrix.maxCoeff(&i, &j);
00075 matrix(i, j) = NAN;
00076 EXPECT_NEAR(min, matrix.minCoeffOfFinites(), 1e-10);
00077
00078 matrix.setConstant(NAN);
00079 EXPECT_TRUE(std::isnan(matrix.minCoeffOfFinites()));
00080 matrix(i, j) = -1.0;
00081 EXPECT_NEAR(-1.0, matrix.minCoeffOfFinites(), 1e-10);
00082 }
00083
00084 TEST(EigenMatrixBaseAddons, maxCoeffOfFinites)
00085 {
00086 Matrix<double, 7, 18> matrix;
00087 matrix.setRandom();
00088 double max = matrix.maxCoeff();
00089 EXPECT_NEAR(max, matrix.maxCoeffOfFinites(), 1e-10);
00090
00091 int i, j;
00092 matrix.minCoeff(&i, &j);
00093 matrix(i, j) = NAN;
00094 EXPECT_NEAR(max, matrix.maxCoeffOfFinites(), 1e-10);
00095
00096 matrix.setConstant(NAN);
00097 EXPECT_TRUE(std::isnan(matrix.maxCoeffOfFinites()));
00098 matrix(i, j) = -1.0;
00099 EXPECT_NEAR(-1.0, matrix.maxCoeffOfFinites(), 1e-10);
00100 }
00101
00102 TEST(EigenMatrixBaseAddons, clamp)
00103 {
00104 Eigen::VectorXf vector(Eigen::VectorXf::LinSpaced(9, 1.0, 9.0));
00105 Eigen::Matrix3f matrix;
00106 matrix << vector.segment(0, 3), vector.segment(3, 3), vector.segment(6, 3);
00107 matrix(1, 1) = NAN;
00108 matrix = matrix.unaryExpr(grid_map::Clamp<float>(2.1, 7.0));
00109 EXPECT_NEAR(2.1, matrix(0, 0), 1e-7);
00110 EXPECT_NEAR(2.1, matrix(1, 0), 1e-7);
00111 EXPECT_NEAR(3.0, matrix(2, 0), 1e-7);
00112 EXPECT_TRUE(std::isnan(matrix(1, 1)));
00113 EXPECT_NEAR(7.0, matrix(2, 2), 1e-7);
00114 }