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
icvCreatePOSITObject(CvPoint3D32f * points,int numPoints,CvPOSITObject ** ppObject)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
icvPOSIT(CvPOSITObject * pObject,CvPoint2D32f * imagePoints,float focalLength,CvTermCriteria criteria,CvMatr32f rotation,CvVect32f translation)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
icvReleasePOSITObject(CvPOSITObject ** ppObject)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
icvPseudoInverse3D(float * a,float * b,int n,int method)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 *
cvCreatePOSITObject(CvPoint3D32f * points,int numPoints)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
cvPOSIT(CvPOSITObject * pObject,CvPoint2D32f * imagePoints,double focalLength,CvTermCriteria criteria,CvMatr32f rotation,CvVect32f translation)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
cvReleasePOSITObject(CvPOSITObject ** ppObject)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