diff options
Diffstat (limited to 'cv/src/cvposit.cpp')
-rw-r--r-- | cv/src/cvposit.cpp | 378 |
1 files changed, 378 insertions, 0 deletions
diff --git a/cv/src/cvposit.cpp b/cv/src/cvposit.cpp new file mode 100644 index 0000000..65da46d --- /dev/null +++ b/cv/src/cvposit.cpp @@ -0,0 +1,378 @@ +/*M/////////////////////////////////////////////////////////////////////////////////////// +// +// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. +// +// By downloading, copying, installing or using the software you agree to this license. +// If you do not agree to this license, do not download, install, +// copy or use the software. +// +// +// Intel License Agreement +// For Open Source Computer Vision Library +// +// Copyright (C) 2000, Intel Corporation, all rights reserved. +// Third party copyrights are property of their respective owners. +// +// Redistribution and use in source and binary forms, with or without modification, +// are permitted provided that the following conditions are met: +// +// * Redistribution's of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// +// * Redistribution's in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// * The name of Intel Corporation may not be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// This software is provided by the copyright holders and contributors "as is" and +// any express or implied warranties, including, but not limited to, the implied +// warranties of merchantability and fitness for a particular purpose are disclaimed. +// In no event shall the Intel Corporation or contributors be liable for any direct, +// indirect, incidental, special, exemplary, or consequential damages +// (including, but not limited to, procurement of substitute goods or services; +// loss of use, data, or profits; or business interruption) however caused +// and on any theory of liability, whether in contract, strict liability, +// or tort (including negligence or otherwise) arising in any way out of +// the use of this software, even if advised of the possibility of such damage. +// +//M*/ +#include "_cv.h" + +/* POSIT structure */ +struct CvPOSITObject +{ + int N; + float* inv_matr; + float* obj_vecs; + float* img_vecs; +}; + +static void icvPseudoInverse3D( float *a, float *b, int n, int method ); + +static CvStatus icvCreatePOSITObject( CvPoint3D32f *points, + int numPoints, + CvPOSITObject **ppObject ) +{ + int i; + + /* Compute size of required memory */ + /* buffer for inverse matrix = N*3*float */ + /* buffer for storing weakImagePoints = numPoints * 2 * float */ + /* buffer for storing object vectors = N*3*float */ + /* buffer for storing image vectors = N*2*float */ + + int N = numPoints - 1; + int inv_matr_size = N * 3 * sizeof( float ); + int obj_vec_size = inv_matr_size; + int img_vec_size = N * 2 * sizeof( float ); + CvPOSITObject *pObject; + + /* check bad arguments */ + if( points == NULL ) + return CV_NULLPTR_ERR; + if( numPoints < 4 ) + return CV_BADSIZE_ERR; + if( ppObject == NULL ) + return CV_NULLPTR_ERR; + + /* memory allocation */ + pObject = (CvPOSITObject *) cvAlloc( sizeof( CvPOSITObject ) + + inv_matr_size + obj_vec_size + img_vec_size ); + + if( !pObject ) + return CV_OUTOFMEM_ERR; + + /* part the memory between all structures */ + pObject->N = N; + pObject->inv_matr = (float *) ((char *) pObject + sizeof( CvPOSITObject )); + pObject->obj_vecs = (float *) ((char *) (pObject->inv_matr) + inv_matr_size); + pObject->img_vecs = (float *) ((char *) (pObject->obj_vecs) + obj_vec_size); + +/****************************************************************************************\ +* Construct object vectors from object points * +\****************************************************************************************/ + for( i = 0; i < numPoints - 1; i++ ) + { + pObject->obj_vecs[i] = points[i + 1].x - points[0].x; + pObject->obj_vecs[N + i] = points[i + 1].y - points[0].y; + pObject->obj_vecs[2 * N + i] = points[i + 1].z - points[0].z; + } +/****************************************************************************************\ +* Compute pseudoinverse matrix * +\****************************************************************************************/ + icvPseudoInverse3D( pObject->obj_vecs, pObject->inv_matr, N, 0 ); + + *ppObject = pObject; + return CV_NO_ERR; +} + + +static CvStatus icvPOSIT( CvPOSITObject *pObject, CvPoint2D32f *imagePoints, + float focalLength, CvTermCriteria criteria, + CvMatr32f rotation, CvVect32f translation ) +{ + int i, j, k; + int count = 0, converged = 0; + float inorm, jnorm, invInorm, invJnorm, invScale, scale = 0, inv_Z = 0; + float diff = (float)criteria.epsilon; + float inv_focalLength = 1 / focalLength; + + /* init variables */ + int N = pObject->N; + float *objectVectors = pObject->obj_vecs; + float *invMatrix = pObject->inv_matr; + float *imgVectors = pObject->img_vecs; + + /* Check bad arguments */ + if( imagePoints == NULL ) + return CV_NULLPTR_ERR; + if( pObject == NULL ) + return CV_NULLPTR_ERR; + if( focalLength <= 0 ) + return CV_BADFACTOR_ERR; + if( !rotation ) + return CV_NULLPTR_ERR; + if( !translation ) + return CV_NULLPTR_ERR; + if( (criteria.type == 0) || (criteria.type > (CV_TERMCRIT_ITER | CV_TERMCRIT_EPS))) + return CV_BADFLAG_ERR; + if( (criteria.type & CV_TERMCRIT_EPS) && criteria.epsilon < 0 ) + return CV_BADFACTOR_ERR; + if( (criteria.type & CV_TERMCRIT_ITER) && criteria.max_iter <= 0 ) + return CV_BADFACTOR_ERR; + + while( !converged ) + { + if( count == 0 ) + { + /* subtract out origin to get image vectors */ + for( i = 0; i < N; i++ ) + { + imgVectors[i] = imagePoints[i + 1].x - imagePoints[0].x; + imgVectors[N + i] = imagePoints[i + 1].y - imagePoints[0].y; + } + } + else + { + diff = 0; + /* Compute new SOP (scaled orthograthic projection) image from pose */ + for( i = 0; i < N; i++ ) + { + /* objectVector * k */ + float old; + float tmp = objectVectors[i] * rotation[6] /*[2][0]*/ + + objectVectors[N + i] * rotation[7] /*[2][1]*/ + + objectVectors[2 * N + i] * rotation[8] /*[2][2]*/; + + tmp *= inv_Z; + tmp += 1; + + old = imgVectors[i]; + imgVectors[i] = imagePoints[i + 1].x * tmp - imagePoints[0].x; + + diff = MAX( diff, (float) fabs( imgVectors[i] - old )); + + old = imgVectors[N + i]; + imgVectors[N + i] = imagePoints[i + 1].y * tmp - imagePoints[0].y; + + diff = MAX( diff, (float) fabs( imgVectors[N + i] - old )); + } + } + + /* calculate I and J vectors */ + for( i = 0; i < 2; i++ ) + { + for( j = 0; j < 3; j++ ) + { + rotation[3*i+j] /*[i][j]*/ = 0; + for( k = 0; k < N; k++ ) + { + rotation[3*i+j] /*[i][j]*/ += invMatrix[j * N + k] * imgVectors[i * N + k]; + } + } + } + + inorm = rotation[0] /*[0][0]*/ * rotation[0] /*[0][0]*/ + + rotation[1] /*[0][1]*/ * rotation[1] /*[0][1]*/ + + rotation[2] /*[0][2]*/ * rotation[2] /*[0][2]*/; + + jnorm = rotation[3] /*[1][0]*/ * rotation[3] /*[1][0]*/ + + rotation[4] /*[1][1]*/ * rotation[4] /*[1][1]*/ + + rotation[5] /*[1][2]*/ * rotation[5] /*[1][2]*/; + + invInorm = cvInvSqrt( inorm ); + invJnorm = cvInvSqrt( jnorm ); + + inorm *= invInorm; + jnorm *= invJnorm; + + rotation[0] /*[0][0]*/ *= invInorm; + rotation[1] /*[0][1]*/ *= invInorm; + rotation[2] /*[0][2]*/ *= invInorm; + + rotation[3] /*[1][0]*/ *= invJnorm; + rotation[4] /*[1][1]*/ *= invJnorm; + rotation[5] /*[1][2]*/ *= invJnorm; + + /* row2 = row0 x row1 (cross product) */ + rotation[6] /*->m[2][0]*/ = rotation[1] /*->m[0][1]*/ * rotation[5] /*->m[1][2]*/ - + rotation[2] /*->m[0][2]*/ * rotation[4] /*->m[1][1]*/; + + rotation[7] /*->m[2][1]*/ = rotation[2] /*->m[0][2]*/ * rotation[3] /*->m[1][0]*/ - + rotation[0] /*->m[0][0]*/ * rotation[5] /*->m[1][2]*/; + + rotation[8] /*->m[2][2]*/ = rotation[0] /*->m[0][0]*/ * rotation[4] /*->m[1][1]*/ - + rotation[1] /*->m[0][1]*/ * rotation[3] /*->m[1][0]*/; + + scale = (inorm + jnorm) / 2.0f; + inv_Z = scale * inv_focalLength; + + count++; + converged = ((criteria.type & CV_TERMCRIT_EPS) && (diff < criteria.epsilon)); + converged |= ((criteria.type & CV_TERMCRIT_ITER) && (count == criteria.max_iter)); + } + invScale = 1 / scale; + translation[0] = imagePoints[0].x * invScale; + translation[1] = imagePoints[0].y * invScale; + translation[2] = 1 / inv_Z; + + return CV_NO_ERR; +} + + +static CvStatus icvReleasePOSITObject( CvPOSITObject ** ppObject ) +{ + cvFree( ppObject ); + return CV_NO_ERR; +} + +/*F/////////////////////////////////////////////////////////////////////////////////////// +// Name: icvPseudoInverse3D +// Purpose: Pseudoinverse N x 3 matrix N >= 3 +// Context: +// Parameters: +// a - input matrix +// b - pseudoinversed a +// n - number of rows in a +// method - if 0, then b = inv(transpose(a)*a) * transpose(a) +// if 1, then SVD used. +// Returns: +// Notes: Both matrix are stored by n-dimensional vectors. +// Now only method == 0 supported. +//F*/ +void +icvPseudoInverse3D( float *a, float *b, int n, int method ) +{ + int k; + + if( method == 0 ) + { + float ata00 = 0; + float ata11 = 0; + float ata22 = 0; + float ata01 = 0; + float ata02 = 0; + float ata12 = 0; + float det = 0; + + /* compute matrix ata = transpose(a) * a */ + for( k = 0; k < n; k++ ) + { + float a0 = a[k]; + float a1 = a[n + k]; + float a2 = a[2 * n + k]; + + ata00 += a0 * a0; + ata11 += a1 * a1; + ata22 += a2 * a2; + + ata01 += a0 * a1; + ata02 += a0 * a2; + ata12 += a1 * a2; + } + /* inverse matrix ata */ + { + float inv_det; + float p00 = ata11 * ata22 - ata12 * ata12; + float p01 = -(ata01 * ata22 - ata12 * ata02); + float p02 = ata12 * ata01 - ata11 * ata02; + + float p11 = ata00 * ata22 - ata02 * ata02; + float p12 = -(ata00 * ata12 - ata01 * ata02); + float p22 = ata00 * ata11 - ata01 * ata01; + + det += ata00 * p00; + det += ata01 * p01; + det += ata02 * p02; + + inv_det = 1 / det; + + /* compute resultant matrix */ + for( k = 0; k < n; k++ ) + { + float a0 = a[k]; + float a1 = a[n + k]; + float a2 = a[2 * n + k]; + + b[k] = (p00 * a0 + p01 * a1 + p02 * a2) * inv_det; + b[n + k] = (p01 * a0 + p11 * a1 + p12 * a2) * inv_det; + b[2 * n + k] = (p02 * a0 + p12 * a1 + p22 * a2) * inv_det; + } + } + } + + /*if ( method == 1 ) + { + } + */ + + return; +} + +CV_IMPL CvPOSITObject * +cvCreatePOSITObject( CvPoint3D32f * points, int numPoints ) +{ + CvPOSITObject *pObject = 0; + + CV_FUNCNAME( "cvCreatePOSITObject" ); + + __BEGIN__; + + IPPI_CALL( icvCreatePOSITObject( points, numPoints, &pObject )); + + __END__; + + return pObject; +} + + +CV_IMPL void +cvPOSIT( CvPOSITObject * pObject, CvPoint2D32f * imagePoints, + double focalLength, CvTermCriteria criteria, + CvMatr32f rotation, CvVect32f translation ) +{ + CV_FUNCNAME( "cvPOSIT" ); + + __BEGIN__; + + IPPI_CALL( icvPOSIT( pObject, imagePoints,(float) focalLength, criteria, + rotation, translation )); + + __END__; +} + +CV_IMPL void +cvReleasePOSITObject( CvPOSITObject ** ppObject ) +{ + CV_FUNCNAME( "cvReleasePOSITObject" ); + + __BEGIN__; + + IPPI_CALL( icvReleasePOSITObject( ppObject )); + + __END__; +} + +/* End of file. */ |