From 4db6d9986d0a46dbcbb9195d10805ee25b388e42 Mon Sep 17 00:00:00 2001 From: Pavel Rojtberg Date: Fri, 25 Dec 2015 13:27:26 +0100 Subject: [PATCH] also port Rodrigues in Affine to Matx expressions so results are numerically equivalent --- modules/core/include/opencv2/core/affine.hpp | 15 +++++---------- 1 file changed, 5 insertions(+), 10 deletions(-) diff --git a/modules/core/include/opencv2/core/affine.hpp b/modules/core/include/opencv2/core/affine.hpp index 2bce5b989..7f8deb5d7 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); }