Update to 2.0.0 tree from current Fremantle build
[opencv] / cv / src / cvposit.cpp
diff --git a/cv/src/cvposit.cpp b/cv/src/cvposit.cpp
deleted file mode 100644 (file)
index 65da46d..0000000
+++ /dev/null
@@ -1,378 +0,0 @@
-/*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. */