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