transformations.cpp
2.05 KB
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
#include "transformations.h"
/*! Decomposition of rotation matrix in Euler angles ZYZ:
* \f[ R = R_z(\theta_1) R_y(\rho) R_z(\theta_2)\f]
* \param[in] R rotation matrix
* \param[out] t1 \f$\theta_1\f$
* \param[out] rho \f$\rho\f$
* \param[out] t2 \f$\theta_2\f$
*/
void rotmat::decomposeEulerZYZ(const cv::Mat &R, double &t1, double &rho, double &t2){
cv::Matx33d rotmat = R;
std::cout << rotmat;
if (rotmat(2,2) < 1){
if (rotmat(2,2) > -1){
rho = acos(rotmat(2,2));
t1 = atan2(rotmat(1,2),rotmat(0,2));
t2 = atan2(rotmat(2,1),-rotmat(2,0));
}
else{
rho = PI;
t1 = -atan2(rotmat(1,0),rotmat(1,1));
t2 = 0;
}
}
else{
rho = 0;
t1 = atan2(rotmat(1,0),rotmat(1,1));
t2 = 0;
}
}
/// Get rotation matrix from affine camera matrix \f$P_{3\times4}\f$
cv::Mat rotmat::fromCameraMatrixAffine(const cv::Mat &P){
P.col(1).row(2) = 5;
if (P.rows != 3 || P.cols != 4)
std::runtime_error("assert failed (P.rows == 3 && P.cols == 4)");
cv::Mat K,R;
cv::Mat temp;
temp = P.colRange(0,0+3).clone();
cv::Point3d p0(temp.row(0).clone());
cv::Point3d p1(temp.row(1).clone());
temp.row(2) = cv::Mat(p0.cross(p1)).t();
cv::RQDecomp3x3(temp,K,R);
if (K.at<double>(2,2) < 0){
K = -K;
R = -R;
}
return R;
}
/*! Transform a vector \f$(\theta_1,\rho,\theta_2)\f$ reprensenting the euler (Rzyz) angles
* into a rotation matrix:
* \f[ R = R_z(\theta_1) R_y(\rho) R_z(\theta_2)\f]
*/
cv::Mat rotmat::fromEulerZYZ(double t1, double rho, double t2){
double c0,c1,c2,s0,s1,s2;
c0 = cos(t1);
c1 = cos(rho);
c2 = cos(t2);
s0 = sin(t1);
s1 = sin(rho);
s2 = sin(t2);
cv::Matx33d R;
R(0,0) = c0*c1*c2 - s0*s2;
R(0,1) = -c0*c1*s2 - s0*c2;
R(0,2) = c0*s1;
R(1,0) = s0*c1*c2+c0*s2 ;
R(1,1) = -s0*c1*s2 + c0*c2 ;
R(1,2) = s0*s1;
R(2,0) = -s1*c2;
R(2,1) = s1*s2;
R(2,2) = c1;
return ((cv::Mat)R).clone();
}