Eigen norm() with Boost.Units

779 views Asked by At

I am trying to use Boost.Units with Eigen 3.3.1, but after following the instructions here, and some pieces of informations found around, I still cannot figure out how to make norm() work.

Here is what I have so far (sorry for the long code block):

#include <boost/units/quantity.hpp>
#include <boost/units/systems/si/length.hpp>
#include <boost/units/systems/si/area.hpp>
#include <boost/units/cmath.hpp>
#include <Eigen/Geometry>

namespace Eigen {

//specialization of numeric traits
using boost::units::quantity;
template <typename Unit, typename Scalar>
struct NumTraits<quantity<Unit, Scalar>>
        : GenericNumTraits<quantity<Unit, Scalar>>
{
    typedef quantity<Unit, Scalar> Real;
    typedef quantity<Unit, Scalar> NonInteger;
    typedef quantity<Unit, Scalar> Nested;
    typedef quantity<Unit, Scalar> Literal;

    static inline Real epsilon() { return quantity<Unit, Scalar>(0); }
    static inline Real dummy_precision() { return quantity<Unit, Scalar>(1e-6 * Unit()); }
    static inline Real digits10() { return quantity<Unit, Scalar>(0); }

    enum {
        IsComplex = 0,
        IsInteger = 0,
        IsSigned = 1,
        RequireInitialization = 1,
        ReadCost = 1,
        AddCost = 3,
        MulCost = 3
    };
};

//specialization of sum operator
template <typename Unit, typename Scalar>
struct ScalarBinaryOpTraits<quantity<Unit, Scalar>, quantity<Unit, Scalar>, internal::scalar_sum_op<quantity<Unit, Scalar>, quantity<Unit, Scalar>>> {
    typedef typename boost::units::add_typeof_helper<quantity<Unit, Scalar>, quantity<Unit, Scalar>>::type ReturnType;
};

//specialization of product operator
template <typename Unit, typename Scalar>
struct ScalarBinaryOpTraits<Scalar, quantity<Unit, Scalar>,internal::scalar_product_op<Scalar, quantity<Unit, Scalar>>> {
    typedef Scalar X;
    typedef quantity<Unit, Scalar> Y;
    typedef typename boost::units::multiply_typeof_helper<X, Y>::type ReturnType;
};
template <typename Unit, typename Scalar>
struct ScalarBinaryOpTraits<quantity<Unit, Scalar>, Scalar, internal::scalar_product_op<quantity<Unit, Scalar>, Scalar>> {
    typedef quantity<Unit, Scalar> X;
    typedef Scalar Y;
    typedef typename boost::units::multiply_typeof_helper<X, Y>::type ReturnType;
};
template <typename Unit, typename Scalar>
struct ScalarBinaryOpTraits<quantity<Unit, Scalar>, quantity<Unit, Scalar>, internal::scalar_product_op<quantity<Unit, Scalar>, quantity<Unit, Scalar>>> {
    typedef quantity<Unit, Scalar> X;
    typedef quantity<Unit, Scalar> Y;
    typedef typename boost::units::multiply_typeof_helper<X, Y>::type ReturnType;
};

namespace internal {

//specialization for abs2()
template<typename Unit, typename Scalar>
struct abs2_impl<quantity<Unit, Scalar>>
{
    typedef quantity<Unit, Scalar> X;
    typedef quantity<Unit, Scalar> Y;
    typedef typename boost::units::multiply_typeof_helper<X, Y>::type ReturnType;

    EIGEN_DEVICE_FUNC
    static inline ReturnType run(const quantity<Unit, Scalar>& x)
    {
        return x * x;
    }
};


} // namespace internal

} // namespace Eigen

namespace boost {

namespace units {

//required functions
using namespace boost::units::si;
inline quantity<area, double> abs2(const quantity<length, double>& x)  { return x * x; }

} // namespace units

} // namespace boost

int main(int /*argc*/, char** /*argv[]*/)
{
    //unit typedefs
    using namespace boost::units;
    using namespace boost::units::si;
    using Length = quantity<length, double>;
    using Area = quantity<area, double>;

    //eigen typedefs
    using LengthVector = Eigen::Matrix<Length, 3, 1>;
    using AreaVector = Eigen::Matrix<Area, 3, 1>;
    using LengthMatrix = Eigen::Matrix<Length, 3, 3>;

    //test norm
    LengthVector vector1;
    Length result4 = vector1.norm();
}

But this fails to compile (gcc 5.4.0) with an error like

could not convert "boost::units::sqrt... (some undecipherable template error)"

and

could not convert "Eigen::internal::abs2_impl... (some obscure template error)"

1

There are 1 answers

0
Yuechuan Xue On
template<typename Derived>
EIGEN_STRONG_INLINE
typename NumTraits<typename internal::traits<Derived>::Scalar>::Real
MatrixBase<Derived>::squaredNorm() const
{
  return numext::real((*this).cwiseAbs2().sum());
}

template<typename Derived>
inline typename NumTraits<typename internal::traits<Derived>::Scalar>::Real
MatrixBase<Derived>::norm() const
{
     return numext::sqrt(squaredNorm());
}

As shown above, Eigen3 norm() function makes use of the squaredNorm(). However, the declaration of squaredNorm() requires the return type should be same to the Matrix element type Derived, which means the return value units should be same to the matrix element units. For example, a displacement vector of meter units, its squaredNorm should return a value with meter_squared unit, which conflicts with the declaration. So it may not be possible to use squaredNorm() or norm() directly without changing the implementation of Eigen.

My idea is to write a utility function outside of Eigen to implement squaredNorm(), norm() and normalized() :

template<typename T, int Row, int Col>
EIGEN_STRONG_INLINE
static T norm(const Eigen::Matrix<T, Row, Col>& m)
{
     return Eigen::numext::sqrt(squared_norm(m));
}

https://github.com/iastate-robotics/eigen3-units