Blame view
src/transformations.cpp
2.05 KB
b0bb08d1c
init
|
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 |
#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();
}
|