Download modules/calib3d/src/posit.cpp from tgollogly/test: direct link, hf CLI and curl.
- Browser
- Download file 12.6 kB
-
https://huggingface.co/tgollogly/test/resolve/main/modules/calib3d/src/posit.cpp
- Command line
-
hf download hf://tgollogly/test/modules/calib3d/src/posit.cpp
-
curl -L -o posit.cpp https://huggingface.co/tgollogly/test/resolve/main/modules/calib3d/src/posit.cpp
12.6 kB
| /*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*/ | |
| /* 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, | |
| float* rotation, float* translation ) | |
| { | |
| int i, j, k; | |
| int count = 0; | |
| bool converged = false; | |
| float scale = 0, inv_Z = 0; | |
| float diff = (float)criteria.epsilon; | |
| /* 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; | |
| /* init variables */ | |
| float inv_focalLength = 1 / focalLength; | |
| int N = pObject->N; | |
| float *objectVectors = pObject->obj_vecs; | |
| float *invMatrix = pObject->inv_matr; | |
| float *imgVectors = pObject->img_vecs; | |
| 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]; | |
| } | |
| } | |
| } | |
| float 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]*/; | |
| float 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]*/; | |
| const float invInorm = cvInvSqrt( inorm ); | |
| const float 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)) | |
| || ((criteria.type & CV_TERMCRIT_ITER) && (count == criteria.max_iter)); | |
| } | |
| const float 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 ) | |
| { | |
| if( method == 0 ) | |
| { | |
| float ata00 = 0; | |
| float ata11 = 0; | |
| float ata22 = 0; | |
| float ata01 = 0; | |
| float ata02 = 0; | |
| float ata12 = 0; | |
| int k; | |
| /* 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 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; | |
| float det = 0; | |
| det += ata00 * p00; | |
| det += ata01 * p01; | |
| det += ata02 * p02; | |
| const float 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; | |
| IPPI_CALL( icvCreatePOSITObject( points, numPoints, &pObject )); | |
| return pObject; | |
| } | |
| CV_IMPL void | |
| cvPOSIT( CvPOSITObject * pObject, CvPoint2D32f * imagePoints, | |
| double focalLength, CvTermCriteria criteria, | |
| float* rotation, float* translation ) | |
| { | |
| IPPI_CALL( icvPOSIT( pObject, imagePoints,(float) focalLength, criteria, | |
| rotation, translation )); | |
| } | |
| CV_IMPL void | |
| cvReleasePOSITObject( CvPOSITObject ** ppObject ) | |
| { | |
| IPPI_CALL( icvReleasePOSITObject( ppObject )); | |
| } | |
| /* End of file. */ | |