Home | History | Annotate | Download | only in src
      1 /*M///////////////////////////////////////////////////////////////////////////////////////
      2 //
      3 //  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
      4 //
      5 //  By downloading, copying, installing or using the software you agree to this license.
      6 //  If you do not agree to this license, do not download, install,
      7 //  copy or use the software.
      8 //
      9 //
     10 //                        Intel License Agreement
     11 //                For Open Source Computer Vision Library
     12 //
     13 // Copyright (C) 2000, Intel Corporation, all rights reserved.
     14 // Third party copyrights are property of their respective owners.
     15 //
     16 // Redistribution and use in source and binary forms, with or without modification,
     17 // are permitted provided that the following conditions are met:
     18 //
     19 //   * Redistribution's of source code must retain the above copyright notice,
     20 //     this list of conditions and the following disclaimer.
     21 //
     22 //   * Redistribution's in binary form must reproduce the above copyright notice,
     23 //     this list of conditions and the following disclaimer in the documentation
     24 //     and/or other materials provided with the distribution.
     25 //
     26 //   * The name of Intel Corporation may not be used to endorse or promote products
     27 //     derived from this software without specific prior written permission.
     28 //
     29 // This software is provided by the copyright holders and contributors "as is" and
     30 // any express or implied warranties, including, but not limited to, the implied
     31 // warranties of merchantability and fitness for a particular purpose are disclaimed.
     32 // In no event shall the Intel Corporation or contributors be liable for any direct,
     33 // indirect, incidental, special, exemplary, or consequential damages
     34 // (including, but not limited to, procurement of substitute goods or services;
     35 // loss of use, data, or profits; or business interruption) however caused
     36 // and on any theory of liability, whether in contract, strict liability,
     37 // or tort (including negligence or otherwise) arising in any way out of
     38 // the use of this software, even if advised of the possibility of such damage.
     39 //
     40 //M*/
     41 #include "_cv.h"
     42 
     43 /* POSIT structure */
     44 struct CvPOSITObject
     45 {
     46     int N;
     47     float* inv_matr;
     48     float* obj_vecs;
     49     float* img_vecs;
     50 };
     51 
     52 static void icvPseudoInverse3D( float *a, float *b, int n, int method );
     53 
     54 static  CvStatus  icvCreatePOSITObject( CvPoint3D32f *points,
     55                                         int numPoints,
     56                                         CvPOSITObject **ppObject )
     57 {
     58     int i;
     59 
     60     /* Compute size of required memory */
     61     /* buffer for inverse matrix = N*3*float */
     62     /* buffer for storing weakImagePoints = numPoints * 2 * float */
     63     /* buffer for storing object vectors = N*3*float */
     64     /* buffer for storing image vectors = N*2*float */
     65 
     66     int N = numPoints - 1;
     67     int inv_matr_size = N * 3 * sizeof( float );
     68     int obj_vec_size = inv_matr_size;
     69     int img_vec_size = N * 2 * sizeof( float );
     70     CvPOSITObject *pObject;
     71 
     72     /* check bad arguments */
     73     if( points == NULL )
     74         return CV_NULLPTR_ERR;
     75     if( numPoints < 4 )
     76         return CV_BADSIZE_ERR;
     77     if( ppObject == NULL )
     78         return CV_NULLPTR_ERR;
     79 
     80     /* memory allocation */
     81     pObject = (CvPOSITObject *) cvAlloc( sizeof( CvPOSITObject ) +
     82                                          inv_matr_size + obj_vec_size + img_vec_size );
     83 
     84     if( !pObject )
     85         return CV_OUTOFMEM_ERR;
     86 
     87     /* part the memory between all structures */
     88     pObject->N = N;
     89     pObject->inv_matr = (float *) ((char *) pObject + sizeof( CvPOSITObject ));
     90     pObject->obj_vecs = (float *) ((char *) (pObject->inv_matr) + inv_matr_size);
     91     pObject->img_vecs = (float *) ((char *) (pObject->obj_vecs) + obj_vec_size);
     92 
     93 /****************************************************************************************\
     94 *          Construct object vectors from object points                                   *
     95 \****************************************************************************************/
     96     for( i = 0; i < numPoints - 1; i++ )
     97     {
     98         pObject->obj_vecs[i] = points[i + 1].x - points[0].x;
     99         pObject->obj_vecs[N + i] = points[i + 1].y - points[0].y;
    100         pObject->obj_vecs[2 * N + i] = points[i + 1].z - points[0].z;
    101     }
    102 /****************************************************************************************\
    103 *   Compute pseudoinverse matrix                                                         *
    104 \****************************************************************************************/
    105     icvPseudoInverse3D( pObject->obj_vecs, pObject->inv_matr, N, 0 );
    106 
    107     *ppObject = pObject;
    108     return CV_NO_ERR;
    109 }
    110 
    111 
    112 static  CvStatus  icvPOSIT( CvPOSITObject *pObject, CvPoint2D32f *imagePoints,
    113                             float focalLength, CvTermCriteria criteria,
    114                             CvMatr32f rotation, CvVect32f translation )
    115 {
    116     int i, j, k;
    117     int count = 0, converged = 0;
    118     float inorm, jnorm, invInorm, invJnorm, invScale, scale = 0, inv_Z = 0;
    119     float diff = (float)criteria.epsilon;
    120     float inv_focalLength = 1 / focalLength;
    121 
    122     /* init variables */
    123     int N = pObject->N;
    124     float *objectVectors = pObject->obj_vecs;
    125     float *invMatrix = pObject->inv_matr;
    126     float *imgVectors = pObject->img_vecs;
    127 
    128     /* Check bad arguments */
    129     if( imagePoints == NULL )
    130         return CV_NULLPTR_ERR;
    131     if( pObject == NULL )
    132         return CV_NULLPTR_ERR;
    133     if( focalLength <= 0 )
    134         return CV_BADFACTOR_ERR;
    135     if( !rotation )
    136         return CV_NULLPTR_ERR;
    137     if( !translation )
    138         return CV_NULLPTR_ERR;
    139     if( (criteria.type == 0) || (criteria.type > (CV_TERMCRIT_ITER | CV_TERMCRIT_EPS)))
    140         return CV_BADFLAG_ERR;
    141     if( (criteria.type & CV_TERMCRIT_EPS) && criteria.epsilon < 0 )
    142         return CV_BADFACTOR_ERR;
    143     if( (criteria.type & CV_TERMCRIT_ITER) && criteria.max_iter <= 0 )
    144         return CV_BADFACTOR_ERR;
    145 
    146     while( !converged )
    147     {
    148         if( count == 0 )
    149         {
    150             /* subtract out origin to get image vectors */
    151             for( i = 0; i < N; i++ )
    152             {
    153                 imgVectors[i] = imagePoints[i + 1].x - imagePoints[0].x;
    154                 imgVectors[N + i] = imagePoints[i + 1].y - imagePoints[0].y;
    155             }
    156         }
    157         else
    158         {
    159             diff = 0;
    160             /* Compute new SOP (scaled orthograthic projection) image from pose */
    161             for( i = 0; i < N; i++ )
    162             {
    163                 /* objectVector * k */
    164                 float old;
    165                 float tmp = objectVectors[i] * rotation[6] /*[2][0]*/ +
    166                     objectVectors[N + i] * rotation[7]     /*[2][1]*/ +
    167                     objectVectors[2 * N + i] * rotation[8] /*[2][2]*/;
    168 
    169                 tmp *= inv_Z;
    170                 tmp += 1;
    171 
    172                 old = imgVectors[i];
    173                 imgVectors[i] = imagePoints[i + 1].x * tmp - imagePoints[0].x;
    174 
    175                 diff = MAX( diff, (float) fabs( imgVectors[i] - old ));
    176 
    177                 old = imgVectors[N + i];
    178                 imgVectors[N + i] = imagePoints[i + 1].y * tmp - imagePoints[0].y;
    179 
    180                 diff = MAX( diff, (float) fabs( imgVectors[N + i] - old ));
    181             }
    182         }
    183 
    184         /* calculate I and J vectors */
    185         for( i = 0; i < 2; i++ )
    186         {
    187             for( j = 0; j < 3; j++ )
    188             {
    189                 rotation[3*i+j] /*[i][j]*/ = 0;
    190                 for( k = 0; k < N; k++ )
    191                 {
    192                     rotation[3*i+j] /*[i][j]*/ += invMatrix[j * N + k] * imgVectors[i * N + k];
    193                 }
    194             }
    195         }
    196 
    197         inorm = rotation[0] /*[0][0]*/ * rotation[0] /*[0][0]*/ +
    198                 rotation[1] /*[0][1]*/ * rotation[1] /*[0][1]*/ +
    199                 rotation[2] /*[0][2]*/ * rotation[2] /*[0][2]*/;
    200 
    201         jnorm = rotation[3] /*[1][0]*/ * rotation[3] /*[1][0]*/ +
    202                 rotation[4] /*[1][1]*/ * rotation[4] /*[1][1]*/ +
    203                 rotation[5] /*[1][2]*/ * rotation[5] /*[1][2]*/;
    204 
    205         invInorm = cvInvSqrt( inorm );
    206         invJnorm = cvInvSqrt( jnorm );
    207 
    208         inorm *= invInorm;
    209         jnorm *= invJnorm;
    210 
    211         rotation[0] /*[0][0]*/ *= invInorm;
    212         rotation[1] /*[0][1]*/ *= invInorm;
    213         rotation[2] /*[0][2]*/ *= invInorm;
    214 
    215         rotation[3] /*[1][0]*/ *= invJnorm;
    216         rotation[4] /*[1][1]*/ *= invJnorm;
    217         rotation[5] /*[1][2]*/ *= invJnorm;
    218 
    219         /* row2 = row0 x row1 (cross product) */
    220         rotation[6] /*->m[2][0]*/ = rotation[1] /*->m[0][1]*/ * rotation[5] /*->m[1][2]*/ -
    221                                     rotation[2] /*->m[0][2]*/ * rotation[4] /*->m[1][1]*/;
    222 
    223         rotation[7] /*->m[2][1]*/ = rotation[2] /*->m[0][2]*/ * rotation[3] /*->m[1][0]*/ -
    224                                     rotation[0] /*->m[0][0]*/ * rotation[5] /*->m[1][2]*/;
    225 
    226         rotation[8] /*->m[2][2]*/ = rotation[0] /*->m[0][0]*/ * rotation[4] /*->m[1][1]*/ -
    227                                     rotation[1] /*->m[0][1]*/ * rotation[3] /*->m[1][0]*/;
    228 
    229         scale = (inorm + jnorm) / 2.0f;
    230         inv_Z = scale * inv_focalLength;
    231 
    232         count++;
    233         converged = ((criteria.type & CV_TERMCRIT_EPS) && (diff < criteria.epsilon));
    234         converged |= ((criteria.type & CV_TERMCRIT_ITER) && (count == criteria.max_iter));
    235     }
    236     invScale = 1 / scale;
    237     translation[0] = imagePoints[0].x * invScale;
    238     translation[1] = imagePoints[0].y * invScale;
    239     translation[2] = 1 / inv_Z;
    240 
    241     return CV_NO_ERR;
    242 }
    243 
    244 
    245 static  CvStatus  icvReleasePOSITObject( CvPOSITObject ** ppObject )
    246 {
    247     cvFree( ppObject );
    248     return CV_NO_ERR;
    249 }
    250 
    251 /*F///////////////////////////////////////////////////////////////////////////////////////
    252 //    Name:       icvPseudoInverse3D
    253 //    Purpose:    Pseudoinverse N x 3 matrix     N >= 3
    254 //    Context:
    255 //    Parameters:
    256 //                a - input matrix
    257 //                b - pseudoinversed a
    258 //                n - number of rows in a
    259 //                method - if 0, then b = inv(transpose(a)*a) * transpose(a)
    260 //                         if 1, then SVD used.
    261 //    Returns:
    262 //    Notes:      Both matrix are stored by n-dimensional vectors.
    263 //                Now only method == 0 supported.
    264 //F*/
    265 void
    266 icvPseudoInverse3D( float *a, float *b, int n, int method )
    267 {
    268     int k;
    269 
    270     if( method == 0 )
    271     {
    272         float ata00 = 0;
    273         float ata11 = 0;
    274         float ata22 = 0;
    275         float ata01 = 0;
    276         float ata02 = 0;
    277         float ata12 = 0;
    278         float det = 0;
    279 
    280         /* compute matrix ata = transpose(a) * a  */
    281         for( k = 0; k < n; k++ )
    282         {
    283             float a0 = a[k];
    284             float a1 = a[n + k];
    285             float a2 = a[2 * n + k];
    286 
    287             ata00 += a0 * a0;
    288             ata11 += a1 * a1;
    289             ata22 += a2 * a2;
    290 
    291             ata01 += a0 * a1;
    292             ata02 += a0 * a2;
    293             ata12 += a1 * a2;
    294         }
    295         /* inverse matrix ata */
    296         {
    297             float inv_det;
    298             float p00 = ata11 * ata22 - ata12 * ata12;
    299             float p01 = -(ata01 * ata22 - ata12 * ata02);
    300             float p02 = ata12 * ata01 - ata11 * ata02;
    301 
    302             float p11 = ata00 * ata22 - ata02 * ata02;
    303             float p12 = -(ata00 * ata12 - ata01 * ata02);
    304             float p22 = ata00 * ata11 - ata01 * ata01;
    305 
    306             det += ata00 * p00;
    307             det += ata01 * p01;
    308             det += ata02 * p02;
    309 
    310             inv_det = 1 / det;
    311 
    312             /* compute resultant matrix */
    313             for( k = 0; k < n; k++ )
    314             {
    315                 float a0 = a[k];
    316                 float a1 = a[n + k];
    317                 float a2 = a[2 * n + k];
    318 
    319                 b[k] = (p00 * a0 + p01 * a1 + p02 * a2) * inv_det;
    320                 b[n + k] = (p01 * a0 + p11 * a1 + p12 * a2) * inv_det;
    321                 b[2 * n + k] = (p02 * a0 + p12 * a1 + p22 * a2) * inv_det;
    322             }
    323         }
    324     }
    325 
    326     /*if ( method == 1 )
    327        {
    328        }
    329      */
    330 
    331     return;
    332 }
    333 
    334 CV_IMPL CvPOSITObject *
    335 cvCreatePOSITObject( CvPoint3D32f * points, int numPoints )
    336 {
    337     CvPOSITObject *pObject = 0;
    338 
    339     CV_FUNCNAME( "cvCreatePOSITObject" );
    340 
    341     __BEGIN__;
    342 
    343     IPPI_CALL( icvCreatePOSITObject( points, numPoints, &pObject ));
    344 
    345     __END__;
    346 
    347     return pObject;
    348 }
    349 
    350 
    351 CV_IMPL void
    352 cvPOSIT( CvPOSITObject * pObject, CvPoint2D32f * imagePoints,
    353          double focalLength, CvTermCriteria criteria,
    354          CvMatr32f rotation, CvVect32f translation )
    355 {
    356     CV_FUNCNAME( "cvPOSIT" );
    357 
    358     __BEGIN__;
    359 
    360     IPPI_CALL( icvPOSIT( pObject, imagePoints,(float) focalLength, criteria,
    361                          rotation, translation ));
    362 
    363     __END__;
    364 }
    365 
    366 CV_IMPL void
    367 cvReleasePOSITObject( CvPOSITObject ** ppObject )
    368 {
    369     CV_FUNCNAME( "cvReleasePOSITObject" );
    370 
    371     __BEGIN__;
    372 
    373     IPPI_CALL( icvReleasePOSITObject( ppObject ));
    374 
    375     __END__;
    376 }
    377 
    378 /* End of file. */
    379