Re: [eigen] Mapping array of scalars into quaternions

[Benoit Jacob <jacob.benoit.1@xxxxxxxxx>]

Hi,

this is really good stuff, i can confirm that you've correctly
understood the various issues about which you were unsure.
Incidentally we had been talking about something like this with Rohit
recently.

There are so many things to do at the moment, could you please address
these points before we apply your patch, that would help a lot:
1) need to extend the quaternion unit tests to cover that. The file is
test/geo_quaternion.cpp. No need to adapt all of it, just append a few
sanity checks for Quaternion<Map...> at the end, e.g. build a
quaternion that maps the array of a plain quaternion, do some
arithmetic on both and check (VERIFY_IS_APPROX) that the results are
the same.
2) i've made a few comments inline below.

2009/10/21 Mathieu Gautier <mathieu.gautier@xxxxxx>:
> Hi,
>
> I want to implement a class which hold both a quaternion and a vector3 to
> discribe a position of a frame in respect to an other frame (Something
> similar to Frame in KDL). For some historic reason, the memory layout must
> be a continuous array of scalar (Scalar[7]), the 3 first scalars are the
> vector3 and the other are the quaternion. Besides I have to interface this
> structure with other libraries which return an array of 7 scalars. I also
> have to map array of 4 scalars into the Quaternion class.

OK. So indeed with the current quaternion class you were out of luck
because its array is 16-byte aligned.

>
> I though of something like that :
>
> template<typename _Scalars>
> struct Frame {
>    _Scalars[7] data;
>    Map<Matrix<3,1,_Scalars>> v;
>    Map<Quaternion<_Scalars>> q;
> };
>
> and setting the Map accordingly. However, there's no such concept of Map for
> the Quaternion class or RotationBase.

Indeed.

>
> So, I modified the Quaternion class to use either a Matrix<Scalar,4,1> to
> store the quaternion coefficients or a Map<Matrix<Scalar,4,1>> to map an
> array of scalars. I added a argument to the template prototype of the
> quaternion class which I call StorageType which is either the Matrix or
> Map<Matrix>.

Good.
In the Quaternion constructor, you could add some sanity checks for that type:
EIGEN_STATIC_ASSERT_VECTOR_ONLY(StorageType)
EIGEN_STATIC_ASSERT(
  int(StorageType::Flags)&DirectAccessBit,
  THIS_METHOD_IS_ONLY_FOR_EXPRESSIONS_WITH_DIRECT_MEMORY_ACCESS_SUCH_AS_MAP_OR_PLAIN_MATRICES
)

> Since I have almost no experience with Eigen, I don't know if it's the
> better choice and if it could cause alignement issues. I have attached my
> modifications as a patch. This implementation may be incomplete.

That's good and there's no alignment issue, because the StorageType
would only claim to be aligned if you explicitly tell it that the
pointer is aligned.
Currently we have a little performance trouble: there's no way to tell
Map that the pointer is aligned, but see other new thread, this is
taken care of.

>
> So, to summarize, the Frame class would be:
>
> template<typename _Scalars>
> struct Frame {
>    _Scalars[7] data;
>    Map<Matrix<3,1,_Scalars>> v;
>    QuaternionMap<_Scalars> q;
> };
>
> where v is a mapping of data and q a mapping of data+3.

Looks good.

Some more comments inline in the patch:

>
> --> -template<typename _Scalar> struct ei_traits<Quaternion<_Scalar> >
> +template<typename _Scalar, class StorageType> struct
> ei_traits<Quaternion<_Scalar, StorageType> >
>  {
>   typedef _Scalar Scalar;
>  };
>
> -template<typename _Scalar>
> -class Quaternion : public RotationBase<Quaternion<_Scalar>,3>
> +template<typename _Scalar, class StorageType = Matrix<_Scalar, 4, 1>>
> +class Quaternion : public RotationBase<Quaternion<_Scalar, StorageType>,3>

Why make it templated in both the Scalar and StorageType? The Scalar
could be deduced from the storage type:

  typedef typename StorageType::Scalar Scalar;

fewer template argument passing, is better.

>   /** the type of the Coefficients 4-vector */
> -  typedef Matrix<Scalar, 4, 1> Coefficients;
> +//  typedef Matrix<Scalar, 4, 1> Coefficients;
> +  typedef StorageType Coefficients;

This should be harmonized, no need to keep 2 names. How about renaming
StorageType to just Coefficients? or CoeffsType?
Actually there was a small problem with StorageType, that is, when it
was Vector4f it felt strange because when we talk about the storage of
a Vector4f we mean the underlying float[4].

> -  inline static Quaternion Identity() { return Quaternion(1, 0, 0, 0); }
> +  inline static Quaternion<Scalar> Identity() { return
> Quaternion<Scalar>(1, 0, 0, 0); }

well done :) indeed one must be careful, when returning by value it
must carry its own storage.

> -  inline Quaternion operator* (const Quaternion& q) const;
> -  inline Quaternion& operator*= (const Quaternion& q);
> +  template<class OtherStorageType> inline Quaternion<Scalar> operator*
> (const Quaternion<Scalar, OtherStorageType>& q) const;
> +  template<class OtherStorageType> inline Quaternion& operator*= (const
> Quaternion<Scalar, OtherStorageType>& q);

well done, indeed introducting this template param allows for
interoperability between quaternion types.

> +/** \ingroup Geometry_Module
> +  * double precision quaternion type */
> +typedef Quaternion<float>::QuaternionMap QuaternionMapf;
> +/** \ingroup Geometry_Module
> +  * double precision quaternion type */
> +typedef Quaternion<double>::QuaternionMap QuaternionMapd;

you wrote "double" twice in the doc.

i'll stop here, trust you for the rest if you write the unit test.

Thanks
Benoit

> +
>  // Generic Quaternion * Quaternion product
> -template<int Arch,typename Scalar> inline Quaternion<Scalar>
> -ei_quaternion_product(const Quaternion<Scalar>& a, const
> Quaternion<Scalar>& b)
> +template<int Arch,typename Scalar, class StorageType, class
> OtherStorageType> inline Quaternion<Scalar>
> +ei_quaternion_product(const Quaternion<Scalar, StorageType>& a, const
> Quaternion<Scalar, OtherStorageType>& b)
>  {
>   return Quaternion<Scalar>
>   (
> @@ -239,15 +253,17 @@
>  }
>
>  /** \returns the concatenation of two rotations as a quaternion-quaternion
> product */
> -template <typename Scalar>
> -inline Quaternion<Scalar> Quaternion<Scalar>::operator* (const Quaternion&
> other) const
> +template <typename Scalar, class StorageType>
> +template <class OtherStorageType>
> +inline Quaternion<Scalar> Quaternion<Scalar, StorageType>::operator* (const
> Quaternion<Scalar, OtherStorageType>& other) const
>  {
> -  return ei_quaternion_product<EiArch>(*this,other);
> +  return ei_quaternion_product<EiArch, Scalar, StorageType,
> OtherStorageType>(*this, other);
>  }
>
>  /** \sa operator*(Quaternion) */
> -template <typename Scalar>
> -inline Quaternion<Scalar>& Quaternion<Scalar>::operator*= (const
> Quaternion& other)
> +template <typename Scalar, class StorageType>
> +template <class OtherStorageType>
> +inline Quaternion<Scalar, StorageType>& Quaternion<Scalar,
> StorageType>::operator*= (const Quaternion<Scalar, OtherStorageType>& other)
>  {
>   return (*this = *this * other);
>  }
> @@ -259,9 +275,9 @@
>   *   - Quaternion:    30n
>   *   - Via a Matrix3: 24 + 15n
>   */
> -template <typename Scalar>
> -inline typename Quaternion<Scalar>::Vector3
> -Quaternion<Scalar>::_transformVector(Vector3 v) const
> +template <typename Scalar, class StorageType>
> +inline typename Quaternion<Scalar, StorageType>::Vector3
> +Quaternion<Scalar, StorageType>::_transformVector(Vector3 v) const
>  {
>     // Note that this algorithm comes from the optimization by hand
>     // of the conversion to a Matrix followed by a Matrix/Vector product.
> @@ -272,8 +288,8 @@
>     return v + this->w() * uv + this->vec().cross(uv);
>  }
>
> -template<typename Scalar>
> -inline Quaternion<Scalar>& Quaternion<Scalar>::operator=(const Quaternion&
> other)
> +template<typename Scalar, class StorageType>
> +inline Quaternion<Scalar, StorageType>& Quaternion<Scalar,
> StorageType>::operator=(const Quaternion& other)
>  {
>   m_coeffs = other.m_coeffs;
>   return *this;
> @@ -281,8 +297,8 @@
>
>  /** Set \c *this from an angle-axis \a aa and returns a reference to \c
> *this
>   */
> -template<typename Scalar>
> -inline Quaternion<Scalar>& Quaternion<Scalar>::operator=(const
> AngleAxisType& aa)
> +template<typename Scalar, class StorageType>
> +inline Quaternion<Scalar, StorageType>& Quaternion<Scalar,
> StorageType>::operator=(const AngleAxisType& aa)
>  {
>   Scalar ha = Scalar(0.5)*aa.angle(); // Scalar(0.5) to suppress precision
> loss warnings
>   this->w() = ei_cos(ha);
> @@ -295,9 +311,9 @@
>   *   - if \a xpr is a 3x3 matrix, then \a xpr is assumed to be rotation
> matrix
>   *     and \a xpr is converted to a quaternion
>   */
> -template<typename Scalar>
> +template<typename Scalar, class StorageType>
>  template<typename Derived>
> -inline Quaternion<Scalar>& Quaternion<Scalar>::operator=(const
> MatrixBase<Derived>& xpr)
> +inline Quaternion<Scalar, StorageType>& Quaternion<Scalar,
> StorageType>::operator=(const MatrixBase<Derived>& xpr)
>  {
>   ei_quaternion_assign_impl<Derived>::run(*this, xpr.derived());
>   return *this;
> @@ -306,9 +322,9 @@
>  /** Convert the quaternion to a 3x3 rotation matrix. The quaternion is
> required to
>   * be normalized, otherwise the result is undefined.
>   */
> -template<typename Scalar>
> -inline typename Quaternion<Scalar>::Matrix3
> -Quaternion<Scalar>::toRotationMatrix(void) const
> +template<typename Scalar, class StorageType>
> +inline typename Quaternion<Scalar, StorageType>::Matrix3
> +Quaternion<Scalar, StorageType>::toRotationMatrix(void) const
>  {
>   // NOTE if inlined, then gcc 4.2 and 4.4 get rid of the temporary (not gcc
> 4.3 !!)
>   // if not inlined then the cost of the return by value is huge ~ +35%,
> @@ -352,9 +368,9 @@
>   * Note that the two input vectors do \b not have to be normalized, and
>   * do not need to have the same norm.
>   */
> -template<typename Scalar>
> +template<typename Scalar, class StorageType>
>  template<typename Derived1, typename Derived2>
> -inline Quaternion<Scalar>& Quaternion<Scalar>::setFromTwoVectors(const
> MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b)
> +inline Quaternion<Scalar, StorageType>& Quaternion<Scalar,
> StorageType>::setFromTwoVectors(const MatrixBase<Derived1>& a, const
> MatrixBase<Derived2>& b)
>  {
>   Vector3 v0 = a.normalized();
>   Vector3 v1 = b.normalized();
> @@ -395,17 +411,17 @@
>   *
>   * \sa Quaternion::conjugate()
>   */
> -template <typename Scalar>
> -inline Quaternion<Scalar> Quaternion<Scalar>::inverse() const
> +template <typename Scalar, class StorageType>
> +inline Quaternion<Scalar> Quaternion<Scalar, StorageType>::inverse() const
>  {
>   // FIXME should this function be called multiplicativeInverse and
> conjugate() be called inverse() or opposite()  ??
>   Scalar n2 = this->squaredNorm();
>   if (n2 > 0)
> -    return Quaternion(conjugate().coeffs() / n2);
> +    return Quaternion<Scalar>(conjugate().coeffs() / n2);
>   else
>   {
>     // return an invalid result to flag the error
> -    return Quaternion(Coefficients::Zero());
> +    return Quaternion<Scalar>(Coefficients::Zero());
>   }
>  }
>
> @@ -415,17 +431,18 @@
>   *
>   * \sa Quaternion::inverse()
>   */
> -template <typename Scalar>
> -inline Quaternion<Scalar> Quaternion<Scalar>::conjugate() const
> +template <typename Scalar, class StorageType>
> +inline Quaternion<Scalar> Quaternion<Scalar, StorageType>::conjugate()
> const
>  {
> -  return Quaternion(this->w(),-this->x(),-this->y(),-this->z());
> +  return Quaternion<Scalar>(this->w(),-this->x(),-this->y(),-this->z());
>  }
>
>  /** \returns the angle (in radian) between two rotations
>   * \sa dot()
>   */
> -template <typename Scalar>
> -inline Scalar Quaternion<Scalar>::angularDistance(const Quaternion& other)
> const
> +template <typename Scalar, class StorageType>
> +template <class OtherStorageType>
> +inline Scalar Quaternion<Scalar, StorageType>::angularDistance(const
> Quaternion<Scalar, OtherStorageType>& other) const
>  {
>   double d = ei_abs(this->dot(other));
>   if (d>=1.0)
> @@ -436,14 +453,15 @@
>  /** \returns the spherical linear interpolation between the two quaternions
>   * \c *this and \a other at the parameter \a t
>   */
> -template <typename Scalar>
> -Quaternion<Scalar> Quaternion<Scalar>::slerp(Scalar t, const Quaternion&
> other) const
> +template <typename Scalar, class StorageType>
> +template <class OtherStorageType>
> +Quaternion<Scalar> Quaternion<Scalar, StorageType>::slerp(Scalar t, const
> Quaternion<Scalar, OtherStorageType>& other) const
>  {
>   static const Scalar one = Scalar(1) - precision<Scalar>();
>   Scalar d = this->dot(other);
>   Scalar absD = ei_abs(d);
>   if (absD>=one)
> -    return *this;
> +    return Quaternion<Scalar>(*this);
>
>   // theta is the angle between the 2 quaternions
>   Scalar theta = std::acos(absD);
> @@ -454,7 +472,7 @@
>   if (d<0)
>     scale1 = -scale1;
>
> -  return Quaternion(scale0 * m_coeffs + scale1 * other.m_coeffs);
> +  return Quaternion<Scalar>(scale0 * m_coeffs + scale1 * other.coeffs());
>  }
>
>  // set from a rotation matrix
>
>