122 lines
3.4 KiB
C++
122 lines
3.4 KiB
C++
#include "rotationConverters.hpp"
|
|
|
|
#include <cmath>
|
|
#include <opencv2/calib3d.hpp>
|
|
#include <opencv2/core.hpp>
|
|
|
|
#define CALIB_PI 3.14159265358979323846
|
|
#define CALIB_PI_2 1.57079632679489661923
|
|
|
|
void calib::Euler(const cv::Mat& src, cv::Mat& dst, int argType)
|
|
{
|
|
if((src.rows == 3) && (src.cols == 3))
|
|
{
|
|
//convert rotaion matrix to 3 angles (pitch, yaw, roll)
|
|
dst = cv::Mat(3, 1, CV_64F);
|
|
double pitch, yaw, roll;
|
|
|
|
if(src.at<double>(0,2) < -0.998)
|
|
{
|
|
pitch = -atan2(src.at<double>(1,0), src.at<double>(1,1));
|
|
yaw = -CALIB_PI_2;
|
|
roll = 0.;
|
|
}
|
|
else if(src.at<double>(0,2) > 0.998)
|
|
{
|
|
pitch = atan2(src.at<double>(1,0), src.at<double>(1,1));
|
|
yaw = CALIB_PI_2;
|
|
roll = 0.;
|
|
}
|
|
else
|
|
{
|
|
pitch = atan2(-src.at<double>(1,2), src.at<double>(2,2));
|
|
yaw = asin(src.at<double>(0,2));
|
|
roll = atan2(-src.at<double>(0,1), src.at<double>(0,0));
|
|
}
|
|
|
|
if(argType == CALIB_DEGREES)
|
|
{
|
|
pitch *= 180./CALIB_PI;
|
|
yaw *= 180./CALIB_PI;
|
|
roll *= 180./CALIB_PI;
|
|
}
|
|
else if(argType != CALIB_RADIANS)
|
|
CV_Error(cv::Error::StsBadFlag, "Invalid argument type");
|
|
|
|
dst.at<double>(0,0) = pitch;
|
|
dst.at<double>(1,0) = yaw;
|
|
dst.at<double>(2,0) = roll;
|
|
}
|
|
else if( (src.cols == 1 && src.rows == 3) ||
|
|
(src.cols == 3 && src.rows == 1 ) )
|
|
{
|
|
//convert vector which contains 3 angles (pitch, yaw, roll) to rotaion matrix
|
|
double pitch, yaw, roll;
|
|
if(src.cols == 1 && src.rows == 3)
|
|
{
|
|
pitch = src.at<double>(0,0);
|
|
yaw = src.at<double>(1,0);
|
|
roll = src.at<double>(2,0);
|
|
}
|
|
else{
|
|
pitch = src.at<double>(0,0);
|
|
yaw = src.at<double>(0,1);
|
|
roll = src.at<double>(0,2);
|
|
}
|
|
|
|
if(argType == CALIB_DEGREES)
|
|
{
|
|
pitch *= CALIB_PI / 180.;
|
|
yaw *= CALIB_PI / 180.;
|
|
roll *= CALIB_PI / 180.;
|
|
}
|
|
else if(argType != CALIB_RADIANS)
|
|
CV_Error(cv::Error::StsBadFlag, "Invalid argument type");
|
|
|
|
dst = cv::Mat(3, 3, CV_64F);
|
|
cv::Mat M(3, 3, CV_64F);
|
|
cv::Mat i = cv::Mat::eye(3, 3, CV_64F);
|
|
i.copyTo(dst);
|
|
i.copyTo(M);
|
|
|
|
double* pR = dst.ptr<double>();
|
|
pR[4] = cos(pitch);
|
|
pR[7] = sin(pitch);
|
|
pR[8] = pR[4];
|
|
pR[5] = -pR[7];
|
|
|
|
double* pM = M.ptr<double>();
|
|
pM[0] = cos(yaw);
|
|
pM[2] = sin(yaw);
|
|
pM[8] = pM[0];
|
|
pM[6] = -pM[2];
|
|
|
|
dst *= M;
|
|
i.copyTo(M);
|
|
pM[0] = cos(roll);
|
|
pM[3] = sin(roll);
|
|
pM[4] = pM[0];
|
|
pM[1] = -pM[3];
|
|
|
|
dst *= M;
|
|
}
|
|
else
|
|
CV_Error(cv::Error::StsBadFlag, "Input matrix must be 1x3, 3x1 or 3x3" );
|
|
}
|
|
|
|
void calib::RodriguesToEuler(const cv::Mat& src, cv::Mat& dst, int argType)
|
|
{
|
|
CV_Assert((src.cols == 1 && src.rows == 3) || (src.cols == 3 && src.rows == 1));
|
|
cv::Mat R;
|
|
cv::Rodrigues(src, R);
|
|
Euler(R, dst, argType);
|
|
}
|
|
|
|
void calib::EulerToRodrigues(const cv::Mat& src, cv::Mat& dst, int argType)
|
|
{
|
|
CV_Assert((src.cols == 1 && src.rows == 3) || (src.cols == 3 && src.rows == 1));
|
|
cv::Mat R;
|
|
Euler(src, R, argType);
|
|
cv::Rodrigues(R, dst);
|
|
}
|