dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Matrix: convert slice test to gtest

This commit is contained in:
Matthias Grob 2022-02-23 15:04:13 +01:00
parent 46df8ab60c
commit 555ed9b9d8
2 changed files with 71 additions and 43 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/lib/matrix/test/CMakeLists.txt
View File

@ -6,7 +6,6 @@ add_compile_options(
)
set(tests
slice
vectorAssignment
transpose
vector
@ -43,5 +42,6 @@ px4_add_unit_gtest(SRC MatrixMultiplicationTest.cpp)
px4_add_unit_gtest(SRC MatrixPseudoInverseTest.cpp)
px4_add_unit_gtest(SRC MatrixScalarMultiplicationTest.cpp)
px4_add_unit_gtest(SRC MatrixSetIdentityTest.cpp)
px4_add_unit_gtest(SRC MatrixSliceTest.cpp)
px4_add_unit_gtest(SRC MatrixSparseVectorTest.cpp)
px4_add_unit_gtest(SRC MatrixUnwrapTest.cpp)

112
src/lib/matrix/test/slice.cpp → src/lib/matrix/test/MatrixSliceTest.cpp
View File

@ -1,12 +1,42 @@
#include "test_macros.hpp"
/****************************************************************************
*
* Copyright (C) 2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include <gtest/gtest.h>
#include <matrix/math.hpp>
using namespace matrix;
template class matrix::Slice<float, 2, 3, 4, 5>; // so that we get full coverage results
int main()
TEST(MatrixSliceTest, Slice)
{
float data[9] = {0, 2, 3,
4, 5, 6,
@ -21,7 +51,7 @@ int main()
7, 8, 10
};
Matrix<float, 2, 3> B_check_rowslice(data_check_rowslice);
TEST(isEqual(B_rowslice, B_check_rowslice));
EXPECT_EQ(B_rowslice, B_check_rowslice);
// Test column slicing
Matrix<float, 3, 2> B_colslice(A.slice<3, 2>(0, 1));
@ -31,7 +61,7 @@ int main()
8, 10
};
Matrix<float, 3, 2> B_check_colslice(data_check_colslice);
TEST(isEqual(B_colslice, B_check_colslice));
EXPECT_EQ(B_colslice, B_check_colslice);
// Test slicing both
Matrix<float, 2, 2> B_bothslice(A.slice<2, 2>(1, 1));
@ -40,7 +70,7 @@ int main()
8, 10
};
Matrix<float, 2, 2> B_check_bothslice(data_check_bothslice);
TEST(isEqual(B_bothslice, B_check_bothslice));
EXPECT_EQ(B_bothslice, B_check_bothslice);
//Test block writing
float data_2[4] = {
@ -57,7 +87,7 @@ int main()
7, 13, 14
};
Matrix<float, 3, 3> D(data_2_check);
TEST(isEqual(A, D));
EXPECT_EQ(A, D);
//Test writing to slices
Matrix<float, 3, 1> E;
@ -73,11 +103,11 @@ int main()
float data_3_check[3] = {9, 11, 3};
Matrix<float, 3, 1> G(data_3_check);
TEST(isEqual(E, G));
TEST(isEqual(E, Matrix<float, 3, 1>(E.slice<3, 1>(0, 0))));
EXPECT_EQ(E, G);
EXPECT_EQ(E, (Matrix<float, 3, 1>(E.slice<3, 1>(0, 0))));
Matrix<float, 2, 1> H = E.slice<2, 1>(0, 0);
TEST(isEqual(H, F));
EXPECT_EQ(H, F);
float data_4_check[5] = {3, 11, 9, 0, 0};
{
@ -89,7 +119,7 @@ int main()
K.row(0) = J.row(2);
Matrix<float, 5, 1> K_check(data_4_check);
TEST(isEqual(K, K_check));
EXPECT_EQ(K, K_check);
}
{
// assigning col slices to each other
@ -100,14 +130,14 @@ int main()
K.col(0) = J.col(2);
Matrix<float, 1, 5> K_check(data_4_check);
TEST(isEqual(K, K_check));
EXPECT_EQ(K, K_check);
}
// check that slice of a slice works for reading
const Matrix<float, 3, 3> cm33(data);
Matrix<float, 2, 1> topRight = cm33.slice<2, 3>(0, 0).slice<2, 1>(0, 2);
float top_right_check[2] = {3, 6};
TEST(isEqual(topRight, Matrix<float, 2, 1>(top_right_check)));
EXPECT_EQ(topRight, (Matrix<float, 2, 1>(top_right_check)));
// check that slice of a slice works for writing
Matrix<float, 3, 3> m33(data);
@ -116,21 +146,21 @@ int main()
4, 5, 0,
7, 8, 10
};
TEST(isEqual(m33, Matrix<float, 3, 3>(data_check)));
EXPECT_EQ(m33, (Matrix<float, 3, 3>(data_check)));
// longerThan
Vector3f v5;
v5(0) = 3;
v5(1) = 4;
v5(2) = 9;
TEST(v5.xy().longerThan(4.99f));
TEST(!v5.xy().longerThan(5.f));
TEST(isEqualF(5.f, v5.xy().norm()));
EXPECT_TRUE(v5.xy().longerThan(4.99f));
EXPECT_FALSE(v5.xy().longerThan(5.f));
EXPECT_FLOAT_EQ(v5.xy().norm(), 5.f);
// min/max
TEST(m33.row(1).max() == 5);
TEST(m33.col(0).min() == 0);
TEST((m33.slice<2, 2>(1, 1).max()) == 10);
EXPECT_FLOAT_EQ(m33.row(1).max(), 5.f);
EXPECT_FLOAT_EQ(m33.col(0).min(), 0.f);
EXPECT_FLOAT_EQ((m33.slice<2, 2>(1, 1).max()), 10.f);
// assign scalar value to slice
Matrix<float, 3, 1> L;
@ -142,7 +172,7 @@ int main()
float data_5_check[3] = {0, 0, 3};
Matrix<float, 3, 1> M(data_5_check);
TEST(isEqual(L, M));
EXPECT_EQ(L, M);
// return diagonal elements
float data_6[9] = {0, 2, 3,
@ -153,19 +183,19 @@ int main()
Vector3f v6 = N.slice<3, 3>(0, 0).diag();
Vector3f v6_check = {0, 5, 10};
TEST(isEqual(v6, v6_check));
EXPECT_EQ(v6, v6_check);
Vector2f v7 = N.slice<2, 3>(1, 0).diag();
Vector2f v7_check = {4, 8};
TEST(isEqual(v7, v7_check));
EXPECT_EQ(v7, v7_check);
Vector2f v8 = N.slice<3, 2>(0, 1).diag();
Vector2f v8_check = {2, 6};
TEST(isEqual(v8, v8_check));
EXPECT_EQ(v8, v8_check);
Vector2f v9(N.slice<1, 2>(1, 1));
Vector2f v9_check = {5, 6};
TEST(isEqual(v9, v9_check));
EXPECT_EQ(v9, v9_check);
Vector3f v10(N.slice<1, 3>(1, 0));
Vector3f v10_check = {4, 5, 6};
TEST(isEqual(v10, v10_check));
EXPECT_EQ(v10, v10_check);
// Different assignment operators
SquareMatrix3f O(data);
@ -174,63 +204,61 @@ int main()
O.slice<2, 2>(1, 0) += operand;
float O_check_data_1 [9] = {0, 2, 3, 6, 6, 6, 4, 7, 10};
TEST(isEqual(O, SquareMatrix3f(O_check_data_1)));
EXPECT_EQ(O, SquareMatrix3f(O_check_data_1));
O = SquareMatrix3f(data);
O.slice<2, 1>(1, 1) += operand.slice<2, 1>(0, 0);
float O_check_data_2 [9] = {0, 2, 3, 4, 7, 6, 7, 5, 10};
TEST(isEqual(O, SquareMatrix3f(O_check_data_2)));
EXPECT_EQ(O, SquareMatrix3f(O_check_data_2));
O = SquareMatrix3f(data);
O.slice<3, 3>(0, 0) += -1;
float O_check_data_3 [9] = {-1, 1, 2, 3, 4, 5, 6, 7, 9};
TEST(isEqual(O, SquareMatrix3f(O_check_data_3)));
EXPECT_EQ(O, SquareMatrix3f(O_check_data_3));
O = SquareMatrix3f(data);
O.col(1) += Vector3f{1, -2, 3};
float O_check_data_4 [9] = {0, 3, 3, 4, 3, 6, 7, 11, 10};
TEST(isEqual(O, SquareMatrix3f(O_check_data_4)));
EXPECT_EQ(O, SquareMatrix3f(O_check_data_4));
O = SquareMatrix3f(data);
O.slice<2, 2>(1, 0) -= operand;
float O_check_data_5 [9] = {0, 2, 3, 2, 4, 6, 10, 9, 10};
TEST(isEqual(O, SquareMatrix3f(O_check_data_5)));
EXPECT_EQ(O, SquareMatrix3f(O_check_data_5));
O = SquareMatrix3f(data);
O.slice<2, 1>(1, 1) -= operand.slice<2, 1>(0, 0);
float O_check_data_6 [9] = {0, 2, 3, 4, 3, 6, 7, 11, 10};
TEST(isEqual(O, SquareMatrix3f(O_check_data_6)));
EXPECT_EQ(O, SquareMatrix3f(O_check_data_6));
O = SquareMatrix3f(data);
O.slice<3, 3>(0, 0) -= -1;
float O_check_data_7 [9] = {1, 3, 4, 5, 6, 7, 8, 9, 11};
TEST(isEqual(O, SquareMatrix3f(O_check_data_7)));
EXPECT_EQ(O, SquareMatrix3f(O_check_data_7));
O = SquareMatrix3f(data);
O.col(1) -= Vector3f{1, -2, 3};
float O_check_data_8 [9] = {0, 1, 3, 4, 7, 6, 7, 5, 10};
TEST(isEqual(O, SquareMatrix3f(O_check_data_8)));
EXPECT_EQ(O, SquareMatrix3f(O_check_data_8));
O = SquareMatrix3f(data);
O.slice<2, 1>(1, 1) *= 5.f;
float O_check_data_9 [9] = {0, 2, 3, 4, 25, 6, 7, 40, 10};
TEST(isEqual(O, SquareMatrix3f(O_check_data_9)));
EXPECT_EQ(O, SquareMatrix3f(O_check_data_9));
O = SquareMatrix3f(data);
O.slice<2, 1>(1, 1) /= 2.f;
float O_check_data_10 [9] = {0, 2, 3, 4, 2.5, 6, 7, 4, 10};
TEST(isEqual(O, SquareMatrix3f(O_check_data_10)));
EXPECT_EQ(O, SquareMatrix3f(O_check_data_10));
// Different operations
O = SquareMatrix3f(data);
SquareMatrix<float, 2> res_11(O.slice<2, 2>(1, 1) * 2.f);
float O_check_data_11 [4] = {10, 12, 16, 20};
TEST(isEqual(res_11, SquareMatrix<float, 2>(O_check_data_11)));
EXPECT_EQ(res_11, (SquareMatrix<float, 2>(O_check_data_11)));
O = SquareMatrix3f(data);
SquareMatrix<float, 2> res_12(O.slice<2, 2>(1, 1) / 2.f);
float O_check_data_12 [4] = {2.5, 3, 4, 5};
TEST(isEqual(res_12, SquareMatrix<float, 2>(O_check_data_12)));
return 0;
EXPECT_EQ(res_12, (SquareMatrix<float, 2>(O_check_data_12)));
}