EigenConverter.h

Go to the documentation of this file.

/*
* This file is part of ArmarX.
*
* ArmarX is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License version 2 as
* published by the Free Software Foundation.
*
* ArmarX is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*
* @author     Fabian Peller (fabian dot peller at kit dot edu)
* @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
*             GNU General Public License
*/
 
#pragma once
 
// STD/STL
#include <memory>
#include <numeric>
#include <string>
 
// Eigen
#include <Eigen/Core>
#include <Eigen/Geometry>
 
 
// ArmarX
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 
#include <RobotAPI/interface/aron.h>
#include <RobotAPI/libraries/aron/core/data/variant/complex/NDArray.h>
 
namespace armarx::aron::data::converter
{
    class AronEigenConverter
    {
        AronEigenConverter() = delete;
 
    public:
        // TODO: Add inplace methods
 
        static Eigen::Quaternion<float> ConvertToQuaternionf(const data::NDArrayPtr&);
        static Eigen::Quaternion<float> ConvertToQuaternionf(const data::NDArray&);
        static Eigen::Quaternion<double> ConvertToQuaterniond(const data::NDArrayPtr&);
        static Eigen::Quaternion<double> ConvertToQuaterniond(const data::NDArray&);
        static Eigen::Vector3f ConvertToVector3f(const data::NDArrayPtr&);
        static Eigen::Vector3f ConvertToVector3f(const data::NDArray&);
        static Eigen::Vector3d ConvertToVector3d(const data::NDArrayPtr&);
        static Eigen::Vector3d ConvertToVector3d(const data::NDArray&);
        static Eigen::Matrix4f ConvertToMatrix4f(const data::NDArrayPtr&);
        static Eigen::Matrix4f ConvertToMatrix4f(const data::NDArray&);
        static Eigen::Matrix4d ConvertToMatrix4d(const data::NDArrayPtr&);
        static Eigen::Matrix4d ConvertToMatrix4d(const data::NDArray&);
 
        template <typename T>
        static Eigen::Quaternion<T>
        ConvertToQuaternion(const data::NDArrayPtr& nav)
        {
            ARMARX_CHECK_NOT_NULL(nav);
            return ConvertToQuaternion<T>(*nav);
        }
 
        template <typename T>
        static Eigen::Quaternion<T>
        ConvertToQuaternion(const data::NDArray& nav)
        {
            checkDimensions(nav, {1, 4, sizeof(T)}, "ConvertToQuaternion");
            auto dims = nav.getShape();
 
            Eigen::Map<Eigen::Quaternion<T>> ret(reinterpret_cast<T*>(nav.getData()));
            return ret;
        }
 
        template <typename T, int Size>
        static Eigen::Matrix<T, Size, 1>
        ConvertToVector(const data::NDArrayPtr& nav)
        {
            ARMARX_CHECK_NOT_NULL(nav);
            return ConvertToVector<T, Size>(*nav);
        }
 
        template <typename T, int Size>
        static Eigen::Matrix<T, Size, 1>
        ConvertToVector(const data::NDArray& nav)
        {
            checkDimensions(nav, {Size, 1, sizeof(T)}, "ConvertToVector");
            auto dims = nav.getShape();
 
            Eigen::Map<Eigen::Matrix<T, Size, 1>> ret(reinterpret_cast<T*>(nav.getData()));
            return ret;
        }
 
        template <typename T, int Rows, int Cols>
        static Eigen::Matrix<T, Rows, Cols>
        ConvertToMatrix(const data::NDArrayPtr& nav)
        {
            ARMARX_CHECK_NOT_NULL(nav);
            return ConvertToMatrix<T, Rows, Cols>(*nav);
        }
 
        template <typename T>
        static Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>
        ConvertToDynamicMatrix(const data::NDArray& nav)
        {
            using MatrixT = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>;
 
            const auto dims = nav.getShape();
            ARMARX_CHECK(dims.size() == 2 or dims.size() == 3); // for now ...
 
            Eigen::Map<MatrixT> map(reinterpret_cast<T*>(nav.getData()), dims.at(0), dims.at(1));
            return map;
        }
 
        template <typename T, int Rows = Eigen::Dynamic, int Cols = Eigen::Dynamic>
        static Eigen::Matrix<T, Rows, Cols>
        ConvertToMatrix(const data::NDArray& nav)
        {
            if constexpr (Rows == Eigen::Dynamic and Cols == Eigen::Dynamic)
            {
                return ConvertToDynamicMatrix<T>(nav);
            }
            else
            {
                checkDimensions(nav, {Rows, Cols, sizeof(T)}, "ConvertToMatrix");
 
                Eigen::Map<Eigen::Matrix<T, Rows, Cols>> map(reinterpret_cast<T*>(nav.getData()));
                return map;
            }
        }
 
        template <typename T>
        static data::NDArrayPtr
        ConvertFromQuaternion(const Eigen::Quaternion<T>& quat)
        {
            data::NDArrayPtr ndArr(new data::NDArray);
            ndArr->setShape({1, 4});
            ndArr->setData(sizeof(T) * 4,
                           reinterpret_cast<const unsigned char*>(quat.coeffs().data()));
 
            return ndArr;
        }
 
        template <typename T>
        static data::NDArrayPtr
        ConvertFromMatrix(const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& mat)
        {
            data::NDArrayPtr ndArr(new data::NDArray);
 
            ndArr->setShape({static_cast<int>(mat.rows()), static_cast<int>(mat.cols())});
            ndArr->setData(sizeof(T) * mat.size(),
                           reinterpret_cast<const unsigned char*>(mat.data()));
 
            return ndArr;
        }
 
        template <typename T, int Rows = Eigen::Dynamic, int Cols = Eigen::Dynamic>
        static data::NDArrayPtr
        ConvertFromMatrix(const Eigen::Matrix<T, Rows, Cols>& mat)
        {
            data::NDArrayPtr ndArr(new data::NDArray);
 
            ndArr->setShape({Rows, Cols});
            ndArr->setData(sizeof(T) * mat.size(),
                           reinterpret_cast<const unsigned char*>(mat.data()));
 
            return ndArr;
        }
 
        // Eigen::Array
 
        template <typename T>
        static Eigen::Array<T, Eigen::Dynamic, Eigen::Dynamic>
        ConvertToDynamicArray(const data::NDArray& nav)
        {
            const auto dims = nav.getShape();
            ARMARX_CHECK_EQUAL(dims.size(), 2);
 
            using ArrayT = Eigen::Array<T, Eigen::Dynamic, Eigen::Dynamic>;
 
            ArrayT ret =
                Eigen::Map<ArrayT>(reinterpret_cast<T*>(nav.getData()), dims.at(0), dims.at(1));
            return ret;
        }
 
        template <typename T, int Rows = Eigen::Dynamic, int Cols = Eigen::Dynamic>
        static Eigen::Array<T, Rows, Cols>
        ConvertToArray(const data::NDArray& nav)
        {
            if constexpr (Rows == Eigen::Dynamic and Cols == Eigen::Dynamic)
            {
                return ConvertToDynamicArray<T>(nav);
            }
 
            checkDimensions(nav, {Rows, Cols, sizeof(T)}, "ConvertToArray");
            auto dims = nav.getShape();
 
            Eigen::Map<Eigen::Array<T, Rows, Cols>> ret(
                reinterpret_cast<T*>(nav.getData()), dims.at(0), dims.at(1));
            return ret;
        }
 
        template <typename T>
        static data::NDArrayPtr
        ConvertFromArray(const Eigen::Array<T, Eigen::Dynamic, Eigen::Dynamic>& mat)
        {
            data::NDArrayPtr ndArr(new data::NDArray);
 
            ndArr->setShape({static_cast<int>(mat.rows()), static_cast<int>(mat.cols())});
            ndArr->setData(sizeof(T) * mat.size(),
                           reinterpret_cast<const unsigned char*>(mat.data()));
 
            return ndArr;
        }
 
 
    private:
        /**
         * @throw `error::AronException` If `nav`'s dimensions do not match `expected`.
         */
        static void checkDimensions(const data::NDArray& nav,
                                    const std::vector<int>& expected,
                                    const std::string& method,
                                    const std::string& caller = "AronEigenConverter");
    };
 
} // namespace armarx::aron::data::converter
 
// legacy
namespace armarx::aron::converter
{
    using AronEigenConverter = ::armarx::aron::data::converter::AronEigenConverter;
}