RigidTransform.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_RIGID_TRANSFORM_H_
#define _MIRA_RIGID_TRANSFORM_H_

#include <algorithm>
#include <limits>

#include <math/Angle.h>
#include <math/Eigen.h>
#include <math/Lerp.h>
#include <math/YawPitchRoll.h>

#include <serialization/GetterSetter.h>

#include <platform/Types.h>

namespace mira {



template< int D,
        typename TransformDerived,
        typename OtherDerived,
        int OtherRows=OtherDerived::RowsAtCompileTime,
        int OtherCols=OtherDerived::ColsAtCompileTime>
struct ei_rigidtransform_product_impl
{
    static_assert(D!=D,"You are trying to apply a rigid transform to a matrix "
                       "of invalid size");
};


template <typename T, int D>
class RigidTransform
{
    static_assert(D!=D,"RigidTransform is not defined for this dimension. "
                       "Only 2 and 3 dimensions are available.");
};


template <typename T, int D>
class RigidTransformCov : public RigidTransform<T,D>
{
    static_assert(D!=D,"RigidTransformCov is not defined for this dimension. "
                       "Only 2 and 3 dimensions are available.");
};


template <typename T, int D, typename TRotation, typename TransformDerived>
class RigidTransformBase
{
public:

    typedef T Type;

    typedef Eigen::Matrix<T,D,1> TranslationType;

    typedef TRotation RotationType;

    enum {
        Dim  = D,    
        HDim = D+1   
    };

    typedef Eigen::Matrix<T,HDim,HDim> MatrixType;

public:

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    RigidTransformBase(TranslationType translation, RotationType rotation) :
        t(translation), r(rotation) {}


public:

    TransformDerived inverse() const
    {
        RotationType inv = r.inverse();
        return TransformDerived(inv.RotationType::operator*(-t), inv);
    }

    bool isApprox(const RigidTransformBase& other,
                  T prec = std::numeric_limits<T>::epsilon()) const
    {
        return t.isApprox(other.t,prec) && r.isApprox(other.r, prec);
    }

public:

    TransformDerived& operator*= (const RigidTransformBase& other)
    {
        t += r * other.t;
        r *= other.r;
        return *This();
    }

    friend TransformDerived operator* (const RigidTransformBase& a,
                                       const RigidTransformBase& b) {
        return mul(a,b);
    }

    template <typename OtherDerived>
    typename ei_rigidtransform_product_impl<D, TransformDerived, OtherDerived>::TResult
    operator*(const Eigen::MatrixBase<OtherDerived> &other) const
    {
        return ei_rigidtransform_product_impl<D, TransformDerived, OtherDerived>::run(*this, other.derived());
    }

public:

    operator Eigen::Transform<T,D,Eigen::Isometry>()
    {
        // order of transforms is from right to left (i.e. rotate first and
        // translate afterwards)
        return Eigen::Translation<T,D>(t)*r;
    }

public:

    MatrixType getMatrix() const
    {
        MatrixType m = MatrixType::Identity ();
        m.template block<Dim,Dim>(0,0) = r.toRotationMatrix();
        m.template block<Dim,1>(0,Dim) = t.transpose();
        return m;
    }

protected:

    TransformDerived* This() { return static_cast<TransformDerived*>(this); }
    const TransformDerived* This() const {
        return static_cast<const TransformDerived*>(this);
    }

protected:

    static TransformDerived mul(const RigidTransformBase& a,
                                const RigidTransformBase& b)
    {
        RotationType r = a.r*b.r; // this must be assigned to a temp variable
                                  // (due to a bug in Eigen::Rotation2D<>? )
        return TransformDerived(a.r * b.t + a.t, r);
    }

public:

    TranslationType t;

    RotationType    r;
};


template <typename T>
class RigidTransform<T,2> : public RigidTransformBase<T, 2,
                                     Eigen::Rotation2D<T>, RigidTransform<T,2> >
{
    typedef RigidTransformBase<T, 2, Eigen::Rotation2D<T>, RigidTransform<T,2> > Base;

public:

    RigidTransform() :
        Base(Eigen::Matrix<T,2,1>(0,0),Eigen::Rotation2D<T>(0)) {}

    RigidTransform(const Eigen::Matrix<T,2,1>& translation,
                   const Eigen::Rotation2D<T>& rotation) :
        Base(translation,rotation) {}

    RigidTransform(const Eigen::Matrix<T,2,1>& translation, T angle) :
        Base(translation,Eigen::Rotation2D<T>(angle)) {}

    RigidTransform(T x, T y, T angle) :
        Base(Eigen::Matrix<T,2,1>(x,y),
        Eigen::Rotation2D<T>(angle)) {}

    template <typename UnitTag, typename Derived>
    RigidTransform(T x, T y, const AngleBase<T,UnitTag,Derived>& angle) :
        Base(Eigen::Matrix<T,2,1>(x,y),
        Eigen::Rotation2D<T>(angle.rad())) {}

public:

    template <typename U>
    RigidTransform<U,2> cast() const {
        return RigidTransform<U,2>(this->t.template cast<U>(),
                                   this->r.template cast<U>());
    }

public:

    T& x() { return this->t.x(); }
    T  x() const { return this->t.x(); }

    T& y() { return this->t.y(); }
    T  y() const { return this->t.y(); }

    T& phi() { return this->r.angle(); }
    T  phi() const { return this->r.angle(); }

public:

    template <typename Reflector>
    void reflect(Reflector& m)
    {
        m.property("X", this->t[0], "The translational part of the transform");
        m.property("Y", this->t[1], "The translational part of the transform");
        m.property("Phi", getter<T>(rad2deg<T>, this->r.angle()),
                          setter<T>(deg2rad<T>, this->r.angle()),
                          "The orientation in degrees");
    }

    friend std::ostream& operator<<(std::ostream& os, const RigidTransform& tf)
    {
        os << "t: " << tf.t.transpose() << "\n";
        os << "r: " << tf.r.angle() << "\n";
        return os;
    }
};


template <typename T>
class RigidTransformCov<T,2> : public RigidTransform<T, 2>
{
    typedef RigidTransform<T, 2> Base;

public:

    static const int CovarianceDim  = 3;

    typedef Eigen::Matrix<T,CovarianceDim,CovarianceDim> CovMatrixType;

    static CovMatrixType nullCov() {
        return CovMatrixType::Zero();
    }

public:

    RigidTransformCov() : Base(), cov(nullCov())
    {}

    RigidTransformCov(const Base& transform) : Base(transform),
            cov(nullCov()) {}

    RigidTransformCov(const Eigen::Matrix<T,2,1>& translation,
                      const Eigen::Rotation2D<T>& rotation,
                      const CovMatrixType& covariance) :
        Base(translation,rotation), cov(covariance) {}

    RigidTransformCov(const Eigen::Matrix<T,2,1>& translation, T angle,
                      const CovMatrixType& covariance) :
        Base(translation,Eigen::Rotation2D<T>(angle)),
        cov(covariance) {}

    RigidTransformCov(T x, T y, T angle, const CovMatrixType& covariance) :
        Base(Eigen::Matrix<T,2,1>(x,y),
        Eigen::Rotation2D<T>(angle)), cov(covariance) {}

    template <typename UnitTag, typename Derived>
    RigidTransformCov(T x, T y, const AngleBase<T,UnitTag,Derived>& angle,
                      const CovMatrixType& covariance) :
        Base(Eigen::Matrix<T,2,1>(x,y),
        Eigen::Rotation2D<T>(angle.rad())), cov(covariance) {}

    RigidTransformCov(T x, T y, T angle, T covX, T covY, T covAngle) :
        Base(Eigen::Matrix<T,2,1>(x,y),
        Eigen::Rotation2D<T>(angle)),
        cov(Eigen::DiagonalMatrix<T,3>(covX, covY, covAngle)) {}

    template <typename UnitTag, typename Derived>
    RigidTransformCov(T x, T y, const AngleBase<T,UnitTag,Derived>& angle,
                      T covX, T covY, T covAngle) :
        Base(Eigen::Matrix<T,2,1>(x,y),
        Eigen::Rotation2D<T>(angle.rad())),
        cov(Eigen::DiagonalMatrix<T,3>(covX, covY, covAngle)) {}

public:

    template <typename U>
    RigidTransformCov<U,2> cast() const {
        return RigidTransformCov<U,2>(this->t.template cast<U>(),
                                      this->r.template cast<U>(),
                                      this->cov.template cast<U>());
    }

public:

    template <typename Reflector>
    void reflect(Reflector& r)
    {
        MIRA_REFLECT_BASE(r, Base);
        r.member("Cov", cov, "The covariance matrix (using radians for phi)");
    }

    friend std::ostream& operator<<(std::ostream& os, const RigidTransformCov& tf)
    {
        os << "t: " << tf.t.transpose() << "\n";
        os << "r: " << tf.r.angle() << "\n";
        os << "cov:\n" << tf.cov << "\n";
        return os;
    }

public:
    // overwritten to implement covariance propagation

    RigidTransformCov inverse() const
    {
        typedef typename Base::RotationType RotationType;
        RotationType inv = this->r.inverse();

        CovMatrixType J;

        T sinr = std::sin(this->r.angle());
        T cosr = std::cos(this->r.angle());

        // compute the jacobians
        J << -cosr, -sinr,  this->t(0)*sinr -this->t(1)*cosr,
              sinr, -cosr,  this->t(0)*cosr +this->t(1)*sinr,
                 0,     0,                                -1;

        CovMatrixType c = J*cov*J.transpose();
        return RigidTransformCov(inv.RotationType::operator*(-this->t), inv, c);
    }

    RigidTransformCov& operator*= (const RigidTransformCov& other)
    {
        propagateCov(cov,*this,other); //< must be done here, since operator* changes our value
        Base::operator*=(other);
        return *this;
    }

    RigidTransformCov& operator*= (const Base& other)
    {
        propagateCov(cov,*this,other); //< must be done here, since operator* changes our value
        Base::operator*=(other);
        return *this;
    }

    friend RigidTransformCov operator* (const RigidTransformCov& a,
                                        const RigidTransformCov& b)
    {
        RigidTransformCov t = Base::mul(a,b);
        propagateCov(t.cov,a,b);
        return t;
    }

    friend RigidTransformCov operator* (const RigidTransformCov& a,
                                        const Base& b)
    {
        RigidTransformCov t = Base::mul(a,b);
        propagateCov(t.cov,a,b);
        return t;
    }

    friend RigidTransformCov operator* (const Base& a,
                                        const RigidTransformCov& b)
    {
        RigidTransformCov t = Base::mul(a,b);
        propagateCov(t.cov,a,b);
        return t;
    }

private:

    // propagates the covariance matices of both transforms (impl. see below)
    static void propagateCov(CovMatrixType& oC, const RigidTransformCov& a,
                             const RigidTransformCov& w);
    // same as above, but w does not carry any covariance (impl. see below)
    static void propagateCov(CovMatrixType& oC, const RigidTransformCov& a,
                             const Base& w);
    // same as above, but a does not carry any covariance (impl. see below)
    static void propagateCov(CovMatrixType& C, const Base& a,
                             const RigidTransformCov& w);

public:

    CovMatrixType cov;
};


template <typename T>
class RigidTransform<T,3> : public RigidTransformBase<T, 3,
                                     Eigen::Quaternion<T>, RigidTransform<T,3> >
{
    typedef RigidTransformBase<T, 3, Eigen::Quaternion<T>, RigidTransform<T,3> > Base;

public:

    RigidTransform() :
        Base(Eigen::Matrix<T,3,1>(0,0,0), Eigen::Quaternion<T>::Identity()) {}

    RigidTransform(const Eigen::Matrix<T,3,1>& translation,
                   const Eigen::Quaternion<T>& rotation) :
        Base(translation,rotation) {}

    RigidTransform(T x, T y, T z, T yaw, T pitch, T roll) :
        Base(Eigen::Matrix<T,3,1>(x,y,z),
             quaternionFromYawPitchRoll(yaw,pitch,roll)) {}

public:

    template <typename U>
    RigidTransform<U,3> cast() const {
        return RigidTransform<U,3>(this->t.template cast<U>(),
                                   this->r.template cast<U>());
    }

public:

    T& x() { return this->t.x(); }
    T  x() const { return this->t.x(); }

    T& y() { return this->t.y(); }
    T  y() const { return this->t.y(); }

    T& z() { return this->t.z(); }
    T  z() const { return this->t.z(); }


    T yaw() const { return eulerAngles(this->r.toRotationMatrix(), 2,1,0)(0,0); }

    T pitch() const { return eulerAngles(this->r.toRotationMatrix(), 2,1,0)(1,0); }

    T roll() const { return eulerAngles(this->r.toRotationMatrix(), 2,1,0)(2,0); }

public:

    template <typename Derived>
    void reflect(BinarySerializer<Derived>& r)
    {
        r.property("X",   this->t[0],   "The translational part of the transform");
        r.property("Y",   this->t[1],   "The translational part of the transform");
        r.property("Z",   this->t[2],   "The translational part of the transform");
        // rotation
        r.member("Rotation", this->r.coeffs(), "The rotational part");
    }

    template <typename Derived>
    void reflect(BinaryDeserializer<Derived>& r)
    {
        r.property("X",   this->t[0],   "The translational part of the transform");
        r.property("Y",   this->t[1],   "The translational part of the transform");
        r.property("Z",   this->t[2],   "The translational part of the transform");
        // rotation
        r.member("Rotation", this->r.coeffs(), "The rotational part");
    }

    template <typename Reflector>
    void reflectRead(Reflector& r)
    {
        r.property("X",   this->t[0],   "The translational part of the transform");
        r.property("Y",   this->t[1],   "The translational part of the transform");
        r.property("Z",   this->t[2],   "The translational part of the transform");

        auto ypr = quaternionToYawPitchRoll(this->r);
        T yaw   = rad2deg(ypr.template get<0>());
        T pitch = rad2deg(ypr.template get<1>());
        T roll  = rad2deg(ypr.template get<2>());
        r.member("Yaw"  , yaw,   "The yaw angle (phi, in degrees)");
        r.member("Pitch", pitch, "The pitch angle (theta, in degrees)");
        r.member("Roll" , roll,  "The roll angle (psi, in degrees)");
    }

    template <typename Reflector>
    void reflectWrite(Reflector& r)
    {
        T yaw, pitch, roll;
        r.property("X",   this->t[0],   "The translational part of the transform");
        r.property("Y",   this->t[1],   "The translational part of the transform");
        r.property("Z",   this->t[2],   "The translational part of the transform");
        r.member("Yaw"  , yaw,   "The yaw angle (phi, in degrees)");
        r.member("Pitch", pitch, "The pitch angle (theta, in degrees)");
        r.member("Roll" , roll,  "The roll angle (psi, in degrees)");
        this->r = quaternionFromYawPitchRoll(deg2rad(yaw),deg2rad(pitch),deg2rad(roll));
    }

    MIRA_SPLIT_REFLECT_MEMBER

    friend std::ostream& operator<<(std::ostream& os, const RigidTransform& tf)
    {
        os << "t: " << tf.t.transpose() << "\n";
        os << "r: " << tf.r.w() << " " << tf.r.x() << " " << tf.r.y() << " " << tf.r.z() << "\n";
        return os;
    }
};


template <typename T>
class RigidTransformCov<T,3> : public RigidTransform<T, 3>
{
    typedef RigidTransform<T, 3> Base;

public:
    static const int CovarianceDim = 7;

    static const int EulerCovarianceDim = 6;

    typedef Eigen::Matrix<T,CovarianceDim,CovarianceDim> CovMatrixType;

    typedef Eigen::Matrix<T,EulerCovarianceDim,EulerCovarianceDim> YawPitchRollCovMatrixType;

public:

    static CovMatrixType nullCov() {
        return CovMatrixType::Zero();
    }

    static YawPitchRollCovMatrixType nullYawPitchRollCov() {
        return YawPitchRollCovMatrixType::Zero();
    }

public:

    RigidTransformCov() : Base(), cov(nullCov()) {}

    RigidTransformCov(const Base& transform) : Base(transform),
            cov(nullCov()) {}

    RigidTransformCov(const Eigen::Matrix<T,3,1>& translation,
                      const Eigen::Quaternion<T>& rotation,
                      const CovMatrixType& covariance) :
        Base(translation,rotation), cov(covariance) {}

    RigidTransformCov(const Eigen::Matrix<T,3,1>& translation,
                      const Eigen::Quaternion<T>& rotation,
                      const YawPitchRollCovMatrixType& yawPitchRollCov) :
        Base(translation, rotation),
        cov(quaternionCovFromYawPitchRollCov(yawPitchRollCov, rotation)){}

    RigidTransformCov(T x, T y, T z, T yaw, T pitch, T roll,
                      const YawPitchRollCovMatrixType& yawPitchRollCov)  {
        Eigen::Quaternion<T> rotation;
        quaternionCovFromYawPitchRollCov(yawPitchRollCov,  yaw, pitch, roll,
                                         rotation, cov);
        this->t = Eigen::Matrix<T,3,1>(x,y,z);
        this->r = rotation;
    }

public:

    template <typename U>
    RigidTransformCov<U,3> cast() const {
        typedef typename RigidTransformCov<U,3>::CovMatrixType CovMatrixTypeU;
        return RigidTransformCov<U,3>(this->t.template cast<U>(),
                                      this->r.template cast<U>(),
                                      CovMatrixTypeU(this->cov.template cast<U>()));
    }

public:

    YawPitchRollCovMatrixType getYawPitchRollCov() const {
        return quaternionCovToYawPitchRollCov(cov,this->r);
    }

public:

    template <typename Derived>
    void reflect(BinarySerializer<Derived>& r)
    {
        r.property("X",   this->t[0],   "The translational part of the transform");
        r.property("Y",   this->t[1],   "The translational part of the transform");
        r.property("Z",   this->t[2],   "The translational part of the transform");
        // rotation
        r.member("Rotation", this->r.coeffs(), "The rotational part");
        // 7x7 cov matrix
        r.member("Cov", cov, "The covariance matrix");
    }

    template <typename Derived>
    void reflect(BinaryDeserializer<Derived>& r)
    {
        r.property("X",   this->t[0],   "The translational part of the transform");
        r.property("Y",   this->t[1],   "The translational part of the transform");
        r.property("Z",   this->t[2],   "The translational part of the transform");
        // rotation
        r.member("Rotation", this->r.coeffs(), "The rotational part");
        // 7x7 cov matrix
        r.member("Cov", cov, "The covariance matrix");
    }

    template <typename Reflector>
    void reflectRead(Reflector& r)
    {
        r.property("X",   this->t[0],   "The translational part of the transform");
        r.property("Y",   this->t[1],   "The translational part of the transform");
        r.property("Z",   this->t[2],   "The translational part of the transform");

        auto ypr = quaternionToYawPitchRoll(this->r);
        T yaw   = rad2deg(ypr.template get<0>());
        T pitch = rad2deg(ypr.template get<1>());
        T roll  = rad2deg(ypr.template get<2>());
        r.member("Yaw"  , yaw,   "The yaw angle (phi, in degrees)");
        r.member("Pitch", pitch, "The pitch angle (theta, in degrees)");
        r.member("Roll" , roll,  "The roll angle (psi, in degrees)");

        // convert to yaw pitch roll cov
        YawPitchRollCovMatrixType yprcov = quaternionCovToYawPitchRollCov(this->cov, this->r);
        r.member("Cov", yprcov, "The covariance matrix for [x,y,z,qw,qx,qy,qz]");
    }

    template <typename Reflector>
    void reflectWrite(Reflector& r)
    {
        T yaw, pitch, roll;
        r.property("X", this->t[0], "The translational part of the transform");
        r.property("Y", this->t[1], "The translational part of the transform");
        r.property("Z", this->t[2], "The translational part of the transform");
        r.member("Yaw"  , yaw,   "The yaw angle (phi, in degrees)");
        r.member("Pitch", pitch, "The pitch angle (theta, in degrees)");
        r.member("Roll" , roll,  "The roll angle (psi, in degrees)");

        yaw   = deg2rad(yaw);
        pitch = deg2rad(pitch);
        roll  = deg2rad(roll);
        this->r = quaternionFromYawPitchRoll(yaw,pitch,roll);

        YawPitchRollCovMatrixType yprcov;
        r.member("Cov", yprcov, "The covariance matrix for [x,y,z,qw,qx,qy,qz]");

        // convert to quaternion cov
        this->cov = quaternionCovFromYawPitchRollCov(yprcov, yaw, pitch, roll);
    }

    MIRA_SPLIT_REFLECT_MEMBER

    friend std::ostream& operator<<(std::ostream& os, const RigidTransformCov& tf)
    {
        os << "t: " << tf.t.transpose() << "\n";
        os << "r: " << tf.r.w() << " " << tf.r.x() << " " << tf.r.y() << " " << tf.r.z() << "\n";
        os << "cov:\n" << tf.cov << "\n";
        return os;
    }

public:

    // overwritten to implement covariance propagation

    RigidTransformCov inverse() const
    {
        typedef typename Base::RotationType RotationType;
        RotationType inv = this->r.inverse();

        CovMatrixType J = -CovMatrixType::Identity();

        J(3,3) = 1.0;
        J.template block<3,3>(0,0) <<
                 (T)2.0 * (this->r.y() * this->r.y() + this->r.z() * this->r.z()) - (T)1.0,
                -(T)2.0 * (this->r.w() * this->r.z() + this->r.x() * this->r.y())      ,
                 (T)2.0 * (this->r.w() * this->r.y() - this->r.x() * this->r.z())      ,

                 (T)2.0 * (this->r.w() * this->r.z() - this->r.x() * this->r.y())      ,
                 (T)2.0 * (this->r.x() * this->r.x() + this->r.z() * this->r.z()) - (T)1.0,
                -(T)2.0 * (this->r.w() * this->r.x() + this->r.y() * this->r.z())      ,

                -(T)2.0 * (this->r.w() * this->r.y() + this->r.x() * this->r.z())      ,
                 (T)2.0 * (this->r.w() * this->r.x() - this->r.y() * this->r.z())      ,
                 (T)2.0 * (this->r.x() * this->r.x() + this->r.y() * this->r.y()) - (T)1.0;

        // signs seem to be partially wrong in blanco2010se3 (see YawPitchRoll.h)
        // this now corresponds to MRPT 1.5.5 CPose3DQuat::inverseComposePoint
        J.template block<3,4>(0,3) <<
                -(T)2.0 * (this->r.z() * this->t(1) -          this->r.y() * this->t(2)                                 ),
                -(T)2.0 * (this->r.y() * this->t(1) +          this->r.z() * this->t(2)                                 ),
                -(T)2.0 * (this->r.x() * this->t(1) - (T)2.0 * this->r.y() * this->t(0) -          this->r.w() * this->t(2)),
                -(T)2.0 * (this->r.x() * this->t(2) +          this->r.w() * this->t(1) - (T)2.0 * this->r.z() * this->t(0)),

                -(T)2.0 * (this->r.x() * this->t(2) -          this->r.z() * this->t(0)                                 ),
                -(T)2.0 * (this->r.y() * this->t(0) - (T)2.0 * this->r.x() * this->t(1) +       this->r.w() * this->t(2)),
                -(T)2.0 * (this->r.x() * this->t(0) +          this->r.z() * this->t(2)                                 ),
                -(T)2.0 * (this->r.y() * this->t(2) - (T)2.0 * this->r.z() * this->t(1) -       this->r.w() * this->t(0)),

                -(T)2.0 * (this->r.y() * this->t(0) -       this->r.x() * this->t(1)                                 ),
                -(T)2.0 * (this->r.z() * this->t(0) -       this->r.w() * this->t(1) - (T)2.0 * this->r.x() * this->t(2)),
                -(T)2.0 * (this->r.z() * this->t(1) +       this->r.w() * this->t(0) - (T)2.0 * this->r.y() * this->t(2)),
                -(T)2.0 * (this->r.x() * this->t(0) +       this->r.y() * this->t(1)                                 );

        CovMatrixType c = J*cov*J.transpose();
        return RigidTransformCov(inv.RotationType::operator*(-this->t), inv, c);
    }

    RigidTransformCov& operator*= (const RigidTransformCov& other)
    {
        propagateCov(cov,*this,other); // must be done here, since operator* changes our value
        Base::operator*=(other);
        return *this;
    }

    RigidTransformCov& operator*= (const Base& other)
    {
        propagateCov(cov,*this,other); // must be done here, since operator* changes our value
        Base::operator*=(other);
        return *this;
    }

    friend RigidTransformCov operator* (const RigidTransformCov& a,
                                        const RigidTransformCov& b)
    {
        RigidTransformCov t = Base::mul(a,b);
        propagateCov(t.cov,a,b);
        return t;
    }

    friend RigidTransformCov operator* (const RigidTransformCov& a,
                                        const Base& b)
    {
        RigidTransformCov t = Base::mul(a,b);
        propagateCov(t.cov,a,b);
        return t;
    }

    friend RigidTransformCov operator* (const Base& a,
                                        const RigidTransformCov& b)
    {
        RigidTransformCov t = Base::mul(a,b);
        propagateCov(t.cov,a,b);
        return t;
    }

private:

    // propagates the covariance matices to of both transforms (impl. see below)
    static void propagateCov(CovMatrixType& oC,
                        const RigidTransformCov& w, const RigidTransformCov& a);
    // same as above, but w does not carry any covariance (impl. see below)
    static void propagateCov(CovMatrixType& oC,
                        const RigidTransformCov& w, const Base& a);
    // same as above, but a does not carry any covariance (impl. see below)
    static void propagateCov(CovMatrixType& C,
                        const Base& w, const RigidTransformCov& a);

public:

    CovMatrixType cov;
};


typedef RigidTransform<float,2> RigidTransform2f;
typedef RigidTransform<float,3> RigidTransform3f;

typedef RigidTransform<double,2> RigidTransform2d;
typedef RigidTransform<double,3> RigidTransform3d;

typedef RigidTransformCov<float,2> RigidTransformCov2f;
typedef RigidTransformCov<float,3> RigidTransformCov3f;

typedef RigidTransformCov<double,2> RigidTransformCov2d;
typedef RigidTransformCov<double,3> RigidTransformCov3d;

// Covariance propagation maths

// 2D case:
//
// g(a,w) = w * a
//        = [ Rw * ta + tw ;
//             phia + phiw ]
//
// Ja = d g(a,w) / da
// Jw = d g(a,w) / dw
// C = Ja*Ca*Ja' + Jw*Cw*Jw'

template <typename T>
inline void RigidTransformCov<T,2>::propagateCov(CovMatrixType& oC,
                                                 const RigidTransformCov& w,
                                                 const RigidTransformCov& a)
{
    // NOTE: change oC at the end only, since it may be a reference on
    //       the parameter w or a!

    typedef Eigen::Matrix<T,3,3> Mat3;
    Mat3 Ja, Jw;

    T sinrw = std::sin(w.r.angle());
    T cosrw = std::cos(w.r.angle());

    // compute the jacobians
    Ja << cosrw, -sinrw, 0,
          sinrw,  cosrw, 0,
              0,      0, 1;

    Jw << 1, 0, -a.t(0)*sinrw -a.t(1)*cosrw,
          0, 1,  a.t(0)*cosrw -a.t(1)*sinrw,
          0, 0,  1;

    // propagate the covariances:
    oC = Ja * a.cov * Ja.transpose() + Jw * w.cov * Jw.transpose();
}

// same as above, but w does not carry any covariance
template <typename T>
inline void RigidTransformCov<T,2>::propagateCov(CovMatrixType& oC,
                                                 const RigidTransformCov& w,
                                                 const Base& a)
{
    // NOTE: change oC at the end only,
    //       since it may be a reference on the parameter w or a!

    typedef Eigen::Matrix<T,3,3> Mat3;
    Mat3 Jw;

    T sinrw = std::sin(w.r.angle());
    T cosrw = std::cos(w.r.angle());

    // compute the jacobians
    Jw << 1, 0, -a.t(0)*sinrw -a.t(1)*cosrw,
          0, 1,  a.t(0)*cosrw -a.t(1)*sinrw,
          0, 0,  1;

    // propagate the covariances:
    oC = Jw * w.cov * Jw.transpose();
}

template <typename T>
inline void RigidTransformCov<T,2>::propagateCov(CovMatrixType& oC,
                                                 const Base& w,
                                                 const RigidTransformCov& a)
{
    // NOTE: change oC at the end only,
    //       since it may be a reference on the parameter w or a!

    typedef Eigen::Matrix<T,3,3> Mat3;
    Mat3 Ja;

    T sinrw = std::sin(w.r.angle());
    T cosrw = std::cos(w.r.angle());

    // compute the jacobians
    Ja << cosrw, -sinrw, 0,
          sinrw,  cosrw, 0,
              0,      0, 1;

    // propagate the covariances:
    oC = Ja * a.cov * Ja.transpose();
}

// 3D case:
//
//
// g(a,w) = w * a
//        = [ qw * ta + tw ;
//            qw * qa ]
//
// Ja = d g(a,w) / da
// Jw = d g(a,w) / dw
// C = Ja*Ca*Ja' + Jw*Cw*Jw'
template <typename T>
inline void RigidTransformCov<T,3>::propagateCov(CovMatrixType& oC,
                                                 const RigidTransformCov& w,
                                                 const RigidTransformCov& a)
{
    // NOTE: change oC at the end only,
    //       since it may be a reference on the parameter w or a!

    CovMatrixType Jw = CovMatrixType::Identity();

    Jw.template block<3,4>(0,3) <<
                                    -    w.r.z()*a.t(1,0)+    w.r.y()*a.t(2,0),
                                         w.r.y()*a.t(1,0)+    w.r.z()*a.t(2,0),
            -(T)2.0*w.r.y()*a.t(0,0)+    w.r.x()*a.t(1,0)+    w.r.w()*a.t(2,0),
            -(T)2.0*w.r.z()*a.t(0,0)-    w.r.w()*a.t(1,0)+    w.r.x()*a.t(2,0),

                 w.r.z()*a.t(0,0)                        -    w.r.x()*a.t(2,0),
                 w.r.y()*a.t(0,0)-(T)2.0*w.r.x()*a.t(1,0)-    w.r.w()*a.t(2,0),
                 w.r.x()*a.t(0,0)                        +    w.r.z()*a.t(2,0),
                 w.r.w()*a.t(0,0)-(T)2.0*w.r.z()*a.t(1,0)+    w.r.y()*a.t(2,0),

                -w.r.y()*a.t(0,0)+    w.r.x()*a.t(1,0)                        ,
                 w.r.z()*a.t(0,0)+    w.r.w()*a.t(1,0)-(T)2.0*w.r.x()*a.t(2,0),
                -w.r.w()*a.t(0,0)+    w.r.z()*a.t(1,0)-(T)2.0*w.r.y()*a.t(2,0),
                 w.r.x()*a.t(0,0)+    w.r.y()*a.t(1,0);
    Jw.template block<3,4>(0,3) *= (T)2.0;

    Jw.template block<4,4>(3,3) <<
            a.r.w(),-a.r.x(),-a.r.y(),-a.r.z(),
            a.r.x(), a.r.w(), a.r.z(),-a.r.y(),
            a.r.y(),-a.r.z(), a.r.w(), a.r.x(),
            a.r.z(), a.r.y(),-a.r.x(), a.r.w();

    CovMatrixType Ja = CovMatrixType::Identity();

    Ja.template block<3,3>(0,0) <<
            (T)0.5-w.r.y()*w.r.y()-w.r.z()*w.r.z(),
                   w.r.x()*w.r.y()-w.r.w()*w.r.z(),
                   w.r.w()*w.r.y()+w.r.x()*w.r.z(),

                   w.r.w()*w.r.z()+w.r.x()*w.r.y(),
            (T)0.5-w.r.x()*w.r.x()-w.r.z()*w.r.z(),
                   w.r.y()*w.r.z()-w.r.w()*w.r.x(),

                   w.r.x()*w.r.z()-w.r.w()*w.r.y(),
                   w.r.w()*w.r.x()+w.r.y()*w.r.z(),
            (T)0.5-w.r.x()*w.r.x()-w.r.y()*w.r.y();
    Ja.template block<3,3>(0,0) *= (T)2.0;

    Ja.template block<4,4>(3,3) <<
            w.r.w(), -w.r.x(), -w.r.y(), -w.r.z(),
            w.r.x(),  w.r.w(), -w.r.z(),  w.r.y(),
            w.r.y(),  w.r.z(),  w.r.w(), -w.r.x(),
            w.r.z(), -w.r.y(),  w.r.x(),  w.r.w();

    // propagate the covariances:
    oC = Jw * w.cov * Jw.transpose() + Ja * a.cov * Ja.transpose();
}

template <typename T>
inline void RigidTransformCov<T,3>::propagateCov(CovMatrixType& oC,
                                                 const RigidTransformCov& w,
                                                 const Base& a)
{
    // NOTE: change oC at the end only,
    //       since it may be a reference on the parameter w or a!

    CovMatrixType Jw = CovMatrixType::Identity();

    Jw.template block<3,4>(0,3) <<
                                    -    w.r.z()*a.t(1,0)+    w.r.y()*a.t(2,0),
                                         w.r.y()*a.t(1,0)+    w.r.z()*a.t(2,0),
            -(T)2.0*w.r.y()*a.t(0,0)+    w.r.x()*a.t(1,0)+    w.r.w()*a.t(2,0),
            -(T)2.0*w.r.z()*a.t(0,0)-    w.r.w()*a.t(1,0)+    w.r.x()*a.t(2,0),

                 w.r.z()*a.t(0,0)                        -    w.r.x()*a.t(2,0),
                 w.r.y()*a.t(0,0)-(T)2.0*w.r.x()*a.t(1,0)-    w.r.w()*a.t(2,0),
                 w.r.x()*a.t(0,0)                        +    w.r.z()*a.t(2,0),
                 w.r.w()*a.t(0,0)-(T)2.0*w.r.z()*a.t(1,0)+    w.r.y()*a.t(2,0),

                -w.r.y()*a.t(0,0)+    w.r.x()*a.t(1,0)                     ,
                 w.r.z()*a.t(0,0)+    w.r.w()*a.t(1,0)-2.0*w.r.x()*a.t(2,0),
                -w.r.w()*a.t(0,0)+    w.r.z()*a.t(1,0)-2.0*w.r.y()*a.t(2,0),
                 w.r.x()*a.t(0,0)+    w.r.y()*a.t(1,0);
    Jw.template block<3,4>(0,3) *= (T)2.0;

    Jw.template block<4,4>(3,3) <<
            a.r.w(),-a.r.x(),-a.r.y(),-a.r.z(),
            a.r.x(), a.r.w(), a.r.z(),-a.r.y(),
            a.r.y(),-a.r.z(), a.r.w(), a.r.x(),
            a.r.z(), a.r.y(),-a.r.x(), a.r.w();

    // propagate the covariances:
    oC = Jw * w.cov * Jw.transpose();
}

template <typename T>
inline void RigidTransformCov<T,3>::propagateCov(CovMatrixType& oC,
                                                 const Base& w,
                                                 const RigidTransformCov& a)
{
    // NOTE: change oC at the end only,
    //       since it may be a reference on the parameter w or a!

    CovMatrixType Ja = CovMatrixType::Identity();

    Ja.template block<3,3>(0,0) <<
            (T)0.5-w.r.y()*w.r.y()-w.r.z()*w.r.z(),
                   w.r.x()*w.r.y()-w.r.w()*w.r.z(),
                   w.r.w()*w.r.y()+w.r.x()*w.r.z(),

                   w.r.w()*w.r.z()+w.r.x()*w.r.y(),
            (T)0.5-w.r.x()*w.r.x()-w.r.z()*w.r.z(),
                   w.r.y()*w.r.z()-w.r.w()*w.r.x(),

                   w.r.x()*w.r.z()-w.r.w()*w.r.y(),
                   w.r.w()*w.r.x()+w.r.y()*w.r.z(),
            (T)0.5-w.r.x()*w.r.x()-w.r.y()*w.r.y();
    Ja.template block<3,3>(0,0) *= (T)2.0;

    Ja.template block<4,4>(3,3) <<
            w.r.w(), -w.r.x(), -w.r.y(), -w.r.z(),
            w.r.x(),  w.r.w(), -w.r.z(),  w.r.y(),
            w.r.y(),  w.r.z(),  w.r.w(), -w.r.x(),
            w.r.z(), -w.r.y(),  w.r.x(),  w.r.w();

    // propagate the covariances:
    oC = Ja * a.cov * Ja.transpose();
}

// Linear interpolation stuff

template <typename T, int D, typename S>
RigidTransform<T, D> lerp(const RigidTransform<T, D>& t1,
                          const RigidTransform<T, D>& t2,
                          S alpha)
{
    return RigidTransform<T, D>(lerp(t1.t, t2.t, alpha),  // interpolate translation
                                lerp(t1.r, t2.r, alpha)); // interpolate rotation
}

template <typename T, int D, typename S>
RigidTransformCov<T, D> lerp(const RigidTransformCov<T, D>& t1,
                             const RigidTransformCov<T, D>& t2,
                             S alpha)
{
    return RigidTransformCov<T, D>(lerp(t1.t, t2.t, alpha),  // interpolate translation
                                   lerp(t1.r, t2.r, alpha),  // interpolate rotation
                                   lerp(t1.cov, t2.cov, alpha)); // interpolate covariance
}


// Sanity checks

template<typename T, int D>
bool isWellDefined(const RigidTransform<T, D>& p)
{
    const auto transformMatrix = p.getMatrix();
    constexpr auto matrixDim = RigidTransform<T, D>::HDim;
    for (int i = 0; i < matrixDim; ++i) {
        for (int j = 0; j < matrixDim; ++j) {
            if (boost::math::isnan(transformMatrix(i,j))) {
                return false;
            }
        }
    }
    return true;
}

template<typename T, int D>
bool isWellDefined(const RigidTransformCov<T, D>& p)
{
    const auto transformValid = isWellDefined(static_cast<const RigidTransform<T, D>&>(p));
    if (!transformValid) {
        return false;
    }

    constexpr auto precision = Eigen::NumTraits<T>::dummy_precision();
    constexpr auto covDim = RigidTransformCov<T, D>::CovarianceDim;

    // check for nan
    for (int i = 0; i < covDim; ++i) {
        for (int j = 0; j < covDim; ++j) {
            if (boost::math::isnan(p.cov(i, j))) {
                return false;
            }
        }
    }

    // check for symmetry
    for (int i = 0; i < covDim; ++i) {
        for (int j = i + 1; j < covDim; ++j) {
            const bool almostEqual = std::abs(p.cov(i, j) - p.cov(j, i)) <= precision;
            if (!almostEqual) {
                return false;
            }
        }
    }

    // is positive semi definite?
    Eigen::LDLT<typename RigidTransformCov<T, D>::CovMatrixType> ldlt(p.cov);
    const auto covValid = ldlt.info() != Eigen::NumericalIssue && ldlt.isPositive();
    return covValid;
}

// specializations for Dx1 and dynamic matrices

template< int D, typename TransformDerived, typename OtherDerived>
struct ei_rigidtransform_product_impl<D, TransformDerived, OtherDerived,D, 1>
{
    typedef typename OtherDerived::Scalar Scalar;
    typedef RigidTransformBase<Scalar, D, typename TransformDerived::RotationType, TransformDerived> TTransform;
    typedef const Eigen::Matrix<Scalar, D, 1> TResult;


    static TResult run(const TTransform& t, const OtherDerived& v)
    {
        // TODO: increase performance by using expression templates see Eigen/Geometry/Transform.h
        return t.r*v + t.t;
    }
};

template< int D, typename TransformDerived, typename OtherDerived>
struct ei_rigidtransform_product_impl<D, TransformDerived, OtherDerived,Eigen::Dynamic, Eigen::Dynamic>
{
    typedef typename OtherDerived::Scalar Scalar;
    typedef RigidTransformBase<Scalar, D, typename TransformDerived::RotationType, TransformDerived> TTransform;
    typedef const Eigen::Matrix<Scalar, D, 1> TResult;


    static TResult run(const TTransform& t, const OtherDerived& v)
    {
        // TODO: increase performance by using expression templates see Eigen/Geometry/Transform.h
        return t.r*v + t.t;
    }
};



}

#endif