diff --git a/modules/calib3d/src/calibration.cpp b/modules/calib3d/src/calibration.cpp index dbf92c7d5f..b8f569aeb8 100644 --- a/modules/calib3d/src/calibration.cpp +++ b/modules/calib3d/src/calibration.cpp @@ -287,7 +287,6 @@ CV_IMPL int cvRodrigues2( const CvMat* src, CvMat* dst, CvMat* jacobian ) if( src->cols == 1 || src->rows == 1 ) { - double rx, ry, rz, theta; int step = src->rows > 1 ? src->step / elem_size : 1; if( src->rows + src->cols*CV_MAT_CN(src->type) - 1 != 3 ) @@ -296,19 +295,21 @@ CV_IMPL int cvRodrigues2( const CvMat* src, CvMat* dst, CvMat* jacobian ) if( dst->rows != 3 || dst->cols != 3 || CV_MAT_CN(dst->type) != 1 ) CV_Error( CV_StsBadSize, "Output matrix must be 3x3, single-channel floating point matrix" ); + Point3d r; if( depth == CV_32F ) { - rx = src->data.fl[0]; - ry = src->data.fl[step]; - rz = src->data.fl[step*2]; + r.x = src->data.fl[0]; + r.y = src->data.fl[step]; + r.z = src->data.fl[step*2]; } else { - rx = src->data.db[0]; - ry = src->data.db[step]; - rz = src->data.db[step*2]; + r.x = src->data.db[0]; + r.y = src->data.db[step]; + r.z = src->data.db[step*2]; } - theta = std::sqrt(rx*rx + ry*ry + rz*rz); + + double theta = norm(r); if( theta < DBL_EPSILON ) { @@ -323,54 +324,48 @@ CV_IMPL int cvRodrigues2( const CvMat* src, CvMat* dst, CvMat* jacobian ) } else { - const double I[] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 }; - double c = cos(theta); double s = sin(theta); double c1 = 1. - c; double itheta = theta ? 1./theta : 0.; - rx *= itheta; ry *= itheta; rz *= itheta; + r *= itheta; - double rrt[] = { rx*rx, rx*ry, rx*rz, rx*ry, ry*ry, ry*rz, rx*rz, ry*rz, rz*rz }; - double _r_x_[] = { 0, -rz, ry, rz, 0, -rx, -ry, rx, 0 }; - double R[9]; - CvMat matR = cvMat( 3, 3, CV_64F, R ); + Matx33d rrt( r.x*r.x, r.x*r.y, r.x*r.z, r.x*r.y, r.y*r.y, r.y*r.z, r.x*r.z, r.y*r.z, r.z*r.z ); + Matx33d r_x( 0, -r.z, r.y, + r.z, 0, -r.x, + -r.y, r.x, 0 ); // R = cos(theta)*I + (1 - cos(theta))*r*rT + sin(theta)*[r_x] - // where [r_x] is [0 -rz ry; rz 0 -rx; -ry rx 0] - for( k = 0; k < 9; k++ ) - R[k] = c*I[k] + c1*rrt[k] + s*_r_x_[k]; + Matx33d R = c*Matx33d::eye() + c1*rrt + s*r_x; - cvConvert( &matR, dst ); + Mat(R).convertTo(cvarrToMat(dst), dst->type); if( jacobian ) { - double drrt[] = { rx+rx, ry, rz, ry, 0, 0, rz, 0, 0, - 0, rx, 0, rx, ry+ry, rz, 0, rz, 0, - 0, 0, rx, 0, 0, ry, rx, ry, rz+rz }; + const double I[] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 }; + double drrt[] = { r.x+r.x, r.y, r.z, r.y, 0, 0, r.z, 0, 0, + 0, r.x, 0, r.x, r.y+r.y, r.z, 0, r.z, 0, + 0, 0, r.x, 0, 0, r.y, r.x, r.y, r.z+r.z }; double d_r_x_[] = { 0, 0, 0, 0, 0, -1, 0, 1, 0, 0, 0, 1, 0, 0, 0, -1, 0, 0, 0, -1, 0, 1, 0, 0, 0, 0, 0 }; for( i = 0; i < 3; i++ ) { - double ri = i == 0 ? rx : i == 1 ? ry : rz; + double ri = i == 0 ? r.x : i == 1 ? r.y : r.z; double a0 = -s*ri, a1 = (s - 2*c1*itheta)*ri, a2 = c1*itheta; double a3 = (c - s*itheta)*ri, a4 = s*itheta; for( k = 0; k < 9; k++ ) - J[i*9+k] = a0*I[k] + a1*rrt[k] + a2*drrt[i*9+k] + - a3*_r_x_[k] + a4*d_r_x_[i*9+k]; + J[i*9+k] = a0*I[k] + a1*rrt.val[k] + a2*drrt[i*9+k] + + a3*r_x.val[k] + a4*d_r_x_[i*9+k]; } } } } else if( src->cols == 3 && src->rows == 3 ) { - double R[9], U[9], V[9], W[3], rx, ry, rz; - CvMat matR = cvMat( 3, 3, CV_64F, R ); - CvMat matU = cvMat( 3, 3, CV_64F, U ); - CvMat matV = cvMat( 3, 3, CV_64F, V ); - CvMat matW = cvMat( 3, 1, CV_64F, W ); + Matx33d U, Vt; + Vec3d W; double theta, s, c; int step = dst->rows > 1 ? dst->step / elem_size : 1; @@ -378,8 +373,9 @@ CV_IMPL int cvRodrigues2( const CvMat* src, CvMat* dst, CvMat* jacobian ) (dst->rows != 3 || dst->cols != 1 || CV_MAT_CN(dst->type) != 1)) CV_Error( CV_StsBadSize, "Output matrix must be 1x3 or 3x1" ); - cvConvert( src, &matR ); - if( !cvCheckArr( &matR, CV_CHECK_RANGE+CV_CHECK_QUIET, -100, 100 ) ) + Matx33d R = cvarrToMat(src); + + if( !checkRange(R, true, NULL, -100, 100) ) { cvZero(dst); if( jacobian ) @@ -387,15 +383,13 @@ CV_IMPL int cvRodrigues2( const CvMat* src, CvMat* dst, CvMat* jacobian ) return 0; } - cvSVD( &matR, &matW, &matU, &matV, CV_SVD_MODIFY_A + CV_SVD_U_T + CV_SVD_V_T ); - cvGEMM( &matU, &matV, 1, 0, 0, &matR, CV_GEMM_A_T ); + SVD::compute(R, W, U, Vt); + R = U*Vt; - rx = R[7] - R[5]; - ry = R[2] - R[6]; - rz = R[3] - R[1]; + Point3d r(R(2, 1) - R(1, 2), R(0, 2) - R(2, 0), R(1, 0) - R(0, 1)); - s = std::sqrt((rx*rx + ry*ry + rz*rz)*0.25); - c = (R[0] + R[4] + R[8] - 1)*0.5; + s = std::sqrt((r.x*r.x + r.y*r.y + r.z*r.z)*0.25); + c = (R(0, 0) + R(1, 1) + R(2, 2) - 1)*0.5; c = c > 1. ? 1. : c < -1. ? -1. : c; theta = acos(c); @@ -404,21 +398,19 @@ CV_IMPL int cvRodrigues2( const CvMat* src, CvMat* dst, CvMat* jacobian ) double t; if( c > 0 ) - rx = ry = rz = 0; + r = Point3d(0, 0, 0); else { - t = (R[0] + 1)*0.5; - rx = std::sqrt(MAX(t,0.)); - t = (R[4] + 1)*0.5; - ry = std::sqrt(MAX(t,0.))*(R[1] < 0 ? -1. : 1.); - t = (R[8] + 1)*0.5; - rz = std::sqrt(MAX(t,0.))*(R[2] < 0 ? -1. : 1.); - if( fabs(rx) < fabs(ry) && fabs(rx) < fabs(rz) && (R[5] > 0) != (ry*rz > 0) ) - rz = -rz; - theta /= std::sqrt(rx*rx + ry*ry + rz*rz); - rx *= theta; - ry *= theta; - rz *= theta; + t = (R(0, 0) + 1)*0.5; + r.x = std::sqrt(MAX(t,0.)); + t = (R(1, 1) + 1)*0.5; + r.y = std::sqrt(MAX(t,0.))*(R(0, 1) < 0 ? -1. : 1.); + t = (R(2, 2) + 1)*0.5; + r.z = std::sqrt(MAX(t,0.))*(R(0, 2) < 0 ? -1. : 1.); + if( fabs(r.x) < fabs(r.y) && fabs(r.x) < fabs(r.z) && (R(1, 2) > 0) != (r.y*r.z > 0) ) + r.z = -r.z; + theta /= norm(r); + r *= theta; } if( jacobian ) @@ -455,16 +447,16 @@ CV_IMPL int cvRodrigues2( const CvMat* src, CvMat* dst, CvMat* jacobian ) // var2 = [om;theta] double dvar2dvar[] = { - vth, 0, 0, rx, 0, - 0, vth, 0, ry, 0, - 0, 0, vth, rz, 0, + vth, 0, 0, r.x, 0, + 0, vth, 0, r.y, 0, + 0, 0, vth, r.z, 0, 0, 0, 0, 0, 1 }; double domegadvar2[] = { - theta, 0, 0, rx*vth, - 0, theta, 0, ry*vth, - 0, 0, theta, rz*vth + theta, 0, 0, r.x*vth, + 0, theta, 0, r.y*vth, + 0, 0, theta, r.z*vth }; CvMat _dvardR = cvMat( 5, 9, CV_64FC1, dvardR ); @@ -483,20 +475,20 @@ CV_IMPL int cvRodrigues2( const CvMat* src, CvMat* dst, CvMat* jacobian ) } vth *= theta; - rx *= vth; ry *= vth; rz *= vth; + r *= vth; } if( depth == CV_32F ) { - dst->data.fl[0] = (float)rx; - dst->data.fl[step] = (float)ry; - dst->data.fl[step*2] = (float)rz; + dst->data.fl[0] = (float)r.x; + dst->data.fl[step] = (float)r.y; + dst->data.fl[step*2] = (float)r.z; } else { - dst->data.db[0] = rx; - dst->data.db[step] = ry; - dst->data.db[step*2] = rz; + dst->data.db[0] = r.x; + dst->data.db[step] = r.y; + dst->data.db[step*2] = r.z; } } diff --git a/modules/core/include/opencv2/core/affine.hpp b/modules/core/include/opencv2/core/affine.hpp index 2bce5b9895..7f8deb5d77 100644 --- a/modules/core/include/opencv2/core/affine.hpp +++ b/modules/core/include/opencv2/core/affine.hpp @@ -241,30 +241,25 @@ void cv::Affine3::rotation(const Mat3& R) template inline void cv::Affine3::rotation(const Vec3& _rvec) { - double rx = _rvec[0], ry = _rvec[1], rz = _rvec[2]; - double theta = std::sqrt(rx*rx + ry*ry + rz*rz); + double theta = norm(_rvec); if (theta < DBL_EPSILON) rotation(Mat3::eye()); else { - const double I[] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 }; - double c = std::cos(theta); double s = std::sin(theta); double c1 = 1. - c; double itheta = (theta != 0) ? 1./theta : 0.; - rx *= itheta; ry *= itheta; rz *= itheta; + Point3_ r = _rvec*itheta; - double rrt[] = { rx*rx, rx*ry, rx*rz, rx*ry, ry*ry, ry*rz, rx*rz, ry*rz, rz*rz }; - double _r_x_[] = { 0, -rz, ry, rz, 0, -rx, -ry, rx, 0 }; - Mat3 R; + Mat3 rrt( r.x*r.x, r.x*r.y, r.x*r.z, r.x*r.y, r.y*r.y, r.y*r.z, r.x*r.z, r.y*r.z, r.z*r.z ); + Mat3 r_x( 0, -r.z, r.y, r.z, 0, -r.x, -r.y, r.x, 0 ); // R = cos(theta)*I + (1 - cos(theta))*r*rT + sin(theta)*[r_x] // where [r_x] is [0 -rz ry; rz 0 -rx; -ry rx 0] - for(int k = 0; k < 9; ++k) - R.val[k] = static_cast(c*I[k] + c1*rrt[k] + s*_r_x_[k]); + Mat3 R = c*Mat3::eye() + c1*rrt + s*r_x; rotation(R); }