git.maemo.org Git - opencv/blob - cv/src/cvposit.cpp

   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. */