--- /dev/null
+/*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;
+ IPPI_CALL( icvCreatePOSITObject( points, numPoints, &pObject ));
+ return pObject;
+}
+
+
+CV_IMPL void
+cvPOSIT( CvPOSITObject * pObject, CvPoint2D32f * imagePoints,
+ double focalLength, CvTermCriteria criteria,
+ CvMatr32f rotation, CvVect32f translation )
+{
+ IPPI_CALL( icvPOSIT( pObject, imagePoints,(float) focalLength, criteria,
+ rotation, translation ));
+}
+
+CV_IMPL void
+cvReleasePOSITObject( CvPOSITObject ** ppObject )
+{
+ IPPI_CALL( icvReleasePOSITObject( ppObject ));
+}
+
+/* End of file. */
projects / opencv / blobdiff