calibration.diff
| calibration.cpp (Arbeitskopie) | ||
|---|---|---|
| 2913 | 2913 | |
| 2914 | 2914 |
/* Find Givens rotation for y axis. */ |
| 2915 | 2915 |
/* |
| 2916 |
( c 0 s )
|
|
| 2917 |
Qy = ( 0 1 0 ), c = m33/sqrt(m31^2 + m33^2), s = m31/sqrt(m31^2 + m33^2) |
|
| 2918 |
(-s 0 c )
|
|
| 2916 |
( c 0 -s )
|
|
| 2917 |
Qy = ( 0 1 0 ), c = m33/sqrt(m31^2 + m33^2), s = -m31/sqrt(m31^2 + m33^2)
|
|
| 2918 |
( s 0 c )
|
|
| 2919 | 2919 |
*/ |
| 2920 |
s = matR[2][0]; |
|
| 2920 |
s = -matR[2][0];
|
|
| 2921 | 2921 |
c = matR[2][2]; |
| 2922 | 2922 |
z = 1./sqrt(c * c + s * s + DBL_EPSILON); |
| 2923 | 2923 |
c *= z; |
| 2924 | 2924 |
s *= z; |
| 2925 | 2925 | |
| 2926 |
double _Qy[3][3] = { {c, 0, s}, {0, 1, 0}, {-s, 0, c} };
|
|
| 2926 |
double _Qy[3][3] = { {c, 0, -s}, {0, 1, 0}, {s, 0, c} };
|
|
| 2927 | 2927 |
CvMat Qy = cvMat(3, 3, CV_64F, _Qy); |
| 2928 | 2928 |
cvMatMul(&R, &Qy, &M); |
| 2929 | 2929 | |
| ... | ... | |
| 3016 | 3016 |
if( eulerAngles ) |
| 3017 | 3017 |
{
|
| 3018 | 3018 |
eulerAngles->x = acos(_Qx[1][1]) * (_Qx[1][2] >= 0 ? 1 : -1) * (180.0 / CV_PI); |
| 3019 |
eulerAngles->y = acos(_Qy[0][0]) * (_Qy[0][2] >= 0 ? 1 : -1) * (180.0 / CV_PI);
|
|
| 3019 |
eulerAngles->y = acos(_Qy[0][0]) * (_Qy[2][0] >= 0 ? 1 : -1) * (180.0 / CV_PI);
|
|
| 3020 | 3020 |
eulerAngles->z = acos(_Qz[0][0]) * (_Qz[0][1] >= 0 ? 1 : -1) * (180.0 / CV_PI); |
| 3021 | 3021 |
} |
| 3022 | 3022 | |