CV_IMPL void
cvDecomposeProjectionMatrix( const CvMat *projMatr, CvMat *calibMatr,
CvMat *rotMatr, CvMat *posVect,
CvMat *rotMatrX, CvMat *rotMatrY,
CvMat *rotMatrZ, CvPoint3D64f *eulerAngles)
{
CvMat *tmpProjMatr = 0;
CvMat *tmpMatrixD = 0;
CvMat *tmpMatrixV = 0;
CvMat *tmpMatrixM = 0;
CV_FUNCNAME("cvDecomposeProjectionMatrix");
__BEGIN__;
/* Validate parameters. */
if(projMatr == 0 || calibMatr == 0 || rotMatr == 0 || posVect == 0)
CV_ERROR(CV_StsNullPtr, "Some of parameters is a NULL pointer!");
if(!CV_IS_MAT(projMatr) || !CV_IS_MAT(calibMatr) || !CV_IS_MAT(rotMatr) || !CV_IS_MAT(posVect))
CV_ERROR(CV_StsUnsupportedFormat, "Input parameters must be a matrices!");
if(projMatr->cols != 4 || projMatr->rows != 3)
CV_ERROR(CV_StsUnmatchedSizes, "Size of projection matrix must be 3x4!");
if(calibMatr->cols != 3 || calibMatr->rows != 3 || rotMatr->cols != 3 || rotMatr->rows != 3)
CV_ERROR(CV_StsUnmatchedSizes, "Size of calibration and rotation matrices must be 3x3!");
if(posVect->cols != 1 || posVect->rows != 4)
CV_ERROR(CV_StsUnmatchedSizes, "Size of position vector must be 4x1!");
CV_CALL(tmpProjMatr = cvCreateMat(4, 4, CV_64F));
CV_CALL(tmpMatrixD = cvCreateMat(4, 4, CV_64F));
CV_CALL(tmpMatrixV = cvCreateMat(4, 4, CV_64F));
CV_CALL(tmpMatrixM = cvCreateMat(3, 3, CV_64F));
/* Compute position vector. */
cvSetZero(tmpProjMatr); // Add zero row to make matrix square.
int i, k;
for(i = 0; i < 3; i++)
for(k = 0; k < 4; k++)
cvmSet(tmpProjMatr, i, k, cvmGet(projMatr, i, k));
CV_CALL(cvSVD(tmpProjMatr, tmpMatrixD, NULL, tmpMatrixV, CV_SVD_MODIFY_A + CV_SVD_V_T));
/* Save position vector. */
for(i = 0; i < 4; i++)
cvmSet(posVect, i, 0, cvmGet(tmpMatrixV, 3, i)); // Solution is last row of V.
/* Compute calibration and rotation matrices via RQ decomposition. */
cvGetCols(projMatr, tmpMatrixM, 0, 3); // M is first square matrix of P.
assert(cvDet(tmpMatrixM) != 0.0); // So far only finite cameras could be decomposed, so M has to be nonsingular [det(M) != 0].
CV_CALL(cvRQDecomp3x3(tmpMatrixM, calibMatr, rotMatr, rotMatrX, rotMatrY, rotMatrZ, eulerAngles));
__END__;
cvReleaseMat(&tmpProjMatr);
cvReleaseMat(&tmpMatrixD);
cvReleaseMat(&tmpMatrixV);
cvReleaseMat(&tmpMatrixM);
}
CV_IMPL void
cvEigenVV( CvArr* srcarr, CvArr* evectsarr, CvArr* evalsarr, double eps )
{
CV_FUNCNAME( "cvEigenVV" );
__BEGIN__;
CvMat sstub, *src = (CvMat*)srcarr;
CvMat estub1, *evects = (CvMat*)evectsarr;
CvMat estub2, *evals = (CvMat*)evalsarr;
if( !CV_IS_MAT( src ))
CV_CALL( src = cvGetMat( src, &sstub ));
if( !CV_IS_MAT( evects ))
CV_CALL( evects = cvGetMat( evects, &estub1 ));
if( !CV_IS_MAT( evals ))
CV_CALL( evals = cvGetMat( evals, &estub2 ));
if( src->cols != src->rows )
CV_ERROR( CV_StsUnmatchedSizes, "source is not quadratic matrix" );
if( !CV_ARE_SIZES_EQ( src, evects) )
CV_ERROR( CV_StsUnmatchedSizes, "eigenvectors matrix has inappropriate size" );
if( (evals->rows != src->rows || evals->cols != 1) &&
(evals->cols != src->rows || evals->rows != 1))
CV_ERROR( CV_StsBadSize, "eigenvalues vector has inappropriate size" );
if( !CV_ARE_TYPES_EQ( src, evects ) || !CV_ARE_TYPES_EQ( src, evals ))
CV_ERROR( CV_StsUnmatchedFormats,
"input matrix, eigenvalues and eigenvectors must have the same type" );
if( !CV_IS_MAT_CONT( src->type & evals->type & evects->type ))
CV_ERROR( CV_BadStep, "all the matrices must be continuous" );
if( CV_MAT_TYPE(src->type) == CV_32FC1 )
{
IPPI_CALL( icvJacobiEigens_32f( src->data.fl,
evects->data.fl,
evals->data.fl, src->cols, (float)eps ));
}
else if( CV_MAT_TYPE(src->type) == CV_64FC1 )
{
IPPI_CALL( icvJacobiEigens_64d( src->data.db,
evects->data.db,
evals->data.db, src->cols, eps ));
}
else
{
CV_ERROR( CV_StsUnsupportedFormat, "Only 32fC1 and 64fC1 types are supported" );
}
CV_CHECK_NANS( evects );
CV_CHECK_NANS( evals );
__END__;
}
/*F///////////////////////////////////////////////////////////////////////////////////////
// Name: cvMeanShift
// Purpose: MeanShift algorithm
// Context:
// Parameters:
// imgProb - 2D object probability distribution
// windowIn - CvRect of CAMSHIFT Window intial size
// numIters - If CAMSHIFT iterates this many times, stop
// windowOut - Location, height and width of converged CAMSHIFT window
// len - If != NULL, return equivalent len
// width - If != NULL, return equivalent width
// itersUsed - Returns number of iterations CAMSHIFT took to converge
// Returns:
// The function itself returns the area found
// Notes:
//F*/
CV_IMPL int
cvMeanShift( const void* imgProb, CvRect windowIn,
CvTermCriteria criteria, CvConnectedComp* comp )
{
CvMoments moments;
int i = 0, eps;
CvMat stub, *mat = (CvMat*)imgProb;
CvMat cur_win;
CvRect cur_rect = windowIn;
CV_FUNCNAME( "cvMeanShift" );
if( comp )
comp->rect = windowIn;
moments.m00 = moments.m10 = moments.m01 = 0;
__BEGIN__;
CV_CALL( mat = cvGetMat( mat, &stub ));
if( CV_MAT_CN( mat->type ) > 1 )
CV_ERROR( CV_BadNumChannels, cvUnsupportedFormat );
if( windowIn.height <= 0 || windowIn.width <= 0 )
CV_ERROR( CV_StsBadArg, "Input window has non-positive sizes" );
if( windowIn.x < 0 || windowIn.x + windowIn.width > mat->cols ||
windowIn.y < 0 || windowIn.y + windowIn.height > mat->rows )
CV_ERROR( CV_StsBadArg, "Initial window is not inside the image ROI" );
CV_CALL( criteria = cvCheckTermCriteria( criteria, 1., 100 ));
eps = cvRound( criteria.epsilon * criteria.epsilon );
for( i = 0; i < criteria.max_iter; i++ )
{
int dx, dy, nx, ny;
double inv_m00;
CV_CALL( cvGetSubRect( mat, &cur_win, cur_rect ));
CV_CALL( cvMoments( &cur_win, &moments ));
/* Calculating center of mass */
if( fabs(moments.m00) < DBL_EPSILON )
break;
inv_m00 = moments.inv_sqrt_m00*moments.inv_sqrt_m00;
dx = cvRound( moments.m10 * inv_m00 - windowIn.width*0.5 );
dy = cvRound( moments.m01 * inv_m00 - windowIn.height*0.5 );
nx = cur_rect.x + dx;
ny = cur_rect.y + dy;
if( nx < 0 )
nx = 0;
else if( nx + cur_rect.width > mat->cols )
nx = mat->cols - cur_rect.width;
if( ny < 0 )
ny = 0;
else if( ny + cur_rect.height > mat->rows )
ny = mat->rows - cur_rect.height;
dx = nx - cur_rect.x;
dy = ny - cur_rect.y;
cur_rect.x = nx;
cur_rect.y = ny;
/* Check for coverage centers mass & window */
if( dx*dx + dy*dy < eps )
break;
}
__END__;
if( comp )
{
comp->rect = cur_rect;
comp->area = (float)moments.m00;
}
return i;
}
CV_IMPL void
cvKMeans2( const CvArr* samples_arr, int cluster_count,
CvArr* labels_arr, CvTermCriteria termcrit )
{
CvMat* centers = 0;
CvMat* old_centers = 0;
CvMat* counters = 0;
CV_FUNCNAME( "cvKMeans2" );
__BEGIN__;
CvMat samples_stub, labels_stub;
CvMat* samples = (CvMat*)samples_arr;
CvMat* labels = (CvMat*)labels_arr;
CvMat* temp = 0;
CvRNG rng = CvRNG(-1);
int i, j, k, sample_count, dims;
int ids_delta, iter;
double max_dist;
if( !CV_IS_MAT( samples ))
CV_CALL( samples = cvGetMat( samples, &samples_stub ));
if( !CV_IS_MAT( labels ))
CV_CALL( labels = cvGetMat( labels, &labels_stub ));
if( cluster_count < 1 )
CV_ERROR( CV_StsOutOfRange, "Number of clusters should be positive" );
if( CV_MAT_DEPTH(samples->type) != CV_32F || CV_MAT_TYPE(labels->type) != CV_32SC1 )
CV_ERROR( CV_StsUnsupportedFormat,
"samples should be floating-point matrix, cluster_idx - integer vector" );
if( labels->rows != 1 && (labels->cols != 1 || !CV_IS_MAT_CONT(labels->type)) ||
labels->rows + labels->cols - 1 != samples->rows )
CV_ERROR( CV_StsUnmatchedSizes,
"cluster_idx should be 1D vector of the same number of elements as samples' number of rows" );
CV_CALL( termcrit = cvCheckTermCriteria( termcrit, 1e-6, 100 ));
termcrit.epsilon *= termcrit.epsilon;
sample_count = samples->rows;
if( cluster_count > sample_count )
cluster_count = sample_count;
dims = samples->cols*CV_MAT_CN(samples->type);
ids_delta = labels->step ? labels->step/(int)sizeof(int) : 1;
CV_CALL( centers = cvCreateMat( cluster_count, dims, CV_64FC1 ));
CV_CALL( old_centers = cvCreateMat( cluster_count, dims, CV_64FC1 ));
CV_CALL( counters = cvCreateMat( 1, cluster_count, CV_32SC1 ));
// init centers
for( i = 0; i < sample_count; i++ )
labels->data.i[i] = cvRandInt(&rng) % cluster_count;
counters->cols = cluster_count; // cut down counters
max_dist = termcrit.epsilon*2;
for( iter = 0; iter < termcrit.max_iter; iter++ )
{
// computer centers
cvZero( centers );
cvZero( counters );
for( i = 0; i < sample_count; i++ )
{
float* s = (float*)(samples->data.ptr + i*samples->step);
k = labels->data.i[i*ids_delta];
double* c = (double*)(centers->data.ptr + k*centers->step);
for( j = 0; j <= dims - 4; j += 4 )
{
double t0 = c[j] + s[j];
double t1 = c[j+1] + s[j+1];
c[j] = t0;
c[j+1] = t1;
t0 = c[j+2] + s[j+2];
t1 = c[j+3] + s[j+3];
c[j+2] = t0;
c[j+3] = t1;
}
for( ; j < dims; j++ )
c[j] += s[j];
counters->data.i[k]++;
}
if( iter > 0 )
max_dist = 0;
for( k = 0; k < cluster_count; k++ )
{
double* c = (double*)(centers->data.ptr + k*centers->step);
if( counters->data.i[k] != 0 )
{
double scale = 1./counters->data.i[k];
for( j = 0; j < dims; j++ )
c[j] *= scale;
}
else
{
i = cvRandInt( &rng ) % sample_count;
float* s = (float*)(samples->data.ptr + i*samples->step);
for( j = 0; j < dims; j++ )
c[j] = s[j];
}
if( iter > 0 )
{
double dist = 0;
double* c_o = (double*)(old_centers->data.ptr + k*old_centers->step);
for( j = 0; j < dims; j++ )
{
double t = c[j] - c_o[j];
dist += t*t;
}
if( max_dist < dist )
max_dist = dist;
}
}
// assign labels
for( i = 0; i < sample_count; i++ )
{
float* s = (float*)(samples->data.ptr + i*samples->step);
int k_best = 0;
double min_dist = DBL_MAX;
for( k = 0; k < cluster_count; k++ )
{
double* c = (double*)(centers->data.ptr + k*centers->step);
double dist = 0;
j = 0;
for( ; j <= dims - 4; j += 4 )
{
double t0 = c[j] - s[j];
double t1 = c[j+1] - s[j+1];
dist += t0*t0 + t1*t1;
t0 = c[j+2] - s[j+2];
t1 = c[j+3] - s[j+3];
dist += t0*t0 + t1*t1;
}
for( ; j < dims; j++ )
{
double t = c[j] - s[j];
dist += t*t;
}
if( min_dist > dist )
{
min_dist = dist;
k_best = k;
}
}
labels->data.i[i*ids_delta] = k_best;
}
if( max_dist < termcrit.epsilon )
break;
CV_SWAP( centers, old_centers, temp );
}
cvZero( counters );
for( i = 0; i < sample_count; i++ )
counters->data.i[labels->data.i[i]]++;
// ensure that we do not have empty clusters
for( k = 0; k < cluster_count; k++ )
if( counters->data.i[k] == 0 )
for(;;)
{
i = cvRandInt(&rng) % sample_count;
j = labels->data.i[i];
if( counters->data.i[j] > 1 )
{
labels->data.i[i] = k;
counters->data.i[j]--;
counters->data.i[k]++;
break;
}
}
__END__;
cvReleaseMat( ¢ers );
cvReleaseMat( &old_centers );
cvReleaseMat( &counters );
}
CV_IMPL CvArr*
cvRange( CvArr* arr, double start, double end )
{
int ok = 0;
CV_FUNCNAME( "cvRange" );
__BEGIN__;
CvMat stub, *mat = (CvMat*)arr;
double delta;
int type, step;
double val = start;
int i, j;
int rows, cols;
if( !CV_IS_MAT(mat) )
CV_CALL( mat = cvGetMat( mat, &stub) );
rows = mat->rows;
cols = mat->cols;
type = CV_MAT_TYPE(mat->type);
delta = (end-start)/(rows*cols);
if( CV_IS_MAT_CONT(mat->type) )
{
cols *= rows;
rows = 1;
step = 1;
}
else
step = mat->step / CV_ELEM_SIZE(type);
if( type == CV_32SC1 )
{
int* idata = mat->data.i;
int ival = cvRound(val), idelta = cvRound(delta);
if( fabs(val - ival) < DBL_EPSILON &&
fabs(delta - idelta) < DBL_EPSILON )
{
for( i = 0; i < rows; i++, idata += step )
for( j = 0; j < cols; j++, ival += idelta )
idata[j] = ival;
}
else
{
for( i = 0; i < rows; i++, idata += step )
for( j = 0; j < cols; j++, val += delta )
idata[j] = cvRound(val);
}
}
else if( type == CV_32FC1 )
{
float* fdata = mat->data.fl;
for( i = 0; i < rows; i++, fdata += step )
for( j = 0; j < cols; j++, val += delta )
fdata[j] = (float)val;
}
else
CV_ERROR( CV_StsUnsupportedFormat, "The function only supports 32sC1 and 32fC1 datatypes" );
ok = 1;
__END__;
return ok ? arr : 0;
}
// convert CvScalar to specified type
void
cvScalarToRawData( CvScalar* scalar, int flags, void* data, int extend_to_12 )
{
CV_FUNCNAME( "cvScalarToRawData" );
__BEGIN__;
int type = CV_ARR_TYPE( flags );
int cn = CV_ARR_CN( type );
int depth = type & CV_ARR_DEPTH_MASK;
assert( scalar && data );
assert( (unsigned)(cn - 1) < 4 );
switch( depth )
{
case CV_8UC1:
while( cn-- )
{
int t = cvRound( scalar->val[cn] );
((uchar*)data)[cn] = CV_CAST_8U(t);
}
break;
case CV_8SC1:
while( cn-- )
{
int t = cvRound( scalar->val[cn] );
((char*)data)[cn] = CV_CAST_8S(t);
}
break;
case CV_16SC1:
while( cn-- )
{
int t = cvRound( scalar->val[cn] );
((short*)data)[cn] = CV_CAST_16S(t);
}
break;
case CV_32SC1:
while( cn-- )
((int*)data)[cn] = cvRound( scalar->val[cn] );
break;
case CV_32FC1:
while( cn-- )
((float*)data)[cn] = (float)(scalar->val[cn]);
break;
case CV_64FC1:
while( cn-- )
((double*)data)[cn] = (double)(scalar->val[cn]);
break;
default:
assert(0);
CV_ERROR_FROM_CODE( CV_BadDepth );
}
if( extend_to_12 )
{
int pix_size = icvPixSize[type];
int offset = icvPixSize[depth]*12;
do
{
offset -= pix_size;
memcpy( (char*)data + offset, data, pix_size );
}
while( offset > pix_size );
}
__END__;
}
/*
Finds real roots of cubic, quadratic or linear equation.
The original code has been taken from Ken Turkowski web page
(http://www.worldserver.com/turk/opensource/) and adopted for OpenCV.
Here is the copyright notice.
-----------------------------------------------------------------------
Copyright (C) 1978-1999 Ken Turkowski. <*****@*****.**>
All rights reserved.
Warranty Information
Even though I have reviewed this software, I make no warranty
or representation, either express or implied, with respect to this
software, its quality, accuracy, merchantability, or fitness for a
particular purpose. As a result, this software is provided "as is,"
and you, its user, are assuming the entire risk as to its quality
and accuracy.
This code may be used and freely distributed as long as it includes
this copyright notice and the above warranty information.
-----------------------------------------------------------------------
*/
CV_IMPL int
cvSolveCubic( const CvMat* coeffs, CvMat* roots )
{
int n = 0;
CV_FUNCNAME( "cvSolveCubic" );
__BEGIN__;
double a0 = 1., a1, a2, a3;
double x0 = 0., x1 = 0., x2 = 0.;
int step = 1, coeff_count;
if( !CV_IS_MAT(coeffs) )
CV_ERROR( !coeffs ? CV_StsNullPtr : CV_StsBadArg, "Input parameter is not a valid matrix" );
if( !CV_IS_MAT(roots) )
CV_ERROR( !roots ? CV_StsNullPtr : CV_StsBadArg, "Output parameter is not a valid matrix" );
if( CV_MAT_TYPE(coeffs->type) != CV_32FC1 && CV_MAT_TYPE(coeffs->type) != CV_64FC1 ||
CV_MAT_TYPE(roots->type) != CV_32FC1 && CV_MAT_TYPE(roots->type) != CV_64FC1 )
CV_ERROR( CV_StsUnsupportedFormat,
"Both matrices should be floating-point (single or double precision)" );
coeff_count = coeffs->rows + coeffs->cols - 1;
if( coeffs->rows != 1 && coeffs->cols != 1 || coeff_count != 3 && coeff_count != 4 )
CV_ERROR( CV_StsBadSize,
"The matrix of coefficients must be 1-dimensional vector of 3 or 4 elements" );
if( roots->rows != 1 && roots->cols != 1 ||
roots->rows + roots->cols - 1 != 3 )
CV_ERROR( CV_StsBadSize,
"The matrix of roots must be 1-dimensional vector of 3 elements" );
if( CV_MAT_TYPE(coeffs->type) == CV_32FC1 )
{
const float* c = coeffs->data.fl;
if( coeffs->rows > 1 )
step = coeffs->step/sizeof(c[0]);
if( coeff_count == 4 )
a0 = c[0], c += step;
a1 = c[0];
a2 = c[step];
a3 = c[step*2];
}
else
{
const double* c = coeffs->data.db;
if( coeffs->rows > 1 )
step = coeffs->step/sizeof(c[0]);
if( coeff_count == 4 )
a0 = c[0], c += step;
a1 = c[0];
a2 = c[step];
a3 = c[step*2];
}
if( a0 == 0 )
{
if( a1 == 0 )
{
if( a2 == 0 )
n = a3 == 0 ? -1 : 0;
else
{
// linear equation
x0 = a3/a2;
n = 1;
}
}
else
{
// quadratic equation
double d = a2*a2 - 4*a1*a3;
if( d >= 0 )
{
d = sqrt(d);
double q = (-a2 + (a2 < 0 ? -d : d)) * 0.5;
x0 = q / a1;
x1 = a3 / q;
n = d > 0 ? 2 : 1;
}
}
}
else
{
a0 = 1./a0;
a1 *= a0;
a2 *= a0;
a3 *= a0;
double Q = (a1 * a1 - 3 * a2) * (1./9);
double R = (2 * a1 * a1 * a1 - 9 * a1 * a2 + 27 * a3) * (1./54);
double Qcubed = Q * Q * Q;
double d = Qcubed - R * R;
if( d >= 0 )
{
double theta = acos(R / sqrt(Qcubed));
double sqrtQ = sqrt(Q);
double t0 = -2 * sqrtQ;
double t1 = theta * (1./3);
double t2 = a1 * (1./3);
x0 = t0 * cos(t1) - t2;
x1 = t0 * cos(t1 + (2.*CV_PI/3)) - t2;
x2 = t0 * cos(t1 + (4.*CV_PI/3)) - t2;
n = 3;
}
else
{
double e;
d = sqrt(-d);
e = pow(d + fabs(R), 0.333333333333);
if( R > 0 )
e = -e;
x0 = (e + Q / e) - a1 * (1./3);
n = 1;
}
}
step = 1;
if( CV_MAT_TYPE(roots->type) == CV_32FC1 )
{
float* r = roots->data.fl;
if( roots->rows > 1 )
step = roots->step/sizeof(r[0]);
r[0] = (float)x0;
r[step] = (float)x1;
r[step*2] = (float)x2;
}
else
{
double* r = roots->data.db;
if( roots->rows > 1 )
step = roots->step/sizeof(r[0]);
r[0] = x0;
r[step] = x1;
r[step*2] = x2;
}
__END__;
return n;
}
/*!
* \brief CvCapture_GStreamer::open Open the given file with gstreamer
* \param type CvCapture type. One of CV_CAP_GSTREAMER_*
* \param filename Filename to open in case of CV_CAP_GSTREAMER_FILE
* \return boolean. Specifies if opening was succesful.
*
* In case of CV_CAP_GSTREAMER_V4L(2), a pipelin is constructed as follows:
* v4l2src ! autoconvert ! appsink
*
*
* The 'filename' parameter is not limited to filesystem paths, and may be one of the following:
*
* - a normal filesystem path:
* e.g. video.avi or /path/to/video.avi or C:\\video.avi
* - an uri:
* e.g. file:///path/to/video.avi or rtsp:///path/to/stream.asf
* - a gstreamer pipeline description:
* e.g. videotestsrc ! videoconvert ! appsink
* the appsink name should be either 'appsink0' (the default) or 'opencvsink'
*
* When dealing with a file, CvCapture_GStreamer will not drop frames if the grabbing interval
* larger than the framerate period. (Unlike the uri or manual pipeline description, which assume
* a live source)
*
* The pipeline will only be started whenever the first frame is grabbed. Setting pipeline properties
* is really slow if we need to restart the pipeline over and over again.
*
* TODO: the 'type' parameter is imo unneeded. for v4l2, filename 'v4l2:///dev/video0' can be used.
* I expect this to be the same for CV_CAP_GSTREAMER_1394. Is anyone actually still using v4l (v1)?
*
*/
bool CvCapture_GStreamer::open( int type, const char* filename )
{
CV_FUNCNAME("cvCaptureFromCAM_GStreamer");
__BEGIN__;
gst_initializer::init();
bool stream = false;
bool manualpipeline = false;
char *uri = NULL;
uridecodebin = NULL;
GstElementFactory * testfac;
if (type == CV_CAP_GSTREAMER_V4L){
testfac = gst_element_factory_find("v4lsrc");
if (!testfac){
return false;
}
g_object_unref(G_OBJECT(testfac));
filename = "v4lsrc ! "COLOR_ELEM" ! appsink";
}
if (type == CV_CAP_GSTREAMER_V4L2){
testfac = gst_element_factory_find("v4l2src");
if (!testfac){
return false;
}
g_object_unref(G_OBJECT(testfac));
filename = "v4l2src ! "COLOR_ELEM" ! appsink";
}
// test if we have a valid uri. If so, open it with an uridecodebin
// else, we might have a file or a manual pipeline.
// if gstreamer cannot parse the manual pipeline, we assume we were given and
// ordinary file path.
if(!gst_uri_is_valid(filename))
{
uri = realpath(filename, NULL);
stream = false;
if(uri)
{
uri = g_filename_to_uri(uri, NULL, NULL);
if(!uri) {
CV_WARN("GStreamer: Error opening file\n");
close();
return false;
}
}
else
{
GError *err = NULL;
uridecodebin = gst_parse_launch(filename, &err);
if(!uridecodebin) {
//fprintf(stderr, "GStreamer: Error opening bin: %s\n", err->message);
//close();
return false;
}
stream = true;
manualpipeline = true;
}
} else {
stream = true;
uri = g_strdup(filename);
}
bool element_from_uri = false;
if(!uridecodebin)
{
// At this writing, the v4l2 element (and maybe others too) does not support caps renegotiation.
// This means that we cannot use an uridecodebin when dealing with v4l2, since setting
// capture properties will not work.
// The solution (probably only until gstreamer 1.2) is to make an element from uri when dealing with v4l2.
gchar * protocol = gst_uri_get_protocol(uri);
if (!strcasecmp(protocol , "v4l2"))
{
#if GST_VERSION_MAJOR == 0
uridecodebin = gst_element_make_from_uri(GST_URI_SRC, uri, "src");
#else
uridecodebin = gst_element_make_from_uri(GST_URI_SRC, uri, "src", NULL);
#endif
element_from_uri = true;
}else{
uridecodebin = gst_element_factory_make ("uridecodebin", NULL);
g_object_set(G_OBJECT(uridecodebin),"uri",uri, NULL);
}
g_free(protocol);
if(!uridecodebin) {
//fprintf(stderr, "GStreamer: Error opening bin: %s\n", err->message);
close();
return false;
}
}
if(manualpipeline)
{
GstIterator *it = NULL;
#if GST_VERSION_MAJOR == 0
it = gst_bin_iterate_sinks(GST_BIN(uridecodebin));
if(gst_iterator_next(it, (gpointer *)&sink) != GST_ITERATOR_OK) {
CV_ERROR(CV_StsError, "GStreamer: cannot find appsink in manual pipeline\n");
return false;
}
#else
it = gst_bin_iterate_sinks (GST_BIN(uridecodebin));
gboolean done = FALSE;
GstElement *element = NULL;
gchar* name = NULL;
GValue value = G_VALUE_INIT;
while (!done) {
switch (gst_iterator_next (it, &value)) {
case GST_ITERATOR_OK:
element = GST_ELEMENT (g_value_get_object (&value));
name = gst_element_get_name(element);
if (name){
if(strstr(name, "opencvsink") != NULL || strstr(name, "appsink") != NULL) {
sink = GST_ELEMENT ( gst_object_ref (element) );
done = TRUE;
}
g_free(name);
}
g_value_unset (&value);
break;
case GST_ITERATOR_RESYNC:
gst_iterator_resync (it);
break;
case GST_ITERATOR_ERROR:
case GST_ITERATOR_DONE:
done = TRUE;
break;
}
}
gst_iterator_free (it);
if (!sink){
CV_ERROR(CV_StsError, "GStreamer: cannot find appsink in manual pipeline\n");
return false;
}
#endif
pipeline = uridecodebin;
}
else
{
pipeline = gst_pipeline_new (NULL);
// videoconvert (in 0.10: ffmpegcolorspace) automatically selects the correct colorspace
// conversion based on caps.
color = gst_element_factory_make(COLOR_ELEM, NULL);
sink = gst_element_factory_make("appsink", NULL);
gst_bin_add_many(GST_BIN(pipeline), uridecodebin, color, sink, NULL);
if(element_from_uri) {
if(!gst_element_link(uridecodebin, color)) {
CV_ERROR(CV_StsError, "GStreamer: cannot link color -> sink\n");
gst_object_unref(pipeline);
return false;
}
}else{
g_signal_connect(uridecodebin, "pad-added", G_CALLBACK(newPad), color);
}
if(!gst_element_link(color, sink)) {
CV_ERROR(CV_StsError, "GStreamer: cannot link color -> sink\n");
gst_object_unref(pipeline);
return false;
}
}
//TODO: is 1 single buffer really high enough?
gst_app_sink_set_max_buffers (GST_APP_SINK(sink), 1);
gst_app_sink_set_drop (GST_APP_SINK(sink), stream);
//do not emit signals: all calls will be synchronous and blocking
gst_app_sink_set_emit_signals (GST_APP_SINK(sink), 0);
#if GST_VERSION_MAJOR == 0
caps = gst_caps_new_simple("video/x-raw-rgb",
"bpp", G_TYPE_INT, 24,
"red_mask", G_TYPE_INT, 0x0000FF,
"green_mask", G_TYPE_INT, 0x00FF00,
"blue_mask", G_TYPE_INT, 0xFF0000,
NULL);
#else
// support 1 and 3 channel 8 bit data, as well as bayer (also 1 channel, 8bit)
caps = gst_caps_from_string("video/x-raw, format=(string){BGR, GRAY8}; video/x-bayer,format=(string){rggb,bggr,grbg,gbrg}");
#endif
gst_app_sink_set_caps(GST_APP_SINK(sink), caps);
gst_caps_unref(caps);
//we do not start recording here just yet.
// the user probably wants to set capture properties first, so start recording whenever the first frame is requested
__END__;
return true;
}
static double
icvGetThreshVal_Otsu( const CvHistogram* hist )
{
double max_val = 0;
CV_FUNCNAME( "icvGetThreshVal_Otsu" );
__BEGIN__;
int i, count;
const float* h;
double sum = 0, mu = 0;
bool uniform = false;
double low = 0, high = 0, delta = 0;
float* nu_thresh = 0;
double mu1 = 0, q1 = 0;
double max_sigma = 0;
if( !CV_IS_HIST(hist) || CV_IS_SPARSE_HIST(hist) || hist->mat.dims != 1 )
CV_ERROR( CV_StsBadArg,
"The histogram in Otsu method must be a valid dense 1D histogram" );
count = hist->mat.dim[0].size;
h = (float*)cvPtr1D( hist->bins, 0 );
if( !CV_HIST_HAS_RANGES(hist) || CV_IS_UNIFORM_HIST(hist) )
{
if( CV_HIST_HAS_RANGES(hist) )
{
low = hist->thresh[0][0];
high = hist->thresh[0][1];
}
else
{
low = 0;
high = count;
}
delta = (high-low)/count;
low += delta*0.5;
uniform = true;
}
else
nu_thresh = hist->thresh2[0];
for( i = 0; i < count; i++ )
{
sum += h[i];
if( uniform )
mu += (i*delta + low)*h[i];
else
mu += (nu_thresh[i*2] + nu_thresh[i*2+1])*0.5*h[i];
}
sum = fabs(sum) > FLT_EPSILON ? 1./sum : 0;
mu *= sum;
mu1 = 0;
q1 = 0;
for( i = 0; i < count; i++ )
{
double p_i, q2, mu2, val_i, sigma;
p_i = h[i]*sum;
mu1 *= q1;
q1 += p_i;
q2 = 1. - q1;
if( MIN(q1,q2) < FLT_EPSILON || MAX(q1,q2) > 1. - FLT_EPSILON )
continue;
if( uniform )
val_i = i*delta + low;
else
val_i = (nu_thresh[i*2] + nu_thresh[i*2+1])*0.5;
mu1 = (mu1 + val_i*p_i)/q1;
mu2 = (mu - q1*mu1)/q2;
sigma = q1*q2*(mu1 - mu2)*(mu1 - mu2);
if( sigma > max_sigma )
{
max_sigma = sigma;
max_val = val_i;
}
}
__END__;
return max_val;
}
/*!
* \brief CvVideoWriter_GStreamer::writeFrame
* \param image
* \return
* Pushes the given frame on the pipeline.
* The timestamp for the buffer is generated from the framerate set in open
* and ensures a smooth video
*/
bool CvVideoWriter_GStreamer::writeFrame( const IplImage * image )
{
CV_FUNCNAME("CvVideoWriter_GStreamer::writerFrame");
GstClockTime duration, timestamp;
GstFlowReturn ret;
int size;
__BEGIN__;
handleMessage(pipeline);
if (input_pix_fmt == GST_VIDEO_FORMAT_BGR) {
if (image->nChannels != 3 || image->depth != IPL_DEPTH_8U) {
CV_ERROR(CV_StsUnsupportedFormat, "cvWriteFrame() needs images with depth = IPL_DEPTH_8U and nChannels = 3.");
}
}
#if FULL_GST_VERSION >= VERSION_NUM(0,10,29)
else if (input_pix_fmt == GST_VIDEO_FORMAT_GRAY8) {
if (image->nChannels != 1 || image->depth != IPL_DEPTH_8U) {
CV_ERROR(CV_StsUnsupportedFormat, "cvWriteFrame() needs images with depth = IPL_DEPTH_8U and nChannels = 1.");
}
}
#endif
else {
assert(false);
}
size = image->imageSize;
duration = ((double)1/framerate) * GST_SECOND;
timestamp = num_frames * duration;
//gst_app_src_push_buffer takes ownership of the buffer, so we need to supply it a copy
#if GST_VERSION_MAJOR == 0
buffer = gst_buffer_new_and_alloc (size);
memcpy(GST_BUFFER_DATA (buffer), (guint8*)image->imageData, size);
GST_BUFFER_DURATION(buffer) = duration;
GST_BUFFER_TIMESTAMP(buffer) = timestamp;
#else
buffer = gst_buffer_new_allocate (NULL, size, NULL);
GstMapInfo info;
gst_buffer_map(buffer, &info, (GstMapFlags)GST_MAP_READ);
memcpy(info.data, (guint8*)image->imageData, size);
gst_buffer_unmap(buffer, &info);
GST_BUFFER_DURATION(buffer) = duration;
GST_BUFFER_PTS(buffer) = timestamp;
GST_BUFFER_DTS(buffer) = timestamp;
#endif
//set the current number in the frame
GST_BUFFER_OFFSET(buffer) = num_frames;
ret = gst_app_src_push_buffer(GST_APP_SRC(source), buffer);
if (ret != GST_FLOW_OK) {
/* something wrong, stop pushing */
assert(false);
}
//gst_debug_bin_to_dot_file (GST_BIN(pipeline), GST_DEBUG_GRAPH_SHOW_ALL, "pipeline");
++num_frames;
__END__;
return true;
}
/*!
* \brief handleMessage
* Handles gstreamer bus messages. Mainly for debugging purposes and ensuring clean shutdown on error
*/
void handleMessage(GstElement * pipeline)
{
CV_FUNCNAME("handlemessage");
GError *err = NULL;
gchar *debug = NULL;
GstBus* bus = NULL;
GstStreamStatusType tp;
GstElement * elem = NULL;
GstMessage* msg = NULL;
__BEGIN__;
bus = gst_element_get_bus(pipeline);
while(gst_bus_have_pending(bus)) {
msg = gst_bus_pop(bus);
//printf("Got %s message\n", GST_MESSAGE_TYPE_NAME(msg));
if(gst_is_missing_plugin_message(msg))
{
CV_ERROR(CV_StsError, "GStreamer: your gstreamer installation is missing a required plugin\n");
}
else
{
switch (GST_MESSAGE_TYPE (msg)) {
case GST_MESSAGE_STATE_CHANGED:
GstState oldstate, newstate, pendstate;
gst_message_parse_state_changed(msg, &oldstate, &newstate, &pendstate);
//fprintf(stderr, "state changed from %s to %s (pending: %s)\n", gst_element_state_get_name(oldstate),
// gst_element_state_get_name(newstate), gst_element_state_get_name(pendstate));
break;
case GST_MESSAGE_ERROR:
gst_message_parse_error(msg, &err, &debug);
//fprintf(stderr, "GStreamer Plugin: Embedded video playback halted; module %s reported: %s\n",
// gst_element_get_name(GST_MESSAGE_SRC (msg)), err->message);
g_error_free(err);
g_free(debug);
gst_element_set_state(GST_ELEMENT(pipeline), GST_STATE_NULL);
break;
case GST_MESSAGE_EOS:
//fprintf(stderr, "reached the end of the stream.");
break;
case GST_MESSAGE_STREAM_STATUS:
gst_message_parse_stream_status(msg,&tp,&elem);
//fprintf(stderr, "stream status: elem %s, %i\n", GST_ELEMENT_NAME(elem), tp);
break;
default:
//fprintf(stderr, "unhandled message\n");
break;
}
}
gst_message_unref(msg);
}
gst_object_unref(GST_OBJECT(bus));
__END__
}
/*!
* \brief CvVideoWriter_GStreamer::open
* \param filename filename to output to
* \param fourcc desired codec fourcc
* \param fps desired framerate
* \param frameSize the size of the expected frames
* \param is_color color or grayscale
* \return success
*
* We support 2 modes of operation. Either the user enters a filename and a fourcc
* code, or enters a manual pipeline description like in CvVideoCapture_Gstreamer.
* In the latter case, we just push frames on the appsink with appropriate caps.
* In the former case, we try to deduce the correct container from the filename,
* and the correct encoder from the fourcc profile.
*
* If the file extension did was not recognize, an avi container is used
*
*/
bool CvVideoWriter_GStreamer::open( const char * filename, int fourcc,
double fps, CvSize frameSize, bool is_color )
{
CV_FUNCNAME("CvVideoWriter_GStreamer::open");
// check arguments
assert (filename);
assert (fps > 0);
assert (frameSize.width > 0 && frameSize.height > 0);
// init gstreamer
gst_initializer::init();
// init vars
bool manualpipeline = true;
int bufsize = 0;
GError *err = NULL;
const char* mime = NULL;
GstStateChangeReturn stateret;
GstCaps* caps = NULL;
GstCaps* videocaps = NULL;
#if FULL_GST_VERSION >= VERSION_NUM(0,10,32)
GstCaps* containercaps = NULL;
GstEncodingContainerProfile* containerprofile = NULL;
GstEncodingVideoProfile* videoprofile = NULL;
#endif
GstIterator *it = NULL;
// we first try to construct a pipeline from the given string.
// if that fails, we assume it is an ordinary filename
__BEGIN__;
encodebin = gst_parse_launch(filename, &err);
if(!encodebin) {
manualpipeline = false;
}
if(manualpipeline)
{
#if GST_VERSION_MAJOR == 0
it = gst_bin_iterate_sources(GST_BIN(encodebin));
if(gst_iterator_next(it, (gpointer *)&source) != GST_ITERATOR_OK) {
CV_ERROR(CV_StsError, "GStreamer: cannot find appsink in manual pipeline\n");
return false;
}
#else
it = gst_bin_iterate_sources (GST_BIN(encodebin));
gboolean done = FALSE;
GstElement *element = NULL;
gchar* name = NULL;
GValue value = G_VALUE_INIT;
while (!done) {
switch (gst_iterator_next (it, &value)) {
case GST_ITERATOR_OK:
element = GST_ELEMENT (g_value_get_object (&value));
name = gst_element_get_name(element);
if (name){
if(strstr(name, "opencvsrc") != NULL || strstr(name, "appsrc") != NULL) {
source = GST_ELEMENT ( gst_object_ref (element) );
done = TRUE;
}
g_free(name);
}
g_value_unset (&value);
break;
case GST_ITERATOR_RESYNC:
gst_iterator_resync (it);
break;
case GST_ITERATOR_ERROR:
case GST_ITERATOR_DONE:
done = TRUE;
break;
}
}
gst_iterator_free (it);
if (!source){
CV_ERROR(CV_StsError, "GStreamer: cannot find appsrc in manual pipeline\n");
return false;
}
#endif
pipeline = encodebin;
}
else
{
pipeline = gst_pipeline_new (NULL);
// we just got a filename and a fourcc code.
// first, try to guess the container from the filename
//encodebin = gst_element_factory_make("encodebin", NULL);
//proxy old non existing fourcc ids. These were used in previous opencv versions,
//but do not even exist in gstreamer any more
if (fourcc == CV_FOURCC('M','P','1','V')) fourcc = CV_FOURCC('M', 'P', 'G' ,'1');
if (fourcc == CV_FOURCC('M','P','2','V')) fourcc = CV_FOURCC('M', 'P', 'G' ,'2');
if (fourcc == CV_FOURCC('D','R','A','C')) fourcc = CV_FOURCC('d', 'r', 'a' ,'c');
//create encoder caps from fourcc
videocaps = gst_riff_create_video_caps(fourcc, NULL, NULL, NULL, NULL, NULL);
if (!videocaps){
CV_ERROR( CV_StsUnsupportedFormat, "Gstreamer Opencv backend does not support this codec.");
}
//create container caps from file extension
mime = filenameToMimetype(filename);
if (!mime) {
CV_ERROR( CV_StsUnsupportedFormat, "Gstreamer Opencv backend does not support this file type.");
}
#if FULL_GST_VERSION >= VERSION_NUM(0,10,32)
containercaps = gst_caps_from_string(mime);
//create encodebin profile
containerprofile = gst_encoding_container_profile_new("container", "container", containercaps, NULL);
videoprofile = gst_encoding_video_profile_new(videocaps, NULL, NULL, 1);
gst_encoding_container_profile_add_profile(containerprofile, (GstEncodingProfile *) videoprofile);
#endif
//create pipeline elements
encodebin = gst_element_factory_make("encodebin", NULL);
#if FULL_GST_VERSION >= VERSION_NUM(0,10,32)
g_object_set(G_OBJECT(encodebin), "profile", containerprofile, NULL);
#endif
source = gst_element_factory_make("appsrc", NULL);
file = gst_element_factory_make("filesink", NULL);
g_object_set(G_OBJECT(file), "location", filename, NULL);
}
if (is_color)
{
input_pix_fmt = GST_VIDEO_FORMAT_BGR;
bufsize = frameSize.width * frameSize.height * 3;
#if GST_VERSION_MAJOR == 0
caps = gst_video_format_new_caps(GST_VIDEO_FORMAT_BGR,
frameSize.width,
frameSize.height,
int(fps), 1,
1, 1);
#else
caps = gst_caps_new_simple("video/x-raw",
"format", G_TYPE_STRING, "BGR",
"width", G_TYPE_INT, frameSize.width,
"height", G_TYPE_INT, frameSize.height,
"framerate", GST_TYPE_FRACTION, int(fps), 1,
NULL);
caps = gst_caps_fixate(caps);
#endif
}
else
{
#if FULL_GST_VERSION >= VERSION_NUM(0,10,29)
input_pix_fmt = GST_VIDEO_FORMAT_GRAY8;
bufsize = frameSize.width * frameSize.height;
#if GST_VERSION_MAJOR == 0
caps = gst_video_format_new_caps(GST_VIDEO_FORMAT_GRAY8,
frameSize.width,
frameSize.height,
int(fps), 1,
1, 1);
#else
caps = gst_caps_new_simple("video/x-raw",
"format", G_TYPE_STRING, "GRAY8",
"width", G_TYPE_INT, frameSize.width,
"height", G_TYPE_INT, frameSize.height,
"framerate", GST_TYPE_FRACTION, int(fps), 1,
NULL);
caps = gst_caps_fixate(caps);
#endif
#else
CV_Assert(!"Gstreamer 0.10.29 or newer is required for grayscale input");
#endif
}
gst_app_src_set_caps(GST_APP_SRC(source), caps);
gst_app_src_set_stream_type(GST_APP_SRC(source), GST_APP_STREAM_TYPE_STREAM);
gst_app_src_set_size (GST_APP_SRC(source), -1);
g_object_set(G_OBJECT(source), "format", GST_FORMAT_TIME, NULL);
g_object_set(G_OBJECT(source), "block", 1, NULL);
g_object_set(G_OBJECT(source), "is-live", 0, NULL);
g_object_set(G_OBJECT(source), "emit-signals", 1, NULL);
if(!manualpipeline)
{
g_object_set(G_OBJECT(file), "buffer-size", bufsize, NULL);
gst_bin_add_many(GST_BIN(pipeline), source, encodebin, file, NULL);
if(!gst_element_link_many(source, encodebin, file, NULL)) {
CV_ERROR(CV_StsError, "GStreamer: cannot link elements\n");
}
}
stateret = gst_element_set_state(GST_ELEMENT(pipeline), GST_STATE_PLAYING);
if(stateret == GST_STATE_CHANGE_FAILURE) {
CV_ERROR(CV_StsError, "GStreamer: cannot put pipeline to play\n");
}
handleMessage(pipeline);
framerate = fps;
num_frames = 0;
__END__;
return true;
}
/////////////////////////////////
// cv3dTrackerCalibrateCameras //
/////////////////////////////////
CV_IMPL CvBool cv3dTrackerCalibrateCameras(int num_cameras,
const Cv3dTrackerCameraIntrinsics camera_intrinsics[], // size is num_cameras
CvSize etalon_size,
float square_size,
IplImage *samples[], // size is num_cameras
Cv3dTrackerCameraInfo camera_info[]) // size is num_cameras
{
CV_FUNCNAME("cv3dTrackerCalibrateCameras");
const int num_points = etalon_size.width * etalon_size.height;
int cameras_done = 0; // the number of cameras whose positions have been determined
CvPoint3D32f *object_points = NULL; // real-world coordinates of checkerboard points
CvPoint2D32f *points = NULL; // 2d coordinates of checkerboard points as seen by a camera
IplImage *gray_img = NULL; // temporary image for color conversion
IplImage *tmp_img = NULL; // temporary image used by FindChessboardCornerGuesses
int c, i, j;
if (etalon_size.width < 3 || etalon_size.height < 3)
CV_ERROR(CV_StsBadArg, "Chess board size is invalid");
for (c = 0; c < num_cameras; c++)
{
// CV_CHECK_IMAGE is not available in the cvaux library
// so perform the checks inline.
//CV_CALL(CV_CHECK_IMAGE(samples[c]));
if( samples[c] == NULL )
CV_ERROR( CV_HeaderIsNull, "Null image" );
if( samples[c]->dataOrder != IPL_DATA_ORDER_PIXEL && samples[c]->nChannels > 1 )
CV_ERROR( CV_BadOrder, "Unsupported image format" );
if( samples[c]->maskROI != 0 || samples[c]->tileInfo != 0 )
CV_ERROR( CV_StsBadArg, "Unsupported image format" );
if( samples[c]->imageData == 0 )
CV_ERROR( CV_BadDataPtr, "Null image data" );
if( samples[c]->roi &&
((samples[c]->roi->xOffset | samples[c]->roi->yOffset
| samples[c]->roi->width | samples[c]->roi->height) < 0 ||
samples[c]->roi->xOffset + samples[c]->roi->width > samples[c]->width ||
samples[c]->roi->yOffset + samples[c]->roi->height > samples[c]->height ||
(unsigned) (samples[c]->roi->coi) > (unsigned) (samples[c]->nChannels)))
CV_ERROR( CV_BadROISize, "Invalid ROI" );
// End of CV_CHECK_IMAGE inline expansion
if (samples[c]->depth != IPL_DEPTH_8U)
CV_ERROR(CV_BadDepth, "Channel depth of source image must be 8");
if (samples[c]->nChannels != 3 && samples[c]->nChannels != 1)
CV_ERROR(CV_BadNumChannels, "Source image must have 1 or 3 channels");
}
CV_CALL(object_points = (CvPoint3D32f *)cvAlloc(num_points * sizeof(CvPoint3D32f)));
CV_CALL(points = (CvPoint2D32f *)cvAlloc(num_points * sizeof(CvPoint2D32f)));
// fill in the real-world coordinates of the checkerboard points
FillObjectPoints(object_points, etalon_size, square_size);
for (c = 0; c < num_cameras; c++)
{
CvSize image_size = cvSize(samples[c]->width, samples[c]->height);
IplImage *img;
// The input samples are not required to all have the same size or color
// format. If they have different sizes, the temporary images are
// reallocated as necessary.
if (samples[c]->nChannels == 3)
{
// convert to gray
if (gray_img == NULL || gray_img->width != samples[c]->width ||
gray_img->height != samples[c]->height )
{
if (gray_img != NULL)
cvReleaseImage(&gray_img);
CV_CALL(gray_img = cvCreateImage(image_size, IPL_DEPTH_8U, 1));
}
CV_CALL(cvCvtColor(samples[c], gray_img, CV_BGR2GRAY));
img = gray_img;
}
else
{
// no color conversion required
img = samples[c];
}
if (tmp_img == NULL || tmp_img->width != samples[c]->width ||
tmp_img->height != samples[c]->height )
{
if (tmp_img != NULL)
cvReleaseImage(&tmp_img);
CV_CALL(tmp_img = cvCreateImage(image_size, IPL_DEPTH_8U, 1));
}
int count = num_points;
bool found = cvFindChessBoardCornerGuesses(img, tmp_img, 0,
etalon_size, points, &count) != 0;
if (count == 0)
continue;
// If found is true, it means all the points were found (count = num_points).
// If found is false but count is non-zero, it means that not all points were found.
cvFindCornerSubPix(img, points, count, cvSize(5,5), cvSize(-1,-1),
cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS, 10, 0.01f));
// If the image origin is BL (bottom-left), fix the y coordinates
// so they are relative to the true top of the image.
if (samples[c]->origin == IPL_ORIGIN_BL)
{
for (i = 0; i < count; i++)
points[i].y = samples[c]->height - 1 - points[i].y;
}
if (found)
{
// Make sure x coordinates are increasing and y coordinates are decreasing.
// (The y coordinate of point (0,0) should be the greatest, because the point
// on the checkerboard that is the origin is nearest the bottom of the image.)
// This is done after adjusting the y coordinates according to the image origin.
if (points[0].x > points[1].x)
{
// reverse points in each row
for (j = 0; j < etalon_size.height; j++)
{
CvPoint2D32f *row = &points[j*etalon_size.width];
for (i = 0; i < etalon_size.width/2; i++)
std::swap(row[i], row[etalon_size.width-i-1]);
}
}
if (points[0].y < points[etalon_size.width].y)
{
// reverse points in each column
for (i = 0; i < etalon_size.width; i++)
{
for (j = 0; j < etalon_size.height/2; j++)
std::swap(points[i+j*etalon_size.width],
points[i+(etalon_size.height-j-1)*etalon_size.width]);
}
}
}
DrawEtalon(samples[c], points, count, etalon_size, found);
if (!found)
continue;
float rotVect[3];
float rotMatr[9];
float transVect[3];
cvFindExtrinsicCameraParams(count,
image_size,
points,
object_points,
const_cast<float *>(camera_intrinsics[c].focal_length),
camera_intrinsics[c].principal_point,
const_cast<float *>(camera_intrinsics[c].distortion),
rotVect,
transVect);
// Check result against an arbitrary limit to eliminate impossible values.
// (If the chess board were truly that far away, the camera wouldn't be able to
// see the squares.)
if (transVect[0] > 1000*square_size
|| transVect[1] > 1000*square_size
|| transVect[2] > 1000*square_size)
{
// ignore impossible results
continue;
}
CvMat rotMatrDescr = cvMat(3, 3, CV_32FC1, rotMatr);
CvMat rotVectDescr = cvMat(3, 1, CV_32FC1, rotVect);
/* Calc rotation matrix by Rodrigues Transform */
cvRodrigues2( &rotVectDescr, &rotMatrDescr );
//combine the two transformations into one matrix
//order is important! rotations are not commutative
float tmat[4][4] = { { 1.f, 0.f, 0.f, 0.f },
{ 0.f, 1.f, 0.f, 0.f },
{ 0.f, 0.f, 1.f, 0.f },
{ transVect[0], transVect[1], transVect[2], 1.f } };
float rmat[4][4] = { { rotMatr[0], rotMatr[1], rotMatr[2], 0.f },
{ rotMatr[3], rotMatr[4], rotMatr[5], 0.f },
{ rotMatr[6], rotMatr[7], rotMatr[8], 0.f },
{ 0.f, 0.f, 0.f, 1.f } };
MultMatrix(camera_info[c].mat, tmat, rmat);
// change the transformation of the cameras to put them in the world coordinate
// system we want to work with.
// Start with an identity matrix; then fill in the values to accomplish
// the desired transformation.
float smat[4][4] = { { 1.f, 0.f, 0.f, 0.f },
{ 0.f, 1.f, 0.f, 0.f },
{ 0.f, 0.f, 1.f, 0.f },
{ 0.f, 0.f, 0.f, 1.f } };
// First, reflect through the origin by inverting all three axes.
smat[0][0] = -1.f;
smat[1][1] = -1.f;
smat[2][2] = -1.f;
MultMatrix(tmat, camera_info[c].mat, smat);
// Scale x and y coordinates by the focal length (allowing for non-square pixels
// and/or non-symmetrical lenses).
smat[0][0] = 1.0f / camera_intrinsics[c].focal_length[0];
smat[1][1] = 1.0f / camera_intrinsics[c].focal_length[1];
smat[2][2] = 1.0f;
MultMatrix(camera_info[c].mat, smat, tmat);
camera_info[c].principal_point = camera_intrinsics[c].principal_point;
camera_info[c].valid = true;
cameras_done++;
}
exit:
cvReleaseImage(&gray_img);
cvReleaseImage(&tmp_img);
cvFree(&object_points);
cvFree(&points);
return cameras_done == num_cameras;
}
unsigned int cvLabel (IplImage const *img, IplImage *imgOut, CvBlobs &blobs)
{
CV_FUNCNAME("cvLabel");
__CV_BEGIN__;
{
CV_ASSERT(img&&(img->depth==IPL_DEPTH_8U)&&(img->nChannels==1));
CV_ASSERT(imgOut&&(imgOut->depth==IPL_DEPTH_LABEL)&&(imgOut->nChannels==1));
unsigned int numPixels=0;
cvSetZero(imgOut);
CvLabel label=0;
cvReleaseBlobs(blobs);
unsigned int stepIn = img->widthStep / (img->depth / 8);
unsigned int stepOut = imgOut->widthStep / (imgOut->depth / 8);
unsigned int imgIn_width = img->width;
unsigned int imgIn_height = img->height;
unsigned int imgIn_offset = 0;
unsigned int imgOut_width = imgOut->width;
unsigned int imgOut_height = imgOut->height;
unsigned int imgOut_offset = 0;
if(img->roi)
{
imgIn_width = img->roi->width;
imgIn_height = img->roi->height;
imgIn_offset = img->roi->xOffset + (img->roi->yOffset * stepIn);
}
if(imgOut->roi)
{
imgOut_width = imgOut->roi->width;
imgOut_height = imgOut->roi->height;
imgOut_offset = imgOut->roi->xOffset + (imgOut->roi->yOffset * stepOut);
}
unsigned char *imgDataIn = (unsigned char *)img->imageData + imgIn_offset;
CvLabel *imgDataOut = (CvLabel *)imgOut->imageData + imgOut_offset;
#define imageIn(X, Y) imgDataIn[(X) + (Y)*stepIn]
#define imageOut(X, Y) imgDataOut[(X) + (Y)*stepOut]
CvLabel lastLabel = 0;
CvBlob *lastBlob = NULL;
for (unsigned int y=0; y<imgIn_height; y++)
{
for (unsigned int x=0; x<imgIn_width; x++)
{
if (imageIn(x, y))
{
bool labeled = imageOut(x, y);
if ((!imageOut(x, y))&&((y==0)||(!imageIn(x, y-1))))
{
labeled = true;
// Label contour.
label++;
CV_ASSERT(label!=CV_BLOB_MAX_LABEL);
imageOut(x, y) = label;
numPixels++;
// XXX This is not necessary at all. I only do this for consistency.
if (y>0)
imageOut(x, y-1) = CV_BLOB_MAX_LABEL;
CvBlob *blob = new CvBlob;
blob->label = label;
blob->area = 1;
blob->minx = x; blob->maxx = x;
blob->miny = y; blob->maxy = y;
blob->m10=x; blob->m01=y;
blob->m11=x*y;
blob->m20=x*x; blob->m02=y*y;
blob->internalContours.clear();
blobs.insert(CvLabelBlob(label,blob));
lastLabel = label;
lastBlob = blob;
blob->contour.startingPoint = cvPoint(x, y);
unsigned char direction=1;
unsigned int xx = x;
unsigned int yy = y;
bool contourEnd = false;
do
{
for (unsigned int numAttempts=0; numAttempts<3; numAttempts++)
{
bool found = false;
for (unsigned char i=0; i<3; i++)
{
int nx = xx+movesE[direction][i][0];
int ny = yy+movesE[direction][i][1];
if ((nx<imgIn_width)&&(nx>=0)&&(ny<imgIn_height)&&(ny>=0))
{
if (imageIn(nx, ny))
{
found = true;
blob->contour.chainCode.push_back(movesE[direction][i][3]);
xx=nx;
yy=ny;
direction=movesE[direction][i][2];
break;
}
else
{
imageOut(nx, ny) = CV_BLOB_MAX_LABEL;
}
}
}
if (!found)
direction=(direction+1)%4;
else
{
if (imageOut(xx, yy) != label)
{
imageOut(xx, yy) = label;
numPixels++;
if (xx<blob->minx) blob->minx = xx;
else if (xx>blob->maxx) blob->maxx = xx;
if (yy<blob->miny) blob->miny = yy;
else if (yy>blob->maxy) blob->maxy = yy;
blob->area++;
blob->m10+=xx; blob->m01+=yy;
blob->m11+=xx*yy;
blob->m20+=xx*xx; blob->m02+=yy*yy;
}
break;
}
if (contourEnd = ((xx==x) && (yy==y) && (direction==1)))
break;
}
}
while (!contourEnd);
}
if ((y+1<imgIn_height)&&(!imageIn(x, y+1))&&(!imageOut(x, y+1)))
{
labeled = true;
// Label internal contour
CvLabel l;
CvBlob *blob = NULL;
if (!imageOut(x, y))
{
/*if (!imageOut(x-1, y))
{
cerr << "!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!" << endl;
continue;
}*/
l = imageOut(x-1, y);
imageOut(x, y) = l;
numPixels++;
if (l==lastLabel)
blob = lastBlob;
else
{
blob = blobs.find(l)->second;
lastLabel = l;
lastBlob = blob;
}
blob->area++;
blob->m10+=x; blob->m01+=y;
blob->m11+=x*y;
blob->m20+=x*x; blob->m02+=y*y;
}
else
{
l = imageOut(x, y);
if (l==lastLabel)
blob = lastBlob;
else
{
blob = blobs.find(l)->second;
lastLabel = l;
lastBlob = blob;
}
}
// XXX This is not necessary (I believe). I only do this for consistency.
imageOut(x, y+1) = CV_BLOB_MAX_LABEL;
CvContourChainCode *contour = new CvContourChainCode;
contour->startingPoint = cvPoint(x, y);
unsigned char direction = 3;
unsigned int xx = x;
unsigned int yy = y;
do
{
for (unsigned int numAttempts=0; numAttempts<3; numAttempts++)
{
bool found = false;
for (unsigned char i=0; i<3; i++)
{
int nx = xx+movesI[direction][i][0];
int ny = yy+movesI[direction][i][1];
if (imageIn(nx, ny))
{
found = true;
contour->chainCode.push_back(movesI[direction][i][3]);
xx=nx;
yy=ny;
direction=movesI[direction][i][2];
break;
}
else
{
imageOut(nx, ny) = CV_BLOB_MAX_LABEL;
}
}
if (!found)
direction=(direction+1)%4;
else
{
if (!imageOut(xx, yy))
{
imageOut(xx, yy) = l;
numPixels++;
blob->area++;
blob->m10+=xx; blob->m01+=yy;
blob->m11+=xx*yy;
blob->m20+=xx*xx; blob->m02+=yy*yy;
}
break;
}
}
}
while (!(xx==x && yy==y));
blob->internalContours.push_back(contour);
}
//else if (!imageOut(x, y))
if (!labeled)
{
// Internal pixel
CvLabel l = imageOut(x-1, y);
imageOut(x, y) = l;
numPixels++;
CvBlob *blob = NULL;
if (l==lastLabel)
blob = lastBlob;
else
{
blob = blobs.find(l)->second;
lastLabel = l;
lastBlob = blob;
}
blob->area++;
blob->m10+=x; blob->m01+=y;
blob->m11+=x*y;
blob->m20+=x*x; blob->m02+=y*y;
}
}
}
}
for (CvBlobs::iterator it=blobs.begin(); it!=blobs.end(); ++it)
{
cvCentroid((*it).second);
(*it).second->u11 = (*it).second->m11 - ((*it).second->m10*(*it).second->m01)/(*it).second->m00;
(*it).second->u20 = (*it).second->m20 - ((*it).second->m10*(*it).second->m10)/(*it).second->m00;
(*it).second->u02 = (*it).second->m02 - ((*it).second->m01*(*it).second->m01)/(*it).second->m00;
double m00_2 = (*it).second->m00 * (*it).second->m00;
(*it).second->n11 = (*it).second->u11 / m00_2;
(*it).second->n20 = (*it).second->u20 / m00_2;
(*it).second->n02 = (*it).second->u02 / m00_2;
(*it).second->p1 = (*it).second->n20 + (*it).second->n02;
double nn = (*it).second->n20 - (*it).second->n02;
(*it).second->p2 = nn*nn + 4.*((*it).second->n11*(*it).second->n11);
}
return numPixels;
}
__CV_END__;
}
// initalize IplImage header, allocated by the user
CV_IMPL IplImage*
cvInitImageHeader( IplImage * image, CvSize size, int depth,
int channels, int origin, int align, int clear )
{
IplImage* result = 0;
CV_FUNCNAME( "cvInitImageHeader" );
__BEGIN__;
if( !image )
CV_ERROR( IPL_HeaderIsNull, "null pointer to header" );
if( clear )
{
char *colorModel, *channelSeq;
memset( image, 0, sizeof( *image ));
image->nSize = sizeof( *image );
CV_CALL( icvGetColorModel( channels, &colorModel, &channelSeq ));
strncpy( image->colorModel, colorModel, 4 );
strncpy( image->channelSeq, channelSeq, 4 );
}
if( size.width < 0 || size.height < 0 )
CV_ERROR( CV_BadROISize, "Bad input roi" );
if( (depth != (int)IPL_DEPTH_1U && depth != (int)IPL_DEPTH_8U &&
depth != (int)IPL_DEPTH_8S && depth != (int)IPL_DEPTH_16U &&
depth != (int)IPL_DEPTH_16S && depth != (int)IPL_DEPTH_32S &&
depth != (int)IPL_DEPTH_32F && depth != (int)IPL_DEPTH_64F) ||
channels < 0 )
CV_ERROR( IPL_BadDepth, "Unsupported format" );
if( origin != CV_ORIGIN_BL && origin != CV_ORIGIN_TL )
CV_ERROR( IPL_BadOrigin, "Bad input origin" );
if( align != 4 && align != 8 )
CV_ERROR( IPL_BadAlign, "Bad input align" );
image->width = size.width;
image->height = size.height;
if( image->roi )
{
image->roi->coi = 0;
image->roi->xOffset = image->roi->yOffset = 0;
image->roi->width = size.width;
image->roi->height = size.height;
}
image->nChannels = MAX( channels, 1 );
image->depth = depth;
image->align = align;
image->widthStep = (((image->width * image->nChannels *
(image->depth & ~IPL_DEPTH_SIGN) + 7)/8)+ align - 1) & (~(align - 1));
image->origin = origin;
image->imageSize = image->widthStep * image->height;
result = image;
__END__;
return result;
}
CV_IMPL double
cvThreshold( const void* srcarr, void* dstarr, double thresh, double maxval, int type )
{
CvHistogram* hist = 0;
CV_FUNCNAME( "cvThreshold" );
__BEGIN__;
CvSize roi;
int src_step, dst_step;
CvMat src_stub, *src = (CvMat*)srcarr;
CvMat dst_stub, *dst = (CvMat*)dstarr;
CvMat src0, dst0;
int coi1 = 0, coi2 = 0;
int ithresh, imaxval, cn;
bool use_otsu;
CV_CALL( src = cvGetMat( src, &src_stub, &coi1 ));
CV_CALL( dst = cvGetMat( dst, &dst_stub, &coi2 ));
if( coi1 + coi2 )
CV_ERROR( CV_BadCOI, "COI is not supported by the function" );
if( !CV_ARE_CNS_EQ( src, dst ) )
CV_ERROR( CV_StsUnmatchedFormats, "Both arrays must have equal number of channels" );
cn = CV_MAT_CN(src->type);
if( cn > 1 )
{
src = cvReshape( src, &src0, 1 );
dst = cvReshape( dst, &dst0, 1 );
}
use_otsu = (type & ~CV_THRESH_MASK) == CV_THRESH_OTSU;
type &= CV_THRESH_MASK;
if( use_otsu )
{
float _ranges[] = { 0, 256 };
float* ranges = _ranges;
int hist_size = 256;
void* srcarr0 = src;
if( CV_MAT_TYPE(src->type) != CV_8UC1 )
CV_ERROR( CV_StsNotImplemented, "Otsu method can only be used with 8uC1 images" );
CV_CALL( hist = cvCreateHist( 1, &hist_size, CV_HIST_ARRAY, &ranges ));
cvCalcArrHist( &srcarr0, hist );
thresh = cvFloor(icvGetThreshVal_Otsu( hist ));
}
if( !CV_ARE_DEPTHS_EQ( src, dst ) )
{
if( CV_MAT_TYPE(dst->type) != CV_8UC1 )
CV_ERROR( CV_StsUnsupportedFormat, "In case of different types destination should be 8uC1" );
if( type != CV_THRESH_BINARY && type != CV_THRESH_BINARY_INV )
CV_ERROR( CV_StsBadArg,
"In case of different types only CV_THRESH_BINARY "
"and CV_THRESH_BINARY_INV thresholding types are supported" );
if( maxval < 0 )
{
CV_CALL( cvSetZero( dst ));
}
else
{
CV_CALL( cvCmpS( src, thresh, dst, type == CV_THRESH_BINARY ? CV_CMP_GT : CV_CMP_LE ));
if( maxval < 255 )
CV_CALL( cvAndS( dst, cvScalarAll( maxval ), dst ));
}
EXIT;
}
if( !CV_ARE_SIZES_EQ( src, dst ) )
CV_ERROR( CV_StsUnmatchedSizes, "" );
roi = cvGetMatSize( src );
if( CV_IS_MAT_CONT( src->type & dst->type ))
{
roi.width *= roi.height;
roi.height = 1;
src_step = dst_step = CV_STUB_STEP;
}
else
{
src_step = src->step;
dst_step = dst->step;
}
switch( CV_MAT_DEPTH(src->type) )
{
case CV_8U:
ithresh = cvFloor(thresh);
imaxval = cvRound(maxval);
if( type == CV_THRESH_TRUNC )
imaxval = ithresh;
imaxval = CV_CAST_8U(imaxval);
if( ithresh < 0 || ithresh >= 255 )
{
if( type == CV_THRESH_BINARY || type == CV_THRESH_BINARY_INV ||
((type == CV_THRESH_TRUNC || type == CV_THRESH_TOZERO_INV) && ithresh < 0) ||
(type == CV_THRESH_TOZERO && ithresh >= 255) )
{
int v = type == CV_THRESH_BINARY ? (ithresh >= 255 ? 0 : imaxval) :
type == CV_THRESH_BINARY_INV ? (ithresh >= 255 ? imaxval : 0) :
type == CV_THRESH_TRUNC ? imaxval : 0;
cvSet( dst, cvScalarAll(v) );
EXIT;
}
else
{
cvCopy( src, dst );
EXIT;
}
}
if( type == CV_THRESH_BINARY || type == CV_THRESH_BINARY_INV )
{
if( icvCompareC_8u_C1R_cv_p && icvAndC_8u_C1R_p )
{
IPPI_CALL( icvCompareC_8u_C1R_cv_p( src->data.ptr, src_step,
(uchar)ithresh, dst->data.ptr, dst_step, roi,
type == CV_THRESH_BINARY ? cvCmpGreater : cvCmpLessEq ));
if( imaxval < 255 )
IPPI_CALL( icvAndC_8u_C1R_p( dst->data.ptr, dst_step,
(uchar)imaxval, dst->data.ptr, dst_step, roi ));
EXIT;
}
}
else if( type == CV_THRESH_TRUNC || type == CV_THRESH_TOZERO_INV )
{
if( icvThreshold_GTVal_8u_C1R_p )
{
IPPI_CALL( icvThreshold_GTVal_8u_C1R_p( src->data.ptr, src_step,
dst->data.ptr, dst_step, roi, (uchar)ithresh,
(uchar)(type == CV_THRESH_TRUNC ? ithresh : 0) ));
EXIT;
}
}
else
{
assert( type == CV_THRESH_TOZERO );
if( icvThreshold_LTVal_8u_C1R_p )
{
ithresh = cvFloor(thresh+1.);
ithresh = CV_CAST_8U(ithresh);
IPPI_CALL( icvThreshold_LTVal_8u_C1R_p( src->data.ptr, src_step,
dst->data.ptr, dst_step, roi, (uchar)ithresh, 0 ));
EXIT;
}
}
icvThresh_8u_C1R( src->data.ptr, src_step,
dst->data.ptr, dst_step, roi,
(uchar)ithresh, (uchar)imaxval, type );
break;
case CV_32F:
if( type == CV_THRESH_TRUNC || type == CV_THRESH_TOZERO_INV )
{
if( icvThreshold_GTVal_32f_C1R_p )
{
IPPI_CALL( icvThreshold_GTVal_32f_C1R_p( src->data.fl, src_step,
dst->data.fl, dst_step, roi, (float)thresh,
type == CV_THRESH_TRUNC ? (float)thresh : 0 ));
EXIT;
}
}
else if( type == CV_THRESH_TOZERO )
{
if( icvThreshold_LTVal_32f_C1R_p )
{
IPPI_CALL( icvThreshold_LTVal_32f_C1R_p( src->data.fl, src_step,
dst->data.fl, dst_step, roi, (float)(thresh*(1 + FLT_EPSILON)), 0 ));
EXIT;
}
}
icvThresh_32f_C1R( src->data.fl, src_step, dst->data.fl, dst_step, roi,
(float)thresh, (float)maxval, type );
break;
default:
CV_ERROR( CV_BadDepth, cvUnsupportedFormat );
}
__END__;
if( hist )
cvReleaseHist( &hist );
return thresh;
}
// set underlying CvMat or IplImage data
CV_IMPL void
cvSetData( CvArr* arr, void* data, int step )
{
CV_FUNCNAME( "cvSetData" );
__BEGIN__;
int pix_size, min_step;
if( _CV_IS_ARR( arr ))
{
CvMat* mat = (CvMat*)arr;
pix_size = icvPixSize[CV_ARR_TYPE(mat->type)];
min_step = mat->width*pix_size & ((mat->height <= 1) - 1);
if( step != CV_AUTOSTEP )
{
if( step < min_step && data != 0 )
CV_ERROR_FROM_CODE( CV_BadStep );
mat->step = step & ((mat->height <= 1) - 1);
}
else
{
mat->step = min_step;
}
mat->data.ptr = (uchar*)data;
CV_CALL( cvSetMatShapeFlags( mat ));
}
else if( _CV_IS_IMAGE( arr ))
{
IplImage* img = (IplImage*)arr;
pix_size = icvGetBtPix( img );
min_step = img->width*pix_size;
if( step != CV_AUTOSTEP && img->height > 1 )
{
if( step < min_step && data != 0 )
CV_ERROR_FROM_CODE( CV_BadStep );
img->widthStep = step;
}
else
{
img->widthStep = min_step;
}
img->imageSize = img->widthStep * img->height;
img->imageData = img->imageDataOrigin = (char*)data;
if( (((int)(long)data | step) & 7) == 0 &&
icvAlign(img->width * pix_size, 7) == step )
{
img->align = 8;
}
else
{
img->align = 4;
}
}
else
{
CV_ERROR( CV_StsBadArg, "" );
}
__END__;
}
CV_IMPL void
cvUndistort2( const CvArr* _src, CvArr* _dst, const CvMat* A, const CvMat* dist_coeffs )
{
static int inittab = 0;
uchar* buffer = 0;
CV_FUNCNAME( "cvUndistort2" );
__BEGIN__;
float a[9], k[4];
int coi1 = 0, coi2 = 0;
CvMat srcstub, *src = (CvMat*)_src;
CvMat dststub, *dst = (CvMat*)_dst;
CvMat _a = cvMat( 3, 3, CV_32F, a ), _k;
int cn, src_step, dst_step;
CvSize size;
if( !inittab )
{
icvInitLinearCoeffTab();
icvInitCubicCoeffTab();
inittab = 1;
}
CV_CALL( src = cvGetMat( src, &srcstub, &coi1 ));
CV_CALL( dst = cvGetMat( dst, &dststub, &coi2 ));
if( coi1 != 0 || coi2 != 0 )
CV_ERROR( CV_BadCOI, "The function does not support COI" );
if( CV_MAT_DEPTH(src->type) != CV_8U )
CV_ERROR( CV_StsUnsupportedFormat, "Only 8-bit images are supported" );
if( src->data.ptr == dst->data.ptr )
CV_ERROR( CV_StsNotImplemented, "In-place undistortion is not implemented" );
if( !CV_ARE_TYPES_EQ( src, dst ))
CV_ERROR( CV_StsUnmatchedFormats, "" );
if( !CV_ARE_SIZES_EQ( src, dst ))
CV_ERROR( CV_StsUnmatchedSizes, "" );
if( !CV_IS_MAT(A) || A->rows != 3 || A->cols != 3 ||
CV_MAT_TYPE(A->type) != CV_32FC1 && CV_MAT_TYPE(A->type) != CV_64FC1 )
CV_ERROR( CV_StsBadArg, "Intrinsic matrix must be a valid 3x3 floating-point matrix" );
if( !CV_IS_MAT(dist_coeffs) || dist_coeffs->rows != 1 && dist_coeffs->cols != 1 ||
dist_coeffs->rows*dist_coeffs->cols*CV_MAT_CN(dist_coeffs->type) != 4 ||
CV_MAT_DEPTH(dist_coeffs->type) != CV_64F &&
CV_MAT_DEPTH(dist_coeffs->type) != CV_32F )
CV_ERROR( CV_StsBadArg,
"Distortion coefficients must be 1x4 or 4x1 floating-point vector" );
cvConvert( A, &_a );
_k = cvMat( dist_coeffs->rows, dist_coeffs->cols,
CV_MAKETYPE(CV_32F, CV_MAT_CN(dist_coeffs->type)), k );
cvConvert( dist_coeffs, &_k );
cn = CV_MAT_CN(src->type);
size = cvGetMatSize(src);
src_step = src->step ? src->step : CV_STUB_STEP;
dst_step = dst->step ? dst->step : CV_STUB_STEP;
if( fabs((double)k[2]) < 1e-5 && fabs((double)k[3]) < 1e-5 && icvUndistortGetSize_p )
{
int buf_size = 0;
CvUndistortRadialIPPFunc func =
cn == 1 ? (CvUndistortRadialIPPFunc)icvUndistortRadial_8u_C1R_p :
(CvUndistortRadialIPPFunc)icvUndistortRadial_8u_C3R_p;
if( func && icvUndistortGetSize_p( size, &buf_size ) >= 0 && buf_size > 0 )
{
CV_CALL( buffer = (uchar*)cvAlloc( buf_size ));
if( func( src->data.ptr, src_step, dst->data.ptr,
dst_step, size, a[0], a[4],
a[2], a[5], k[0], k[1], buffer ) >= 0 )
EXIT;
}
}
icvUnDistort_8u_CnR( src->data.ptr, src_step,
dst->data.ptr, dst_step, size, a, k, cn );
__END__;
cvFree( &buffer );
}
// convert array (CvMat or IplImage) to CvMat
CV_IMPL CvMat*
cvGetMat( const CvArr* array, CvMat* mat, int* pCOI )
{
CvMat* result = 0;
CvMat* src = (CvMat*)array;
int coi = 0;
CV_FUNCNAME( "cvGetMat" );
__BEGIN__;
if( !mat || !src )
CV_ERROR_FROM_CODE( CV_StsNullPtr );
if( !_CV_IS_ARR(src) )
{
const IplImage* img = (const IplImage*)src;
int depth, order;
if( img->nSize != sizeof(IplImage))
CV_ERROR_FROM_CODE( CV_StsBadFlag );
if( img->imageData == 0 )
CV_ERROR_FROM_CODE( CV_StsNullPtr );
if( img->nChannels > 4 )
CV_ERROR_FROM_CODE( CV_BadNumChannels );
depth = icvIplToCvDepth( img->depth );
if( depth < 0 )
CV_ERROR_FROM_CODE( CV_BadDepth );
order = img->dataOrder & (img->nChannels > 1 ? -1 : 0);
if( img->roi )
{
if( order == IPL_DATA_ORDER_PLANE )
{
int type = depth;
if( img->roi->coi == 0 )
CV_ERROR( CV_StsBadFlag, "Planar order should be used with coi != 0" );
CV_CALL( cvInitMatHeader( mat, img->roi->height,
img->roi->width, type,
img->imageData + (img->roi->coi-1)*img->imageSize +
img->roi->yOffset*img->widthStep +
img->roi->xOffset*icvPixSize[type],
img->widthStep ));
}
else /* pixel order */
{
int type = depth + (img->nChannels - 1)*8;
coi = img->roi->coi;
CV_CALL( cvInitMatHeader( mat, img->roi->height,
img->roi->width, type,
img->imageData +
img->roi->yOffset*img->widthStep +
img->roi->xOffset*icvPixSize[type],
img->widthStep ));
}
}
else
{
int type = depth + (img->nChannels - 1)*8;
if( order != IPL_DATA_ORDER_PIXEL )
CV_ERROR( CV_StsBadFlag, "Pixel order should be used with coi == 0" );
CV_CALL( cvInitMatHeader( mat, img->height,
img->width, type,
img->imageData, img->widthStep ));
}
result = mat;
}
else
{
if( !src->data.ptr )
CV_ERROR_FROM_CODE( CV_StsNullPtr );
result = (CvMat*)src;
}
__END__;
if( pCOI )
*pCOI = coi;
return result;
}
CV_IMPL void
cvInitUndistortMap( const CvMat* A, const CvMat* dist_coeffs,
CvArr* mapxarr, CvArr* mapyarr )
{
uchar* buffer = 0;
CV_FUNCNAME( "cvInitUndistortMap" );
__BEGIN__;
float a[9], k[4];
int coi1 = 0, coi2 = 0;
CvMat mapxstub, *_mapx = (CvMat*)mapxarr;
CvMat mapystub, *_mapy = (CvMat*)mapyarr;
float *mapx, *mapy;
CvMat _a = cvMat( 3, 3, CV_32F, a ), _k;
int mapxstep, mapystep;
int u, v;
float u0, v0, fx, fy, _fx, _fy, k1, k2, p1, p2;
CvSize size;
CV_CALL( _mapx = cvGetMat( _mapx, &mapxstub, &coi1 ));
CV_CALL( _mapy = cvGetMat( _mapy, &mapystub, &coi2 ));
if( coi1 != 0 || coi2 != 0 )
CV_ERROR( CV_BadCOI, "The function does not support COI" );
if( CV_MAT_TYPE(_mapx->type) != CV_32FC1 )
CV_ERROR( CV_StsUnsupportedFormat, "Both maps must have 32fC1 type" );
if( !CV_ARE_TYPES_EQ( _mapx, _mapy ))
CV_ERROR( CV_StsUnmatchedFormats, "" );
if( !CV_ARE_SIZES_EQ( _mapx, _mapy ))
CV_ERROR( CV_StsUnmatchedSizes, "" );
if( !CV_IS_MAT(A) || A->rows != 3 || A->cols != 3 ||
CV_MAT_TYPE(A->type) != CV_32FC1 && CV_MAT_TYPE(A->type) != CV_64FC1 )
CV_ERROR( CV_StsBadArg, "Intrinsic matrix must be a valid 3x3 floating-point matrix" );
if( !CV_IS_MAT(dist_coeffs) || dist_coeffs->rows != 1 && dist_coeffs->cols != 1 ||
dist_coeffs->rows*dist_coeffs->cols*CV_MAT_CN(dist_coeffs->type) != 4 ||
CV_MAT_DEPTH(dist_coeffs->type) != CV_64F &&
CV_MAT_DEPTH(dist_coeffs->type) != CV_32F )
CV_ERROR( CV_StsBadArg,
"Distortion coefficients must be 1x4 or 4x1 floating-point vector" );
cvConvert( A, &_a );
_k = cvMat( dist_coeffs->rows, dist_coeffs->cols,
CV_MAKETYPE(CV_32F, CV_MAT_CN(dist_coeffs->type)), k );
cvConvert( dist_coeffs, &_k );
u0 = a[2]; v0 = a[5];
fx = a[0]; fy = a[4];
_fx = 1.f/fx; _fy = 1.f/fy;
k1 = k[0]; k2 = k[1];
p1 = k[2]; p2 = k[3];
mapxstep = _mapx->step ? _mapx->step : CV_STUB_STEP;
mapystep = _mapy->step ? _mapy->step : CV_STUB_STEP;
mapx = _mapx->data.fl;
mapy = _mapy->data.fl;
size = cvGetMatSize(_mapx);
/*if( icvUndistortGetSize_p && icvCreateMapCameraUndistort_32f_C1R_p )
{
int buf_size = 0;
if( icvUndistortGetSize_p( size, &buf_size ) && buf_size > 0 )
{
CV_CALL( buffer = (uchar*)cvAlloc( buf_size ));
if( icvCreateMapCameraUndistort_32f_C1R_p(
mapx, mapxstep, mapy, mapystep, size,
a[0], a[4], a[2], a[5], k[0], k[1], k[2], k[3], buffer ) >= 0 )
EXIT;
}
}*/
mapxstep /= sizeof(mapx[0]);
mapystep /= sizeof(mapy[0]);
for( v = 0; v < size.height; v++, mapx += mapxstep, mapy += mapystep )
{
float y = (v - v0)*_fy;
float y2 = y*y;
float _2p1y = 2*p1*y;
float _3p1y2 = 3*p1*y2;
float p2y2 = p2*y2;
for( u = 0; u < size.width; u++ )
{
float x = (u - u0)*_fx;
float x2 = x*x;
float r2 = x2 + y2;
float d = 1 + (k1 + k2*r2)*r2;
float _u = fx*(x*(d + _2p1y) + p2y2 + (3*p2)*x2) + u0;
float _v = fy*(y*(d + (2*p2)*x) + _3p1y2 + p1*x2) + v0;
mapx[u] = _u;
mapy[u] = _v;
}
}
__END__;
cvFree( &buffer );
}
CV_IMPL void cvNormalize( const CvArr* src, CvArr* dst,
double a, double b, int norm_type, const CvArr* mask )
{
CvMat* tmp = 0;
CV_FUNCNAME( "cvNormalize" );
__BEGIN__;
double scale, shift;
if( norm_type == CV_MINMAX )
{
double smin = 0, smax = 0;
double dmin = MIN( a, b ), dmax = MAX( a, b );
cvMinMaxLoc( src, &smin, &smax, 0, 0, mask );
scale = (dmax - dmin)*(smax - smin > DBL_EPSILON ? 1./(smax - smin) : 0);
shift = dmin - smin*scale;
}
else if( norm_type == CV_L2 || norm_type == CV_L1 || norm_type == CV_C )
{
CvMat *s = (CvMat*)src, *d = (CvMat*)dst;
if( CV_IS_MAT(s) && CV_IS_MAT(d) && CV_IS_MAT_CONT(s->type & d->type) &&
CV_ARE_TYPES_EQ(s,d) && CV_ARE_SIZES_EQ(s,d) && !mask &&
s->cols*s->rows <= CV_MAX_INLINE_MAT_OP_SIZE*CV_MAX_INLINE_MAT_OP_SIZE )
{
int i, len = s->cols*s->rows;
double norm = 0, v;
if( CV_MAT_TYPE(s->type) == CV_32FC1 )
{
const float* sptr = s->data.fl;
float* dptr = d->data.fl;
if( norm_type == CV_L2 )
{
for( i = 0; i < len; i++ )
{
v = sptr[i];
norm += v*v;
}
norm = sqrt(norm);
}
else if( norm_type == CV_L1 )
for( i = 0; i < len; i++ )
{
v = fabs((double)sptr[i]);
norm += v;
}
else
for( i = 0; i < len; i++ )
{
v = fabs((double)sptr[i]);
norm = MAX(norm,v);
}
norm = norm > DBL_EPSILON ? 1./norm : 0.;
for( i = 0; i < len; i++ )
dptr[i] = (float)(sptr[i]*norm);
EXIT;
}
if( CV_MAT_TYPE(s->type) == CV_64FC1 )
{
const double* sptr = s->data.db;
double* dptr = d->data.db;
if( norm_type == CV_L2 )
{
for( i = 0; i < len; i++ )
{
v = sptr[i];
norm += v*v;
}
norm = sqrt(norm);
}
else if( norm_type == CV_L1 )
for( i = 0; i < len; i++ )
{
v = fabs(sptr[i]);
norm += v;
}
else
for( i = 0; i < len; i++ )
{
v = fabs(sptr[i]);
norm = MAX(norm,v);
}
norm = norm > DBL_EPSILON ? 1./norm : 0.;
for( i = 0; i < len; i++ )
dptr[i] = sptr[i]*norm;
EXIT;
}
}
scale = cvNorm( src, 0, norm_type, mask );
scale = scale > DBL_EPSILON ? 1./scale : 0.;
shift = 0;
}
else
CV_ERROR( CV_StsBadArg, "Unknown/unsupported norm type" );
if( !mask )
cvConvertScale( src, dst, scale, shift );
else
{
CvMat stub, *dmat;
CV_CALL( dmat = cvGetMat(dst, &stub));
CV_CALL( tmp = cvCreateMat(dmat->rows, dmat->cols, dmat->type) );
cvConvertScale( src, tmp, scale, shift );
cvCopy( tmp, dst, mask );
}
__END__;
if( tmp )
cvReleaseMat( &tmp );
}
void cvUpdateTracks(CvBlobs const &blobs, CvTracks &tracks, const double thDistance, const unsigned int thInactive, const unsigned int thActive)
{
CV_FUNCNAME("cvUpdateTracks");
__CV_BEGIN__;
unsigned int nBlobs = blobs.size();
unsigned int nTracks = tracks.size();
// Proximity matrix:
// Last row/column is for ID/label.
// Last-1 "/" is for accumulation.
CvID *close = new unsigned int[(nBlobs+2)*(nTracks+2)]; // XXX Must be same type than CvLabel.
try
{
// Inicialization:
unsigned int i=0;
for (CvBlobs::const_iterator it = blobs.begin(); it!=blobs.end(); ++it, i++)
{
AB(i) = 0;
IB(i) = it->second->label;
}
CvID maxTrackID = 0;
unsigned int j=0;
for (CvTracks::const_iterator jt = tracks.begin(); jt!=tracks.end(); ++jt, j++)
{
AT(j) = 0;
IT(j) = jt->second->id;
if (jt->second->id > maxTrackID)
maxTrackID = jt->second->id;
}
// Proximity matrix calculation and "used blob" list inicialization:
for (i=0; i<nBlobs; i++)
for (j=0; j<nTracks; j++)
if (C(i, j) = (distantBlobTrack(B(i), T(j)) < thDistance))
{
AB(i)++;
AT(j)++;
}
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Detect inactive tracks
for (j=0; j<nTracks; j++)
{
unsigned int c = AT(j);
if (c==0)
{
//cout << "Inactive track: " << j << endl;
// Inactive track.
CvTrack *track = T(j);
track->inactive++;
track->label = 0;
}
}
// Detect new tracks
for (i=0; i<nBlobs; i++)
{
unsigned int c = AB(i);
if (c==0)
{
//cout << "Blob (new track): " << maxTrackID+1 << endl;
//cout << *B(i) << endl;
// New track.
maxTrackID++;
CvBlob *blob = B(i);
CvTrack *track = new CvTrack;
track->id = maxTrackID;
track->label = blob->label;
track->minx = blob->minx;
track->miny = blob->miny;
track->maxx = blob->maxx;
track->maxy = blob->maxy;
track->centroid = blob->centroid;
track->lifetime = 0;
track->active = 0;
track->inactive = 0;
tracks.insert(CvIDTrack(maxTrackID, track));
}
}
// Clustering
for (j=0; j<nTracks; j++)
{
unsigned int c = AT(j);
if (c)
{
list<CvTrack*> tt; tt.push_back(T(j));
list<CvBlob*> bb;
getClusterForTrack(j, close, nBlobs, nTracks, blobs, tracks, bb, tt);
// Select track
CvTrack *track;
unsigned int area = 0;
for (list<CvTrack*>::const_iterator it = tt.begin(); it != tt.end(); ++it)
{
CvTrack *t = *it;
unsigned int a = (t->maxx - t->minx)*(t->maxy - t->miny);
if (a > area)
{
area = a;
track = t;
}
}
// Select blob
CvBlob *blob;
area = 0;
//cout << "Matching blobs: ";
for (list<CvBlob*>::const_iterator it=bb.begin(); it!=bb.end(); ++it)
{
CvBlob *b = *it;
//cout << b->label << " ";
if (b->area>area)
{
area = b->area;
blob = b;
}
}
//cout << endl;
// Update track
//cout << "Matching: track=" << track->id << ", blob=" << blob->label << endl;
track->label = blob->label;
track->centroid = blob->centroid;
track->minx = blob->minx;
track->miny = blob->miny;
track->maxx = blob->maxx;
track->maxy = blob->maxy;
if (track->inactive)
track->active = 0;
track->inactive = 0;
// Others to inactive
for (list<CvTrack*>::const_iterator it=tt.begin(); it!=tt.end(); ++it)
{
CvTrack *t = *it;
if (t!=track)
{
//cout << "Inactive: track=" << t->id << endl;
t->inactive++;
t->label = 0;
}
}
}
}
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
for (CvTracks::iterator jt=tracks.begin(); jt!=tracks.end();)
if ((jt->second->inactive>=thInactive)||((jt->second->inactive)&&(thActive)&&(jt->second->active<thActive)))
{
delete jt->second;
tracks.erase(jt++);
}
else
{
jt->second->lifetime++;
if (!jt->second->inactive)
jt->second->active++;
++jt;
}
}
catch (...)
{
delete[] close;
throw; // TODO: OpenCV style.
}
delete[] close;
__CV_END__;
}
CV_IMPL void cvRandShuffle( CvArr* arr, CvRNG* rng, double iter_factor )
{
CV_FUNCNAME( "cvRandShuffle" );
__BEGIN__;
const int sizeof_int = (int)sizeof(int);
CvMat stub, *mat = (CvMat*)arr;
int i, j, k, iters, delta = 0;
int cont_flag, arr_size, elem_size, cols, step;
const int pair_buf_sz = 100;
int* pair_buf = (int*)cvStackAlloc( pair_buf_sz*sizeof(pair_buf[0])*2 );
CvMat _pair_buf = cvMat( 1, pair_buf_sz*2, CV_32S, pair_buf );
CvRNG _rng = cvRNG(-1);
uchar* data = 0;
int* idata = 0;
if( !CV_IS_MAT(mat) )
CV_CALL( mat = cvGetMat( mat, &stub ));
if( !rng )
rng = &_rng;
cols = mat->cols;
step = mat->step;
arr_size = cols*mat->rows;
iters = cvRound(iter_factor*arr_size)*2;
cont_flag = CV_IS_MAT_CONT(mat->type);
elem_size = CV_ELEM_SIZE(mat->type);
if( elem_size % sizeof_int == 0 && (cont_flag || step % sizeof_int == 0) )
{
idata = mat->data.i;
step /= sizeof_int;
elem_size /= sizeof_int;
}
else
data = mat->data.ptr;
for( i = 0; i < iters; i += delta )
{
delta = MIN( iters - i, pair_buf_sz*2 );
_pair_buf.cols = delta;
cvRandArr( rng, &_pair_buf, CV_RAND_UNI, cvRealScalar(0), cvRealScalar(arr_size) );
if( cont_flag )
{
if( idata )
for( j = 0; j < delta; j += 2 )
{
int* p = idata + pair_buf[j]*elem_size, *q = idata + pair_buf[j+1]*elem_size, t;
for( k = 0; k < elem_size; k++ )
CV_SWAP( p[k], q[k], t );
}
else
for( j = 0; j < delta; j += 2 )
{
uchar* p = data + pair_buf[j]*elem_size, *q = data + pair_buf[j+1]*elem_size, t;
for( k = 0; k < elem_size; k++ )
CV_SWAP( p[k], q[k], t );
}
}
else
{
if( idata )
for( j = 0; j < delta; j += 2 )
{
int idx1 = pair_buf[j], idx2 = pair_buf[j+1], row1, row2;
int* p, *q, t;
row1 = idx1/step; row2 = idx2/step;
p = idata + row1*step + (idx1 - row1*cols)*elem_size;
q = idata + row2*step + (idx2 - row2*cols)*elem_size;
for( k = 0; k < elem_size; k++ )
CV_SWAP( p[k], q[k], t );
}
else
for( j = 0; j < delta; j += 2 )
{
int idx1 = pair_buf[j], idx2 = pair_buf[j+1], row1, row2;
uchar* p, *q, t;
row1 = idx1/step; row2 = idx2/step;
p = data + row1*step + (idx1 - row1*cols)*elem_size;
q = data + row2*step + (idx2 - row2*cols)*elem_size;
for( k = 0; k < elem_size; k++ )
CV_SWAP( p[k], q[k], t );
}
}
}
__END__;
}
void cvRenderTracks(CvTracks const tracks, IplImage *imgSource, IplImage *imgDest, unsigned short mode, CvFont *font)
{
CV_FUNCNAME("cvRenderTracks");
__CV_BEGIN__;
CV_ASSERT(imgDest&&(imgDest->depth==IPL_DEPTH_8U)&&(imgDest->nChannels==3));
if ((mode&CV_TRACK_RENDER_ID)&&(!font))
{
if (!defaultFont)
{
font = defaultFont = new CvFont;
cvInitFont(font, CV_FONT_HERSHEY_DUPLEX, 0.5, 0.5, 0, 1);
// Other fonts:
// CV_FONT_HERSHEY_SIMPLEX, CV_FONT_HERSHEY_PLAIN,
// CV_FONT_HERSHEY_DUPLEX, CV_FONT_HERSHEY_COMPLEX,
// CV_FONT_HERSHEY_TRIPLEX, CV_FONT_HERSHEY_COMPLEX_SMALL,
// CV_FONT_HERSHEY_SCRIPT_SIMPLEX, CV_FONT_HERSHEY_SCRIPT_COMPLEX
}
else
font = defaultFont;
}
if (mode)
{
for (CvTracks::const_iterator it=tracks.begin(); it!=tracks.end(); ++it)
{
if (mode&CV_TRACK_RENDER_ID)
if (!it->second->inactive)
{
stringstream buffer;
buffer << it->first;
cvPutText(imgDest, buffer.str().c_str(), cvPoint((int)it->second->centroid.x, (int)it->second->centroid.y), font, CV_RGB(0.,255.,0.));
}
if (mode&CV_TRACK_RENDER_BOUNDING_BOX)
if (it->second->inactive)
cvRectangle(imgDest, cvPoint(it->second->minx, it->second->miny), cvPoint(it->second->maxx-1, it->second->maxy-1), CV_RGB(0., 0., 50.));
else
cvRectangle(imgDest, cvPoint(it->second->minx, it->second->miny), cvPoint(it->second->maxx-1, it->second->maxy-1), CV_RGB(0., 0., 255.));
if (mode&CV_TRACK_RENDER_TO_LOG)
{
clog << "Track " << it->second->id << endl;
if (it->second->inactive)
clog << " - Inactive for " << it->second->inactive << " frames" << endl;
else
clog << " - Associated with blob " << it->second->label << endl;
clog << " - Lifetime " << it->second->lifetime << endl;
clog << " - Active " << it->second->active << endl;
clog << " - Bounding box: (" << it->second->minx << ", " << it->second->miny << ") - (" << it->second->maxx << ", " << it->second->maxy << ")" << endl;
clog << " - Centroid: (" << it->second->centroid.x << ", " << it->second->centroid.y << ")" << endl;
clog << endl;
}
if (mode&CV_TRACK_RENDER_TO_STD)
{
cout << "Track " << it->second->id << endl;
if (it->second->inactive)
cout << " - Inactive for " << it->second->inactive << " frames" << endl;
else
cout << " - Associated with blobs " << it->second->label << endl;
cout << " - Lifetime " << it->second->lifetime << endl;
cout << " - Active " << it->second->active << endl;
cout << " - Bounding box: (" << it->second->minx << ", " << it->second->miny << ") - (" << it->second->maxx << ", " << it->second->maxy << ")" << endl;
cout << " - Centroid: (" << it->second->centroid.x << ", " << it->second->centroid.y << ")" << endl;
cout << endl;
}
}
}
__CV_END__;
}
void CvBoxFilter::init( int _max_width, int _src_type, int _dst_type,
bool _normalized, CvSize _ksize,
CvPoint _anchor, int _border_mode,
CvScalar _border_value )
{
CV_FUNCNAME( "CvBoxFilter::init" );
__BEGIN__;
sum = 0;
normalized = _normalized;
if( normalized && CV_MAT_TYPE(_src_type) != CV_MAT_TYPE(_dst_type) ||
!normalized && CV_MAT_CN(_src_type) != CV_MAT_CN(_dst_type))
CV_ERROR( CV_StsUnmatchedFormats,
"In case of normalized box filter input and output must have the same type.\n"
"In case of unnormalized box filter the number of input and output channels must be the same" );
min_depth = CV_MAT_DEPTH(_src_type) == CV_8U ? CV_32S : CV_64F;
CvBaseImageFilter::init( _max_width, _src_type, _dst_type, 1, _ksize,
_anchor, _border_mode, _border_value );
scale = normalized ? 1./(ksize.width*ksize.height) : 1;
if( CV_MAT_DEPTH(src_type) == CV_8U )
x_func = (CvRowFilterFunc)icvSumRow_8u32s;
else if( CV_MAT_DEPTH(src_type) == CV_32F )
x_func = (CvRowFilterFunc)icvSumRow_32f64f;
else
CV_ERROR( CV_StsUnsupportedFormat, "Unknown/unsupported input image format" );
if( CV_MAT_DEPTH(dst_type) == CV_8U )
{
if( !normalized )
CV_ERROR( CV_StsBadArg, "Only normalized box filter can be used for 8u->8u transformation" );
y_func = (CvColumnFilterFunc)icvSumCol_32s8u;
}
else if( CV_MAT_DEPTH(dst_type) == CV_16S )
{
if( normalized || CV_MAT_DEPTH(src_type) != CV_8U )
CV_ERROR( CV_StsBadArg, "Only 8u->16s unnormalized box filter is supported in case of 16s output" );
y_func = (CvColumnFilterFunc)icvSumCol_32s16s;
}
else if( CV_MAT_DEPTH(dst_type) == CV_32S )
{
if( normalized || CV_MAT_DEPTH(src_type) != CV_8U )
CV_ERROR( CV_StsBadArg, "Only 8u->32s unnormalized box filter is supported in case of 32s output");
y_func = (CvColumnFilterFunc)icvSumCol_32s32s;
}
else if( CV_MAT_DEPTH(dst_type) == CV_32F )
{
if( CV_MAT_DEPTH(src_type) != CV_32F )
CV_ERROR( CV_StsBadArg, "Only 32f->32f box filter (normalized or not) is supported in case of 32f output" );
y_func = (CvColumnFilterFunc)icvSumCol_64f32f;
}
else{
CV_ERROR( CV_StsBadArg, "Unknown/unsupported destination image format" );
}
__END__;
}
CV_IMPL void cvFindStereoCorrespondenceGC( const CvArr* _left, const CvArr* _right,
CvArr* _dispLeft, CvArr* _dispRight, CvStereoGCState* state, int useDisparityGuess )
{
CvStereoGCState2 state2;
state2.orphans = 0;
state2.maxOrphans = 0;
CV_FUNCNAME( "cvFindStereoCorrespondenceGC" );
__BEGIN__;
CvMat lstub, *left = cvGetMat( _left, &lstub );
CvMat rstub, *right = cvGetMat( _right, &rstub );
CvMat dlstub, *dispLeft = cvGetMat( _dispLeft, &dlstub );
CvMat drstub, *dispRight = cvGetMat( _dispRight, &drstub );
CvSize size;
int iter, i, nZeroExpansions = 0;
CvRNG rng = cvRNG(-1);
int* disp;
CvMat _disp;
int64 E;
CV_ASSERT( state != 0 );
CV_ASSERT( CV_ARE_SIZES_EQ(left, right) && CV_ARE_TYPES_EQ(left, right) &&
CV_MAT_TYPE(left->type) == CV_8UC1 );
CV_ASSERT( !dispLeft ||
(CV_ARE_SIZES_EQ(dispLeft, left) && CV_MAT_CN(dispLeft->type) == 1) );
CV_ASSERT( !dispRight ||
(CV_ARE_SIZES_EQ(dispRight, left) && CV_MAT_CN(dispRight->type) == 1) );
size = cvGetSize(left);
if( !state->left || state->left->width != size.width || state->left->height != size.height )
{
int pcn = (int)(sizeof(GCVtx*)/sizeof(int));
int vcn = (int)(sizeof(GCVtx)/sizeof(int));
int ecn = (int)(sizeof(GCEdge)/sizeof(int));
cvReleaseMat( &state->left );
cvReleaseMat( &state->right );
cvReleaseMat( &state->ptrLeft );
cvReleaseMat( &state->ptrRight );
cvReleaseMat( &state->dispLeft );
cvReleaseMat( &state->dispRight );
state->left = cvCreateMat( size.height, size.width, CV_8UC3 );
state->right = cvCreateMat( size.height, size.width, CV_8UC3 );
state->dispLeft = cvCreateMat( size.height, size.width, CV_16SC1 );
state->dispRight = cvCreateMat( size.height, size.width, CV_16SC1 );
state->ptrLeft = cvCreateMat( size.height, size.width, CV_32SC(pcn) );
state->ptrRight = cvCreateMat( size.height, size.width, CV_32SC(pcn) );
state->vtxBuf = cvCreateMat( 1, size.height*size.width*2, CV_32SC(vcn) );
state->edgeBuf = cvCreateMat( 1, size.height*size.width*12 + 16, CV_32SC(ecn) );
}
if( !useDisparityGuess )
{
cvSet( state->dispLeft, cvScalarAll(OCCLUDED));
cvSet( state->dispRight, cvScalarAll(OCCLUDED));
}
else
{
CV_ASSERT( dispLeft && dispRight );
cvConvert( dispLeft, state->dispLeft );
cvConvert( dispRight, state->dispRight );
}
state2.Ithreshold = state->Ithreshold;
state2.interactionRadius = state->interactionRadius;
state2.lambda = cvRound(state->lambda*DENOMINATOR);
state2.lambda1 = cvRound(state->lambda1*DENOMINATOR);
state2.lambda2 = cvRound(state->lambda2*DENOMINATOR);
state2.K = cvRound(state->K*DENOMINATOR);
icvInitStereoConstTabs();
icvInitGraySubpix( left, right, state->left, state->right );
disp = (int*)cvStackAlloc( state->numberOfDisparities*sizeof(disp[0]) );
_disp = cvMat( 1, state->numberOfDisparities, CV_32S, disp );
cvRange( &_disp, state->minDisparity, state->minDisparity + state->numberOfDisparities );
cvRandShuffle( &_disp, &rng );
if( state2.lambda < 0 && (state2.K < 0 || state2.lambda1 < 0 || state2.lambda2 < 0) )
{
float L = icvComputeK(state)*0.2f;
state2.lambda = cvRound(L*DENOMINATOR);
}
if( state2.K < 0 )
state2.K = state2.lambda*5;
if( state2.lambda1 < 0 )
state2.lambda1 = state2.lambda*3;
if( state2.lambda2 < 0 )
state2.lambda2 = state2.lambda;
icvInitStereoTabs( &state2 );
E = icvComputeEnergy( state, &state2, !useDisparityGuess );
for( iter = 0; iter < state->maxIters; iter++ )
{
for( i = 0; i < state->numberOfDisparities; i++ )
{
int alpha = disp[i];
int64 Enew = icvAlphaExpand( E, -alpha, state, &state2 );
if( Enew < E )
{
nZeroExpansions = 0;
E = Enew;
}
else if( ++nZeroExpansions >= state->numberOfDisparities )
break;
}
}
if( dispLeft )
cvConvert( state->dispLeft, dispLeft );
if( dispRight )
cvConvert( state->dispRight, dispRight );
__END__;
cvFree( &state2.orphans );
}
/*F///////////////////////////////////////////////////////////////////////////////////////
// Name: cvCamShift
// Purpose: CAMSHIFT algorithm
// Context:
// Parameters:
// imgProb - 2D object probability distribution
// windowIn - CvRect of CAMSHIFT Window intial size
// criteria - criteria of stop finding window
// windowOut - Location, height and width of converged CAMSHIFT window
// orientation - If != NULL, return distribution orientation
// len - If != NULL, return equivalent len
// width - If != NULL, return equivalent width
// area - sum of all elements in result window
// itersUsed - Returns number of iterations CAMSHIFT took to converge
// Returns:
// The function itself returns the area found
// Notes:
//F*/
CV_IMPL int
cvCamShift( const void* imgProb, CvRect windowIn,
CvTermCriteria criteria,
CvConnectedComp* _comp,
CvBox2D* box )
{
const int TOLERANCE = 10;
CvMoments moments;
double m00 = 0, m10, m01, mu20, mu11, mu02, inv_m00;
double a, b, c, xc, yc;
double rotate_a, rotate_c;
double theta = 0, square;
double cs, sn;
double length = 0, width = 0;
int itersUsed = 0;
CvConnectedComp comp;
CvMat cur_win, stub, *mat = (CvMat*)imgProb;
CV_FUNCNAME( "cvCamShift" );
comp.rect = windowIn;
__BEGIN__;
CV_CALL( mat = cvGetMat( mat, &stub ));
CV_CALL( itersUsed = cvMeanShift( mat, windowIn, criteria, &comp ));
windowIn = comp.rect;
windowIn.x -= TOLERANCE;
if( windowIn.x < 0 )
windowIn.x = 0;
windowIn.y -= TOLERANCE;
if( windowIn.y < 0 )
windowIn.y = 0;
windowIn.width += 2 * TOLERANCE;
if( windowIn.x + windowIn.width > mat->width )
windowIn.width = mat->width - windowIn.x;
windowIn.height += 2 * TOLERANCE;
if( windowIn.y + windowIn.height > mat->height )
windowIn.height = mat->height - windowIn.y;
CV_CALL( cvGetSubRect( mat, &cur_win, windowIn ));
/* Calculating moments in new center mass */
CV_CALL( cvMoments( &cur_win, &moments ));
m00 = moments.m00;
m10 = moments.m10;
m01 = moments.m01;
mu11 = moments.mu11;
mu20 = moments.mu20;
mu02 = moments.mu02;
if( fabs(m00) < DBL_EPSILON )
EXIT;
inv_m00 = 1. / m00;
xc = cvRound( m10 * inv_m00 + windowIn.x );
yc = cvRound( m01 * inv_m00 + windowIn.y );
a = mu20 * inv_m00;
b = mu11 * inv_m00;
c = mu02 * inv_m00;
/* Calculating width & height */
square = sqrt( 4 * b * b + (a - c) * (a - c) );
/* Calculating orientation */
theta = atan2( 2 * b, a - c + square );
/* Calculating width & length of figure */
cs = cos( theta );
sn = sin( theta );
rotate_a = cs * cs * mu20 + 2 * cs * sn * mu11 + sn * sn * mu02;
rotate_c = sn * sn * mu20 - 2 * cs * sn * mu11 + cs * cs * mu02;
length = sqrt( rotate_a * inv_m00 ) * 4;
width = sqrt( rotate_c * inv_m00 ) * 4;
/* In case, when tetta is 0 or 1.57... the Length & Width may be exchanged */
if( length < width )
{
double t;
CV_SWAP( length, width, t );
CV_SWAP( cs, sn, t );
theta = CV_PI*0.5 - theta;
}
/* Saving results */
if( _comp || box )
{
int t0, t1;
int _xc = cvRound( xc );
int _yc = cvRound( yc );
t0 = cvRound( fabs( length * cs ));
t1 = cvRound( fabs( width * sn ));
t0 = MAX( t0, t1 ) + 2;
comp.rect.width = MIN( t0, (mat->width - _xc) * 2 );
t0 = cvRound( fabs( length * sn ));
t1 = cvRound( fabs( width * cs ));
t0 = MAX( t0, t1 ) + 2;
comp.rect.height = MIN( t0, (mat->height - _yc) * 2 );
comp.rect.x = MAX( 0, _xc - comp.rect.width / 2 );
comp.rect.y = MAX( 0, _yc - comp.rect.height / 2 );
comp.rect.width = MIN( mat->width - comp.rect.x, comp.rect.width );
comp.rect.height = MIN( mat->height - comp.rect.y, comp.rect.height );
comp.area = (float) m00;
}
__END__;
if( _comp )
*_comp = comp;
if( box )
{
box->size.height = (float)length;
box->size.width = (float)width;
box->angle = (float)(theta*180./CV_PI);
box->center = cvPoint2D32f( comp.rect.x + comp.rect.width*0.5f,
comp.rect.y + comp.rect.height*0.5f);
}
return itersUsed;
}
CV_IMPL CvMat*
cvReshape( const CvArr* array, CvMat* header,
int new_cn, int new_rows )
{
CvMat* result = 0;
CV_FUNCNAME( "cvReshape" );
__BEGIN__;
CvMat *mat = (CvMat*)array;
int total_width, new_width;
if( !header )
CV_ERROR( CV_StsNullPtr, "" );
if( (unsigned)(new_cn - 1) > 3 )
CV_ERROR( CV_BadNumChannels, "" );
if( !CV_IS_ARR( mat ))
CV_CALL( mat = cvGetMat( mat, header ));
if( mat != header )
*header = *mat;
total_width = mat->width * CV_ARR_CN( mat->type );
if( new_cn > total_width )
new_rows = mat->height * total_width / new_cn;
if( new_rows != 0 )
{
int total_size = total_width * mat->height;
if( !CV_IS_ARR_CONT( mat->type ))
CV_ERROR( CV_BadStep,
"The matrix is not continuous, thus its number of rows can not be changed" );
if( (unsigned)new_rows > (unsigned)total_size )
CV_ERROR( CV_StsOutOfRange, "Bad new number of rows" );
total_width = total_size / new_rows;
if( total_width * new_rows != total_size )
CV_ERROR( CV_StsBadArg, "The total number of matrix elements "
"is not divisible by the new number of rows" );
header->height = new_rows;
header->step = total_width * icvPixSize[mat->type & CV_ARR_DEPTH_MASK];
}
new_width = total_width / new_cn;
if( new_width * new_cn != total_width )
CV_ERROR( CV_BadNumChannels,
"The total width is not divisible by the new number of channels" );
header->width = new_width;
header->type = (mat->type & ~CV_ARR_CN_MASK) + ((new_cn - 1)*8);
__END__;
return result;
}
CV_IMPL CvSeq*
cvConvexHull2( const CvArr* array, void* hull_storage,
int orientation, int return_points )
{
union { CvContour* c; CvSeq* s; } hull;
CvPoint** pointer = 0;
CvPoint2D32f** pointerf = 0;
int* stack = 0;
CV_FUNCNAME( "cvConvexHull2" );
hull.s = 0;
__BEGIN__;
CvMat* mat = 0;
CvSeqReader reader;
CvSeqWriter writer;
CvContour contour_header;
union { CvContour c; CvSeq s; } hull_header;
CvSeqBlock block, hullblock;
CvSeq* ptseq = 0;
CvSeq* hullseq = 0;
int is_float;
int* t_stack;
int t_count;
int i, miny_ind = 0, maxy_ind = 0, total;
int hulltype;
int stop_idx;
sklansky_func sklansky;
if( CV_IS_SEQ( array ))
{
ptseq = (CvSeq*)array;
if( !CV_IS_SEQ_POINT_SET( ptseq ))
CV_ERROR( CV_StsBadArg, "Unsupported sequence type" );
if( hull_storage == 0 )
hull_storage = ptseq->storage;
}
else
{
CV_CALL( ptseq = cvPointSeqFromMat(
CV_SEQ_KIND_GENERIC, array, &contour_header, &block ));
}
if( CV_IS_STORAGE( hull_storage ))
{
if( return_points )
{
CV_CALL( hullseq = cvCreateSeq(
CV_SEQ_KIND_CURVE|CV_SEQ_ELTYPE(ptseq)|
CV_SEQ_FLAG_CLOSED|CV_SEQ_FLAG_CONVEX,
sizeof(CvContour), sizeof(CvPoint),(CvMemStorage*)hull_storage ));
}
else
{
CV_CALL( hullseq = cvCreateSeq(
CV_SEQ_KIND_CURVE|CV_SEQ_ELTYPE_PPOINT|
CV_SEQ_FLAG_CLOSED|CV_SEQ_FLAG_CONVEX,
sizeof(CvContour), sizeof(CvPoint*), (CvMemStorage*)hull_storage ));
}
}
else
{
if( !CV_IS_MAT( hull_storage ))
CV_ERROR(CV_StsBadArg, "Destination must be valid memory storage or matrix");
mat = (CvMat*)hull_storage;
if( mat->cols != 1 && mat->rows != 1 || !CV_IS_MAT_CONT(mat->type))
CV_ERROR( CV_StsBadArg,
"The hull matrix should be continuous and have a single row or a single column" );
if( mat->cols + mat->rows - 1 < ptseq->total )
CV_ERROR( CV_StsBadSize, "The hull matrix size might be not enough to fit the hull" );
if( CV_MAT_TYPE(mat->type) != CV_SEQ_ELTYPE(ptseq) &&
CV_MAT_TYPE(mat->type) != CV_32SC1 )
CV_ERROR( CV_StsUnsupportedFormat,
"The hull matrix must have the same type as input or 32sC1 (integers)" );
CV_CALL( hullseq = cvMakeSeqHeaderForArray(
CV_SEQ_KIND_CURVE|CV_MAT_TYPE(mat->type)|CV_SEQ_FLAG_CLOSED,
sizeof(contour_header), CV_ELEM_SIZE(mat->type), mat->data.ptr,
mat->cols + mat->rows - 1, &hull_header.s, &hullblock ));
cvClearSeq( hullseq );
}
total = ptseq->total;
if( total == 0 )
{
if( mat )
CV_ERROR( CV_StsBadSize,
"Point sequence can not be empty if the output is matrix" );
EXIT;
}
cvStartAppendToSeq( hullseq, &writer );
is_float = CV_SEQ_ELTYPE(ptseq) == CV_32FC2;
hulltype = CV_SEQ_ELTYPE(hullseq);
sklansky = !is_float ? (sklansky_func)icvSklansky_32s :
(sklansky_func)icvSklansky_32f;
CV_CALL( pointer = (CvPoint**)cvAlloc( ptseq->total*sizeof(pointer[0]) ));
CV_CALL( stack = (int*)cvAlloc( (ptseq->total + 2)*sizeof(stack[0]) ));
pointerf = (CvPoint2D32f**)pointer;
cvStartReadSeq( ptseq, &reader );
for( i = 0; i < total; i++ )
{
pointer[i] = (CvPoint*)reader.ptr;
CV_NEXT_SEQ_ELEM( ptseq->elem_size, reader );
}
// sort the point set by x-coordinate, find min and max y
if( !is_float )
{
icvSortPointsByPointers_32s( pointer, total, 0 );
for( i = 1; i < total; i++ )
{
int y = pointer[i]->y;
if( pointer[miny_ind]->y > y )
miny_ind = i;
if( pointer[maxy_ind]->y < y )
maxy_ind = i;
}
}
else
{
icvSortPointsByPointers_32f( pointerf, total, 0 );
for( i = 1; i < total; i++ )
{
float y = pointerf[i]->y;
if( pointerf[miny_ind]->y > y )
miny_ind = i;
if( pointerf[maxy_ind]->y < y )
maxy_ind = i;
}
}
if( pointer[0]->x == pointer[total-1]->x &&
pointer[0]->y == pointer[total-1]->y )
{
if( hulltype == CV_SEQ_ELTYPE_PPOINT )
{
CV_WRITE_SEQ_ELEM( pointer[0], writer );
}
else if( hulltype == CV_SEQ_ELTYPE_INDEX )
{
int index = 0;
CV_WRITE_SEQ_ELEM( index, writer );
}
else
{
CvPoint pt = pointer[0][0];
CV_WRITE_SEQ_ELEM( pt, writer );
}
goto finish_hull;
}
/*upper half */
{
int *tl_stack = stack;
int tl_count = sklansky( pointer, 0, maxy_ind, tl_stack, -1, 1 );
int *tr_stack = tl_stack + tl_count;
int tr_count = sklansky( pointer, ptseq->total - 1, maxy_ind, tr_stack, -1, -1 );
/* gather upper part of convex hull to output */
if( orientation == CV_COUNTER_CLOCKWISE )
{
CV_SWAP( tl_stack, tr_stack, t_stack );
CV_SWAP( tl_count, tr_count, t_count );
}
if( hulltype == CV_SEQ_ELTYPE_PPOINT )
{
for( i = 0; i < tl_count - 1; i++ )
CV_WRITE_SEQ_ELEM( pointer[tl_stack[i]], writer );
for( i = tr_count - 1; i > 0; i-- )
CV_WRITE_SEQ_ELEM( pointer[tr_stack[i]], writer );
}
else if( hulltype == CV_SEQ_ELTYPE_INDEX )
{
CV_CALL( icvCalcAndWritePtIndices( pointer, tl_stack,
0, tl_count-1, ptseq, &writer ));
CV_CALL( icvCalcAndWritePtIndices( pointer, tr_stack,
tr_count-1, 0, ptseq, &writer ));
}
else
{
for( i = 0; i < tl_count - 1; i++ )
CV_WRITE_SEQ_ELEM( pointer[tl_stack[i]][0], writer );
for( i = tr_count - 1; i > 0; i-- )
CV_WRITE_SEQ_ELEM( pointer[tr_stack[i]][0], writer );
}
stop_idx = tr_count > 2 ? tr_stack[1] : tl_count > 2 ? tl_stack[tl_count - 2] : -1;
}
/* lower half */
{
int *bl_stack = stack;
int bl_count = sklansky( pointer, 0, miny_ind, bl_stack, 1, -1 );
int *br_stack = stack + bl_count;
int br_count = sklansky( pointer, ptseq->total - 1, miny_ind, br_stack, 1, 1 );
if( orientation != CV_COUNTER_CLOCKWISE )
{
CV_SWAP( bl_stack, br_stack, t_stack );
CV_SWAP( bl_count, br_count, t_count );
}
if( stop_idx >= 0 )
{
int check_idx = bl_count > 2 ? bl_stack[1] :
bl_count + br_count > 2 ? br_stack[2-bl_count] : -1;
if( check_idx == stop_idx || check_idx >= 0 &&
pointer[check_idx]->x == pointer[stop_idx]->x &&
pointer[check_idx]->y == pointer[stop_idx]->y )
{
/* if all the points lie on the same line, then
the bottom part of the convex hull is the mirrored top part
(except the exteme points).*/
bl_count = MIN( bl_count, 2 );
br_count = MIN( br_count, 2 );
}
}
if( hulltype == CV_SEQ_ELTYPE_PPOINT )
{
for( i = 0; i < bl_count - 1; i++ )
CV_WRITE_SEQ_ELEM( pointer[bl_stack[i]], writer );
for( i = br_count - 1; i > 0; i-- )
CV_WRITE_SEQ_ELEM( pointer[br_stack[i]], writer );
}
else if( hulltype == CV_SEQ_ELTYPE_INDEX )
{
CV_CALL( icvCalcAndWritePtIndices( pointer, bl_stack,
0, bl_count-1, ptseq, &writer ));
CV_CALL( icvCalcAndWritePtIndices( pointer, br_stack,
br_count-1, 0, ptseq, &writer ));
}
else
{
for( i = 0; i < bl_count - 1; i++ )
CV_WRITE_SEQ_ELEM( pointer[bl_stack[i]][0], writer );
for( i = br_count - 1; i > 0; i-- )
CV_WRITE_SEQ_ELEM( pointer[br_stack[i]][0], writer );
}
}
finish_hull:
CV_CALL( cvEndWriteSeq( &writer ));
if( mat )
{
if( mat->rows > mat->cols )
mat->rows = hullseq->total;
else
mat->cols = hullseq->total;
}
else
{
hull.s = hullseq;
hull.c->rect = cvBoundingRect( ptseq,
ptseq->header_size < (int)sizeof(CvContour) ||
&ptseq->flags == &contour_header.flags );
/*if( ptseq != (CvSeq*)&contour_header )
hullseq->v_prev = ptseq;*/
}
__END__;
cvFree( &pointer );
cvFree( &stack );
return hull.s;
}
CV_IMPL void
cvRQDecomp3x3( const CvMat *matrixM, CvMat *matrixR, CvMat *matrixQ,
CvMat *matrixQx, CvMat *matrixQy, CvMat *matrixQz,
CvPoint3D64f *eulerAngles)
{
CV_FUNCNAME("cvRQDecomp3x3");
__BEGIN__;
double _M[3][3], _R[3][3], _Q[3][3];
CvMat M = cvMat(3, 3, CV_64F, _M);
CvMat R = cvMat(3, 3, CV_64F, _R);
CvMat Q = cvMat(3, 3, CV_64F, _Q);
double z, c, s;
/* Validate parameters. */
CV_ASSERT( CV_IS_MAT(matrixM) && CV_IS_MAT(matrixR) && CV_IS_MAT(matrixQ) &&
matrixM->cols == 3 && matrixM->rows == 3 &&
CV_ARE_SIZES_EQ(matrixM, matrixR) && CV_ARE_SIZES_EQ(matrixM, matrixQ));
cvConvert(matrixM, &M);
{
/* Find Givens rotation Q_x for x axis (left multiplication). */
/*
( 1 0 0 )
Qx = ( 0 c s ), c = m33/sqrt(m32^2 + m33^2), s = m32/sqrt(m32^2 + m33^2)
( 0 -s c )
*/
s = _M[2][1];
c = _M[2][2];
z = 1./sqrt(c * c + s * s + DBL_EPSILON);
c *= z;
s *= z;
double _Qx[3][3] = { {1, 0, 0}, {0, c, s}, {0, -s, c} };
CvMat Qx = cvMat(3, 3, CV_64F, _Qx);
cvMatMul(&M, &Qx, &R);
assert(fabs(_R[2][1]) < FLT_EPSILON);
_R[2][1] = 0;
/* Find Givens rotation for y axis. */
/*
( c 0 s )
Qy = ( 0 1 0 ), c = m33/sqrt(m31^2 + m33^2), s = m31/sqrt(m31^2 + m33^2)
(-s 0 c )
*/
s = _R[2][0];
c = _R[2][2];
z = 1./sqrt(c * c + s * s + DBL_EPSILON);
c *= z;
s *= z;
double _Qy[3][3] = { {c, 0, s}, {0, 1, 0}, {-s, 0, c} };
CvMat Qy = cvMat(3, 3, CV_64F, _Qy);
cvMatMul(&R, &Qy, &M);
assert(fabs(_M[2][0]) < FLT_EPSILON);
_M[2][0] = 0;
/* Find Givens rotation for z axis. */
/*
( c s 0 )
Qz = (-s c 0 ), c = m22/sqrt(m21^2 + m22^2), s = m21/sqrt(m21^2 + m22^2)
( 0 0 1 )
*/
s = _M[1][0];
c = _M[1][1];
z = 1./sqrt(c * c + s * s + DBL_EPSILON);
c *= z;
s *= z;
double _Qz[3][3] = { {c, s, 0}, {-s, c, 0}, {0, 0, 1} };
CvMat Qz = cvMat(3, 3, CV_64F, _Qz);
cvMatMul(&M, &Qz, &R);
assert(fabs(_R[1][0]) < FLT_EPSILON);
_R[1][0] = 0;
// Solve the decomposition ambiguity.
// Diagonal entries of R, except the last one, shall be positive.
// Further rotate R by 180 degree if necessary
if( _R[0][0] < 0 )
{
if( _R[1][1] < 0 )
{
// rotate around z for 180 degree, i.e. a rotation matrix of
// [-1, 0, 0],
// [ 0, -1, 0],
// [ 0, 0, 1]
_R[0][0] *= -1;
_R[0][1] *= -1;
_R[1][1] *= -1;
_Qz[0][0] *= -1;
_Qz[0][1] *= -1;
_Qz[1][0] *= -1;
_Qz[1][1] *= -1;
}
else
{
// rotate around y for 180 degree, i.e. a rotation matrix of
// [-1, 0, 0],
// [ 0, 1, 0],
// [ 0, 0, -1]
_R[0][0] *= -1;
_R[0][2] *= -1;
_R[1][2] *= -1;
_R[2][2] *= -1;
cvTranspose( &Qz, &Qz );
_Qy[0][0] *= -1;
_Qy[0][2] *= -1;
_Qy[2][0] *= -1;
_Qy[2][2] *= -1;
}
}
else if( _R[1][1] < 0 )
{
// ??? for some reason, we never get here ???
// rotate around x for 180 degree, i.e. a rotation matrix of
// [ 1, 0, 0],
// [ 0, -1, 0],
// [ 0, 0, -1]
_R[0][1] *= -1;
_R[0][2] *= -1;
_R[1][1] *= -1;
_R[1][2] *= -1;
_R[2][2] *= -1;
cvTranspose( &Qz, &Qz );
cvTranspose( &Qy, &Qy );
_Qx[1][1] *= -1;
_Qx[1][2] *= -1;
_Qx[2][1] *= -1;
_Qx[2][2] *= -1;
}
// calculate the euler angle
if( eulerAngles )
{
eulerAngles->x = acos(_Qx[1][1]) * (_Qx[1][2] >= 0 ? 1 : -1) * (180.0 / CV_PI);
eulerAngles->y = acos(_Qy[0][0]) * (_Qy[0][2] >= 0 ? 1 : -1) * (180.0 / CV_PI);
eulerAngles->z = acos(_Qz[0][0]) * (_Qz[0][1] >= 0 ? 1 : -1) * (180.0 / CV_PI);
}
/* Calulate orthogonal matrix. */
/*
Q = QzT * QyT * QxT
*/
cvGEMM( &Qz, &Qy, 1, 0, 0, &M, CV_GEMM_A_T + CV_GEMM_B_T );
cvGEMM( &M, &Qx, 1, 0, 0, &Q, CV_GEMM_B_T );
/* Save R and Q matrices. */
cvConvert( &R, matrixR );
cvConvert( &Q, matrixQ );
if( matrixQx )
cvConvert(&Qx, matrixQx);
if( matrixQy )
cvConvert(&Qy, matrixQy);
if( matrixQz )
cvConvert(&Qz, matrixQz);
}
__END__;
}
