From 728c4a7734aeb2e5a1aba5915b0ad596a3d2757a Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Wed, 20 Jul 2022 14:45:35 -0700 Subject: [PATCH 1/3] Add addition operator to Matrix6 Signed-off-by: Louise Poubel --- include/ignition/math/Matrix6.hh | 28 ++++++++++++++++++++++++++++ src/Matrix6_TEST.cc | 32 +++++++++++++++++++++++++++++++- 2 files changed, 59 insertions(+), 1 deletion(-) diff --git a/include/ignition/math/Matrix6.hh b/include/ignition/math/Matrix6.hh index 137ef2675..9d8bc1a48 100644 --- a/include/ignition/math/Matrix6.hh +++ b/include/ignition/math/Matrix6.hh @@ -317,6 +317,34 @@ namespace ignition el(5, 0), el(5, 1), el(5, 2), el(5, 3), el(5, 4), el(5, 5)); } + /// \brief Addition assignment operator. This matrix will + /// become equal to this + _m2. + /// \param[in] _m2 Incoming matrix. + /// \return This matrix + _m2. + public: Matrix6 operator+=(const Matrix6 &_m2) + { + (*this) = (*this) + _m2; + return *this; + } + + /// \brief Addition operator + /// \param[in] _m2 Incoming matrix + /// \return This matrix + _m2 + public: Matrix6 operator+(const Matrix6 &_m2) const + { + auto el = [&](size_t _row, size_t _col) -> T + { + return this->data[_row][_col] + _m2(_row, _col); + }; + return Matrix6( + el(0, 0), el(0, 1), el(0, 2), el(0, 3), el(0, 4), el(0, 5), + el(1, 0), el(1, 1), el(1, 2), el(1, 3), el(1, 4), el(1, 5), + el(2, 0), el(2, 1), el(2, 2), el(2, 3), el(2, 4), el(2, 5), + el(3, 0), el(3, 1), el(3, 2), el(3, 3), el(3, 4), el(3, 5), + el(4, 0), el(4, 1), el(4, 2), el(4, 3), el(4, 4), el(4, 5), + el(5, 0), el(5, 1), el(5, 2), el(5, 3), el(5, 4), el(5, 5)); + } + /// \brief Get the value at the specified row, column index /// \param[in] _col The column index. Index values are clamped to a /// range of [0, 5]. diff --git a/src/Matrix6_TEST.cc b/src/Matrix6_TEST.cc index e134b425d..496de12c1 100644 --- a/src/Matrix6_TEST.cc +++ b/src/Matrix6_TEST.cc @@ -106,7 +106,7 @@ TEST(Matrix6dTest, CoverageExtra) } ///////////////////////////////////////////////// -TEST(Matrix6dTest, Multiply4) +TEST(Matrix6dTest, Multiply) { Matrix6d mat, mat1; @@ -137,6 +137,36 @@ TEST(Matrix6dTest, Multiply4) EXPECT_EQ(mat2, mat4); } +///////////////////////////////////////////////// +TEST(Matrix6dTest, Add) +{ + Matrix6d mat, mat1; + + for (int i = 0; i < 6; ++i) + { + for (int j = 0; j < 6; ++j) + { + mat(i, j) = i - j; + mat1(j, i) = i + j; + } + } + + Matrix6d mat3( + 0, 0, 0, 0, 0, 0, + 2, 2, 2, 2, 2, 2, + 4, 4, 4, 4, 4, 4, + 6, 6, 6, 6, 6, 6, + 8, 8, 8, 8, 8, 8, + 10, 10, 10, 10, 10, 10); + + auto mat2 = mat + mat1; + EXPECT_EQ(mat2, mat3); + + auto mat4 = mat; + mat4 += mat1; + EXPECT_EQ(mat2, mat4); +} + ///////////////////////////////////////////////// TEST(Matrix6dTest, NoIndexException) { From d049d72020a453dff4016232506e0b2d464141ca Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Thu, 21 Jul 2022 23:18:08 +0200 Subject: [PATCH 2/3] Suggestion to 469 (#470) Signed-off-by: ahcorde --- include/ignition/math/Matrix6.hh | 15 +++ src/Matrix6_TEST.cc | 21 ++++ src/python_pybind11/CMakeLists.txt | 1 + src/python_pybind11/src/Matrix6.hh | 4 + src/python_pybind11/test/Matrix6_TEST.py | 124 ++++++++++++++--------- 5 files changed, 115 insertions(+), 50 deletions(-) diff --git a/include/ignition/math/Matrix6.hh b/include/ignition/math/Matrix6.hh index 9d8bc1a48..23b1c3963 100644 --- a/include/ignition/math/Matrix6.hh +++ b/include/ignition/math/Matrix6.hh @@ -125,6 +125,21 @@ namespace ignition { } + /// \brief Set a value in a specific row and col + /// param[in] _row Row of the matrix + /// param[in] _col Col of the matrix + /// param[in] _v Value to assign + /// \return Tru if the value was setted, False otherwise + public: bool SetValue(unsigned _row, unsigned _col, T _v) + { + if (_row < MatrixSize && _col < MatrixSize) + { + this->data[_row][_col] = _v; + return true; + } + return false; + } + /// \brief Change the values /// \param[in] _v00 Row 0, Col 0 value /// \param[in] _v01 Row 0, Col 1 value diff --git a/src/Matrix6_TEST.cc b/src/Matrix6_TEST.cc index 496de12c1..2d37bd99b 100644 --- a/src/Matrix6_TEST.cc +++ b/src/Matrix6_TEST.cc @@ -371,3 +371,24 @@ TEST(Matrix6dTest, SetSubmatrix) 30, 31, 32, 33, 34, 35)); } +///////////////////////////////////////////////// +TEST(Matrix6dTest, SetValue) +{ + Matrix6i mat; + + for (int i = 0; i < 6; ++i) + { + for (int j = 0; j < 6; ++j) + { + mat.SetValue(i, j, i - j); + } + } + + EXPECT_EQ(mat, Matrix6i( + 0, -1, -2, -3, -4, -5, + 1, 0, -1, -2, -3, -4, + 2, 1, 0, -1, -2, -3, + 3, 2, 1, 0, -1, -2, + 4, 3, 2, 1, 0, -1, + 5, 4, 3, 2, 1, 0)); +} diff --git a/src/python_pybind11/CMakeLists.txt b/src/python_pybind11/CMakeLists.txt index 4d8a074aa..e179e138a 100644 --- a/src/python_pybind11/CMakeLists.txt +++ b/src/python_pybind11/CMakeLists.txt @@ -122,6 +122,7 @@ if (BUILD_TESTING) Material_TEST Matrix3_TEST Matrix4_TEST + Matrix6_TEST MovingWindowFilter_TEST OrientedBox_TEST PID_TEST diff --git a/src/python_pybind11/src/Matrix6.hh b/src/python_pybind11/src/Matrix6.hh index fb6456253..9a2e87786 100644 --- a/src/python_pybind11/src/Matrix6.hh +++ b/src/python_pybind11/src/Matrix6.hh @@ -68,6 +68,7 @@ void helpDefineMathMatrix6(py::module &_m, const std::string &_typestr) T, T, T, T, T, T, T, T, T, T, T, T>()) .def(py::self * py::self) + .def(py::self + py::self) .def(py::self == py::self) .def(py::self != py::self) .def("__call__", @@ -76,6 +77,9 @@ void helpDefineMathMatrix6(py::module &_m, const std::string &_typestr) .def("set", &Class::Set, "Set values") + .def("set_value", + &Class::SetValue, + "Set value in a specific row and col.") .def("equal", &Class::Equal, "Equality test operator") diff --git a/src/python_pybind11/test/Matrix6_TEST.py b/src/python_pybind11/test/Matrix6_TEST.py index 718c196d7..85d1cdb54 100644 --- a/src/python_pybind11/test/Matrix6_TEST.py +++ b/src/python_pybind11/test/Matrix6_TEST.py @@ -1,6 +1,6 @@ # Copyright (C) 2022 Open Source Robotics Foundation # -# Licensed under the Apache License, Version 2.0 (the "License"); +# Licensed under the Apache License, Version 2.0 (the "License") # you may not use this file except in compliance with the License. # You may obtain a copy of the License at # @@ -38,48 +38,48 @@ def test_construct(self): 12.0, 13.0, 14.0, 15.0, 16.0, 17.0, 18.0, 19.0, 20.0, 21.0, 22.0, 23.0, 24.0, 25.0, 26.0, 27.0, 28.0, 29.0, - 30.0, 31.0, 32.0, 33.0, 34.0, 35.0); + 30.0, 31.0, 32.0, 33.0, 34.0, 35.0) mat4 = Matrix6d(mat3) self.assertAlmostEqual(mat4, mat3) - self.assertAlmostEqual(mat3(0, 0), 0.0); - self.assertAlmostEqual(mat3(0, 1), 1.0); - self.assertAlmostEqual(mat3(0, 2), 2.0); - self.assertAlmostEqual(mat3(0, 3), 3.0); - self.assertAlmostEqual(mat3(0, 4), 4.0); - self.assertAlmostEqual(mat3(0, 5), 5.0); - self.assertAlmostEqual(mat3(1, 0), 6.0); - self.assertAlmostEqual(mat3(1, 1), 7.0); - self.assertAlmostEqual(mat3(1, 2), 8.0); - self.assertAlmostEqual(mat3(1, 3), 9.0); - self.assertAlmostEqual(mat3(1, 4), 10.0); - self.assertAlmostEqual(mat3(1, 5), 11.0); - self.assertAlmostEqual(mat3(2, 0), 12.0); - self.assertAlmostEqual(mat3(2, 1), 13.0); - self.assertAlmostEqual(mat3(2, 2), 14.0); - self.assertAlmostEqual(mat3(2, 3), 15.0); - self.assertAlmostEqual(mat3(2, 4), 16.0); - self.assertAlmostEqual(mat3(2, 5), 17.0); - self.assertAlmostEqual(mat3(3, 0), 18.0); - self.assertAlmostEqual(mat3(3, 1), 19.0); - self.assertAlmostEqual(mat3(3, 2), 20.0); - self.assertAlmostEqual(mat3(3, 3), 21.0); - self.assertAlmostEqual(mat3(3, 4), 22.0); - self.assertAlmostEqual(mat3(3, 5), 23.0); - self.assertAlmostEqual(mat3(4, 0), 24.0); - self.assertAlmostEqual(mat3(4, 1), 25.0); - self.assertAlmostEqual(mat3(4, 2), 26.0); - self.assertAlmostEqual(mat3(4, 3), 27.0); - self.assertAlmostEqual(mat3(4, 4), 28.0); - self.assertAlmostEqual(mat3(4, 5), 29.0); - self.assertAlmostEqual(mat3(5, 0), 30.0); - self.assertAlmostEqual(mat3(5, 1), 31.0); - self.assertAlmostEqual(mat3(5, 2), 32.0); - self.assertAlmostEqual(mat3(5, 3), 33.0); - self.assertAlmostEqual(mat3(5, 4), 34.0); - self.assertAlmostEqual(mat3(5, 5), 35.0); - self.assertAlmostEqual(mat3(100, 100), 35.0); + self.assertAlmostEqual(mat3(0, 0), 0.0) + self.assertAlmostEqual(mat3(0, 1), 1.0) + self.assertAlmostEqual(mat3(0, 2), 2.0) + self.assertAlmostEqual(mat3(0, 3), 3.0) + self.assertAlmostEqual(mat3(0, 4), 4.0) + self.assertAlmostEqual(mat3(0, 5), 5.0) + self.assertAlmostEqual(mat3(1, 0), 6.0) + self.assertAlmostEqual(mat3(1, 1), 7.0) + self.assertAlmostEqual(mat3(1, 2), 8.0) + self.assertAlmostEqual(mat3(1, 3), 9.0) + self.assertAlmostEqual(mat3(1, 4), 10.0) + self.assertAlmostEqual(mat3(1, 5), 11.0) + self.assertAlmostEqual(mat3(2, 0), 12.0) + self.assertAlmostEqual(mat3(2, 1), 13.0) + self.assertAlmostEqual(mat3(2, 2), 14.0) + self.assertAlmostEqual(mat3(2, 3), 15.0) + self.assertAlmostEqual(mat3(2, 4), 16.0) + self.assertAlmostEqual(mat3(2, 5), 17.0) + self.assertAlmostEqual(mat3(3, 0), 18.0) + self.assertAlmostEqual(mat3(3, 1), 19.0) + self.assertAlmostEqual(mat3(3, 2), 20.0) + self.assertAlmostEqual(mat3(3, 3), 21.0) + self.assertAlmostEqual(mat3(3, 4), 22.0) + self.assertAlmostEqual(mat3(3, 5), 23.0) + self.assertAlmostEqual(mat3(4, 0), 24.0) + self.assertAlmostEqual(mat3(4, 1), 25.0) + self.assertAlmostEqual(mat3(4, 2), 26.0) + self.assertAlmostEqual(mat3(4, 3), 27.0) + self.assertAlmostEqual(mat3(4, 4), 28.0) + self.assertAlmostEqual(mat3(4, 5), 29.0) + self.assertAlmostEqual(mat3(5, 0), 30.0) + self.assertAlmostEqual(mat3(5, 1), 31.0) + self.assertAlmostEqual(mat3(5, 2), 32.0) + self.assertAlmostEqual(mat3(5, 3), 33.0) + self.assertAlmostEqual(mat3(5, 4), 34.0) + self.assertAlmostEqual(mat3(5, 5), 35.0) + self.assertAlmostEqual(mat3(100, 100), 35.0) def test_multiply_mat(self): mat = Matrix6d(0, -1, -2, -3, -4, -5, @@ -111,6 +111,30 @@ def test_multiply_mat(self): mat4 *= mat1 self.assertAlmostEqual(mat2, mat4) + def test_add_mat(self): + mat = Matrix6d() + mat1 = Matrix6d() + + for i in range(6): + for j in range(6): + mat.set_value(i, j, i - j) + mat1.set_value(j, i, i + j) + + mat3 = Matrix6d( + 0, 0, 0, 0, 0, 0, + 2, 2, 2, 2, 2, 2, + 4, 4, 4, 4, 4, 4, + 6, 6, 6, 6, 6, 6, + 8, 8, 8, 8, 8, 8, + 10, 10, 10, 10, 10, 10) + + mat2 = mat + mat1 + self.assertAlmostEqual(mat2, mat3) + + mat4 = mat + mat4 += mat1 + self.assertAlmostEqual(mat2, mat4) + def test_stream_out(self): matA = Matrix6d( 0.0, 1.0, 2.0, 3.0, 4.0, 5.0, @@ -118,7 +142,7 @@ def test_stream_out(self): 12.0, 13.0, 14.0, 15.0, 16.0, 17.0, 18.0, 19.0, 20.0, 21.0, 22.0, 23.0, 24.0, 25.0, 26.0, 27.0, 28.0, 29.0, - 30.0, 31.0, 32.0, 33.0, 34.0, 35.0); + 30.0, 31.0, 32.0, 33.0, 34.0, 35.0) self.assertEqual(str(matA), "0 1 2 3 4 5 " @@ -177,27 +201,27 @@ def test_submatrix(self): 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, - 30, 31, 32, 33, 34, 35); + 30, 31, 32, 33, 34, 35) self.assertAlmostEqual(mat.submatrix(Matrix6dCorner.TOP_LEFT), Matrix3d( 0, 1, 2, 6, 7, 8, - 12, 13, 14)); + 12, 13, 14)) self.assertAlmostEqual(mat.submatrix(Matrix6dCorner.TOP_RIGHT), Matrix3d( 3, 4, 5, 9, 10, 11, - 15, 16, 17)); + 15, 16, 17)) self.assertAlmostEqual(mat.submatrix(Matrix6dCorner.BOTTOM_LEFT), Matrix3d( 18, 19, 20, 24, 25, 26, - 30, 31, 32)); + 30, 31, 32)) self.assertAlmostEqual(mat.submatrix(Matrix6dCorner.BOTTOM_RIGHT), Matrix3d( 21, 22, 23, 27, 28, 29, - 33, 34, 35)); + 33, 34, 35)) def test_set_submatrix(self): mat = Matrix6d() @@ -205,22 +229,22 @@ def test_set_submatrix(self): mat.set_submatrix(Matrix6dCorner.TOP_LEFT, Matrix3d( 0, 1, 2, 6, 7, 8, - 12, 13, 14)); + 12, 13, 14)) mat.set_submatrix(Matrix6dCorner.TOP_RIGHT, Matrix3d( 3, 4, 5, 9, 10, 11, - 15, 16, 17)); + 15, 16, 17)) mat.set_submatrix(Matrix6dCorner.BOTTOM_LEFT, Matrix3d( 18, 19, 20, 24, 25, 26, - 30, 31, 32)); + 30, 31, 32)) mat.set_submatrix(Matrix6dCorner.BOTTOM_RIGHT, Matrix3d( 21, 22, 23, 27, 28, 29, - 33, 34, 35)); + 33, 34, 35)) self.assertAlmostEqual(mat, Matrix6d( 0, 1, 2, 3, 4, 5, @@ -228,7 +252,7 @@ def test_set_submatrix(self): 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, - 30, 31, 32, 33, 34, 35)); + 30, 31, 32, 33, 34, 35)) if __name__ == '__main__': unittest.main() From a9add9ea511e6990d0c3181a37158c0a79fc4725 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Thu, 21 Jul 2022 14:26:49 -0700 Subject: [PATCH 3/3] Test coverage Signed-off-by: Louise Poubel --- include/ignition/math/Matrix6.hh | 2 +- src/Matrix6_TEST.cc | 3 ++- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/include/ignition/math/Matrix6.hh b/include/ignition/math/Matrix6.hh index 23b1c3963..c8dd33135 100644 --- a/include/ignition/math/Matrix6.hh +++ b/include/ignition/math/Matrix6.hh @@ -130,7 +130,7 @@ namespace ignition /// param[in] _col Col of the matrix /// param[in] _v Value to assign /// \return Tru if the value was setted, False otherwise - public: bool SetValue(unsigned _row, unsigned _col, T _v) + public: bool SetValue(size_t _row, size_t _col, T _v) { if (_row < MatrixSize && _col < MatrixSize) { diff --git a/src/Matrix6_TEST.cc b/src/Matrix6_TEST.cc index 2d37bd99b..27c9a56ac 100644 --- a/src/Matrix6_TEST.cc +++ b/src/Matrix6_TEST.cc @@ -380,9 +380,10 @@ TEST(Matrix6dTest, SetValue) { for (int j = 0; j < 6; ++j) { - mat.SetValue(i, j, i - j); + EXPECT_TRUE(mat.SetValue(i, j, i - j)); } } + EXPECT_FALSE(mat.SetValue(100, 100, 100)); EXPECT_EQ(mat, Matrix6i( 0, -1, -2, -3, -4, -5,