generic-library/opencv
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

also port Rodrigues in Affine to Matx expressions

Browse Source 
so results are numerically equivalent
This commit is contained in:
Pavel Rojtberg 2015-12-25 13:27:26 +01:00
parent ac481e6174
commit 4db6d9986d
1 changed files with 5 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
modules/core/include/opencv2/core/affine.hpp
View File

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