static dcache_entry_t *
read_cylinder (uint32 fd, uint32 offset, read_callback_funcp_t read_callback)
{
uint32 nread;
uint8 buf[SECTORS_PER_FLOPPY_CYLINDER * 512], *bufp;
dcache_entry_t *retval;
uint32 new_offset;
new_offset = offset / sizeof buf * sizeof buf;
nread = read_callback (fd, buf, new_offset, sizeof buf);
if (nread != sizeof buf)
retval = NULL;
else
{
for (bufp = buf;
bufp + DCACHE_BLOCK_SIZE <= buf + sizeof buf;
bufp += DCACHE_BLOCK_SIZE, new_offset += DCACHE_BLOCK_SIZE)
{
if (!dcache_entry_lookup (fd, new_offset))
{
uint32 n_written;
n_written = dcache_write (fd, bufp, new_offset,
DCACHE_BLOCK_SIZE, NULL);
if (n_written != DCACHE_BLOCK_SIZE)
warning_unexpected (NULL_STRING);
}
}
retval = dcache_entry_lookup (fd, offset);
}
return retval;
}
void udp_port::on_read(int read_length)
{
read_callback(buffer,read_length);
memset(buffer,0x00,sizeof(buffer));
int b = ReadFile ((HANDLE)sock,&buffer,sizeof(buffer),NULL,&ev);
if (!b && GetLastError () != ERROR_IO_PENDING)
{
printf("ReadFile Failed\n");
throw std::exception("next read failed");
}
}
/* close a disk file. */
void
file_close(int fd)
{
fde *F = &fd_table[fd];
PF *read_callback;
assert(fd >= 0);
assert(F->flags.open);
if ((read_callback = F->read_handler))
{
F->read_handler = NULL;
read_callback(-1, F->read_data);
}
if (F->flags.write_daemon)
{
#if defined(_SQUID_WIN32_) || defined(_SQUID_OS2_)
/*
* on some operating systems, you can not delete or rename
* open files, so we won't allow delayed close.
*/
while (!diskWriteIsComplete(fd))
diskHandleWrite(fd, NULL);
#else
F->flags.close_request = 1;
debug(6, 2) ("file_close: FD %d, delaying close\n", fd);
return;
#endif
}
/*
* Assert there is no write callback. Otherwise we might be
* leaking write state data by closing the descriptor
*/
assert(F->write_handler == NULL);
F->flags.closing = 1;
#if CALL_FSYNC_BEFORE_CLOSE
fsync(fd);
#endif
debug(6, F->flags.close_request ? 2 : 5)
("file_close: FD %d, really closing\n", fd);
fd_close(fd);
close(fd);
statCounter.syscalls.disk.closes++;
}
/*F///////////////////////////////////////////////////////////////////////////////////////
// Names: icvEigenDecomposite_8u32fR
// Purpose: The function calculates all decomposition coefficients for input object
// using previously calculated eigen objects basis and the mean (averaged)
// object
// Context:
// Parameters: obj - input object
// objStep - its step (in bytes)
// nEigObjs - number of eigen objects
// eigInput - pointer either to array of pointers to eigen objects
// or to read callback function (depending on ioFlags)
// eigStep - eigen objects step (in bytes)
// ioFlags - input/output flags
// iserData - pointer to the structure which contains all necessary
// data for the callback function
// avg - pointer to averaged object
// avgStep - its step (in bytes)
// size - ROI size of each source object
// coeffs - calculated coefficients (output data)
//
// Returns: icv status
//
// Notes: see notes for icvCalcEigenObjects_8u32fR function
//F*/
static CvStatus CV_STDCALL
icvEigenDecomposite_8u32fR( uchar * obj, int objStep, int nEigObjs,
void *eigInput, int eigStep, int ioFlags,
void *userData, float *avg, int avgStep,
CvSize size, float *coeffs )
{
int i;
if( nEigObjs < 2 )
return CV_BADFACTOR_ERR;
if( ioFlags < 0 || ioFlags > 1 )
return CV_BADFACTOR_ERR;
if( size.width > objStep || 4 * size.width > eigStep ||
4 * size.width > avgStep || size.height < 1 )
return CV_BADSIZE_ERR;
if( obj == NULL || eigInput == NULL || coeffs == NULL || avg == NULL )
return CV_NULLPTR_ERR;
if( !ioFlags )
for( i = 0; i < nEigObjs; i++ )
if( ((uchar **) eigInput)[i] == NULL )
return CV_NULLPTR_ERR;
if( ioFlags ) /* callback */
{
float *buffer;
CvCallback read_callback = ((CvInput *) & eigInput)->callback;
eigStep = 4 * size.width;
/* memory allocation */
buffer = (float *) cvAlloc( sizeof( float ) * size.width * size.height );
if( buffer == NULL )
return CV_OUTOFMEM_ERR;
for( i = 0; i < nEigObjs; i++ )
{
float w;
CvStatus r = (CvStatus)read_callback( i, (void *) buffer, userData );
if( r )
{
cvFree( &buffer );
return r;
}
w = icvCalcDecompCoeff_8u32fR( obj, objStep, buffer,
eigStep, avg, avgStep, size );
if( w < -1.0e29f )
{
cvFree( &buffer );
return CV_NOTDEFINED_ERR;
}
coeffs[i] = w;
}
cvFree( &buffer );
}
else
/* no callback */
for( i = 0; i < nEigObjs; i++ )
{
float w = icvCalcDecompCoeff_8u32fR( obj, objStep, ((float **) eigInput)[i],
eigStep, avg, avgStep, size );
if( w < -1.0e29f )
return CV_NOTDEFINED_ERR;
coeffs[i] = w;
}
return CV_NO_ERR;
}
/*F///////////////////////////////////////////////////////////////////////////////////////
// Name: icvCalcEigenObjects_8u32fR
// Purpose: The function calculates an orthonormal eigen basis and a mean (averaged)
// object for a group of input objects (images, vectors, etc.). ROI supported.
// Context:
// Parameters: nObjects - number of source objects
// input - pointer either to array of pointers to input objects
// or to read callback function (depending on ioFlags)
// imgStep - full width of each source object row in bytes
// output - pointer either to array of pointers to output eigen objects
// or to write callback function (depending on ioFlags)
// eigStep - full width of each eigenobject row in bytes
// size - ROI size of each source object
// ioFlags - input/output flags (see Notes)
// ioBufSize - input/output buffer size
// userData - pointer to the structure which contains all necessary
// data for the callback functions
// calcLimit - determines the calculation finish conditions
// avg - pointer to averaged object (has the same size as ROI)
// avgStep - full width of averaged object row in bytes
// eigVals - pointer to corresponding eigenvalues (array of <nObjects>
// elements in descending order)
//
// Returns: CV_NO_ERR or error code
//
// Notes: 1. input/output data (that is, input objects and eigen ones) may either
// be allocated in the RAM or be read from/written to the HDD (or any
// other device) by read/write callback functions. It depends on the
// value of ioFlags paramater, which may be the following:
// CV_EIGOBJ_NO_CALLBACK, or 0;
// CV_EIGOBJ_INPUT_CALLBACK;
// CV_EIGOBJ_OUTPUT_CALLBACK;
// CV_EIGOBJ_BOTH_CALLBACK, or
// CV_EIGOBJ_INPUT_CALLBACK | CV_EIGOBJ_OUTPUT_CALLBACK.
// The callback functions as well as the user data structure must be
// developed by the user.
//
// 2. If ioBufSize = 0, or it's too large, the function dermines buffer size
// itself.
//
// 3. Depending on calcLimit parameter, calculations are finished either if
// eigenfaces number comes up to certain value or the relation of the
// current eigenvalue and the largest one comes down to certain value
// (or any of the above conditions takes place). The calcLimit->type value
// must be CV_TERMCRIT_NUMB, CV_TERMCRIT_EPS or
// CV_TERMCRIT_NUMB | CV_TERMCRIT_EPS. The function returns the real
// values calcLimit->max_iter and calcLimit->epsilon.
//
// 4. eigVals may be equal to NULL (if you don't need eigen values in further).
//
//F*/
static CvStatus CV_STDCALL
icvCalcEigenObjects_8u32fR( int nObjects, void* input, int objStep,
void* output, int eigStep, CvSize size,
int ioFlags, int ioBufSize, void* userData,
CvTermCriteria* calcLimit, float* avg,
int avgStep, float *eigVals )
{
int i, j, n, iev = 0, m1 = nObjects - 1, objStep1 = objStep, eigStep1 = eigStep / 4;
CvSize objSize, eigSize, avgSize;
float *c = 0;
float *ev = 0;
float *bf = 0;
uchar *buf = 0;
void *buffer = 0;
float m = 1.0f / (float) nObjects;
CvStatus r;
if( m1 > calcLimit->max_iter && calcLimit->type != CV_TERMCRIT_EPS )
m1 = calcLimit->max_iter;
/* ---- TEST OF PARAMETERS ---- */
if( nObjects < 2 )
return CV_BADFACTOR_ERR;
if( ioFlags < 0 || ioFlags > 3 )
return CV_BADFACTOR_ERR;
if( input == NULL || output == NULL || avg == NULL )
return CV_NULLPTR_ERR;
if( size.width > objStep || 4 * size.width > eigStep ||
4 * size.width > avgStep || size.height < 1 )
return CV_BADSIZE_ERR;
if( !(ioFlags & CV_EIGOBJ_INPUT_CALLBACK) )
for( i = 0; i < nObjects; i++ )
if( ((uchar **) input)[i] == NULL )
return CV_NULLPTR_ERR;
if( !(ioFlags & CV_EIGOBJ_OUTPUT_CALLBACK) )
for( i = 0; i < m1; i++ )
if( ((float **) output)[i] == NULL )
return CV_NULLPTR_ERR;
avgStep /= 4;
eigStep /= 4;
if( objStep == size.width && eigStep == size.width && avgStep == size.width )
{
size.width *= size.height;
size.height = 1;
objStep = objStep1 = eigStep = eigStep1 = avgStep = size.width;
}
objSize = eigSize = avgSize = size;
if( ioFlags & CV_EIGOBJ_INPUT_CALLBACK )
{
objSize.width *= objSize.height;
objSize.height = 1;
objStep = objSize.width;
objStep1 = size.width;
}
if( ioFlags & CV_EIGOBJ_OUTPUT_CALLBACK )
{
eigSize.width *= eigSize.height;
eigSize.height = 1;
eigStep = eigSize.width;
eigStep1 = size.width;
}
n = objSize.height * objSize.width * (ioFlags & CV_EIGOBJ_INPUT_CALLBACK) +
2 * eigSize.height * eigSize.width * (ioFlags & CV_EIGOBJ_OUTPUT_CALLBACK);
/* Buffer size determination */
if( ioFlags )
{
ioBufSize = MIN( icvDefaultBufferSize(), n );
}
/* memory allocation (if necesseay) */
if( ioFlags & CV_EIGOBJ_INPUT_CALLBACK )
{
buf = (uchar *) cvAlloc( sizeof( uchar ) * objSize.width );
if( buf == NULL )
return CV_OUTOFMEM_ERR;
}
if( ioFlags )
{
buffer = (void *) cvAlloc( ioBufSize );
if( buffer == NULL )
{
if( buf )
cvFree( &buf );
return CV_OUTOFMEM_ERR;
}
}
/* Calculation of averaged object */
bf = avg;
for( i = 0; i < avgSize.height; i++, bf += avgStep )
for( j = 0; j < avgSize.width; j++ )
bf[j] = 0.f;
for( i = 0; i < nObjects; i++ )
{
int k, l;
uchar *bu = (ioFlags & CV_EIGOBJ_INPUT_CALLBACK) ? buf : ((uchar **) input)[i];
if( ioFlags & CV_EIGOBJ_INPUT_CALLBACK )
{
CvCallback read_callback = ((CvInput *) & input)->callback;
r = (CvStatus)read_callback( i, (void *) buf, userData );
if( r )
{
if( buffer )
cvFree( &buffer );
if( buf )
cvFree( &buf );
return r;
}
}
bf = avg;
for( k = 0; k < avgSize.height; k++, bf += avgStep, bu += objStep1 )
for( l = 0; l < avgSize.width; l++ )
bf[l] += bu[l];
}
bf = avg;
for( i = 0; i < avgSize.height; i++, bf += avgStep )
for( j = 0; j < avgSize.width; j++ )
bf[j] *= m;
/* Calculation of covariance matrix */
c = (float *) cvAlloc( sizeof( float ) * nObjects * nObjects );
if( c == NULL )
{
if( buffer )
cvFree( &buffer );
if( buf )
cvFree( &buf );
return CV_OUTOFMEM_ERR;
}
r = icvCalcCovarMatrixEx_8u32fR( nObjects, input, objStep1, ioFlags, ioBufSize,
(uchar *) buffer, userData, avg, 4 * avgStep, size, c );
if( r )
{
cvFree( &c );
if( buffer )
cvFree( &buffer );
if( buf )
cvFree( &buf );
return r;
}
/* Calculation of eigenvalues & eigenvectors */
ev = (float *) cvAlloc( sizeof( float ) * nObjects * nObjects );
if( ev == NULL )
{
cvFree( &c );
if( buffer )
cvFree( &buffer );
if( buf )
cvFree( &buf );
return CV_OUTOFMEM_ERR;
}
if( eigVals == NULL )
{
eigVals = (float *) cvAlloc( sizeof( float ) * nObjects );
if( eigVals == NULL )
{
cvFree( &c );
cvFree( &ev );
if( buffer )
cvFree( &buffer );
if( buf )
cvFree( &buf );
return CV_OUTOFMEM_ERR;
}
iev = 1;
}
r = icvJacobiEigens_32f( c, ev, eigVals, nObjects, 0.0f );
cvFree( &c );
if( r )
{
cvFree( &ev );
if( buffer )
cvFree( &buffer );
if( buf )
cvFree( &buf );
if( iev )
cvFree( &eigVals );
return r;
}
/* Eigen objects number determination */
if( calcLimit->type != CV_TERMCRIT_NUMBER )
{
for( i = 0; i < m1; i++ )
if( fabs( eigVals[i] / eigVals[0] ) < calcLimit->epsilon )
break;
m1 = calcLimit->max_iter = i;
}
else
m1 = calcLimit->max_iter;
calcLimit->epsilon = (float) fabs( eigVals[m1 - 1] / eigVals[0] );
for( i = 0; i < m1; i++ )
eigVals[i] = (float) (1.0 / sqrt( (double)eigVals[i] ));
/* ----------------- Calculation of eigenobjects ----------------------- */
if( ioFlags & CV_EIGOBJ_OUTPUT_CALLBACK )
{
int nio, ngr, igr;
nio = ioBufSize / (4 * eigSize.width); /* number of eigen objects in buffer */
ngr = m1 / nio; /* number of io groups */
if( nObjects % nio )
ngr += 1;
for( igr = 0; igr < ngr; igr++ )
{
int io, ie, imin = igr * nio, imax = imin + nio;
if( imax > m1 )
imax = m1;
for(int k = 0; k < eigSize.width * (imax - imin); k++ )
((float *) buffer)[k] = 0.f;
for( io = 0; io < nObjects; io++ )
{
uchar *bu = ioFlags & CV_EIGOBJ_INPUT_CALLBACK ? buf : ((uchar **) input)[io];
if( ioFlags & CV_EIGOBJ_INPUT_CALLBACK )
{
CvCallback read_callback = ((CvInput *) & input)->callback;
r = (CvStatus)read_callback( io, (void *) buf, userData );
if( r )
{
cvFree( &ev );
if( iev )
cvFree( &eigVals );
if( buffer )
cvFree( &buffer );
if( buf )
cvFree( &buf );
return r;
}
}
for( ie = imin; ie < imax; ie++ )
{
int k, l;
uchar *bv = bu;
float e = ev[ie * nObjects + io] * eigVals[ie];
float *be = ((float *) buffer) + ((ie - imin) * eigStep);
bf = avg;
for( k = 0; k < size.height; k++, bv += objStep1,
bf += avgStep, be += eigStep1 )
{
for( l = 0; l < size.width - 3; l += 4 )
{
float f = bf[l];
uchar v = bv[l];
be[l] += e * (v - f);
f = bf[l + 1];
v = bv[l + 1];
be[l + 1] += e * (v - f);
f = bf[l + 2];
v = bv[l + 2];
be[l + 2] += e * (v - f);
f = bf[l + 3];
v = bv[l + 3];
be[l + 3] += e * (v - f);
}
for( ; l < size.width; l++ )
be[l] += e * (bv[l] - bf[l]);
}
}
} /* io */
for( ie = imin; ie < imax; ie++ ) /* calculated eigen objects writting */
{
CvCallback write_callback = ((CvInput *) & output)->callback;
float *be = ((float *) buffer) + ((ie - imin) * eigStep);
r = (CvStatus)write_callback( ie, (void *) be, userData );
if( r )
{
cvFree( &ev );
if( iev )
cvFree( &eigVals );
if( buffer )
cvFree( &buffer );
if( buf )
cvFree( &buf );
return r;
}
}
} /* igr */
}
else
{
int k, p, l;
for( i = 0; i < m1; i++ ) /* e.o. annulation */
{
float *be = ((float **) output)[i];
for( p = 0; p < eigSize.height; p++, be += eigStep )
for( l = 0; l < eigSize.width; l++ )
be[l] = 0.0f;
}
for( k = 0; k < nObjects; k++ )
{
uchar *bv = (ioFlags & CV_EIGOBJ_INPUT_CALLBACK) ? buf : ((uchar **) input)[k];
if( ioFlags & CV_EIGOBJ_INPUT_CALLBACK )
{
CvCallback read_callback = ((CvInput *) & input)->callback;
r = (CvStatus)read_callback( k, (void *) buf, userData );
if( r )
{
cvFree( &ev );
if( iev )
cvFree( &eigVals );
if( buffer )
cvFree( &buffer );
if( buf )
cvFree( &buf );
return r;
}
}
for( i = 0; i < m1; i++ )
{
float v = eigVals[i] * ev[i * nObjects + k];
float *be = ((float **) output)[i];
uchar *bu = bv;
bf = avg;
for( p = 0; p < size.height; p++, bu += objStep1,
bf += avgStep, be += eigStep1 )
{
for( l = 0; l < size.width - 3; l += 4 )
{
float f = bf[l];
uchar u = bu[l];
be[l] += v * (u - f);
f = bf[l + 1];
u = bu[l + 1];
be[l + 1] += v * (u - f);
f = bf[l + 2];
u = bu[l + 2];
be[l + 2] += v * (u - f);
f = bf[l + 3];
u = bu[l + 3];
be[l + 3] += v * (u - f);
}
for( ; l < size.width; l++ )
be[l] += v * (bu[l] - bf[l]);
}
} /* i */
} /* k */
} /* else */
cvFree( &ev );
if( iev )
cvFree( &eigVals );
else
for( i = 0; i < m1; i++ )
eigVals[i] = 1.f / (eigVals[i] * eigVals[i]);
if( buffer )
cvFree( &buffer );
if( buf )
cvFree( &buf );
return CV_NO_ERR;
}
/*F///////////////////////////////////////////////////////////////////////////////////////
// Name: icvCalcCovarMatrixEx_8u32fR
// Purpose: The function calculates a covariance matrix for a group of input objects
// (images, vectors, etc.). ROI supported.
// Context:
// Parameters: nObjects - number of source objects
// objects - array of pointers to ROIs of the source objects
// imgStep - full width of each source object row in bytes
// avg - pointer to averaged object
// avgStep - full width of averaged object row in bytes
// size - ROI size of each source and averaged objects
// covarMatrix - covariance matrix (output parameter; must be allocated
// before call)
//
// Returns: CV_NO_ERR or error code
//
// Notes:
//F*/
static CvStatus CV_STDCALL
icvCalcCovarMatrixEx_8u32fR( int nObjects, void *input, int objStep1,
int ioFlags, int ioBufSize, uchar* buffer,
void *userData, float *avg, int avgStep,
CvSize size, float *covarMatrix )
{
int objStep = objStep1;
/* ---- TEST OF PARAMETERS ---- */
if( nObjects < 2 )
return CV_BADFACTOR_ERR;
if( ioFlags < 0 || ioFlags > 3 )
return CV_BADFACTOR_ERR;
if( ioFlags && ioBufSize < 1024 )
return CV_BADFACTOR_ERR;
if( ioFlags && buffer == NULL )
return CV_NULLPTR_ERR;
if( input == NULL || avg == NULL || covarMatrix == NULL )
return CV_NULLPTR_ERR;
if( size.width > objStep || 4 * size.width > avgStep || size.height < 1 )
return CV_BADSIZE_ERR;
avgStep /= 4;
if( ioFlags & CV_EIGOBJ_INPUT_CALLBACK ) /* ==== USE INPUT CALLBACK ==== */
{
int nio, ngr, igr, n = size.width * size.height, mm = 0;
CvCallback read_callback = ((CvInput *) & input)->callback;
uchar *buffer2;
objStep = n;
nio = ioBufSize / n; /* number of objects in buffer */
ngr = nObjects / nio; /* number of io groups */
if( nObjects % nio )
mm = 1;
ngr += mm;
buffer2 = (uchar *)cvAlloc( sizeof( uchar ) * n );
if( buffer2 == NULL )
return CV_OUTOFMEM_ERR;
for( igr = 0; igr < ngr; igr++ )
{
int k, l;
int io, jo, imin = igr * nio, imax = imin + nio;
uchar *bu1 = buffer, *bu2;
if( imax > nObjects )
imax = nObjects;
/* read igr group */
for( io = imin; io < imax; io++, bu1 += n )
{
CvStatus r;
r = (CvStatus)read_callback( io, (void *) bu1, userData );
if( r )
return r;
}
/* diagonal square calc */
bu1 = buffer;
for( io = imin; io < imax; io++, bu1 += n )
{
bu2 = bu1;
for( jo = io; jo < imax; jo++, bu2 += n )
{
float w = 0.f;
float *fu = avg;
int ij = 0;
for( k = 0; k < size.height; k++, fu += avgStep )
for( l = 0; l < size.width; l++, ij++ )
{
float f = fu[l], u1 = bu1[ij], u2 = bu2[ij];
w += (u1 - f) * (u2 - f);
}
covarMatrix[io * nObjects + jo] = covarMatrix[jo * nObjects + io] = w;
}
}
/* non-diagonal elements calc */
for( jo = imax; jo < nObjects; jo++ )
{
CvStatus r;
bu1 = buffer;
bu2 = buffer2;
/* read jo object */
r = (CvStatus)read_callback( jo, (void *) bu2, userData );
if( r )
return r;
for( io = imin; io < imax; io++, bu1 += n )
{
float w = 0.f;
float *fu = avg;
int ij = 0;
for( k = 0; k < size.height; k++, fu += avgStep )
{
for( l = 0; l < size.width - 3; l += 4, ij += 4 )
{
float f = fu[l];
uchar u1 = bu1[ij];
uchar u2 = bu2[ij];
w += (u1 - f) * (u2 - f);
f = fu[l + 1];
u1 = bu1[ij + 1];
u2 = bu2[ij + 1];
w += (u1 - f) * (u2 - f);
f = fu[l + 2];
u1 = bu1[ij + 2];
u2 = bu2[ij + 2];
w += (u1 - f) * (u2 - f);
f = fu[l + 3];
u1 = bu1[ij + 3];
u2 = bu2[ij + 3];
w += (u1 - f) * (u2 - f);
}
for( ; l < size.width; l++, ij++ )
{
float f = fu[l], u1 = bu1[ij], u2 = bu2[ij];
w += (u1 - f) * (u2 - f);
}
}
covarMatrix[io * nObjects + jo] = covarMatrix[jo * nObjects + io] = w;
}
}
} /* igr */
cvFree( &buffer2 );
} /* if() */
else
/* ==== NOT USE INPUT CALLBACK ==== */
{
int i, j;
uchar **objects = (uchar **) (((CvInput *) & input)->data);
for( i = 0; i < nObjects; i++ )
{
uchar *bu = objects[i];
for( j = i; j < nObjects; j++ )
{
int k, l;
float w = 0.f;
float *a = avg;
uchar *bu1 = bu;
uchar *bu2 = objects[j];
for( k = 0; k < size.height;
k++, bu1 += objStep, bu2 += objStep, a += avgStep )
{
for( l = 0; l < size.width - 3; l += 4 )
{
float f = a[l];
uchar u1 = bu1[l];
uchar u2 = bu2[l];
w += (u1 - f) * (u2 - f);
f = a[l + 1];
u1 = bu1[l + 1];
u2 = bu2[l + 1];
w += (u1 - f) * (u2 - f);
f = a[l + 2];
u1 = bu1[l + 2];
u2 = bu2[l + 2];
w += (u1 - f) * (u2 - f);
f = a[l + 3];
u1 = bu1[l + 3];
u2 = bu2[l + 3];
w += (u1 - f) * (u2 - f);
}
for( ; l < size.width; l++ )
{
float f = a[l];
uchar u1 = bu1[l];
uchar u2 = bu2[l];
w += (u1 - f) * (u2 - f);
}
}
covarMatrix[i * nObjects + j] = covarMatrix[j * nObjects + i] = w;
}
}
} /* else */
return CV_NO_ERR;
}
/*F///////////////////////////////////////////////////////////////////////////////////////
// Names: icvEigenProjection_8u32fR
// Purpose: The function calculates object projection to the eigen sub-space (restores
// an object) using previously calculated eigen objects basis, mean (averaged)
// object and decomposition coefficients of the restored object
// Context:
// Parameters: nEigObjs - Number of eigen objects
// eigens - Array of pointers to eigen objects
// eigStep - Eigen objects step (in bytes)
// coeffs - Previously calculated decomposition coefficients
// avg - Pointer to averaged object
// avgStep - Its step (in bytes)
// rest - Pointer to restored object
// restStep - Its step (in bytes)
// size - ROI size of each object
//
// Returns: CV status
//
// Notes:
//F*/
static CvStatus CV_STDCALL
icvEigenProjection_8u32fR( int nEigObjs, void *eigInput, int eigStep,
int ioFlags, void *userData, float *coeffs,
float *avg, int avgStep, uchar * rest,
int restStep, CvSize size )
{
int i, j, k;
float *buf;
float *buffer = NULL;
float *b;
CvCallback read_callback = ((CvInput *) & eigInput)->callback;
if( size.width > avgStep || 4 * size.width > eigStep || size.height < 1 )
return CV_BADSIZE_ERR;
if( rest == NULL || eigInput == NULL || avg == NULL || coeffs == NULL )
return CV_NULLPTR_ERR;
if( ioFlags < 0 || ioFlags > 1 )
return CV_BADFACTOR_ERR;
if( !ioFlags )
for( i = 0; i < nEigObjs; i++ )
if( ((uchar **) eigInput)[i] == NULL )
return CV_NULLPTR_ERR;
eigStep /= 4;
avgStep /= 4;
if( size.width == restStep && size.width == eigStep && size.width == avgStep )
{
size.width *= size.height;
size.height = 1;
restStep = eigStep = avgStep = size.width;
}
buf = (float *) cvAlloc( sizeof( float ) * size.width * size.height );
if( buf == NULL )
return CV_OUTOFMEM_ERR;
b = buf;
for( i = 0; i < size.height; i++, avg += avgStep, b += size.width )
for( j = 0; j < size.width; j++ )
b[j] = avg[j];
if( ioFlags )
{
buffer = (float *) cvAlloc( sizeof( float ) * size.width * size.height );
if( buffer == NULL )
{
cvFree( &buf );
return CV_OUTOFMEM_ERR;
}
eigStep = size.width;
}
for( k = 0; k < nEigObjs; k++ )
{
float *e = ioFlags ? buffer : ((float **) eigInput)[k];
float c = coeffs[k];
if( ioFlags ) /* read eigen object */
{
CvStatus r = (CvStatus)read_callback( k, (void *) buffer, userData );
if( r )
{
cvFree( &buf );
cvFree( &buffer );
return r;
}
}
b = buf;
for( i = 0; i < size.height; i++, e += eigStep, b += size.width )
{
for( j = 0; j < size.width - 3; j += 4 )
{
float b0 = c * e[j];
float b1 = c * e[j + 1];
float b2 = c * e[j + 2];
float b3 = c * e[j + 3];
b[j] += b0;
b[j + 1] += b1;
b[j + 2] += b2;
b[j + 3] += b3;
}
for( ; j < size.width; j++ )
b[j] += c * e[j];
}
}
b = buf;
for( i = 0; i < size.height; i++, avg += avgStep, b += size.width, rest += restStep )
for( j = 0; j < size.width; j++ )
{
int w = cvRound( b[j] );
w = !(w & ~255) ? w : w < 0 ? 0 : 255;
rest[j] = (uchar) w;
}
cvFree( &buf );
if( ioFlags )
cvFree( &buffer );
return CV_NO_ERR;
}
static THD_FUNCTION(mpu_thread, arg) {
(void)arg;
chRegSetThreadName("MPU Sampling");
static int16_t raw_accel_gyro_mag_tmp[9];
#if USE_MAGNETOMETER
static int mag_cnt = MAG_DIV;
#endif
static systime_t iteration_timer = 0;
static int identical_reads = 0;
iteration_timer = chVTGetSystemTime();
for(;;) {
if (get_raw_accel_gyro(raw_accel_gyro_mag_tmp)) {
int is_identical = 1;
for (int i = 0;i < 6;i++) {
if (raw_accel_gyro_mag_tmp[i] != raw_accel_gyro_mag_no_offset[i]) {
is_identical = 0;
break;
}
}
if (is_identical) {
identical_reads++;
} else {
identical_reads = 0;
}
if (identical_reads >= MAX_IDENTICAL_READS) {
failed_reads++;
chThdSleepMicroseconds(FAIL_DELAY_US);
reset_init_mpu();
iteration_timer = chVTGetSystemTime();
} else {
memcpy((uint16_t*)raw_accel_gyro_mag_no_offset, raw_accel_gyro_mag_tmp, sizeof(raw_accel_gyro_mag));
raw_accel_gyro_mag_tmp[3] -= mpu9150_gyro_offsets[0];
raw_accel_gyro_mag_tmp[4] -= mpu9150_gyro_offsets[1];
raw_accel_gyro_mag_tmp[5] -= mpu9150_gyro_offsets[2];
memcpy((uint16_t*)raw_accel_gyro_mag, raw_accel_gyro_mag_tmp, sizeof(raw_accel_gyro_mag));
update_time_diff = chVTGetSystemTime() - last_update_time;
last_update_time = chVTGetSystemTime();
if (read_callback) {
read_callback();
}
#if USE_MAGNETOMETER
mag_cnt++;
if (mag_cnt >= MAG_DIV) {
mag_cnt = 0;
mag_updated = 1;
int16_t raw_mag_tmp[3];
if (get_raw_mag(raw_mag_tmp)) {
memcpy((uint16_t*)raw_accel_gyro_mag_tmp + 6, raw_mag_tmp, sizeof(raw_mag_tmp));
} else {
failed_mag_reads++;
chThdSleepMicroseconds(FAIL_DELAY_US);
reset_init_mpu();
iteration_timer = chVTGetSystemTime();
}
} else {
mag_updated = 0;
}
#endif
}
} else {
failed_reads++;
chThdSleepMicroseconds(FAIL_DELAY_US);
reset_init_mpu();
iteration_timer = chVTGetSystemTime();
}
iteration_timer += US2ST(ITERATION_TIME_US);
systime_t time_start = chVTGetSystemTime();
if (iteration_timer > time_start) {
chThdSleep(iteration_timer - time_start);
} else {
chThdSleepMicroseconds(MIN_ITERATION_DELAY_US);
iteration_timer = chVTGetSystemTime();
}
}
}
void serial_port::on_read()
{
read_callback();
start_read();
}
