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 */ 44struct CvPOSITObject 45{ 46 int N; 47 float* inv_matr; 48 float* obj_vecs; 49 float* img_vecs; 50}; 51 52static void icvPseudoInverse3D( float *a, float *b, int n, int method ); 53 54static 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 112static 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 245static 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*/ 265void 266icvPseudoInverse3D( 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 334CV_IMPL CvPOSITObject * 335cvCreatePOSITObject( 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 351CV_IMPL void 352cvPOSIT( 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 366CV_IMPL void 367cvReleasePOSITObject( 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