TransformUtils.hpp

Go to the documentation of this file.

#ifndef __TRANSFORM_UTILS__
#define __TRANSFORM_UTILS__

#include "lvr2/types/MatrixTypes.hpp"
#include "lvr2/io/Model.hpp"
#include "lvr2/io/CoordinateTransform.hpp"

#include <Eigen/Dense>

namespace lvr2
{

template<typename T>
void getPoseFromMatrix(BaseVector<T>& position, BaseVector<T>& angles, const Transform<T>& mat);

template<typename T>
Transform<T> transformRegistration(const Transform<T>& transform, const Transform<T>& registration);

template<typename T>
Transform<T> buildTransformation(T* alignxf);

template<typename T>
void transformAndReducePointCloud(ModelPtr model, int modulo, 
        const T& sx, const T& sy, const T& sz, 
        const unsigned char& xPos, 
        const unsigned char& yPos, 
        const unsigned char& zPos);

template<typename T>
void transformAndReducePointCloud(ModelPtr& model, int modulo, const CoordinateTransform<T>& c);

template<typename T>
void transformPointCloud(ModelPtr model, const Transform<T>& transformation);

template<typename T>
Transform<T> transformFrame(const Transform<T>& frame, const CoordinateTransform<T>& ct);

template<typename T>
Transform<T> inverseTransform(const Transform<T>& transform);

template<typename T>
Transform<T> poseToMatrix(const Vector3<T>& position, const Vector3<T>& rotation);

template<typename T>
void matrixToPose(const Transform<T>& mat, Vector3<T>& position, Vector3<T>& rotation);

template <typename T>
static Transform<T> rieglToSLAM6DTransform(const Transform<T>& in)
{
       Transform<T> ret;

        ret(0) = in(5);
        ret(1) = -in(9);
        ret(2) = -in(1);
        ret(3) = -in(13);
        ret(4) = -in(6);
        ret(5) = in(10);
        ret(6) = in(2);
        ret(7) = in(14);
        ret(8) = -in(4);
        ret(9) = in(8);
        ret(10) = in(0);
        ret(11) = in(12);
        ret(12) = -100*in(7);
        ret(13) = 100*in(11);
        ret(14) = 100*in(3);
        ret(15) = in(15);

        return ret;
}

template <typename T>
static Transform<T> slam6dToRieglTransform(const Transform<T> &in)
{
    Transform<T> ret;

    ret(0) = in(10);
    ret(1) = -in(2);
    ret(2) = in(6);
    ret(3) = in(14)/100.0;
    ret(4) = -in(8);
    ret(5) = in(0);
    ret(6) = -in(4);
    ret(7) = -in(12)/100.0;
    ret(8) = in(9);
    ret(9) = -in(1);
    ret(10) = in(5);
    ret(11) = in(13)/100.0;
    ret(12) = in(11);
    ret(13) = -in(3);
    ret(14) = in(7);
    ret(15) = in(15);

    return ret;
}

template <typename T>
static Vector3<T> slam6DToRieglPoint(const Vector3<T> &in)
{
    return Vector3<T>(
        in.coeff(2)   / static_cast<T>(100.0),
        - in.coeff(0) / static_cast<T>(100.0),
        in.coeff(1)   / static_cast<T>(100.0)
    );
}

// Change Coodinate Systems

// 1) CS -> LVR

template <typename T>
static Vector3<T> slam6dToLvr(const Vector3<T>& in)
{
    return Vector3<T>(
        in.coeff(2) / static_cast<T>(100.0),
        - in.coeff(0) / static_cast<T>(100.0),
        in.coeff(1) / static_cast<T>(100.0)
    );
}

template<typename T>
static Rotation<T> slam6dToLvr(const Rotation<T>& in)
{
    Rotation<T> ret;
    
    // use coeff functions to access elements without range check
    // should be faster?

    // OLD CODE: why transpose?
    // ret.coeffRef(0, 0) =  in.coeff(2, 2);
    // ret.coeffRef(0, 1) = -in.coeff(0, 2);
    // ret.coeffRef(0, 2) =  in.coeff(1, 2);
    // ret.coeffRef(1, 0) = -in.coeff(2, 0);
    // ret.coeffRef(1, 1) =  in.coeff(0, 0);
    // ret.coeffRef(1, 2) = -in.coeff(1, 0);
    // ret.coeffRef(2, 0) =  in.coeff(2, 1);
    // ret.coeffRef(2, 1) = -in.coeff(0, 1);
    // ret.coeffRef(2, 2) =  in.coeff(1, 1);

    ret.coeffRef(0, 0) =  in.coeff(2, 2);
    ret.coeffRef(1, 0) = -in.coeff(0, 2);
    ret.coeffRef(2, 0) =  in.coeff(1, 2);
    ret.coeffRef(0, 1) = -in.coeff(2, 0);
    ret.coeffRef(1, 1) =  in.coeff(0, 0);
    ret.coeffRef(2, 1) = -in.coeff(1, 0);
    ret.coeffRef(0, 2) =  in.coeff(2, 1);
    ret.coeffRef(1, 2) = -in.coeff(0, 1);
    ret.coeffRef(2, 2) =  in.coeff(1, 1);
    
    return ret;
}

template <typename T>
static Transform<T> slam6dToLvr(const Transform<T> &in)
{
    Transform<T> ret;

    // Rotation
    const Rotation<T> R = in.template block<3,3>(0,0);
    ret.template block<3,3>(0,0) = slam6dToLvr(R);

    // Translation
    ret.coeffRef(0, 3) =  in.coeff(2, 3)/static_cast<T>(100.0);
    ret.coeffRef(1, 3) = -in.coeff(0, 3)/static_cast<T>(100.0);
    ret.coeffRef(2, 3) =  in.coeff(1, 3)/static_cast<T>(100.0);
    ret.coeffRef(3, 0) =  in.coeff(3, 2)/static_cast<T>(100.0);
    ret.coeffRef(3, 1) = -in.coeff(3, 0)/static_cast<T>(100.0);
    ret.coeffRef(3, 2) =  in.coeff(3, 1)/static_cast<T>(100.0);
    ret.coeffRef(3, 3) =  in.coeff(3, 3);

    return ret;
}

template <typename T>
static Vector3<T> openCvToLvr(const Vector3<T>& in)
{
    return Vector3<T>(
        in.coeff(2),
        -in.coeff(0),
        -in.coeff(1)
    );
}

template <typename T>
static Rotation<T> openCvToLvr(const Rotation<T> &in)
{
    Rotation<T> ret;

    ret.coeffRef(0,0) =  in.coeff(2,2);
    ret.coeffRef(0,1) = -in.coeff(2,0);
    ret.coeffRef(0,2) = -in.coeff(2,1);
    ret.coeffRef(1,0) = -in.coeff(0,2);
    ret.coeffRef(1,1) =  in.coeff(0,0);
    ret.coeffRef(1,2) =  in.coeff(0,1);
    ret.coeffRef(2,0) = -in.coeff(1,2);
    ret.coeffRef(2,1) =  in.coeff(1,0);
    ret.coeffRef(2,2) =  in.coeff(1,1);

    return ret;
}

template <typename T>
static Transform<T> openCvToLvr(const Transform<T> &in)
{
    Transform<T> ret;

    const Rotation<T> R = in.template block<3,3>(0,0);
    ret.template block<3,3>(0,0) = openCvToLvr(R);

    ret.coeffRef(0,3) =  in.coeff(2,3);
    ret.coeffRef(1,3) = -in.coeff(0,3);
    ret.coeffRef(2,3) = -in.coeff(1,3);

    ret.coeffRef(3,0) =  in.coeff(3,2);
    ret.coeffRef(3,1) = -in.coeff(3,0);
    ret.coeffRef(3,2) = -in.coeff(3,1);

    ret.coeffRef(3,3) =  in.coeff(3,3);

    return ret;
}


// 2) Lvr -> CS



template <typename T>
static Vector3<T> lvrToSlam6d(const Vector3<T>& in)
{
    return Vector3<T>(
        -in.coeff(1) * static_cast<T>(100.0),
        in.coeff(2) * static_cast<T>(100.0),
        in.coeff(0) * static_cast<T>(100.0)
    );
}

template<typename T>
static Rotation<T> lvrToSlam6d(const Rotation<T>& in)
{
    Rotation<T> ret;
    
    // use coeff functions to access elements without range check
    // should be faster?

    // OLD CODE: why transpose?
    // ret.coeffRef(0, 0) =  in.coeff(1, 1); // in.coeff(1, 1)
    // ret.coeffRef(0, 1) = -in.coeff(2, 1); // in.coeff(1, 2)
    // ret.coeffRef(0, 2) = -in.coeff(0, 1); // in.coeff(1, 0)
    // ret.coeffRef(1, 0) = -in.coeff(1, 2);
    // ret.coeffRef(1, 1) =  in.coeff(2, 2);
    // ret.coeffRef(1, 2) =  in.coeff(0, 2);
    // ret.coeffRef(2, 0) = -in.coeff(1, 0);
    // ret.coeffRef(2, 1) =  in.coeff(2, 0);
    // ret.coeffRef(2, 2) =  in.coeff(0, 0);

    ret.coeffRef(0, 0) =  in.coeff(1, 1); // in.coeff(1, 1)
    ret.coeffRef(1, 0) = -in.coeff(2, 1); // in.coeff(1, 2)
    ret.coeffRef(2, 0) = -in.coeff(0, 1); // in.coeff(1, 0)
    ret.coeffRef(0, 1) = -in.coeff(1, 2);
    ret.coeffRef(1, 1) =  in.coeff(2, 2);
    ret.coeffRef(2, 1) =  in.coeff(0, 2);
    ret.coeffRef(0, 2) = -in.coeff(1, 0);
    ret.coeffRef(1, 2) =  in.coeff(2, 0);
    ret.coeffRef(2, 2) =  in.coeff(0, 0);
    
    return ret;
}

template <typename T>
static Transform<T> lvrToSlam6d(const Transform<T> &in)
{
    Transform<T> ret;

    // Rotation
    const Rotation<T> R = in.template block<3,3>(0,0);
    ret.template block<3,3>(0,0) = lvrToSlam6d(R);

    // Translation
    ret.coeffRef(0, 3) = -static_cast<T>(100.0) * in.coeff(1, 3);
    ret.coeffRef(1, 3) =  static_cast<T>(100.0) * in.coeff(2, 3);
    ret.coeffRef(2, 3) =  static_cast<T>(100.0) * in.coeff(0, 3);
    ret.coeffRef(3, 0) = -static_cast<T>(100.0) * in.coeff(3, 1);
    ret.coeffRef(3, 1) =  static_cast<T>(100.0) * in.coeff(3, 2);
    ret.coeffRef(3, 2) =  static_cast<T>(100.0) * in.coeff(3, 0);

    ret.coeffRef(3, 3) = in.coeff(3, 3);

    return ret;
}



template <typename T>
static Vector3<T> lvrToOpenCv(const Vector3<T>& in)
{
    return Vector3<T>(
        -in.coeff(1),
        -in.coeff(2),
        in.coeff(0)
    );
}

template <typename T>
static Rotation<T> lvrToOpenCv(const Rotation<T>& in)
{
    Rotation<T> ret;

    ret.coeffRef(0,0) =  in.coeff(1,1);
    ret.coeffRef(0,1) =  in.coeff(1,2);
    ret.coeffRef(0,2) = -in.coeff(1,0);
    ret.coeffRef(1,0) =  in.coeff(2,1);
    ret.coeffRef(1,1) =  in.coeff(2,2);
    ret.coeffRef(1,2) = -in.coeff(2,0);
    ret.coeffRef(2,0) = -in.coeff(0,1);
    ret.coeffRef(2,1) = -in.coeff(0,2);
    ret.coeffRef(2,2) =  in.coeff(0,0);

    return ret;
}

template <typename T>
static Transform<T> lvrToOpenCv(const Transform<T> &in)
{
    Transform<T> ret;

    const Rotation<T> R = in.template block<3,3>(0,0);
    ret.template block<3,3>(0,0) = lvrToOpenCv(R);

    ret.coeffRef(0,3) = -in.coeff(1,3);
    ret.coeffRef(1,3) = -in.coeff(2,3);
    ret.coeffRef(2,3) =  in.coeff(0,3);

    ret.coeffRef(3,0) = -in.coeff(3,1);
    ret.coeffRef(3,1) = -in.coeff(3,2);
    ret.coeffRef(3,2) =  in.coeff(3,0);

    ret.coeffRef(3,3) =  in.coeff(3,3);

    return ret;
}

template<typename T>
void extrinsicsToEuler(Extrinsics<T> mat, T* pose)
{
    T *m = mat.data();
    if (pose != 0)
    {
        float _trX, _trY;
        if (m[0] > 0.0)
        {
            pose[4] = asin(m[8]);
        }
        else
        {
            pose[4] = (float)M_PI - asin(m[8]);
        }
        // rPosTheta[1] =  asin( m[8]);      // Calculate Y-axis angle

        float C = cos(pose[4]);
        if (fabs(C) > 0.005)
        {                     // Gimball lock?
            _trX = m[10] / C; // No, so get X-axis angle
            _trY = -m[9] / C;
            pose[3] = atan2(_trY, _trX);
            _trX = m[0] / C; // Get Z-axis angle
            _trY = -m[4] / C;
            pose[5] = atan2(_trY, _trX);
        }
        else
        {                  // Gimball lock has occurred
            pose[3] = 0.0; // Set X-axis angle to zero
            _trX = m[5];   //1          // And calculate Z-axis angle
            _trY = m[1];   //2
            pose[5] = atan2(_trY, _trX);
        }

        // cout << pose[3] << " " << pose[4] << " " << pose[5] << endl;

        pose[0] = m[12];
        pose[1] = m[13];
        pose[2] = m[14];
    }
}

template<typename T>
void eigenToEuler(Transform<T>& mat, T* pose)
{
    T *m = mat.data();
    if (pose != 0)
    {
        float _trX, _trY;
        if (m[0] > 0.0)
        {
            pose[4] = asin(m[8]);
        }
        else
        {
            pose[4] = (float)M_PI - asin(m[8]);
        }
        // rPosTheta[1] =  asin( m[8]);      // Calculate Y-axis angle

        float C = cos(pose[4]);
        if (fabs(C) > 0.005)
        {                     // Gimball lock?
            _trX = m[10] / C; // No, so get X-axis angle
            _trY = -m[9] / C;
            pose[3] = atan2(_trY, _trX);
            _trX = m[0] / C; // Get Z-axis angle
            _trY = -m[4] / C;
            pose[5] = atan2(_trY, _trX);
        }
        else
        {                  // Gimball lock has occurred
            pose[3] = 0.0; // Set X-axis angle to zero
            _trX = m[5];   //1          // And calculate Z-axis angle
            _trY = m[1];   //2
            pose[5] = atan2(_trY, _trX);
        }

        // cout << pose[3] << " " << pose[4] << " " << pose[5] << endl;

        pose[0] = m[12];
        pose[1] = m[13];
        pose[2] = m[14];
    }
}



} // namespace lvr2

#include "TransformUtils.tcc"

#endif