diff --git a/CHANGELOG.md b/CHANGELOG.md index 0707dc0c968..0f3488494e8 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -14,6 +14,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 - When used in Python, new iDynTree objects can be constructed from generic iterable objects and NumPy arrays (`*.FromPython`), and existing objects can be converted to NumPy arrays (`*.toNumPy`) (https://github.com/robotology/idyntree/pull/726). - iDynTree Python bindings can now be installed with `pip3 install git+https://github.com/robotology/idyntree.git` (https://github.com/robotology/idyntree/pull/733). +- Implement the MatrixView class (https://github.com/robotology/idyntree/pull/734) ### Fixed - Fixed bug in `yarprobotstatepublisher` that caused segmentation fault each time an unknown joint name was read from the input joint states topic (https://github.com/robotology/idyntree/pull/719) diff --git a/src/core/CMakeLists.txt b/src/core/CMakeLists.txt index 33a5c1eda81..ee69859d3c1 100644 --- a/src/core/CMakeLists.txt +++ b/src/core/CMakeLists.txt @@ -44,6 +44,7 @@ set(IDYNTREE_CORE_EXP_HEADERS include/iDynTree/Core/Axis.h include/iDynTree/Core/CubicSpline.h include/iDynTree/Core/Span.h include/iDynTree/Core/SO3Utils.h + include/iDynTree/Core/MatrixView.h # Deprecated headers include/iDynTree/Core/AngularForceVector3.h include/iDynTree/Core/AngularMotionVector3.h diff --git a/src/core/include/iDynTree/Core/EigenHelpers.h b/src/core/include/iDynTree/Core/EigenHelpers.h index 210d126c7c1..1dc8cb899f9 100644 --- a/src/core/include/iDynTree/Core/EigenHelpers.h +++ b/src/core/include/iDynTree/Core/EigenHelpers.h @@ -73,6 +73,105 @@ inline Eigen::Map toEigen(iDynTree::Span vec) { return Eigen::Map(vec.data(),vec.size()); } + +inline Eigen::Map, + 0, + Eigen::Stride> +toEigen(const MatrixView& mat) +{ + using MatrixRowMajor = Eigen::Matrix; + + // This is a trick required to see a ColMajor matrix as a RowMajor matrix. + // + // Given the following matrix + // _ _ _ + // / \ _____ | 1 2 3 4 5 | + // / _ \ |_____| | | + // / ___ \ |_____| | | + // /_/ \_\ |_ 6 7 8 9 10 _| + // + // If the matrix is stored as RowMajor matrix there will be a vector v_row in which + // the elements are saved + // v_row = [1 2 3 4 5 6 7 8 9 10] + // + // If the matrix is stored as ColMajor matrix there will be a vector v_col in which + // the elements are saved + // v_col = [1 6 2 7 3 8 4 9 5 10] + // + // Our goal here is to build a RowMajor Matrix (independently it is RowMajor/ColMajor) + // starting from the raw vactor (v_row, v_col) + // + // From the Eigen documentation https://eigen.tuxfamily.org/dox/classEigen_1_1Stride.html + // The inner stride is the pointer increment between two consecutive entries within a given row of a row-major matrix. + // The outer stride is the pointer increment between two consecutive rows of a row-major matrix. + // + // Starting from v_row we can build a RowMajor matrix by choosing the following pair of strides + // - inner_stride = 1 + // - outer_stride = 5 = number of columns of A + // + // Starting from v_col we can build a RowMajor matrix by choosing the following pair of strides + // - inner_stride = 2 = number of rows of A + // - outer_stride = 1 + + const int innerStride = (mat.storageOrder() == StorageOrder::ColMajor) ? mat.rows() : 1; + const int outerStride = (mat.storageOrder() == StorageOrder::ColMajor) ? 1 : mat.cols(); + + return Eigen::Map>( + mat.data(), + mat.rows(), + mat.cols(), + Eigen::Stride(outerStride, innerStride)); +} + +inline Eigen::Map, + 0, + Eigen::Stride> +toEigen(const MatrixView& mat) +{ + using MatrixRowMajor = Eigen::Matrix; + + // This is a trick required to see a ColMajor matrix as a RowMajor matrix. + // + // Given the following matrix + // _ _ _ + // / \ _____ | 1 2 3 4 5 | + // / _ \ |_____| | | + // / ___ \ |_____| | | + // /_/ \_\ |_ 6 7 8 9 10 _| + // + // If the matrix is stored as RowMajor matrix there will be a vector v_row in which + // the elements are saved as + // v_row = [1 2 3 4 5 6 7 8 9 10] + // + // If the matrix is stored as ColMajor matrix there will be a vector v_col in which + // the elements are saved as + // v_col = [1 6 2 7 3 8 4 9 5 10] + // + // Our goal here is to build a RowMajor Matrix (independently it is RowMajor/ColMajor) + // starting from the raw vactor (v_row, v_col) + // + // From the Eigen documentation https://eigen.tuxfamily.org/dox/classEigen_1_1Stride.html + // The inner stride is the pointer increment between two consecutive entries within a given row of a row-major matrix. + // The outer stride is the pointer increment between two consecutive rows of a row-major matrix. + // + // Starting from v_row we can build a RowMajor matrix by choosing the following pair of strides + // - inner_stride = 1 + // - outer_stride = 5 = number of columns of A + // + // Starting from v_col we can build a RowMajor matrix by choosing the following pair of strides + // - inner_stride = 2 = number of rows of A + // - outer_stride = 1 + + const int innerStride = (mat.storageOrder() == StorageOrder::ColMajor) ? mat.rows() : 1; + const int outerStride = (mat.storageOrder() == StorageOrder::ColMajor) ? 1 : mat.cols(); + + return Eigen::Map>( + mat.data(), + mat.rows(), + mat.cols(), + Eigen::Stride(outerStride, innerStride)); +} + #endif inline Eigen::Map > toEigen(const MatrixDynSize & mat) diff --git a/src/core/include/iDynTree/Core/MatrixDynSize.h b/src/core/include/iDynTree/Core/MatrixDynSize.h index 1f4cb61c6b6..e3f94e97fc3 100644 --- a/src/core/include/iDynTree/Core/MatrixDynSize.h +++ b/src/core/include/iDynTree/Core/MatrixDynSize.h @@ -14,6 +14,8 @@ #include +#include + namespace iDynTree { /** @@ -105,6 +107,15 @@ namespace iDynTree */ MatrixDynSize& operator=(const MatrixDynSize& other); + /** + * Assignment operator + * + * @param other the object to copy into self + * + * @return *this + */ + MatrixDynSize& operator=(const MatrixView& other); + /** * Denstructor * @@ -209,6 +220,14 @@ namespace iDynTree void fillColMajorBuffer(double * colMajorBuf) const; +#if !defined(SWIG_VERSION) || SWIG_VERSION >= 0x030000 + /** Typedefs to enable make_matrix_view. + */ + ///@{ + typedef double value_type; + ///@} +#endif + /** @name Output helpers. * Output helpers. */ diff --git a/src/core/include/iDynTree/Core/MatrixFixSize.h b/src/core/include/iDynTree/Core/MatrixFixSize.h index d90760fe66b..7911043ee43 100644 --- a/src/core/include/iDynTree/Core/MatrixFixSize.h +++ b/src/core/include/iDynTree/Core/MatrixFixSize.h @@ -141,6 +141,14 @@ namespace iDynTree */ void fillColMajorBuffer(double * colMajorBuf) const; + #if !defined(SWIG_VERSION) || SWIG_VERSION >= 0x030000 + /** Typedefs to enable make_matrix_view. + */ + ///@{ + typedef double value_type; + ///@} +#endif + /** @name Output helpers. * Output helpers. diff --git a/src/core/include/iDynTree/Core/MatrixView.h b/src/core/include/iDynTree/Core/MatrixView.h new file mode 100644 index 00000000000..e9c4fae1b8a --- /dev/null +++ b/src/core/include/iDynTree/Core/MatrixView.h @@ -0,0 +1,279 @@ +/* + * Copyright (C) 2020 Fondazione Istituto Italiano di Tecnologia + * + * Licensed under either the GNU Lesser General Public License v3.0 : + * https://www.gnu.org/licenses/lgpl-3.0.html + * or the GNU Lesser General Public License v2.1 : + * https://www.gnu.org/licenses/old-licenses/lgpl-2.1.html + * at your option. + */ + +#ifndef IDYNTREE_MATRIX_VIEW_H +#define IDYNTREE_MATRIX_VIEW_H + +#include + +#include + +#include +#include + +// constexpr workaround for SWIG +#ifdef SWIG +#define IDYNTREE_CONSTEXPR +#else +#define IDYNTREE_CONSTEXPR constexpr +#endif + + +namespace iDynTree +{ + + namespace MatrixViewInternal + { + // this is required to be compatible with c++17 + template struct make_void { typedef void type; }; + template using void_t = typename make_void::type; + + /** + * has_IsRowMajor is used to build a type-dependent expression that check if an + * element has IsRowMajor argument. This specific implementation is used when + * the the object has not IsRowMajor. + */ + template + struct has_IsRowMajor : std::false_type {}; + + /** + * has_IsRowMajor is used to build a type-dependent expression that check if an + * element has IsRowMajor argument. This specific implementation is used when + * the the object has not IsRowMajor, indeed void_t<\endcode> is used to + * detect ill-formed types in SFINAE context. + */ + template + struct has_IsRowMajor> : std::true_type {}; + + } // namespace MatrixViewIntenal + + /** + * Type of storage ordering + */ + enum class StorageOrder + { + RowMajor, + ColMajor + }; + + /** + * MatrixView implements a view interface of Matrices. Both RowMajor and ColMajor matrices are + * supported. + * @note The user should define the storage ordering when the MatrixView is created (the default + order is RowMajor). However if the MatrixView is generated: + * - from an object having a public member called IsRowMajor, the correct storage + order is chosen. + * - from another MatrixView, the correct storage order is chosen. + */ + template class MatrixView + { + public: + using element_type = ElementType; + using value_type = std::remove_cv_t; + using index_type = std::ptrdiff_t; + using pointer = element_type*; + using reference = element_type&; + + private: + pointer m_storage; + index_type m_rows; + index_type m_cols; + + StorageOrder m_storageOrder; + + index_type rawIndex(index_type row, index_type col) const + { + if (m_storageOrder == StorageOrder::RowMajor) + { + return (col + this->m_cols * row); + } else + { + return (this->m_rows * col + row); + } + } + + public: + + MatrixView() + : MatrixView(nullptr, 0, 0, StorageOrder::RowMajor) + {} + MatrixView(const MatrixView& other) + : MatrixView(other.m_storage, other.m_rows, other.m_cols, other.m_storageOrder) + { + } + + template < + class OtherElementType, + class = std::enable_if_t< + details::is_allowed_element_type_conversion::value>> + IDYNTREE_CONSTEXPR MatrixView(const MatrixView& other) + : MatrixView(other.data(), other.rows(), other.cols(), other.storageOrder()) + { + } + + template < + class Container, + std::enable_if_t::value + && std::is_convertible().data()), + pointer>::value + && MatrixViewInternal::has_IsRowMajor::value + && !std::is_same::value, + int> = 0> + MatrixView(const Container& matrix) + : MatrixView(matrix.data(), + matrix.rows(), + matrix.cols(), + Container::IsRowMajor ? StorageOrder::RowMajor + : StorageOrder::ColMajor) + { + } + + template < + class Container, + std::enable_if_t::value + && std::is_convertible().data()), + pointer>::value + && !MatrixViewInternal::has_IsRowMajor::value + && !std::is_same::value, + int> = 0> + MatrixView(const Container& matrix, const StorageOrder& order = StorageOrder::RowMajor) + : MatrixView(matrix.data(), matrix.rows(), matrix.cols(), order) + { + } + + template ().data()), pointer>::value + && MatrixViewInternal::has_IsRowMajor::value + && !std::is_same::value, + int> = 0> + MatrixView(Container& matrix) + : MatrixView(matrix.data(), + matrix.rows(), + matrix.cols(), + Container::IsRowMajor ? StorageOrder::RowMajor + : StorageOrder::ColMajor) + { + } + + template ().data()), pointer>::value + && !MatrixViewInternal::has_IsRowMajor::value + && !std::is_same::value, + int> = 0> + MatrixView(Container& matrix, const StorageOrder& order = StorageOrder::RowMajor) + : MatrixView(matrix.data(), matrix.rows(), matrix.cols(), order) + { + } + + MatrixView(pointer in_data, + index_type in_rows, + index_type in_cols, + const StorageOrder& order = StorageOrder::RowMajor) + : m_storage(in_data) + , m_rows(in_rows) + , m_cols(in_cols) + , m_storageOrder(order) + { + } + + const StorageOrder& storageOrder() const noexcept + { + return m_storageOrder; + } + + pointer data() const noexcept + { + return m_storage; + } + + /** + * @name Matrix interface methods. + * Methods exposing a matrix-like interface to MatrixView. + * + */ + ///@{ + reference operator()(index_type row, const index_type col) const + { + assert(row < this->rows()); + assert(col < this->cols()); + return this->m_storage[rawIndex(row, col)]; + } + + index_type rows() const noexcept + { + return this->m_rows; + } + + index_type cols() const noexcept + { + return this->m_cols; + } + ///@} + }; + + template + IDYNTREE_CONSTEXPR MatrixView + make_matrix_view(ElementType* ptr, + typename MatrixView::index_type rows, + typename MatrixView::index_type cols, + const StorageOrder& order = StorageOrder::RowMajor) + { + return MatrixView(ptr, rows, cols, order); + } + + template ::value + || std::is_same, Container>::value, + int> = 0> + IDYNTREE_CONSTEXPR MatrixView make_matrix_view(Container& cont) + { + return MatrixView(cont); + } + + template ::value + || std::is_same, Container>::value, + int> = 0> + IDYNTREE_CONSTEXPR MatrixView make_matrix_view(const Container& cont) + { + return MatrixView(cont); + } + + template ::value + && !std::is_same, Container>::value, + int> = 0> + IDYNTREE_CONSTEXPR MatrixView + make_matrix_view(Container& cont, + const StorageOrder& order = StorageOrder::RowMajor) + { + return MatrixView(cont, order); + } + + template ::value + && !std::is_same, Container>::value, + int> = 0> + IDYNTREE_CONSTEXPR MatrixView + make_matrix_view(const Container& cont, + const StorageOrder& order = StorageOrder::RowMajor) + { + return MatrixView(cont, order); + } + +} // namespace iDynTree + +#endif /* IDYNTREE_MATRIX_MATRIX_VIEW_H */ diff --git a/src/core/src/MatrixDynSize.cpp b/src/core/src/MatrixDynSize.cpp index a016325440b..150eea0da5c 100644 --- a/src/core/src/MatrixDynSize.cpp +++ b/src/core/src/MatrixDynSize.cpp @@ -107,6 +107,41 @@ MatrixDynSize& MatrixDynSize::operator=(const MatrixDynSize& other) return *this; } +MatrixDynSize& MatrixDynSize::operator=(const MatrixView& other) +{ + m_rows = other.rows(); + m_cols = other.cols(); + + const unsigned requiredCapacity = m_rows * m_cols; + + // if other is empty, return + if (requiredCapacity == 0) return *this; + + // If the copied data fits in the currently allocated buffer, + // use that one (if the user want to free the memory can use + // the shrink_to_fit method). + // Otherwise, allocate a new buffer after deleting the old one) + if (m_capacity < requiredCapacity) { + // need to allocate new buffer + // if old buffer exists, delete it + if (m_capacity > 0) { + delete [] m_data; + } + m_data = new double[requiredCapacity]; + m_capacity = requiredCapacity; + } + + for(unsigned int i = 0; i < m_rows; i++) + { + for(unsigned int j = 0; j < m_cols; j++) + { + this->m_data[this->rawIndexRowMajor(i,j)] = other(i, j); + } + } + + return *this; +} + MatrixDynSize::~MatrixDynSize() { if( this->m_capacity > 0 ) @@ -306,5 +341,4 @@ std::string MatrixDynSize::reservedToString() const return this->toString(); } - } diff --git a/src/core/tests/CMakeLists.txt b/src/core/tests/CMakeLists.txt index a788a887dd4..6e95062422c 100644 --- a/src/core/tests/CMakeLists.txt +++ b/src/core/tests/CMakeLists.txt @@ -32,6 +32,7 @@ add_unit_test(TransformFromMatrix4x4) add_unit_test(CubicSpline) add_unit_test(Span) add_unit_test(SO3Utils) +add_unit_test(MatrixView) # We have also some usages of the API that we want to make sure that do not compile diff --git a/src/core/tests/EigenHelpersUnitTest.cpp b/src/core/tests/EigenHelpersUnitTest.cpp index cfae4be80c2..d4b7f5411dc 100644 --- a/src/core/tests/EigenHelpersUnitTest.cpp +++ b/src/core/tests/EigenHelpersUnitTest.cpp @@ -45,6 +45,27 @@ void testSpanToEigen(const Vector3& input) { ASSERT_EQUAL_VECTOR(input, check); } +template +void testMatrixToEigen(const MatrixType& input) { + MatrixDynSize randMat(input.rows(), input.cols()), check(input.rows(), input.cols()); + getRandomMatrix(randMat); + MatrixView matrixViewCheck(check); + toEigen(matrixViewCheck) = toEigen(randMat); + ASSERT_EQUAL_MATRIX(randMat, check); + toEigen(make_matrix_view(check)) = toEigen(randMat); + toEigen(randMat) = toEigen(make_matrix_view(check)); + toEigen(check) = toEigen(make_matrix_view(input)); + ASSERT_EQUAL_MATRIX(input, check); +} + +void checkMatrixViewStorageOrder(const Eigen::MatrixXd& input) { + auto matrixView = make_matrix_view(input); + + // The toEigen returns a RowMajor matrix even if the original matrix is ColMajor. + // The Eigen::Stride is chosen to have the coherent behaviour. + ASSERT_EQUAL_MATRIX(input, toEigen(matrixView)); +} + int main() { Vector3 vec; @@ -60,5 +81,20 @@ int main() testSpanToEigen(vec); + // test the matrix view + Matrix2x3 mat1; + getRandomMatrix(mat1); + testMatrixToEigen(mat1); + + MatrixDynSize mat2(12,31); + getRandomMatrix(mat2); + testMatrixToEigen(mat2); + + Eigen::MatrixXd mat3(12,31); + getRandomMatrix(mat3); + testMatrixToEigen(mat3); + + checkMatrixViewStorageOrder(mat3); + return EXIT_SUCCESS; } diff --git a/src/core/tests/MatrixDynSizeUnitTest.cpp b/src/core/tests/MatrixDynSizeUnitTest.cpp index 9e5e91dc749..ffa81cddd2a 100644 --- a/src/core/tests/MatrixDynSizeUnitTest.cpp +++ b/src/core/tests/MatrixDynSizeUnitTest.cpp @@ -88,8 +88,19 @@ void checkCopyOperator() } +void checkMatrixView() +{ + MatrixDynSize test1, testToMatrixView; + testToMatrixView.resize(15, 12); + iDynTree::getRandomMatrix(testToMatrixView); + + test1 = iDynTree::make_matrix_view(testToMatrixView); + ASSERT_EQUAL_MATRIX(test1, testToMatrixView); +} + int main() { checkCapacity(); checkCopyOperator(); + checkMatrixView(); } diff --git a/src/core/tests/MatrixViewUnitTest.cpp b/src/core/tests/MatrixViewUnitTest.cpp new file mode 100644 index 00000000000..ba2f89c8470 --- /dev/null +++ b/src/core/tests/MatrixViewUnitTest.cpp @@ -0,0 +1,51 @@ +/* + * Copyright (C) 2015 Fondazione Istituto Italiano di Tecnologia + * + * Licensed under either the GNU Lesser General Public License v3.0 : + * https://www.gnu.org/licenses/lgpl-3.0.html + * or the GNU Lesser General Public License v2.1 : + * https://www.gnu.org/licenses/old-licenses/lgpl-2.1.html + * at your option. + */ + +#include +#include +#include +#include + +using namespace iDynTree; + +template +void areMatricesEqual(const T & mat1, const U & mat2) +{ + ASSERT_EQUAL_DOUBLE(mat1.rows(), mat2.rows()); + ASSERT_EQUAL_DOUBLE(mat1.cols(), mat2.cols()); + + ASSERT_EQUAL_MATRIX(mat1, mat2); +} + +int main() +{ + // test with iDynTree matrix + MatrixDynSize mat1(27, 41); + getRandomMatrix(mat1); + MatrixView view1(mat1); + + areMatricesEqual(view1, mat1); + + // Test with eigen column major matrix + Eigen::MatrixXd mat2(27, 41); + mat2.setRandom(); + MatrixView view2(mat2); + + areMatricesEqual(view2, mat2); + + // Test with eigen row major matrix + Eigen::Matrix mat3(27, 41); + mat3.setRandom(); + MatrixView view3(mat3); + + areMatricesEqual(view3, mat3); + + return 0; +}