Chiebot-Mirror
/
opencv
8
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

calib3d: port Rodrigues to cpp primitives

Browse Source
pull/5873/head
Pavel Rojtberg 9 years ago
parent 8d79285d02
commit ac481e6174
1 changed files with 57 additions and 65 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 122
      modules/calib3d/src/calibration.cpp

122
modules/calib3d/src/calibration.cpp
Unescape View File

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

Write Preview
Loading…
Cancel
Save