/*
Draws a single Oxford-type feature
@param img image on which to draw
@param feat feature to be drawn
@param color color in which to draw
*/
void draw_oxfd_feature( IplImage* img, struct feature* feat, CvScalar color )
{
double m[4] = { feat->a, feat->b, feat->b, feat->c };
double v[4] = { 0 };
double e[2] = { 0 };
CvMat M, V, E;
double alpha, l1, l2;
/* compute axes and orientation of ellipse surrounding affine region */
cvInitMatHeader( &M, 2, 2, CV_64FC1, m, CV_AUTOSTEP );
cvInitMatHeader( &V, 2, 2, CV_64FC1, v, CV_AUTOSTEP );
cvInitMatHeader( &E, 2, 1, CV_64FC1, e, CV_AUTOSTEP );
#if CV_MAJOR_VERSION==1
cvEigenVV( &M, &V, &E, DBL_EPSILON );
#else
cvEigenVV( &M, &V, &E, DBL_EPSILON, -1,-1 );
#endif
l1 = 1 / sqrt( e[1] );
l2 = 1 / sqrt( e[0] );
alpha = -atan2( v[1], v[0] );
alpha *= 180 / CV_PI;
cvEllipse( img, cvPoint( feat->x, feat->y ), cvSize( l2, l1 ), alpha,
0, 360, CV_RGB(0,0,0), 3, 8, 0 );
cvEllipse( img, cvPoint( feat->x, feat->y ), cvSize( l2, l1 ), alpha,
0, 360, color, 1, 8, 0 );
cvLine( img, cvPoint( feat->x+2, feat->y ), cvPoint( feat->x-2, feat->y ),
color, 1, 8, 0 );
cvLine( img, cvPoint( feat->x, feat->y+2 ), cvPoint( feat->x, feat->y-2 ),
color, 1, 8, 0 );
}
/*在图像上画单个OXFD特征点
参数：
img：图像指针
feat：要画的特征点
color：颜色
*/
static void draw_oxfd_feature(IplImage* img, struct feature* feat,
CvScalar color)
{
double m[4] = { feat->a, feat->b, feat->b, feat->c };
double v[4] = { 0 }; //特征向量的数据
double e[2] = { 0 }; //特征值的数据
CvMat M, V, E;
double alpha, l1, l2;
//计算椭圆的轴线和方向
cvInitMatHeader(&M, 2, 2, CV_64FC1, m, CV_AUTOSTEP); //矩阵
cvInitMatHeader(&V, 2, 2, CV_64FC1, v, CV_AUTOSTEP); //2个2*1的特征向量组成的矩阵
cvInitMatHeader(&E, 2, 1, CV_64FC1, e, CV_AUTOSTEP); //特征值
cvEigenVV(&M, &V, &E, DBL_EPSILON, 0, 0); //计算特征值和特征向量
l1 = 1 / sqrt(e[1]);
l2 = 1 / sqrt(e[0]);
alpha = -atan2(v[1], v[0]);
alpha *= 180 / CV_PI;
//画椭圆和十字星
cvEllipse(img, cvPoint(feat->x, feat->y), cvSize(l2, l1), alpha,
0, 360, CV_RGB(0, 0, 0), 3, 8, 0);
cvEllipse(img, cvPoint(feat->x, feat->y), cvSize(l2, l1), alpha,
0, 360, color, 1, 8, 0);
cvLine(img, cvPoint(feat->x + 2, feat->y), cvPoint(feat->x - 2, feat->y),
color, 1, 8, 0);
cvLine(img, cvPoint(feat->x, feat->y + 2), cvPoint(feat->x, feat->y - 2),
color, 1, 8, 0);
}
// 3次方程式を解いて、3x3の実対称行列の固有値と固有ベクトルを求める
static int
eigen33(double e[3], double ev[3][3], double m[3][3], const double rankDiag)
{
double coef[4];
coef[3] = -1.0;
coef[2] = m[0][0] + m[1][1] + m[2][2];
coef[1] = -m[0][0] * m[1][1] - m[1][1] * m[2][2] - m[2][2] * m[0][0]
+ m[1][2] * m[2][1] + m[0][2] * m[2][0] + m[0][1] * m[1][0];
coef[0] = m[0][0] * m[1][1] * m[2][2] + m[0][1] * m[1][2] * m[2][0]
+ m[0][2] * m[1][0] * m[2][1] - m[0][0] * m[1][2] * m[2][1]
- m[0][1] * m[1][0] * m[2][2] - m[0][2] * m[1][1] * m[2][0];
CvMat vCoef, vE, vEv, vM;
vCoef = cvMat(4, 1, CV_64FC1, coef);
vE = cvMat(3, 1, CV_64FC1, e);
vEv = cvMat(3, 3, CV_64FC1, ev);
vM = cvMat(3, 3, CV_64FC1, m);
// ３次方程式を解く
const int nRoot = cvSolveCubic(&vCoef, &vE);
// 出てきた解が実数解
sortRoot(e, nRoot);
cvEigenVV(&vM, &vEv, &vE, rankDiag);
return nRoot;
}
static CvStatus
icvFitLine3D_wods( CvPoint3D32f * points, int count, float *weights, float *line )
{
int i;
float w0 = 0;
float x0 = 0, y0 = 0, z0 = 0;
float x2 = 0, y2 = 0, z2 = 0, xy = 0, yz = 0, xz = 0;
float dx2, dy2, dz2, dxy, dxz, dyz;
float *v;
float n;
float det[9], evc[9], evl[3];
memset( evl, 0, 3*sizeof(evl[0]));
memset( evc, 0, 9*sizeof(evl[0]));
if( weights )
{
for( i = 0; i < count; i++ )
{
float x = points[i].x;
float y = points[i].y;
float z = points[i].z;
float w = weights[i];
x2 += x * x * w;
xy += x * y * w;
xz += x * z * w;
y2 += y * y * w;
yz += y * z * w;
z2 += z * z * w;
x0 += x * w;
y0 += y * w;
z0 += z * w;
w0 += w;
}
}
else
{
for( i = 0; i < count; i++ )
{
float x = points[i].x;
float y = points[i].y;
float z = points[i].z;
x2 += x * x;
xy += x * y;
xz += x * z;
y2 += y * y;
yz += y * z;
z2 += z * z;
x0 += x;
y0 += y;
z0 += z;
}
w0 = (float) count;
}
x2 /= w0;
xy /= w0;
xz /= w0;
y2 /= w0;
yz /= w0;
z2 /= w0;
x0 /= w0;
y0 /= w0;
z0 /= w0;
dx2 = x2 - x0 * x0;
dxy = xy - x0 * y0;
dxz = xz - x0 * z0;
dy2 = y2 - y0 * y0;
dyz = yz - y0 * z0;
dz2 = z2 - z0 * z0;
det[0] = dz2 + dy2;
det[1] = -dxy;
det[2] = -dxz;
det[3] = det[1];
det[4] = dx2 + dz2;
det[5] = -dyz;
det[6] = det[2];
det[7] = det[5];
det[8] = dy2 + dx2;
/* Searching for a eigenvector of det corresponding to the minimal eigenvalue */
#if 1
{
CvMat _det = cvMat( 3, 3, CV_32F, det );
CvMat _evc = cvMat( 3, 3, CV_32F, evc );
CvMat _evl = cvMat( 3, 1, CV_32F, evl );
cvEigenVV( &_det, &_evc, &_evl, 0 );
i = evl[0] < evl[1] ? (evl[0] < evl[2] ? 0 : 2) : (evl[1] < evl[2] ? 1 : 2);
}
#else
{
CvMat _det = cvMat( 3, 3, CV_32F, det );
CvMat _evc = cvMat( 3, 3, CV_32F, evc );
CvMat _evl = cvMat( 1, 3, CV_32F, evl );
cvSVD( &_det, &_evl, &_evc, 0, CV_SVD_MODIFY_A+CV_SVD_U_T );
}
i = 2;
#endif
v = &evc[i * 3];
n = (float) sqrt( (double)v[0] * v[0] + (double)v[1] * v[1] + (double)v[2] * v[2] );
n = (float)MAX(n, eps);
line[0] = v[0] / n;
line[1] = v[1] / n;
line[2] = v[2] / n;
line[3] = x0;
line[4] = y0;
line[5] = z0;
return CV_NO_ERR;
}
void train(CvArr **Means,CvArr **eigenVects,CvArr ** eigenVals,int numGestures,int numTraining,int numFeatures, int indx[]) {
int two=2;
char num[] ="01-01";
double *Features;
char *fo = ".png";
char filename[] = "../../../TrainingData/01-01.png";
int i,j,k;
double avg;
CvScalar t;
CvArr *tmpCov = cvCreateMat(numFeatures,numFeatures,CV_64FC1);
// CvArr *tmpEigVec = cvCreateMat(numFeatures,numFeatures,CV_64FC1);
//1CvArr **tmp= (CvArr **)malloc(sizeof(CvArr *)*numFeatures);
CvArr **tmp= (CvArr **)malloc(sizeof(CvArr *)*numTraining);
for(k=0; k<numTraining; k++) //1numFeatures
tmp[k] = cvCreateMat(1,numFeatures,CV_64FC1); //1tmp[k] = cvCreateMat(1,numTraining,CV_64FC1);
IplImage* src;
for(i=0; i< numGestures; i++) {
Means[i] = cvCreateMat(1,numFeatures,CV_64FC1);
for(j=0;j<numTraining; j++) {
filename[22] = '\0';
num[0] = '0'+indx[i]/10 ;
num[1] = '0'+indx[i]%10;
if((j+1)>9) {
num[3] = '0'+(j+1)/10;
num[4] = '0'+(j+1)%10;
num[5] = '\0';
}
else {
num[3] = '0'+j+1;
num[4] = '\0';
}
strcat(filename,num);
strcat(filename,fo);
//fprintf(stderr,"i=%d j=%d %s \n",i,j,filename);
src = cvLoadImage( filename,CV_LOAD_IMAGE_GRAYSCALE );
Features=computeFDFeatures(src,numFeatures);
//fprintf(stderr,"got contour\n");
for(k=0; k < numFeatures; k++)
*( (double*)CV_MAT_ELEM_PTR( *((CvMat *)tmp[j]),0,k ) ) = Features[k]; //1*((CvMat *)tmp[k]),0,j
//fprintf(stderr,"copied values\n");
free(Features);
//cvReleaseImage( &src );
}
/*for(k=0;k<numFeatures;k++) {
avg=0;
for(j=0;j<numGestures;j++)
avg = avg+CV_MAT_ELEM( *((CvMat*)tmp[k]), double, 0, j );
avg=avg/(double)numGestures;
//fprintf(stderr,"%lf\n",t.val[0]);
*( (double*)CV_MAT_ELEM_PTR( *((CvMat *)Means[i]),0,k ) ) = avg;
}*/
// print the feature vectors
/*for(k=0;k<numTraining;k++) {
for(j=0;j<numFeatures;j++)
fprintf(stderr," %lf ",*( (double*)CV_MAT_ELEM_PTR( *((CvMat *)tmp[k]),0,j ) ));
fprintf(stderr,";\n",i);
}
fprintf(stderr,"covs now\n");*/
//for(i=0;i<numGestures;i++) {
cvCalcCovarMatrix( tmp, numTraining, tmpCov, Means[i],CV_COVAR_SCALE|CV_COVAR_NORMAL); //Means[i] , |2
fprintf(stderr,"%d\n",i);
for(k=0;k<numFeatures;k++) {
for(j=0;j<numFeatures;j++)
fprintf(stderr," %lf ",*( (double*)CV_MAT_ELEM_PTR( *((CvMat *)tmpCov),k,j ) ));
fprintf(stderr,";\n",i);
}
eigenVects[i]=cvCreateMat(numFeatures,numFeatures,CV_64FC1);
eigenVals[i]=cvCreateMat(1,numFeatures,CV_64FC1);
cvEigenVV(tmpCov,eigenVects[i],eigenVals[i],0);
fprintf(stderr,"Eigenvalues:\n");
for(k=0;k<numFeatures;k++)
{
fprintf(stderr," %lf ",*( (double*)CV_MAT_ELEM_PTR( *((CvMat *)eigenVals[i]),0,k ) ));
}
fprintf(stderr,";\n",i);
for(k=0;k<numFeatures;k++) {
for(j=0;j<numFeatures;j++)
fprintf(stderr," %lf ",*( (double*)CV_MAT_ELEM_PTR( *((CvMat *)eigenVects[i]),k,j ) ));
fprintf(stderr,";\n",i);
}
//invCovMat[i] = cvCreateMat(numFeatures,numFeatures,CV_64FC1);
//cvInvert(tmpCov,invCovMat[i],CV_SVD);
//}
}
//fprintf(stderr,"found averages\n");
/*for(i=0;i<numGestures;i++) {
fprintf(stderr,"i=%d ",i);
for(j=0;j<numFeatures;j++)
fprintf(stderr," %lf ",*( (double*)CV_MAT_ELEM_PTR( *((CvMat *)Means[i]),0,j ) ));
fprintf(stderr,"\n",i);
}*/
}
/* for now this function works bad with singular cases
You can see in the code, that when some troubles with
matrices or some variables occur -
box filled with zero values is returned.
However in general function works fine.
*/
static void
icvFitEllipse_F( CvSeq* points, CvBox2D* box )
{
CvMat* D = 0;
CV_FUNCNAME( "icvFitEllipse_F" );
__BEGIN__;
double S[36], C[36], T[36];
int i, j;
double eigenvalues[6], eigenvectors[36];
double a, b, c, d, e, f;
double x0, y0, idet, scale, offx = 0, offy = 0;
int n = points->total;
CvSeqReader reader;
int is_float = CV_SEQ_ELTYPE(points) == CV_32FC2;
CvMat _S = cvMat(6,6,CV_64F,S), _C = cvMat(6,6,CV_64F,C), _T = cvMat(6,6,CV_64F,T);
CvMat _EIGVECS = cvMat(6,6,CV_64F,eigenvectors), _EIGVALS = cvMat(6,1,CV_64F,eigenvalues);
/* create matrix D of input points */
CV_CALL( D = cvCreateMat( n, 6, CV_64F ));
cvStartReadSeq( points, &reader );
/* shift all points to zero */
for( i = 0; i < n; i++ )
{
if( !is_float )
{
offx += ((CvPoint*)reader.ptr)->x;
offy += ((CvPoint*)reader.ptr)->y;
}
else
{
offx += ((CvPoint2D32f*)reader.ptr)->x;
offy += ((CvPoint2D32f*)reader.ptr)->y;
}
CV_NEXT_SEQ_ELEM( points->elem_size, reader );
}
offx /= n;
offy /= n;
// fill matrix rows as (x*x, x*y, y*y, x, y, 1 )
for( i = 0; i < n; i++ )
{
double x, y;
double* Dptr = D->data.db + i*6;
if( !is_float )
{
x = ((CvPoint*)reader.ptr)->x - offx;
y = ((CvPoint*)reader.ptr)->y - offy;
}
else
{
x = ((CvPoint2D32f*)reader.ptr)->x - offx;
y = ((CvPoint2D32f*)reader.ptr)->y - offy;
}
CV_NEXT_SEQ_ELEM( points->elem_size, reader );
Dptr[0] = x * x;
Dptr[1] = x * y;
Dptr[2] = y * y;
Dptr[3] = x;
Dptr[4] = y;
Dptr[5] = 1.;
}
// S = D^t*D
cvMulTransposed( D, &_S, 1 );
cvSVD( &_S, &_EIGVALS, &_EIGVECS, 0, CV_SVD_MODIFY_A + CV_SVD_U_T );
for( i = 0; i < 6; i++ )
{
double a = eigenvalues[i];
a = a < DBL_EPSILON ? 0 : 1./sqrt(sqrt(a));
for( j = 0; j < 6; j++ )
eigenvectors[i*6 + j] *= a;
}
// C = Q^-1 = transp(INVEIGV) * INVEIGV
cvMulTransposed( &_EIGVECS, &_C, 1 );
cvZero( &_S );
S[2] = 2.;
S[7] = -1.;
S[12] = 2.;
// S = Q^-1*S*Q^-1
cvMatMul( &_C, &_S, &_T );
cvMatMul( &_T, &_C, &_S );
// and find its eigenvalues and vectors too
//cvSVD( &_S, &_EIGVALS, &_EIGVECS, 0, CV_SVD_MODIFY_A + CV_SVD_U_T );
cvEigenVV( &_S, &_EIGVECS, &_EIGVALS, 0 );
for( i = 0; i < 3; i++ )
if( eigenvalues[i] > 0 )
break;
if( i >= 3 /*eigenvalues[0] < DBL_EPSILON*/ )
{
box->center.x = box->center.y =
box->size.width = box->size.height =
box->angle = 0.f;
EXIT;
}
// now find truthful eigenvector
_EIGVECS = cvMat( 6, 1, CV_64F, eigenvectors + 6*i );
_T = cvMat( 6, 1, CV_64F, T );
// Q^-1*eigenvecs[0]
cvMatMul( &_C, &_EIGVECS, &_T );
// extract vector components
a = T[0]; b = T[1]; c = T[2]; d = T[3]; e = T[4]; f = T[5];
///////////////// extract ellipse axes from above values ////////////////
/*
1) find center of ellipse
it satisfy equation
| a b/2 | * | x0 | + | d/2 | = |0 |
| b/2 c | | y0 | | e/2 | |0 |
*/
idet = a * c - b * b * 0.25;
idet = idet > DBL_EPSILON ? 1./idet : 0;
// we must normalize (a b c d e f ) to fit (4ac-b^2=1)
scale = sqrt( 0.25 * idet );
if( scale < DBL_EPSILON )
{
box->center.x = (float)offx;
box->center.y = (float)offy;
box->size.width = box->size.height = box->angle = 0.f;
EXIT;
}
a *= scale;
b *= scale;
c *= scale;
d *= scale;
e *= scale;
f *= scale;
x0 = (-d * c + e * b * 0.5) * 2.;
y0 = (-a * e + d * b * 0.5) * 2.;
// recover center
box->center.x = (float)(x0 + offx);
box->center.y = (float)(y0 + offy);
// offset ellipse to (x0,y0)
// new f == F(x0,y0)
f += a * x0 * x0 + b * x0 * y0 + c * y0 * y0 + d * x0 + e * y0;
if( fabs(f) < DBL_EPSILON )
{
box->size.width = box->size.height = box->angle = 0.f;
EXIT;
}
scale = -1. / f;
// normalize to f = 1
a *= scale;
b *= scale;
c *= scale;
// extract axis of ellipse
// one more eigenvalue operation
S[0] = a;
S[1] = S[2] = b * 0.5;
S[3] = c;
_S = cvMat( 2, 2, CV_64F, S );
_EIGVECS = cvMat( 2, 2, CV_64F, eigenvectors );
_EIGVALS = cvMat( 1, 2, CV_64F, eigenvalues );
cvSVD( &_S, &_EIGVALS, &_EIGVECS, 0, CV_SVD_MODIFY_A + CV_SVD_U_T );
// exteract axis length from eigenvectors
box->size.width = (float)(2./sqrt(eigenvalues[0]));
box->size.height = (float)(2./sqrt(eigenvalues[1]));
// calc angle
box->angle = (float)(180 - atan2(eigenvectors[2], eigenvectors[3])*180/CV_PI);
__END__;
cvReleaseMat( &D );
}
void Kalman_Filter::track_object(IplImage *img, vector< vector<float> > despModel
)
{
printf("begin track \n");
//associate
//find feature within the ellipse centered at x_predict with shape sigma_predict
CvMat* shape = cvCreateMat(2,2,CV_32FC1);
CvMat* eigenVec = cvCreateMat(2,2,CV_32FC1);
CvMat* eigenVal = cvCreateMat(2,1,CV_32FC1);
cvmSet(shape, 0, 0, sigma_predict.at<float>(0, 0));
cvmSet(shape, 0, 1, sigma_predict.at<float>(0, 1));
cvmSet(shape, 1, 0, sigma_predict.at<float>(1, 0));
cvmSet(shape, 1, 1, sigma_predict.at<float>(1, 1));
cvEigenVV(shape, eigenVec, eigenVal, 1e-15);
//cvEigenVV(&A, &E, &l); // l = eigenvalues of A (descending order)
// E = corresponding eigenvectors (rows)
float radius = ceil(3*sqrt(cvmGet(eigenVal,0,0)));
radius += patch_size;
CvRect roi;
float minx = max(x_predict.at<float>(0, 0) - radius, 0.0f);
float miny = max(x_predict.at<float>(1, 0) - radius, 0.0f);
float maxx = min(x_predict.at<float>(0, 0) + radius, (float)(img->width-1));
float maxy = min(x_predict.at<float>(1, 0) + radius, (float)img->height-1);
roi.x = minx;
roi.y = miny;
roi.width = maxx-minx;
roi.height = maxy-miny;
printf("roi: %d %d %d %d \n", roi.x, roi.y, roi.width, roi.height);
cvSetImageROI(img, roi);
/* create destination image
Note that cvGetSize will return the width and the height of ROI */
IplImage* search_region = cvCreateImage( cvGetSize(img), img->depth, img->nChannels );
cvZero(search_region);
/* copy subimage */
cvCopy(img, search_region, NULL);
/* always reset the Region of Interest */
cvResetImageROI(img);
printf("ellipse \n");
Object_Detection objectDetector;
std::vector<CvPoint> measurements = objectDetector.do_local_match(despModel, search_region, sigma, threshold);
float minDist = FLT_MAX;
int minIndex = INT_MAX;
CvMat* covMat = cvCreateMat(2,2,CV_32FC1);
CvMat* predictedPoint = cvCreateMat(1,2,CV_32FC1);
cvmSet(covMat, 0, 0, sigma_predict.at<float>(0, 0));
cvmSet(covMat, 0, 1, sigma_predict.at<float>(0, 1));
cvmSet(covMat, 1, 0, sigma_predict.at<float>(1, 0));
cvmSet(covMat, 1, 1, sigma_predict.at<float>(1, 1));
cvmSet(predictedPoint, 0, 0, x_predict.at<float>(0, 0));
cvmSet(predictedPoint, 0, 1, x_predict.at<float>(1, 0));
for(int i = 0; i < measurements.size(); i++)
{
CvMat* hypothesis = cvCreateMat(1,2,CV_32FC1);
cvmSet(hypothesis, 0, 0, measurements[i].x+minx);
cvmSet(hypothesis, 0, 1, measurements[i].y+miny);
CvMat* inverted = cvCreateMat(2, 2, CV_32FC1);
cvInvert( covMat, inverted, CV_LU);
float distance = cvMahalanobis( predictedPoint, hypothesis, inverted);
if(minDist > distance)
{
minDist = distance;
minIndex = i;
}
}
CvPoint measurement = {roi.width/2, roi.height/2};
if (minIndex != INT_MAX && measurements.size() > 0) measurement = measurements[minIndex];
measurement.x += minx;
measurement.y += miny;
//correction
Mat temp_invert = M * sigma_predict * M.t() + sigma_m;
Mat K = sigma_predict * M.t() * temp_invert.inv();
Mat y = Mat::zeros(2, 1, CV_32FC1);
y.at<float>(0, 0) = measurement.x;
y.at<float>(1, 0) = measurement.y;
Mat x_correct = x_predict + K * (y - M * x_predict);
Mat sigma_correct = (I - K * M) * sigma_predict;
//prediction
x_predict = D * x_correct;
sigma_predict = D * sigma_correct * D.t() + sigma_d;
std::cout << "x_correct: \n" << x_correct << std::endl;
m_trajectory.push_back(cvPoint(x_correct.at<float>(0, 0), x_correct.at<float>(1, 0)));
for (int i = 0; i < m_trajectory.size(); i++)
{
cvCircle(img, m_trajectory[i], 3, cvScalar(0,255,0), 2);
}
}
/* for now this function works bad with singular cases
You can see in the code, that when some troubles with
matrices or some variables occur -
box filled with zero values is returned.
However in general function works fine.
*/
static CvStatus icvFitEllipse_32f( CvSeq* points, CvBox2D* box )
{
CvStatus status = CV_OK;
float u[6];
CvMatr32f D = 0;
float S[36]; /* S = D' * D */
float C[36];
float INVQ[36];
/* transposed eigenvectors */
float INVEIGV[36];
/* auxulary matrices */
float TMP1[36];
float TMP2[36];
int i, index = -1;
float eigenvalues[6];
float a, b, c, d, e, f;
float offx, offy;
float *matr;
int n = points->total;
CvSeqReader reader;
int is_float = CV_SEQ_ELTYPE(points) == CV_32FC2;
CvMat _S, _EIGVECS, _EIGVALS;
/* create matrix D of input points */
D = icvCreateMatrix_32f( 6, n );
offx = offy = 0;
cvStartReadSeq( points, &reader );
/* shift all points to zero */
for( i = 0; i < n; i++ )
{
if( !is_float )
{
offx += (float)((CvPoint*)reader.ptr)->x;
offy += (float)((CvPoint*)reader.ptr)->y;
}
else
{
offx += ((CvPoint2D32f*)reader.ptr)->x;
offy += ((CvPoint2D32f*)reader.ptr)->y;
}
CV_NEXT_SEQ_ELEM( points->elem_size, reader );
}
c = 1.f / n;
offx *= c;
offy *= c;
/* fill matrix rows as (x*x, x*y, y*y, x, y, 1 ) */
matr = D;
for( i = 0; i < n; i++ )
{
float x, y;
if( !is_float )
{
x = (float)((CvPoint*)reader.ptr)->x - offx;
y = (float)((CvPoint*)reader.ptr)->y - offy;
}
else
{
x = ((CvPoint2D32f*)reader.ptr)->x - offx;
y = ((CvPoint2D32f*)reader.ptr)->y - offy;
}
CV_NEXT_SEQ_ELEM( points->elem_size, reader );
matr[0] = x * x;
matr[1] = x * y;
matr[2] = y * y;
matr[3] = x;
matr[4] = y;
matr[5] = 1.f;
matr += 6;
}
/* compute S */
icvMulTransMatrixR_32f( D, 6, n, S );
/* fill matrix C */
icvSetZero_32f( C, 6, 6 );
C[2] = 2.f; //icvSetElement_32f( C, 6, 6, 0, 2, 2.f );
C[7] = -1.f; //icvSetElement_32f( C, 6, 6, 1, 1, -1.f );
C[12] = 2.f; //icvSetElement_32f( C, 6, 6, 2, 0, 2.f );
/* find eigenvalues */
//status1 = icvJacobiEigens_32f( S, INVEIGV, eigenvalues, 6, 0.f );
//assert( status1 == CV_OK );
_S = cvMat( 6, 6, CV_32F, S );
_EIGVECS = cvMat( 6, 6, CV_32F, INVEIGV );
_EIGVALS = cvMat( 6, 1, CV_32F, eigenvalues );
cvEigenVV( &_S, &_EIGVECS, &_EIGVALS, 0 );
//avoid troubles with small negative values
for( i = 0; i < 6; i++ )
eigenvalues[i] = (float)fabs(eigenvalues[i]);
cvbSqrt( eigenvalues, eigenvalues, 6 );
cvbInvSqrt( eigenvalues, eigenvalues, 6 );
for( i = 0; i < 6; i++ )
icvScaleVector_32f( &INVEIGV[i * 6], &INVEIGV[i * 6], 6, eigenvalues[i] );
// INVQ = transp(INVEIGV) * INVEIGV
icvMulTransMatrixR_32f( INVEIGV, 6, 6, INVQ );
/* create matrix INVQ*C*INVQ */
icvMulMatrix_32f( INVQ, 6, 6, C, 6, 6, TMP1 );
icvMulMatrix_32f( TMP1, 6, 6, INVQ, 6, 6, TMP2 );
/* find its eigenvalues and vectors */
//status1 = icvJacobiEigens_32f( TMP2, INVEIGV, eigenvalues, 6, 0.f );
//assert( status1 == CV_OK );
_S = cvMat( 6, 6, CV_32F, TMP2 );
cvEigenVV( &_S, &_EIGVECS, &_EIGVALS, 0 );
/* search for positive eigenvalue */
for( i = 0; i < 3; i++ )
{
if( eigenvalues[i] > 0 )
{
index = i;
break;
}
}
/* only 3 eigenvalues must be not zero
and only one of them must be positive
if it is not true - return zero result
*/
if( index == -1 )
{
box->center.x = box->center.y =
box->size.width = box->size.height =
box->angle = 0.f;
goto error;
}
/* now find truthful eigenvector */
icvTransformVector_32f( INVQ, &INVEIGV[index * 6], u, 6, 6 );
/* extract vector components */
a = u[0];
b = u[1];
c = u[2];
d = u[3];
e = u[4];
f = u[5];
{
/* extract ellipse axes from above values */
/*
1) find center of ellipse
it satisfy equation
| a b/2 | * | x0 | + | d/2 | = |0 |
| b/2 c | | y0 | | e/2 | |0 |
*/
float x0, y0;
float idet = 1.f / (a * c - b * b * 0.25f);
/* we must normalize (a b c d e f ) to fit (4ac-b^2=1) */
float scale = cvSqrt( 0.25f * idet );
if (!scale)
{
box->center.x = box->center.y =
box->size.width = box->size.height =
box->angle = 0.f;
goto error;
}
a *= scale;
b *= scale;
c *= scale;
d *= scale;
e *= scale;
f *= scale;
//x0 = box->center.x = (-d * c * 0.5f + e * b * 0.25f) * 4.f;
//y0 = box->center.y = (-a * e * 0.5f + d * b * 0.25f) * 4.f;
x0 = box->center.x = (-d * c + e * b * 0.5f) * 2.f;
y0 = box->center.y = (-a * e + d * b * 0.5f) * 2.f;
/* offset ellipse to (x0,y0) */
/* new f == F(x0,y0) */
f += a * x0 * x0 + b * x0 * y0 + c * y0 * y0 + d * x0 + e * y0;
if (!f)
{
box->center.x = box->center.y =
box->size.width = box->size.height =
box->angle = 0.f;
goto error;
}
scale = -1.f / f;
/* normalize to f = 1 */
a *= scale;
b *= scale;
c *= scale;
}
/* recover center */
box->center.x += offx;
box->center.y += offy;
/* extract axis of ellipse */
/* one more eigenvalue operation */
TMP1[0] = a;
TMP1[1] = TMP1[2] = b * 0.5f;
TMP1[3] = c;
//status1 = icvJacobiEigens_32f( TMP1, INVEIGV, eigenvalues, 2, 0.f );
//assert( status1 == CV_OK );
_S = cvMat( 2, 2, CV_32F, TMP1 );
_EIGVECS = cvMat( 2, 2, CV_32F, INVEIGV );
_EIGVALS = cvMat( 2, 1, CV_32F, eigenvalues );
cvEigenVV( &_S, &_EIGVECS, &_EIGVALS, 0 );
/* exteract axis length from eigenvectors */
box->size.height = 2 * cvInvSqrt( eigenvalues[0] );
box->size.width = 2 * cvInvSqrt( eigenvalues[1] );
if ( !(box->size.height && box->size.width) )
{
assert(0);
}
/* calc angle */
box->angle = cvFastArctan( INVEIGV[3], INVEIGV[2] );
error:
if( D )
icvDeleteMatrix( D );
return status;
}
