OpenCv矩阵操作函数源代码

/* ////////////////////////////////////////////////////////////////////

//

// CvMat, CvMatND, CvSparceMat and IplImage support functions

// (creation, deletion, copying, retrieving and setting elements etc.)

//

// */

#include "_cxcore.h"

static struct

{

Cv_iplCreateImageHeader createHeader;

Cv_iplAllocateImageData allocateData;

Cv_iplDeallocate deallocate;

Cv_iplCreateROI createROI;

Cv_iplCloneImage cloneImage;

}

CvIPL;

// Makes the library use native IPL image allocators

CV_IMPL void

cvSetIPLAllocators( Cv_iplCreateImageHeader createHeader,

Cv_iplAllocateImageData allocateData,

Cv_iplDeallocate deallocate,

Cv_iplCreateROI createROI,

Cv_iplCloneImage cloneImage )

{

CV_FUNCNAME( "cvSetIPLAllocators" );

__BEGIN__;

if( !createHeader || !allocateData || !deallocate || !createROI || !cloneImage )

{

if( createHeader || allocateData || deallocate || createROI || cloneImage ) CV_ERROR( CV_StsBadArg, "Either all the pointers should be null or "

"they all should be non-null" );

}

CvIPL.createHeader = createHeader;

CvIPL.allocateData = allocateData;

CvIPL.deallocate = deallocate;

CvIPL.createROI = createROI;

CvIPL.cloneImage = cloneImage;

__END__;

}

/****************************************************************************** **********\

* CvMat creation and basic operations *

\****************************************************************************** **********/

// Creates CvMat and underlying data

CV_IMPL CvMat*

cvCreateMat( int height, int width, int type )

{

CvMat* arr = 0;

CV_FUNCNAME( "cvCreateMat" );

__BEGIN__;

CV_CALL( arr = cvCreateMatHeader( height, width, type ));

CV_CALL( cvCreateData( arr ));

__END__;

if( cvGetErrStatus() < 0 )

cvReleaseMat( &arr );

return arr;

}

static void icvCheckHuge( CvMat* arr )

{

if( (int64)arr->step*arr->rows > INT_MAX )

arr->type &= ~CV_MA T_CONT_FLAG;

}

// Creates CvMat header only

CV_IMPL CvMat*

cvCreateMatHeader( int rows, int cols, int type )

{

CvMat* arr = 0;

CV_FUNCNAME( "cvCreateMatHeader" );

__BEGIN__;

int min_step;

type = CV_MA T_TYPE(type);

if( rows <= 0 || cols <= 0 )

CV_ERROR( CV_StsBadSize, "Non-positive width or height" );

min_step = CV_ELEM_SIZE(type)*cols;

if( min_step <= 0 )

CV_ERROR( CV_StsUnsupportedFormat, "Invalid matrix type" );

CV_CALL( arr = (CvMat*)cvAlloc( sizeof(*arr)));

arr->step = rows == 1 ? 0 : cvAlign(min_step, CV_DEFAULT_MA T_ROW_ALIGN);

arr->type = CV_MA T_MAGIC_V AL | type |

(arr->step == 0 || arr->step == min_step ? CV_MA T_CONT_FLAG : 0);

arr->rows = rows;

arr->cols = cols;

arr->data.ptr = 0;

arr->refcount = 0;

arr->hdr_refcount = 1;

icvCheckHuge( arr );

__END__;

if( cvGetErrStatus() < 0 )

cvReleaseMat( &arr );

return arr;

}

// Initializes CvMat header, allocated by the user

CV_IMPL CvMat*

cvInitMatHeader( CvMat* arr, int rows, int cols,

int type, void* data, int step )

{

CV_FUNCNAME( "cvInitMatHeader" );

__BEGIN__;

int mask, pix_size, min_step;

if( !arr )

CV_ERROR_FROM_CODE( CV_StsNullPtr );

if( (unsigned)CV_MA T_DEPTH(type) > CV_DEPTH_MAX ) CV_ERROR_FROM_CODE( CV_BadNumChannels );

if( rows <= 0 || cols <= 0 )

CV_ERROR( CV_StsBadSize, "Non-positive cols or rows" );

type = CV_MA T_TYPE( type );

arr->type = type | CV_MA T_MAGIC_V AL;

arr->rows = rows;

arr->cols = cols;

arr->data.ptr = (uchar*)data;

arr->refcount = 0;

arr->hdr_refcount = 0;

mask = (arr->rows <= 1) - 1;

pix_size = CV_ELEM_SIZE(type);

min_step = arr->cols*pix_size & mask;

if( step != CV_AUTOSTEP && step != 0 )

{

if( step < min_step )

CV_ERROR_FROM_CODE( CV_BadStep );

arr->step = step & mask;

}

else

{

arr->step = min_step;

}

arr->type = CV_MA T_MAGIC_V AL | type |

(arr->step == min_step ? CV_MA T_CONT_FLAG : 0);

icvCheckHuge( arr );

__END__;

return arr;

// Deallocates the CvMat structure and underlying data

CV_IMPL void

cvReleaseMat( CvMat** array )

{

CV_FUNCNAME( "cvReleaseMat" );

__BEGIN__;

if( !array )

CV_ERROR_FROM_CODE( CV_HeaderIsNull );

if( *array )

{

CvMat* arr = *array;

if( !CV_IS_MA T_HDR(arr) && !CV_IS_MA TND_HDR(arr) )

CV_ERROR_FROM_CODE( CV_StsBadFlag );

*array = 0;

cvDecRefData( arr );

cvFree( &arr );

}

__END__;

}

// Creates a copy of matrix

CV_IMPL CvMat*

cvCloneMat( const CvMat* src )

{

CvMat* dst = 0;

CV_FUNCNAME( "cvCloneMat" );

__BEGIN__;

if( !CV_IS_MA T_HDR( src ))

CV_ERROR( CV_StsBadArg, "Bad CvMat header" );

CV_CALL( dst = cvCreateMatHeader( src->rows, src->cols, src->type ));

if( src->data.ptr )

{

CV_CALL( cvCreateData( dst ));

CV_CALL( cvCopy( src, dst ));

}

__END__;

return dst;

}

/****************************************************************************** **********\

* CvMatND creation and basic operations *

\****************************************************************************** **********/

CV_IMPL CvMatND*

cvInitMatNDHeader( CvMatND* mat, int dims, const int* sizes,

int type, void* data )

{

CvMatND* result = 0;

CV_FUNCNAME( "cvInitMatNDHeader" );

__BEGIN__;

type = CV_MA T_TYPE(type);

int i;

int64 step = CV_ELEM_SIZE(type);

if( !mat )

CV_ERROR( CV_StsNullPtr, "NULL matrix header pointer" );

if( step == 0 )

CV_ERROR( CV_StsUnsupportedFormat, "invalid array data type" );

if( !sizes )

CV_ERROR( CV_StsNullPtr, "NULL pointer" );

if( dims <= 0 || dims > CV_MAX_DIM )

CV_ERROR( CV_StsOutOfRange,

"non-positive or too large number of dimensions" );

for( i = dims - 1; i >= 0; i-- )

{

if( sizes[i] <= 0 )

CV_ERROR( CV_StsBadSize, "one of dimesion sizes is non-positive" );

mat->dim[i].size = sizes[i];

if( step > INT_MAX )

CV_ERROR( CV_StsOutOfRange, "The array is too big" );

mat->dim[i].step = (int)step;

step *= sizes[i];

}

mat->type = CV_MA TND_MAGIC_V AL | (step <= INT_MAX ? CV_MA T_CONT_FLAG : 0) | type;

mat->dims = dims;

mat->data.ptr = (uchar*)data;

mat->refcount = 0;

mat->hdr_refcount = 0;

result = mat;

__END__;

if( cvGetErrStatus() < 0 && mat )

{

mat->type = 0;

mat->data.ptr = 0;

}

return result;

}

// Creates CvMatND and underlying data

CV_IMPL CvMatND*

cvCreateMatND( int dims, const int* sizes, int type )

{

CvMatND* arr = 0;

CV_FUNCNAME( "cvCreateMatND" );

__BEGIN__;

CV_CALL( arr = cvCreateMatNDHeader( dims, sizes, type ));

CV_CALL( cvCreateData( arr ));

__END__;

if( cvGetErrStatus() < 0 )

cvReleaseMatND( &arr );

return arr;

}

// Creates CvMatND header only

CV_IMPL CvMatND*

cvCreateMatNDHeader( int dims, const int* sizes, int type )

{

CvMatND* arr = 0;

CV_FUNCNAME( "cvCreateMatNDHeader" );

__BEGIN__;

if( dims <= 0 || dims > CV_MAX_DIM )

CV_ERROR( CV_StsOutOfRange,

"non-positive or too large number of dimensions" );

CV_CALL( arr = (CvMatND*)cvAlloc( sizeof(*arr) ));

CV_CALL( cvInitMatNDHeader( arr, dims, sizes, type, 0 ));

arr->hdr_refcount = 1;

__END__;

if( cvGetErrStatus() < 0 )

cvReleaseMatND( &arr );

return arr;

}

// Creates a copy of nD array

CV_IMPL CvMatND*

cvCloneMatND( const CvMatND* src )

{

CvMatND* dst = 0;

CV_FUNCNAME( "cvCloneMatND" );

__BEGIN__;

int i, *sizes;

if( !CV_IS_MA TND_HDR( src ))

CV_ERROR( CV_StsBadArg, "Bad CvMatND header" );

sizes = (int*)alloca( src->dims*sizeof(sizes[0]) );

for( i = 0; i < src->dims; i++ )

sizes[i] = src->dim[i].size;

CV_CALL( dst = cvCreateMatNDHeader( src->dims, sizes, src->type ));

if( src->data.ptr )

{

CV_CALL( cvCreateData( dst ));

CV_CALL( cvCopy( src, dst ));

}

__END__;

return dst;

}

static CvMatND*

cvGetMatND( const CvArr* arr, CvMatND* matnd, int* coi )

{

CvMatND* result = 0;

CV_FUNCNAME( "cvGetMatND" );

__BEGIN__;

if( coi )

*coi = 0;

if( !matnd || !arr )

CV_ERROR( CV_StsNullPtr, "NULL array pointer is passed" );

if( CV_IS_MA TND_HDR(arr))

{

if( !((CvMatND*)arr)->data.ptr )

CV_ERROR( CV_StsNullPtr, "The matrix has NULL data pointer" );

result = (CvMatND*)arr;

}

else

{

CvMat stub, *mat = (CvMat*)arr;

if( CV_IS_IMAGE_HDR( mat ))

CV_CALL( mat = cvGetMat( mat, &stub, coi ));

if( !CV_IS_MA T_HDR( mat ))

CV_ERROR( CV_StsBadArg, "Unrecognized or unsupported array type" );

if( !mat->data.ptr )

CV_ERROR( CV_StsNullPtr, "Input array has NULL data pointer" );

matnd->data.ptr = mat->data.ptr;

matnd->refcount = 0;

matnd->hdr_refcount = 0;

matnd->type = mat->type;

matnd->dims = 2;

matnd->dim[0].size = mat->rows;

matnd->dim[0].step = mat->step;

matnd->dim[1].size = mat->cols;

matnd->dim[1].step = CV_ELEM_SIZE(mat->type);

result = matnd;

}

__END__;

return result;

}

// returns number of dimensions to iterate.

/*

Checks whether arrays have equal type, sizes (mask is optional array

that needs to have the same size, but 8uC1 or 8sC1 type).

Returns number of dimensions to iterate through:

0 means that all arrays are continuous,

1 means that all arrays are vectors of continuous arrays etc.

and the size of largest common continuous part of the arrays

*/

CV_IMPL int

cvInitNArrayIterator( int count, CvArr** arrs,

const CvArr* mask, CvMatND* stubs,

CvNArrayIterator* iterator, int flags )

{

int dims = -1;

CV_FUNCNAME( "cvInitArrayOp" );

__BEGIN__;

int i, j, size, dim0 = -1;

int64 step;

CvMatND* hdr0 = 0;

if( count < 1 || count > CV_MAX_ARR )

CV_ERROR( CV_StsOutOfRange, "Incorrect number of arrays" );

if( !arrs || !stubs )

CV_ERROR( CV_StsNullPtr, "Some of required array pointers is NULL" );

if( !iterator )

CV_ERROR( CV_StsNullPtr, "Iterator pointer is NULL" );

for( i = 0; i <= count; i++ )

{

const CvArr* arr = i < count ? arrs[i] : mask;

CvMatND* hdr;

if( !arr )

{

if( i < count )

CV_ERROR( CV_StsNullPtr, "Some of required array pointers is NULL" );

break;

}

if( CV_IS_MA TND( arr ))

hdr = (CvMatND*)arr;

else

{

int coi = 0;

CV_CALL( hdr = cvGetMatND( arr, stubs + i, &coi ));

if( coi != 0 )

CV_ERROR( CV_BadCOI, "COI set is not allowed here" );

}

iterator->hdr[i] = hdr;

if( i > 0 )

{

if( hdr->dims != hdr0->dims )

CV_ERROR( CV_StsUnmatchedSizes,

"Number of dimensions is the same for all arrays" );

if( i < count )

{

switch( flags & (CV_NO_DEPTH_CHECK|CV_NO_CN_CHECK))

{

case 0:

if( !CV_ARE_TYPES_EQ( hdr, hdr0 ))

CV_ERROR( CV_StsUnmatchedFormats,

"Data type is not the same for all arrays" );

break;

case CV_NO_DEPTH_CHECK:

if( !CV_ARE_CNS_EQ( hdr, hdr0 ))

CV_ERROR( CV_StsUnmatchedFormats,

"Number of channels is not the same for all arrays" );

break;

case CV_NO_CN_CHECK:

if( !CV_ARE_CNS_EQ( hdr, hdr0 ))

CV_ERROR( CV_StsUnmatchedFormats,

"Depth is not the same for all arrays" );

break;

}

}

else

{

if( !CV_IS_MASK_ARR( hdr ))

CV_ERROR( CV_StsBadMask, "Mask should have 8uC1 or 8sC1 data type" );

}

if( !(flags & CV_NO_SIZE_CHECK) )

{

for( j = 0; j < hdr->dims; j++ )

if( hdr->dim[j].size != hdr0->dim[j].size )

CV_ERROR( CV_StsUnmatchedSizes,

"Dimension sizes are the same for all arrays" );

}

}

else

hdr0 = hdr;

step = CV_ELEM_SIZE(hdr->type);

for( j = hdr->dims - 1; j > dim0; j-- )

{

if( step != hdr->dim[j].step )

break;

step *= hdr->dim[j].size;

}

if( j == dim0 && step > INT_MAX )

j++;

if( j > dim0 )

dim0 = j;

iterator->hdr[i] = (CvMatND*)hdr;

iterator->ptr[i] = (uchar*)hdr->data.ptr;

}

size = 1;

for( j = hdr0->dims - 1; j > dim0; j-- )

size *= hdr0->dim[j].size;

dims = dim0 + 1;

iterator->dims = dims;

iterator->count = count;

iterator->size = cvSize(size,1);

for( i = 0; i < dims; i++ )

iterator->stack[i] = hdr0->dim[i].size;

__END__;

return dims;

}

// returns zero value if iteration is finished, non-zero otherwise

CV_IMPL int cvNextNArraySlice( CvNArrayIterator* iterator )

{

assert( iterator != 0 );

int i, dims, size = 0;

for( dims = iterator->dims; dims > 0; dims-- )

{

for( i = 0; i < iterator->count; i++ )

iterator->ptr[i] += iterator->hdr[i]->dim[dims-1].step;

if( --iterator->stack[dims-1] > 0 )

break;

size = iterator->hdr[0]->dim[dims-1].size;

for( i = 0; i < iterator->count; i++ )

iterator->ptr[i] -= (size_t)size*iterator->hdr[i]->dim[dims-1].step;

iterator->stack[dims-1] = size;

}

return dims > 0;

}

/****************************************************************************** **********\

* CvSparseMat creation and basic operations *

\****************************************************************************** **********/

// Creates CvMatND and underlying data

CV_IMPL CvSparseMat*

cvCreateSparseMat( int dims, const int* sizes, int type )

{

CvSparseMat* arr = 0;

CV_FUNCNAME( "cvCreateSparseMat" );

__BEGIN__;

type = CV_MA T_TYPE( type );

int pix_size1 = CV_ELEM_SIZE1(type);

int pix_size = pix_size1*CV_MA T_CN(type);

int i, size;

CvMemStorage* storage;

if( pix_size == 0 )

CV_ERROR( CV_StsUnsupportedFormat, "invalid array data type" );

if( dims <= 0 || dims > CV_MAX_DIM_HEAP )

CV_ERROR( CV_StsOutOfRange, "bad number of dimensions" );

if( !sizes )

CV_ERROR( CV_StsNullPtr, "NULL pointer" );

for( i = 0; i < dims; i++ )

{

if( sizes[i] <= 0 )

CV_ERROR( CV_StsBadSize, "one of dimesion sizes is non-positive" );

}

CV_CALL( arr = (CvSparseMat*)cvAlloc(sizeof(*arr)+MAX(0,dims-CV_MAX_DIM)*sizeof(arr->size[0])));

arr->type = CV_SPARSE_MA T_MAGIC_V AL | type;

arr->dims = dims;

arr->refcount = 0;

arr->hdr_refcount = 1;

memcpy( arr->size, sizes, dims*sizeof(sizes[0]));

arr->valoffset = (int)cvAlign(sizeof(CvSparseNode), pix_size1);

arr->idxoffset = (int)cvAlign(arr->valoffset + pix_size, sizeof(int));

size = (int)cvAlign(arr->idxoffset + dims*sizeof(int), sizeof(CvSetElem));

CV_CALL( storage = cvCreateMemStorage( CV_SPARSE_MA T_BLOCK ));

CV_CALL( arr->heap = cvCreateSet( 0, sizeof(CvSet), size, storage ));

arr->hashsize = CV_SPARSE_HASH_SIZE0;

size = arr->hashsize*sizeof(arr->hashtable[0]);

CV_CALL( arr->hashtable = (void**)cvAlloc( size ));

memset( arr->hashtable, 0, size );

__END__;

if( cvGetErrStatus() < 0 )

cvReleaseSparseMat( &arr );

return arr;

}

// Creates CvMatND and underlying data

CV_IMPL void

cvReleaseSparseMat( CvSparseMat** array )

{

CV_FUNCNAME( "cvReleaseSparseMat" );

__BEGIN__;

if( !array )

CV_ERROR_FROM_CODE( CV_HeaderIsNull );

if( *array )

{

CvSparseMat* arr = *array;

if( !CV_IS_SPARSE_MA T_HDR(arr) )

CV_ERROR_FROM_CODE( CV_StsBadFlag );

*array = 0;

cvReleaseMemStorage( &arr->heap->storage );

cvFree( &arr->hashtable );

cvFree( &arr );

}

__END__;

}

// Creates CvMatND and underlying data

CV_IMPL CvSparseMat*

cvCloneSparseMat( const CvSparseMat* src )

{

CvSparseMat* dst = 0;

CV_FUNCNAME( "cvCloneSparseMat" );

__BEGIN__;

if( !CV_IS_SPARSE_MA T_HDR(src) )

CV_ERROR( CV_StsBadArg, "Invalid sparse array header" );

CV_CALL( dst = cvCreateSparseMat( src->dims, src->size, src->type ));

CV_CALL( cvCopy( src, dst ));

__END__;

if( cvGetErrStatus() < 0 )

cvReleaseSparseMat( &dst );

return dst;

}

CvSparseNode*

cvInitSparseMatIterator( const CvSparseMat* mat, CvSparseMatIterator* iterator ) {

CvSparseNode* node = 0;

CV_FUNCNAME( "cvInitSparseMatIterator" );

__BEGIN__;

int idx;

if( !CV_IS_SPARSE_MA T( mat ))

CV_ERROR( CV_StsBadArg, "Invalid sparse matrix header" );

if( !iterator )

CV_ERROR( CV_StsNullPtr, "NULL iterator pointer" );

iterator->mat = (CvSparseMat*)mat;

iterator->node = 0;

for( idx = 0; idx < mat->hashsize; idx++ )

if( mat->hashtable[idx] )

{

node = iterator->node = (CvSparseNode*)mat->hashtable[idx];

break;

}

iterator->curidx = idx;

__END__;

return node;

}

#define ICV_SPARSE_MA T_HASH_MULTIPLIER 33

static uchar*

icvGetNodePtr( CvSparseMat* mat, const int* idx, int* _type,

int create_node, unsigned* precalc_hashval )

{

uchar* ptr = 0;

CV_FUNCNAME( "icvGetNodePtr" );

__BEGIN__;

int i, tabidx;

unsigned hashval = 0;

CvSparseNode *node;

assert( CV_IS_SPARSE_MA T( mat ));

if( !precalc_hashval )

{

for( i = 0; i < mat->dims; i++ )

{

int t = idx[i];

if( (unsigned)t >= (unsigned)mat->size[i] )

CV_ERROR( CV_StsOutOfRange, "One of indices is out of range" );

hashval = hashval*ICV_SPARSE_MA T_HASH_MULTIPLIER + t;

}

}

else

{

hashval = *precalc_hashval;

}

tabidx = hashval & (mat->hashsize - 1);

hashval &= INT_MAX;

for( node = (CvSparseNode*)mat->hashtable[tabidx];

node != 0; node = node->next )

{

if( node->hashval == hashval )

{

int* nodeidx = CV_NODE_IDX(mat,node);

for( i = 0; i < mat->dims; i++ )

if( idx[i] != nodeidx[i] )

break;

if( i == mat->dims )

{

ptr = (uchar*)CV_NODE_V AL(mat,node);

break;

}

}

}

if( !ptr && create_node )

{

if( mat->heap->active_count >= mat->hashsize*CV_SPARSE_HASH_RA TIO ) {

void** newtable;

int newsize = MAX( mat->hashsize*2, CV_SPARSE_HASH_SIZE0);

int newrawsize = newsize*sizeof(newtable[0]);

CvSparseMatIterator iterator;

assert( (newsize & (newsize - 1)) == 0 );

// resize hash table

CV_CALL( newtable = (void**)cvAlloc( newrawsize ));

memset( newtable, 0, newrawsize );

node = cvInitSparseMatIterator( mat, &iterator );

while( node )

{

CvSparseNode* next = cvGetNextSparseNode( &iterator );

int newidx = node->hashval & (newsize - 1);

node->next = (CvSparseNode*)newtable[newidx];

newtable[newidx] = node;

node = next;

}

cvFree( &mat->hashtable );

mat->hashtable = newtable;

mat->hashsize = newsize;

tabidx = hashval & (newsize - 1);

}

node = (CvSparseNode*)cvSetNew( mat->heap );

node->hashval = hashval;

node->next = (CvSparseNode*)mat->hashtable[tabidx];

mat->hashtable[tabidx] = node;

CV_MEMCPY_INT( CV_NODE_IDX(mat,node), idx, mat->dims );

ptr = (uchar*)CV_NODE_V AL(mat,node);

if( create_node > 0 )

CV_ZERO_CHAR( ptr, CV_ELEM_SIZE(mat->type));

}

if( _type )

*_type = CV_MA T_TYPE(mat->type);

__END__;

return ptr;

}

static void

icvDeleteNode( CvSparseMat* mat, const int* idx, unsigned* precalc_hashval )

{

CV_FUNCNAME( "icvDeleteNode" );

__BEGIN__;

int i, tabidx;

unsigned hashval = 0;

CvSparseNode *node, *prev = 0;

assert( CV_IS_SPARSE_MA T( mat ));

if( !precalc_hashval )

{

for( i = 0; i < mat->dims; i++ )

{

int t = idx[i];

if( (unsigned)t >= (unsigned)mat->size[i] )

CV_ERROR( CV_StsOutOfRange, "One of indices is out of range" );

hashval = hashval*ICV_SPARSE_MA T_HASH_MULTIPLIER + t;

}

}