Skip to content

O
opencv
Commit ac481e61 authored by Pavel Rojtberg's avatar Pavel Rojtberg
Browse files
Options

calib3d: port Rodrigues to cpp primitives

parent 8d79285d
Hide whitespace changes
Inline Side-by-side
Showing with 57 additions and 65 deletions
modules/calib3d/src/calibration.cpp
View file @ ac481e61
...@@ -287,7 +287,6 @@ CV_IMPL int cvRodrigues2( const CvMat* src, CvMat* dst, CvMat* jacobian ) ...@@ -287,7 +287,6 @@ CV_IMPL int cvRodrigues2( const CvMat* src, CvMat* dst, CvMat* jacobian )
if( src->cols == 1 || src->rows == 1 ) if( src->cols == 1 || src->rows == 1 )
{ {
double rx, ry, rz, theta;
int step = src->rows > 1 ? src->step / elem_size : 1; int step = src->rows > 1 ? src->step / elem_size : 1;
if( src->rows + src->cols*CV_MAT_CN(src->type) - 1 != 3 ) 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 ) ...@@ -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 ) 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" ); CV_Error( CV_StsBadSize, "Output matrix must be 3x3, single-channel floating point matrix" );
Point3d r;
if( depth == CV_32F ) if( depth == CV_32F )
{ {
rx = src->data.fl[0]; r.x = src->data.fl[0];
ry = src->data.fl[step]; r.y = src->data.fl[step];
rz = src->data.fl[step*2]; r.z = src->data.fl[step*2];
} }
else else
{ {
rx = src->data.db[0]; r.x = src->data.db[0];
ry = src->data.db[step]; r.y = src->data.db[step];
rz = src->data.db[step*2]; r.z = src->data.db[step*2];
} }
theta = std::sqrt(rx*rx + ry*ry + rz*rz);
double theta = norm(r);
if( theta < DBL_EPSILON ) if( theta < DBL_EPSILON )
{ {
...@@ -323,54 +324,48 @@ CV_IMPL int cvRodrigues2( const CvMat* src, CvMat* dst, CvMat* jacobian ) ...@@ -323,54 +324,48 @@ CV_IMPL int cvRodrigues2( const CvMat* src, CvMat* dst, CvMat* jacobian )
} }
else else
{ {
const double I[] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 };
double c = cos(theta); double c = cos(theta);
double s = sin(theta); double s = sin(theta);
double c1 = 1. - c; double c1 = 1. - c;
double itheta = theta ? 1./theta : 0.; 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 }; 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 );
double _r_x_[] = { 0, -rz, ry, rz, 0, -rx, -ry, rx, 0 }; Matx33d r_x( 0, -r.z, r.y,
double R[9]; r.z, 0, -r.x,
CvMat matR = cvMat( 3, 3, CV_64F, R ); -r.y, r.x, 0 );
// R = cos(theta)*I + (1 - cos(theta))*r*rT + sin(theta)*[r_x] // 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] Matx33d R = c*Matx33d::eye() + c1*rrt + s*r_x;
for( k = 0; k < 9; k++ )
R[k] = c*I[k] + c1*rrt[k] + s*_r_x_[k];
cvConvert( &matR, dst ); Mat(R).convertTo(cvarrToMat(dst), dst->type);
if( jacobian ) if( jacobian )
{ {
double drrt[] = { rx+rx, ry, rz, ry, 0, 0, rz, 0, 0, const double I[] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 };
0, rx, 0, rx, ry+ry, rz, 0, rz, 0, double drrt[] = { r.x+r.x, r.y, r.z, r.y, 0, 0, r.z, 0, 0,
0, 0, rx, 0, 0, ry, rx, ry, rz+rz }; 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, double d_r_x_[] = { 0, 0, 0, 0, 0, -1, 0, 1, 0,
0, 0, 1, 0, 0, 0, -1, 0, 0, 0, 0, 1, 0, 0, 0, -1, 0, 0,
0, -1, 0, 1, 0, 0, 0, 0, 0 }; 0, -1, 0, 1, 0, 0, 0, 0, 0 };
for( i = 0; i < 3; i++ ) 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 a0 = -s*ri, a1 = (s - 2*c1*itheta)*ri, a2 = c1*itheta;
double a3 = (c - s*itheta)*ri, a4 = s*itheta; double a3 = (c - s*itheta)*ri, a4 = s*itheta;
for( k = 0; k < 9; k++ ) for( k = 0; k < 9; k++ )
J[i*9+k] = a0*I[k] + a1*rrt[k] + a2*drrt[i*9+k] + J[i*9+k] = a0*I[k] + a1*rrt.val[k] + a2*drrt[i*9+k] +
a3*_r_x_[k] + a4*d_r_x_[i*9+k]; a3*r_x.val[k] + a4*d_r_x_[i*9+k];
} }
} }
} }
} }
else if( src->cols == 3 && src->rows == 3 ) else if( src->cols == 3 && src->rows == 3 )
{ {
double R[9], U[9], V[9], W[3], rx, ry, rz; Matx33d U, Vt;
CvMat matR = cvMat( 3, 3, CV_64F, R ); Vec3d W;
CvMat matU = cvMat( 3, 3, CV_64F, U );
CvMat matV = cvMat( 3, 3, CV_64F, V );
CvMat matW = cvMat( 3, 1, CV_64F, W );
double theta, s, c; double theta, s, c;
int step = dst->rows > 1 ? dst->step / elem_size : 1; 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 ) ...@@ -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)) (dst->rows != 3 || dst->cols != 1 || CV_MAT_CN(dst->type) != 1))
CV_Error( CV_StsBadSize, "Output matrix must be 1x3 or 3x1" ); CV_Error( CV_StsBadSize, "Output matrix must be 1x3 or 3x1" );
cvConvert( src, &matR ); Matx33d R = cvarrToMat(src);
if( !cvCheckArr( &matR, CV_CHECK_RANGE+CV_CHECK_QUIET, -100, 100 ) )
if( !checkRange(R, true, NULL, -100, 100) )
{ {
cvZero(dst); cvZero(dst);
if( jacobian ) if( jacobian )
...@@ -387,15 +383,13 @@ CV_IMPL int cvRodrigues2( const CvMat* src, CvMat* dst, CvMat* jacobian ) ...@@ -387,15 +383,13 @@ CV_IMPL int cvRodrigues2( const CvMat* src, CvMat* dst, CvMat* jacobian )
return 0; return 0;
} }
cvSVD( &matR, &matW, &matU, &matV, CV_SVD_MODIFY_A + CV_SVD_U_T + CV_SVD_V_T ); SVD::compute(R, W, U, Vt);
cvGEMM( &matU, &matV, 1, 0, 0, &matR, CV_GEMM_A_T ); R = U*Vt;
rx = R[7] - R[5]; Point3d r(R(2, 1) - R(1, 2), R(0, 2) - R(2, 0), R(1, 0) - R(0, 1));
ry = R[2] - R[6];
rz = R[3] - R[1];
s = std::sqrt((rx*rx + ry*ry + rz*rz)*0.25); s = std::sqrt((r.x*r.x + r.y*r.y + r.z*r.z)*0.25);
c = (R[0] + R[4] + R[8] - 1)*0.5; c = (R(0, 0) + R(1, 1) + R(2, 2) - 1)*0.5;
c = c > 1. ? 1. : c < -1. ? -1. : c; c = c > 1. ? 1. : c < -1. ? -1. : c;
theta = acos(c); theta = acos(c);
...@@ -404,21 +398,19 @@ CV_IMPL int cvRodrigues2( const CvMat* src, CvMat* dst, CvMat* jacobian ) ...@@ -404,21 +398,19 @@ CV_IMPL int cvRodrigues2( const CvMat* src, CvMat* dst, CvMat* jacobian )
double t; double t;
if( c > 0 ) if( c > 0 )
rx = ry = rz = 0; r = Point3d(0, 0, 0);
else else
{ {
t = (R[0] + 1)*0.5; t = (R(0, 0) + 1)*0.5;
rx = std::sqrt(MAX(t,0.)); r.x = std::sqrt(MAX(t,0.));
t = (R[4] + 1)*0.5; t = (R(1, 1) + 1)*0.5;
ry = std::sqrt(MAX(t,0.))*(R[1] < 0 ? -1. : 1.); r.y = std::sqrt(MAX(t,0.))*(R(0, 1) < 0 ? -1. : 1.);
t = (R[8] + 1)*0.5; t = (R(2, 2) + 1)*0.5;
rz = std::sqrt(MAX(t,0.))*(R[2] < 0 ? -1. : 1.); r.z = std::sqrt(MAX(t,0.))*(R(0, 2) < 0 ? -1. : 1.);
if( fabs(rx) < fabs(ry) && fabs(rx) < fabs(rz) && (R[5] > 0) != (ry*rz > 0) ) if( fabs(r.x) < fabs(r.y) && fabs(r.x) < fabs(r.z) && (R(1, 2) > 0) != (r.y*r.z > 0) )
rz = -rz; r.z = -r.z;
theta /= std::sqrt(rx*rx + ry*ry + rz*rz); theta /= norm(r);
rx *= theta; r *= theta;
ry *= theta;
rz *= theta;
} }
if( jacobian ) if( jacobian )
...@@ -455,16 +447,16 @@ CV_IMPL int cvRodrigues2( const CvMat* src, CvMat* dst, CvMat* jacobian ) ...@@ -455,16 +447,16 @@ CV_IMPL int cvRodrigues2( const CvMat* src, CvMat* dst, CvMat* jacobian )
// var2 = [om;theta] // var2 = [om;theta]
double dvar2dvar[] = double dvar2dvar[] =
{ {
vth, 0, 0, rx, 0, vth, 0, 0, r.x, 0,
0, vth, 0, ry, 0, 0, vth, 0, r.y, 0,
0, 0, vth, rz, 0, 0, 0, vth, r.z, 0,
0, 0, 0, 0, 1 0, 0, 0, 0, 1
}; };
double domegadvar2[] = double domegadvar2[] =
{ {
theta, 0, 0, rx*vth, theta, 0, 0, r.x*vth,
0, theta, 0, ry*vth, 0, theta, 0, r.y*vth,
0, 0, theta, rz*vth 0, 0, theta, r.z*vth
}; };
CvMat _dvardR = cvMat( 5, 9, CV_64FC1, dvardR ); CvMat _dvardR = cvMat( 5, 9, CV_64FC1, dvardR );
...@@ -483,20 +475,20 @@ CV_IMPL int cvRodrigues2( const CvMat* src, CvMat* dst, CvMat* jacobian ) ...@@ -483,20 +475,20 @@ CV_IMPL int cvRodrigues2( const CvMat* src, CvMat* dst, CvMat* jacobian )
} }
vth *= theta; vth *= theta;
rx *= vth; ry *= vth; rz *= vth; r *= vth;
} }
if( depth == CV_32F ) if( depth == CV_32F )
{ {
dst->data.fl[0] = (float)rx; dst->data.fl[0] = (float)r.x;
dst->data.fl[step] = (float)ry; dst->data.fl[step] = (float)r.y;
dst->data.fl[step*2] = (float)rz; dst->data.fl[step*2] = (float)r.z;
} }
else else
{ {
dst->data.db[0] = rx; dst->data.db[0] = r.x;
dst->data.db[step] = ry; dst->data.db[step] = r.y;
dst->data.db[step*2] = rz; dst->data.db[step*2] = r.z;
} }
} }
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment