f4334277
 Hu Chunming
提交3rdparty
|
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
|
// This file is part of OpenCV project.
// It is subject to the license terms in the LICENSE file found in the top-level directory
// of this distribution and at http://opencv.org/license.html.
#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 rotation 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 rotation 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);
}
|
