Merge pull request #5873 from paroj:cpprodrigues

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;
         }
     }
 

modules/core/include/opencv2/core/affine.hpp

@@ -241,30 +241,25 @@ void cv::Affine3<T>::rotation(const Mat3& R)
 template<typename T> inline
 void cv::Affine3<T>::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_<T> 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<float_type>(c*I[k] + c1*rrt[k] + s*_r_x_[k]);
+        Mat3 R = c*Mat3::eye() + c1*rrt + s*r_x;
 
         rotation(R);
     }