diff --git a/modules/calib3d/src/calibration.cpp b/modules/calib3d/src/calibration.cpp index 9c640540f7..f70bc5bdea 100644 --- a/modules/calib3d/src/calibration.cpp +++ b/modules/calib3d/src/calibration.cpp @@ -250,8 +250,6 @@ CV_IMPL void cvComposeRT( const CvMat* _rvec1, const CvMat* _tvec1, CV_IMPL int cvRodrigues2( const CvMat* src, CvMat* dst, CvMat* jacobian ) { - int depth, elem_size; - int i, k; double J[27] = {0}; CvMat matJ = cvMat( 3, 9, CV_64F, J ); @@ -262,8 +260,8 @@ CV_IMPL int cvRodrigues2( const CvMat* src, CvMat* dst, CvMat* jacobian ) CV_Error( !dst ? CV_StsNullPtr : CV_StsBadArg, "The first output argument is not a valid matrix" ); - depth = CV_MAT_DEPTH(src->type); - elem_size = CV_ELEM_SIZE(depth); + int depth = CV_MAT_DEPTH(src->type); + int elem_size = CV_ELEM_SIZE(depth); if( depth != CV_32F && depth != CV_64F ) CV_Error( CV_StsUnsupportedFormat, "The matrices must have 32f or 64f data type" ); @@ -349,12 +347,12 @@ CV_IMPL int cvRodrigues2( const CvMat* src, CvMat* dst, CvMat* jacobian ) 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++ ) + for( int i = 0; i < 3; i++ ) { 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++ ) + for( int k = 0; k < 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]; } @@ -490,6 +488,10 @@ CV_IMPL int cvRodrigues2( const CvMat* src, CvMat* dst, CvMat* jacobian ) dst->data.db[step*2] = r.z; } } + else + { + CV_Error(CV_StsBadSize, "Input matrix must be 1x3 or 3x1 for a rotation vector, or 3x3 for a rotation matrix"); + } if( jacobian ) { @@ -3219,6 +3221,10 @@ void cv::Rodrigues(InputArray _src, OutputArray _dst, OutputArray _jacobian) CV_INSTRUMENT_REGION(); Mat src = _src.getMat(); + CV_Check(src.rows, (src.rows == 1 && src.cols == 3) || (src.rows == 3 && src.cols == 1) || + (src.rows == 1 && src.cols == 1 && src.channels() == 3) || (src.rows == 3 && src.cols == 3), + "Input matrix must be 1x3 or 3x1 for a rotation vector, or 3x3 for a rotation matrix"); + bool v2m = src.cols == 1 || src.rows == 1; _dst.create(3, v2m ? 3 : 1, src.depth()); Mat dst = _dst.getMat();