EigenHelpers.h

Go to the documentation of this file.

/*
 * Copyright (C) 2012 by
 *   MetraLabs GmbH (MLAB), GERMANY
 * and
 *   Neuroinformatics and Cognitive Robotics Labs (NICR) at TU Ilmenau, GERMANY
 * All rights reserved.
 *
 * Contact: info@mira-project.org
 *
 * Commercial Usage:
 *   Licensees holding valid commercial licenses may use this file in
 *   accordance with the commercial license agreement provided with the
 *   software or, alternatively, in accordance with the terms contained in
 *   a written agreement between you and MLAB or NICR.
 *
 * GNU General Public License Usage:
 *   Alternatively, this file may be used under the terms of the GNU
 *   General Public License version 3.0 as published by the Free Software
 *   Foundation and appearing in the file LICENSE.GPL3 included in the
 *   packaging of this file. Please review the following information to
 *   ensure the GNU General Public License version 3.0 requirements will be
 *   met: http://www.gnu.org/copyleft/gpl.html.
 *   Alternatively you may (at your option) use any later version of the GNU
 *   General Public License if such license has been publicly approved by
 *   MLAB and NICR (or its successors, if any).
 *
 * IN NO EVENT SHALL "MLAB" OR "NICR" BE LIABLE TO ANY PARTY FOR DIRECT,
 * INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES ARISING OUT OF
 * THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF "MLAB" OR
 * "NICR" HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 * "MLAB" AND "NICR" SPECIFICALLY DISCLAIM ANY WARRANTIES, INCLUDING,
 * BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
 * FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER IS
 * ON AN "AS IS" BASIS, AND "MLAB" AND "NICR" HAVE NO OBLIGATION TO
 * PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS OR MODIFICATIONS.
 */

#ifndef _MIRA_TOOLBOXES_PYTHON_EIGENHELPERS_H_
#define _MIRA_TOOLBOXES_PYTHON_EIGENHELPERS_H_

#include <transform/Pose.h>
#include <geometry/Point.h>
#include <geometry/Size.h>

#include <utils/ToString.h>
#include <python/UnitWrapper.h>

#if BOOST_VERSION >= 106500
    #define TYPE_WITH_ALIGNMENT boost::python::detail::type_with_alignment
    #define ALIGNMENT_OF boost::python::detail::alignment_of

    #define IS_CLASS boost::python::detail::is_class
    #define IS_UNION boost::python::detail::is_union
#else
    #define TYPE_WITH_ALIGNMENT type_with_alignment
    #define ALIGNMENT_OF ::boost::alignment_of

    #define IS_CLASS is_class
    #define IS_UNION is_union
#endif

// Py_SET_SIZE was added in Python 3.9, must be used from Python 3.11:
// https://docs.python.org/3/c-api/structures.html#c.Py_SET_SIZE
#if PY_VERSION_HEX >= 0x03090000
    #define SET_SIZE(DEST, SRC) Py_SET_SIZE(DEST, SRC)
#else
    #define SET_SIZE(DEST, SRC) Py_SIZE(DEST) = SRC
#endif

// Note: Boost.Python 1.77ff seems to try to respect alignment of exposed types by itself,
// see https://github.com/boostorg/python/commit/aca3c80c4f80dd3d52219e1ee299e08bb980bcfd
// so this entire helper may not be required anymore at some time

#if BOOST_VERSION >= 107700
    #define INSTANCE_STORAGE_ADDRESS &instance->storage.bytes
#else
    #define INSTANCE_STORAGE_ADDRESS &instance->storage
#endif

// Fix alignment issues when exposing Eigen classes or classes with Eigen members to python
// see http://fhtagn.net/prog/2015/04/16/quaternion_boost_python.html
#define BOOST_PYTHON_ALIGN(DataType, Name) \
typedef boost::python::objects::value_holder<DataType> Name##Holder; \
namespace boost { namespace python { namespace objects { \
template<> \
struct instance<Name##Holder> \
{ \
    typedef Name##Holder Data; \
    PyObject_VAR_HEAD \
    PyObject* dict; \
    PyObject* weakrefs; \
    instance_holder* objects; \
    typedef typename TYPE_WITH_ALIGNMENT< \
            ALIGNMENT_OF<Data>::value \
    >::type align_t; \
    union \
    { \
        align_t align; \
        char bytes[sizeof(Data) + 16]; \
    } storage; \
}; \
template<class Derived> \
struct make_instance_impl<DataType, Name##Holder, Derived> \
{ \
    typedef DataType T; \
    typedef Name##Holder Holder; \
    typedef objects::instance<Holder> instance_t; \
    template <class Arg> \
    static inline PyObject* execute(Arg& x) \
    { \
        BOOST_MPL_ASSERT((mpl::or_<IS_CLASS<T>, IS_UNION<T> >)); \
        PyTypeObject* type = Derived::get_class_object(x); \
        if (type == 0) \
            return python::detail::none(); \
        PyObject* raw_result = type->tp_alloc( \
                type, objects::additional_instance_size<Holder>::value); \
        if (raw_result != 0) \
        { \
            python::detail::decref_guard protect(raw_result); \
            instance_t* instance = (instance_t*)raw_result; \
            Holder* holder = Derived::construct( \
                    INSTANCE_STORAGE_ADDRESS, (PyObject*)instance, x); \
            holder->install(raw_result); \
            size_t holder_offset = reinterpret_cast<size_t>(holder) \
                                 - reinterpret_cast<size_t>(INSTANCE_STORAGE_ADDRESS) \
                                 + offsetof(instance_t, storage); \
            SET_SIZE(instance, holder_offset); \
            protect.cancel(); \
        } \
        return raw_result; \
    } \
}; \
template<> \
struct make_instance<DataType, Name##Holder> \
    : make_instance_impl<DataType, Name##Holder, make_instance<DataType,Name##Holder> > \
{ \
    typedef DataType T; \
    typedef Name##Holder Holder; \
    template <class U> \
    static inline PyTypeObject* get_class_object(U&) \
    { \
        return converter::registered<T>::converters.get_class_object(); \
    } \
    static inline Holder* construct(void* storage, PyObject* instance, reference_wrapper<T const> x) \
    { \
        void* aligned_storage = reinterpret_cast<void*>( \
                (reinterpret_cast<size_t>(storage) & ~(size_t(15))) + 16); \
        Holder* new_holder = new (aligned_storage) Holder(instance, x); \
        return new_holder; \
    } \
}; \
}}}

BOOST_PYTHON_ALIGN(mira::Size2i, Size2i)
BOOST_PYTHON_ALIGN(mira::Point2f, Point2f)
BOOST_PYTHON_ALIGN(mira::Point2i, Point2i)
BOOST_PYTHON_ALIGN(mira::Point3f, Point3f)
BOOST_PYTHON_ALIGN(mira::Point3i, Point3i)
BOOST_PYTHON_ALIGN(mira::Pose2, Pose2)
BOOST_PYTHON_ALIGN(mira::PoseCov2, PoseCov2)
BOOST_PYTHON_ALIGN(mira::Pose3, Pose3)
BOOST_PYTHON_ALIGN(mira::PoseCov3, PoseCov3)
BOOST_PYTHON_ALIGN(Eigen::Quaternionf, Quaternionf)

using Matrix6f = Eigen::Matrix<float,6,6>;
BOOST_PYTHON_ALIGN(Matrix6f, Matrix6f)

namespace mira {
namespace python {


template<typename Derived>
struct EigenBaseAccessor {

    static std::string str(const Derived& m) {
        return toString(m);
    }

    static int rows(const Derived& m) {
        return m.rows();
    }
};

template<typename Derived>
struct EigenMatrixAccessor : public EigenBaseAccessor<Derived> {

    static typename Derived::Scalar get(const Derived& m, int row, int col) {
        return m(row, col);
    }

    static void set(Derived& m, int row, int col,
                    typename Derived::Scalar value) {
        m(row, col) = value;
    }

    static int cols(const Derived& m) {
        return m.cols();
    }
};

template<typename Derived>
struct EigenVectorAccessor : public EigenBaseAccessor<Derived> {
    static typename Derived::Scalar get(const Derived& m, int row) {
        return m(row);
    }

    static void set(Derived& m, int row,
                    typename Derived::Scalar value) {
        m(row) = value;
    }
};

template<typename M>
struct ZeroMatrix
{
    static M Zero()
    {
        return M::Zero();
    }
};

template<>
struct ZeroMatrix<Eigen::VectorXf>
{
    static Eigen::VectorXf Zero(int rows)
    {
        return Eigen::VectorXf::Zero(rows);
    }
};

template<>
struct ZeroMatrix<Eigen::MatrixXf>
{
    static Eigen::MatrixXf Zero(int rows, int cols)
    {
        return Eigen::MatrixXf::Zero(rows, cols);
    }
};


template<typename M>
struct EigenFixedSizeMatrix
{
    static void expose(const std::string& name)
    {
        using namespace boost::python;

        class_<M>(name.c_str(), init<>())
            .def("cols", &EigenMatrixAccessor<M>::cols)
            .def("rows", &EigenMatrixAccessor<M>::rows)
            .def("get", &EigenMatrixAccessor<M>::get)
            .def("set", &EigenMatrixAccessor<M>::set)
            .def("zeros", &ZeroMatrix<M>::Zero).staticmethod("zeros")
            .def("__repr__", &EigenMatrixAccessor<M>::str)
            MIRA_PYTHONCONNECTOR_NAMED_TYPE_FOOTER(M, name.c_str())
        ;
    }
};



} /* namespace python */
} /* namespace mira */

#endif /* _MIRA_TOOLBOXES_PYTHON_EIGENHELPERS_H_ */