Program Listing for File utils.hpp
↰ Return to documentation for file (include/tuw_geometry/utils.hpp
)
#ifndef TUW_GEOMETRY__UTILS_HPP
#define TUW_GEOMETRY__UTILS_HPP
#include <opencv2/core/mat.hpp>
#include <opencv2/core/matx.hpp>
#include <string>
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, 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>
double & 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);
return roll;
}
template<typename Quaternion>
double QuaternionToRoll(const Quaternion & q)
{
double roll;
return QuaternionToYaw(q, roll);
}
template<typename Quaternion>
double & 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);
}
return pitch;
}
template<typename Quaternion>
double QuaternionToPitch(const Quaternion & q)
{
double pitch;
return QuaternionToYaw(q, pitch);
}
template<typename Quaternion>
double & 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);
return yaw;
}
template<typename Quaternion>
double QuaternionToYaw(const Quaternion & q)
{
double yaw;
return QuaternionToYaw(q, yaw);
}
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);
} // namespace tuw
#endif // TUW_GEOMETRY__UTILS_HPP