utils.h

Go to the documentation of this file.

#ifndef TUW_GEOMETRY_UTILS
#define TUW_GEOMETRY_UTILS

#include <string>
#include <opencv2/core/mat.hpp>
#include <opencv2/core/matx.hpp>

namespace tuw
{
typedef cv::Matx<double, 3, 3> Tf2D;

inline cv::Vec<double, 3>  append ( const cv::Vec<double, 2> &src, double value = 1.0 )
{
    return cv::Vec<double, 3> ( src.val[0], src.val[1], value );
}

inline cv::Vec<double, 4>  append ( const cv::Vec<double, 3> &src, double value = 1.0 )
{
    return cv::Vec<double, 4> ( src.val[0], src.val[1], src.val[2], value );
}

inline double angle_normalize ( double angle, double min_angle = -M_PI, double max_angle = +M_PI )
{
    while ( angle > max_angle ) {
        angle -= ( 2.*M_PI );
    }
    while ( angle < min_angle ) {
        angle += ( 2.*M_PI );
    }
    return angle;
}
inline double angle_difference ( double alpha0, double angle1 )
{
    return atan2 ( sin ( alpha0-angle1 ), cos ( alpha0-angle1 ) );
}

template <typename Quaternion>
void EulerPitchToQuaternion ( double pitch, Quaternion& q )
{
    double cp = cos ( pitch * 0.5 );
    double sp = sin ( pitch * 0.5 );

    q.w = cp;
    q.x = 0;
    q.y = sp;
    q.z = 0;

}

template <typename Quaternion>
void EulerToQuaternion ( double roll, const Quaternion& q )
{
    double cr = cos ( roll * 0.5 );
    double sr = sin ( roll * 0.5 );

    q.w = cr;
    q.x = sr;
    q.y = 0.;
    q.z = 0.;
}

template <typename Quaternion>
void EulerYawToQuaternion ( double yaw, Quaternion& q )
{
    double cy = cos ( yaw * 0.5 );
    double sy = sin ( yaw * 0.5 );

    q.w = cy;
    q.x = 0.;
    q.y = 0.;
    q.z = sy;
}

template <typename Quaternion>
void EulerToQuaternion ( double pitch, double roll, double yaw, const Quaternion& q )
{
    double cy = cos ( yaw * 0.5 );
    double sy = sin ( yaw * 0.5 );
    double cr = cos ( roll * 0.5 );
    double sr = sin ( roll * 0.5 );
    double cp = cos ( pitch * 0.5 );
    double sp = sin ( pitch * 0.5 );

    q.w = cy * cr * cp + sy * sr * sp;
    q.x = cy * sr * cp - sy * cr * sp;
    q.y = cy * cr * sp + sy * sr * cp;
    q.z = sy * cr * cp - cy * sr * sp;
    return q;
}

template <typename Quaternion>
void QuaternionToRoll ( const Quaternion& q, double& roll )
{
    // roll (x-axis rotation)
    double sinr = +2.0 * ( q.w * q.x + q.y * q.z );
    double cosr = +1.0 - 2.0 * ( q.x * q.x + q.y * q.y );
    roll = atan2 ( sinr, cosr );
}
template <typename Quaternion>
void QuaternionToPitch ( const Quaternion& q, double& pitch )
{
    // pitch (y-axis rotation)
    double sinp = +2.0 * ( q.w * q.y - q.z * q.x );
    if ( fabs ( sinp ) >= 1 ) {
        pitch = copysign ( M_PI / 2, sinp );    // use 90 degrees if out of range
    } else {
        pitch = asin ( sinp );
    }
}
template <typename Quaternion>
void QuaternionToYaw ( const Quaternion& q, double& yaw )
{
    // yaw (z-axis rotation)
    double siny = +2.0 * ( q.w * q.z + q.x * q.y );
    double cosy = +1.0 - 2.0 * ( q.y * q.y + q.z * q.z );
    yaw = atan2 ( siny, cosy );
}

template <typename Quaternion>
void QuaternionToEuler ( const Quaternion& q, double& roll, double& pitch, double& yaw )
{
    QuaternionToRoll ( q, roll ), QuaternionToPitch ( q, pitch ),  QuaternionToYaw ( q, yaw );
}

std::string format ( const cv::Mat_<int8_t> &m );
std::string format ( const cv::Mat_<int> &m );
std::string format ( const cv::Mat_<float> &m );
std::string format ( const cv::Mat_<double> &m );
std::string format ( const cv::Matx33d &m );
}

#endif // TUW_GEOMETRY_UTILS