TransformCast.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_TRANSFORMCAST_H__
#define _MIRA_TRANSFORMCAST_H__

#include <transform/RigidTransform.h>
#include <math/YawPitchRoll.h>

namespace mira {



template<typename TargetTransform,
         typename TargetT, int TargetDim,
         typename SourceTransform,
         typename SourceT, int SourceDim>
struct TransformCast {
    static TargetTransform invoke(const SourceTransform& other) {
        RigidTransform<TargetT,SourceDim> tfWithTargetT(other.t.template cast<TargetT>(), other.r.template cast<TargetT>());
        return TransformCast<RigidTransform<TargetT,TargetDim>, TargetT, TargetDim,
                             RigidTransform<TargetT,SourceDim>, TargetT, SourceDim>::invoke(tfWithTargetT);
    }
};

template<typename TargetT, int TargetDim,
         typename SourceTransform,
         typename SourceT, int SourceDim>
struct TransformCast<RigidTransformCov<TargetT, TargetDim>, TargetT, TargetDim, SourceTransform, SourceT, SourceDim> {
    static RigidTransformCov<TargetT, TargetDim> invoke(const SourceTransform& other) {
        RigidTransformCov<TargetT,SourceDim> tfWithTargetT(other.template cast<TargetT>());
        return TransformCast<RigidTransformCov<TargetT, TargetDim>, TargetT, TargetDim,
                             RigidTransformCov<TargetT,SourceDim>, TargetT, SourceDim>::invoke(tfWithTargetT);
    }
};

// RigidTransform and RigidTransformCov with same T and Dim need separate specializations,
// otherwise some uses are ambiguous between this and the previous one

template<typename T, int Dim>
struct TransformCast<RigidTransform<T, Dim>, T, Dim, RigidTransform<T, Dim>, T, Dim> {
    static RigidTransform<T, Dim> invoke(const RigidTransform<T, Dim>& other) {
        return other;
    }
};

template<typename T, int Dim>
struct TransformCast<RigidTransformCov<T, Dim>, T, Dim, RigidTransformCov<T, Dim>, T, Dim> {
    static RigidTransformCov<T, Dim> invoke(const RigidTransformCov<T, Dim>& other) {
        return other;
    }
};

// specializations for valid combinations of transforms:

// converts from 3D RigidTransform to 2D RigidTransform
template<typename T>
struct TransformCast<RigidTransform<T,2>, T, 2, RigidTransform<T,3>, T, 3> {
    static RigidTransform<T,2> invoke(const RigidTransform<T,3>& other) {
        // extract: yaw (phi), pitch (theta), roll (psi)
        Eigen::Matrix<T,3,1> euler =
                             eulerAngles(other.r.toRotationMatrix(), 2, 1, 0);
        return RigidTransform<T,2>(other.x(), other.y(), euler(0,0) );
    }
};

// converts from 2D RigidTransform to 3D RigidTransform, z and the two
// additional euler angles are set to 0
template<typename T>
struct TransformCast<RigidTransform<T,3>, T, 3, RigidTransform<T,2>, T, 2> {
    static RigidTransform<T,3> invoke(const RigidTransform<T,2>& other) {
        return RigidTransform<T,3>(Eigen::Matrix<T,3,1>(other.x(), other.y(),0),
             Eigen::Quaternion<T>(Eigen::AngleAxis<T>(other.phi(),
                                  Eigen::Matrix<T,3,1>::UnitZ())));
    }
};

// converts from 3D RigidTransformCov to 2D RigidTransformCov
template<typename T>
struct TransformCast<RigidTransformCov<T,2>, T, 2, RigidTransformCov<T,3>, T, 3> {
    static RigidTransformCov<T,2> invoke(const RigidTransformCov<T,3>& other) {
        // extract: yaw (phi), pitch (theta), roll (psi)
        Eigen::Matrix<T,3,1> euler = 
                              eulerAngles(other.r.toRotationMatrix(), 2, 1, 0);

        typename RigidTransformCov<T,2>::CovMatrixType cov;

        typename RigidTransformCov<T,3>::YawPitchRollCovMatrixType otherEulerCov =
                quaternionCovToYawPitchRollCov(other.cov, other.r);

        // copy 2x2 translation cov part: xy-xy
        cov.template block<2,2>(0,0) = otherEulerCov.template block<2,2>(0,0);
        // xy-phi
        cov.template block<2,1>(0,2) = otherEulerCov.template block<2,1>(0,3);
        // xy-phi
        cov.template block<1,2>(2,0) = otherEulerCov.template block<1,2>(3,0);
        // phi-phi
        cov(2,2) = otherEulerCov(3,3);

        return RigidTransformCov<T,2>(other.x(), other.y(), euler(0,0), cov);
    }
};

// converts from 2D RigidTransformCov to 3D RigidTransformCov,
// z and the two additional euler angles are set to 0
template<typename T>
struct TransformCast<RigidTransformCov<T,3>, T, 3, RigidTransformCov<T,2>, T, 2> {
    static RigidTransformCov<T,3> invoke(const RigidTransformCov<T,2>& other)  {
        typename RigidTransformCov<T,3>::YawPitchRollCovMatrixType eulerCov =
                RigidTransformCov<T,3>::nullYawPitchRollCov();

        // copy 2x2 translation cov part: xy-xy
        eulerCov.template block<2,2>(0,0) = other.cov.template block<2,2>(0,0);
        // xy-phi
        eulerCov.template block<2,1>(0,3) = other.cov.template block<2,1>(0,2);
        // xy-phi
        eulerCov.template block<1,2>(3,0) = other.cov.template block<1,2>(2,0);
        // phi-phi
        eulerCov(3,3) = other.cov(2,2);

        Eigen::Quaternion < T > orientation = Eigen::Quaternion<T>(
            Eigen::AngleAxis<T>(other.phi(), Eigen::Matrix<T, 3, 1>::UnitZ()));

        return RigidTransformCov<T, 3>(
            Eigen::Matrix<T, 3, 1>(other.x(), other.y(), 0),
            orientation, eulerCov);
    }
};

// matches "conversions" from the corresponding RigidTransform to
// RigidTransformCov type
template<typename T, int D>
struct TransformCast<RigidTransformCov<T,D>, T, D, RigidTransform<T,D>, T, D> {
    static RigidTransformCov<T,D> invoke(const RigidTransform<T,D>& other) {
        return RigidTransformCov<T,D>(other);
    }
};

// matches "conversions" from the corresponding RigidTransformCov to
// RigidTransform type
template<typename T, int D>
struct TransformCast<RigidTransform<T,D>, T, D, RigidTransformCov<T,D>, T, D> {
    static RigidTransform<T,D> invoke(const RigidTransformCov<T,D>& other) {
        return RigidTransform<T,D>(other);
    }
};

// matches "conversions" from the RigidTransform type of different
// dimensionality to RigidTransformCov
template<typename T, int TargetD, int SourceD>
struct TransformCast<RigidTransformCov<T,TargetD>, T, TargetD,
                     RigidTransform<T,SourceD>, T, SourceD> {
    static RigidTransformCov<T,TargetD> invoke(const RigidTransform<T,SourceD>& other) {
        // first cast RigidTransform to the target dimensionality
        typedef TransformCast<RigidTransform<T,TargetD>, T, TargetD,
                              RigidTransform<T,SourceD>, T, SourceD> Cast;
        RigidTransform<T,TargetD> tmp = Cast::invoke(other);

        // finally create a RigidTransformCov from it
        return RigidTransformCov<T,TargetD>(tmp);
    }
};

// matches "conversions" from the RigidTransformCov type of different
// dimensionality to RigidTransform
template<typename T, int TargetD, int SourceD>
struct TransformCast<RigidTransform<T,TargetD>, T, TargetD,
                     RigidTransformCov<T,SourceD>, T, SourceD> {
    static RigidTransform<T,TargetD> invoke(const RigidTransformCov<T,SourceD>& other) {
        // cast RigidTransform to the target dimensionality
        typedef TransformCast<RigidTransform<T,TargetD>, T, TargetD,
                              RigidTransform<T,SourceD>, T, SourceD> Cast;
        return Cast::invoke(other);
    }
};



template<typename TargetTransform, typename OtherT, int OtherD>
TargetTransform transform_cast(const RigidTransform<OtherT,OtherD>& other)
{
    return TransformCast<TargetTransform, typename TargetTransform::Type, TargetTransform::Dim,
            RigidTransform<OtherT,OtherD>, OtherT, OtherD>::invoke(other);
}

template<typename TargetTransform, typename OtherT, int OtherD>
TargetTransform transform_cast(const RigidTransformCov<OtherT,OtherD>& other)
{
    return TransformCast<TargetTransform, typename TargetTransform::Type, TargetTransform::Dim,
            RigidTransformCov<OtherT,OtherD>, OtherT, OtherD>::invoke(other);
}


} // namespace

#endif