116 lines
3.0 KiB
C++
116 lines
3.0 KiB
C++
/*
|
|
* EigenMatrixBaseAddonsTest.cpp
|
|
*
|
|
* Created on: Sep 23, 2015
|
|
* Author: Péter Fankhauser
|
|
* Institute: ETH Zurich, ANYbotics
|
|
*/
|
|
|
|
#include "grid_map_core/grid_map_core.hpp"
|
|
|
|
// gtest
|
|
#include <gtest/gtest.h>
|
|
|
|
// Eigen
|
|
#include <Eigen/Core>
|
|
|
|
using Eigen::Matrix;
|
|
|
|
TEST(EigenMatrixBaseAddons, numberOfFinites)
|
|
{
|
|
Eigen::Matrix3f matrix(Eigen::Matrix3f::Ones());
|
|
matrix(0, 0) = NAN;
|
|
matrix(1, 0) = NAN;
|
|
EXPECT_EQ(7, matrix.numberOfFinites());
|
|
|
|
Matrix<double, 13, 10> matrix2;
|
|
matrix2.setOnes();
|
|
EXPECT_EQ(matrix2.rows() * matrix2.cols(), matrix2.numberOfFinites());
|
|
|
|
Matrix<double, 13, 10> matrix3;
|
|
matrix3.setConstant(NAN);
|
|
matrix3.col(3).setConstant(0.0);
|
|
EXPECT_EQ(matrix3.rows(), matrix3.numberOfFinites());
|
|
}
|
|
|
|
TEST(EigenMatrixBaseAddons, sumOfFinites)
|
|
{
|
|
Matrix<double, 7, 18> matrix;
|
|
matrix.setRandom();
|
|
EXPECT_NEAR(matrix.sum(), matrix.sumOfFinites(), 1e-10);
|
|
double finiteSum = matrix.sum() - matrix(0, 0) - matrix(1, 2) - matrix(3, 6) - matrix(6, 12);
|
|
matrix(0, 0) = NAN;
|
|
matrix(1, 2) = NAN;
|
|
matrix(3, 6) = NAN;
|
|
matrix(6, 12) = NAN;
|
|
EXPECT_NEAR(finiteSum, matrix.sumOfFinites(), 1e-10);
|
|
matrix.setConstant(NAN);
|
|
EXPECT_TRUE(std::isnan(matrix.sumOfFinites()));
|
|
matrix(5, 7) = 1.0;
|
|
EXPECT_NEAR(1.0, matrix.sumOfFinites(), 1e-10);
|
|
}
|
|
|
|
TEST(EigenMatrixBaseAddons, meanOfFinites)
|
|
{
|
|
Eigen::Matrix3f matrix(Eigen::Matrix3f::Ones());
|
|
matrix(0, 0) = NAN;
|
|
matrix(1, 1) = NAN;
|
|
EXPECT_DOUBLE_EQ(1.0, matrix.meanOfFinites());
|
|
|
|
Matrix<double, 13, 10> matrix2;
|
|
matrix2.setRandom();
|
|
EXPECT_NEAR(matrix2.mean(), matrix2.meanOfFinites(), 1e-10);
|
|
}
|
|
|
|
TEST(EigenMatrixBaseAddons, minCoeffOfFinites)
|
|
{
|
|
Matrix<double, 7, 18> matrix;
|
|
matrix.setRandom();
|
|
double min = matrix.minCoeff();
|
|
EXPECT_NEAR(min, matrix.minCoeffOfFinites(), 1e-10);
|
|
|
|
int i;
|
|
int j;
|
|
matrix.maxCoeff(&i, &j);
|
|
matrix(i, j) = NAN;
|
|
EXPECT_NEAR(min, matrix.minCoeffOfFinites(), 1e-10);
|
|
|
|
matrix.setConstant(NAN);
|
|
EXPECT_TRUE(std::isnan(matrix.minCoeffOfFinites()));
|
|
matrix(i, j) = -1.0;
|
|
EXPECT_NEAR(-1.0, matrix.minCoeffOfFinites(), 1e-10);
|
|
}
|
|
|
|
TEST(EigenMatrixBaseAddons, maxCoeffOfFinites)
|
|
{
|
|
Matrix<double, 7, 18> matrix;
|
|
matrix.setRandom();
|
|
double max = matrix.maxCoeff();
|
|
EXPECT_NEAR(max, matrix.maxCoeffOfFinites(), 1e-10);
|
|
|
|
int i;
|
|
int j;
|
|
matrix.minCoeff(&i, &j);
|
|
matrix(i, j) = NAN;
|
|
EXPECT_NEAR(max, matrix.maxCoeffOfFinites(), 1e-10);
|
|
|
|
matrix.setConstant(NAN);
|
|
EXPECT_TRUE(std::isnan(matrix.maxCoeffOfFinites()));
|
|
matrix(i, j) = -1.0;
|
|
EXPECT_NEAR(-1.0, matrix.maxCoeffOfFinites(), 1e-10);
|
|
}
|
|
|
|
TEST(EigenMatrixBaseAddons, clamp)
|
|
{
|
|
Eigen::VectorXf vector(Eigen::VectorXf::LinSpaced(9, 1.0, 9.0));
|
|
Eigen::Matrix3f matrix;
|
|
matrix << vector.segment(0, 3), vector.segment(3, 3), vector.segment(6, 3);
|
|
matrix(1, 1) = NAN;
|
|
matrix = matrix.unaryExpr(grid_map::Clamp<float>(2.1, 7.0));
|
|
EXPECT_NEAR(2.1, matrix(0, 0), 1e-7);
|
|
EXPECT_NEAR(2.1, matrix(1, 0), 1e-7);
|
|
EXPECT_NEAR(3.0, matrix(2, 0), 1e-7);
|
|
EXPECT_TRUE(std::isnan(matrix(1, 1)));
|
|
EXPECT_NEAR(7.0, matrix(2, 2), 1e-7);
|
|
}
|