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 "precomp.hpp"
42 #include "opencv2/calib3d/calib3d_c.h"
43
44 /* POSIT structure */
45 struct CvPOSITObject
46 {
47 int N;
48 float* inv_matr;
49 float* obj_vecs;
50 float* img_vecs;
51 };
52
53 static void icvPseudoInverse3D( float *a, float *b, int n, int method );
54
icvCreatePOSITObject(CvPoint3D32f * points,int numPoints,CvPOSITObject ** ppObject)55 static CvStatus icvCreatePOSITObject( CvPoint3D32f *points,
56 int numPoints,
57 CvPOSITObject **ppObject )
58 {
59 int i;
60
61 /* Compute size of required memory */
62 /* buffer for inverse matrix = N*3*float */
63 /* buffer for storing weakImagePoints = numPoints * 2 * float */
64 /* buffer for storing object vectors = N*3*float */
65 /* buffer for storing image vectors = N*2*float */
66
67 int N = numPoints - 1;
68 int inv_matr_size = N * 3 * sizeof( float );
69 int obj_vec_size = inv_matr_size;
70 int img_vec_size = N * 2 * sizeof( float );
71 CvPOSITObject *pObject;
72
73 /* check bad arguments */
74 if( points == NULL )
75 return CV_NULLPTR_ERR;
76 if( numPoints < 4 )
77 return CV_BADSIZE_ERR;
78 if( ppObject == NULL )
79 return CV_NULLPTR_ERR;
80
81 /* memory allocation */
82 pObject = (CvPOSITObject *) cvAlloc( sizeof( CvPOSITObject ) +
83 inv_matr_size + obj_vec_size + img_vec_size );
84
85 if( !pObject )
86 return CV_OUTOFMEM_ERR;
87
88 /* part the memory between all structures */
89 pObject->N = N;
90 pObject->inv_matr = (float *) ((char *) pObject + sizeof( CvPOSITObject ));
91 pObject->obj_vecs = (float *) ((char *) (pObject->inv_matr) + inv_matr_size);
92 pObject->img_vecs = (float *) ((char *) (pObject->obj_vecs) + obj_vec_size);
93
94 /****************************************************************************************\
95 * Construct object vectors from object points *
96 \****************************************************************************************/
97 for( i = 0; i < numPoints - 1; i++ )
98 {
99 pObject->obj_vecs[i] = points[i + 1].x - points[0].x;
100 pObject->obj_vecs[N + i] = points[i + 1].y - points[0].y;
101 pObject->obj_vecs[2 * N + i] = points[i + 1].z - points[0].z;
102 }
103 /****************************************************************************************\
104 * Compute pseudoinverse matrix *
105 \****************************************************************************************/
106 icvPseudoInverse3D( pObject->obj_vecs, pObject->inv_matr, N, 0 );
107
108 *ppObject = pObject;
109 return CV_NO_ERR;
110 }
111
112
icvPOSIT(CvPOSITObject * pObject,CvPoint2D32f * imagePoints,float focalLength,CvTermCriteria criteria,float * rotation,float * translation)113 static CvStatus icvPOSIT( CvPOSITObject *pObject, CvPoint2D32f *imagePoints,
114 float focalLength, CvTermCriteria criteria,
115 float* rotation, float* translation )
116 {
117 int i, j, k;
118 int count = 0, converged = 0;
119 float scale = 0, inv_Z = 0;
120 float diff = (float)criteria.epsilon;
121
122 /* Check bad arguments */
123 if( imagePoints == NULL )
124 return CV_NULLPTR_ERR;
125 if( pObject == NULL )
126 return CV_NULLPTR_ERR;
127 if( focalLength <= 0 )
128 return CV_BADFACTOR_ERR;
129 if( !rotation )
130 return CV_NULLPTR_ERR;
131 if( !translation )
132 return CV_NULLPTR_ERR;
133 if( (criteria.type == 0) || (criteria.type > (CV_TERMCRIT_ITER | CV_TERMCRIT_EPS)))
134 return CV_BADFLAG_ERR;
135 if( (criteria.type & CV_TERMCRIT_EPS) && criteria.epsilon < 0 )
136 return CV_BADFACTOR_ERR;
137 if( (criteria.type & CV_TERMCRIT_ITER) && criteria.max_iter <= 0 )
138 return CV_BADFACTOR_ERR;
139
140 /* init variables */
141 float inv_focalLength = 1 / focalLength;
142 int N = pObject->N;
143 float *objectVectors = pObject->obj_vecs;
144 float *invMatrix = pObject->inv_matr;
145 float *imgVectors = pObject->img_vecs;
146
147 while( !converged )
148 {
149 if( count == 0 )
150 {
151 /* subtract out origin to get image vectors */
152 for( i = 0; i < N; i++ )
153 {
154 imgVectors[i] = imagePoints[i + 1].x - imagePoints[0].x;
155 imgVectors[N + i] = imagePoints[i + 1].y - imagePoints[0].y;
156 }
157 }
158 else
159 {
160 diff = 0;
161 /* Compute new SOP (scaled orthograthic projection) image from pose */
162 for( i = 0; i < N; i++ )
163 {
164 /* objectVector * k */
165 float old;
166 float tmp = objectVectors[i] * rotation[6] /*[2][0]*/ +
167 objectVectors[N + i] * rotation[7] /*[2][1]*/ +
168 objectVectors[2 * N + i] * rotation[8] /*[2][2]*/;
169
170 tmp *= inv_Z;
171 tmp += 1;
172
173 old = imgVectors[i];
174 imgVectors[i] = imagePoints[i + 1].x * tmp - imagePoints[0].x;
175
176 diff = MAX( diff, (float) fabs( imgVectors[i] - old ));
177
178 old = imgVectors[N + i];
179 imgVectors[N + i] = imagePoints[i + 1].y * tmp - imagePoints[0].y;
180
181 diff = MAX( diff, (float) fabs( imgVectors[N + i] - old ));
182 }
183 }
184
185 /* calculate I and J vectors */
186 for( i = 0; i < 2; i++ )
187 {
188 for( j = 0; j < 3; j++ )
189 {
190 rotation[3*i+j] /*[i][j]*/ = 0;
191 for( k = 0; k < N; k++ )
192 {
193 rotation[3*i+j] /*[i][j]*/ += invMatrix[j * N + k] * imgVectors[i * N + k];
194 }
195 }
196 }
197
198 float inorm =
199 rotation[0] /*[0][0]*/ * rotation[0] /*[0][0]*/ +
200 rotation[1] /*[0][1]*/ * rotation[1] /*[0][1]*/ +
201 rotation[2] /*[0][2]*/ * rotation[2] /*[0][2]*/;
202
203 float jnorm =
204 rotation[3] /*[1][0]*/ * rotation[3] /*[1][0]*/ +
205 rotation[4] /*[1][1]*/ * rotation[4] /*[1][1]*/ +
206 rotation[5] /*[1][2]*/ * rotation[5] /*[1][2]*/;
207
208 const float invInorm = cvInvSqrt( inorm );
209 const float invJnorm = cvInvSqrt( jnorm );
210
211 inorm *= invInorm;
212 jnorm *= invJnorm;
213
214 rotation[0] /*[0][0]*/ *= invInorm;
215 rotation[1] /*[0][1]*/ *= invInorm;
216 rotation[2] /*[0][2]*/ *= invInorm;
217
218 rotation[3] /*[1][0]*/ *= invJnorm;
219 rotation[4] /*[1][1]*/ *= invJnorm;
220 rotation[5] /*[1][2]*/ *= invJnorm;
221
222 /* row2 = row0 x row1 (cross product) */
223 rotation[6] /*->m[2][0]*/ = rotation[1] /*->m[0][1]*/ * rotation[5] /*->m[1][2]*/ -
224 rotation[2] /*->m[0][2]*/ * rotation[4] /*->m[1][1]*/;
225
226 rotation[7] /*->m[2][1]*/ = rotation[2] /*->m[0][2]*/ * rotation[3] /*->m[1][0]*/ -
227 rotation[0] /*->m[0][0]*/ * rotation[5] /*->m[1][2]*/;
228
229 rotation[8] /*->m[2][2]*/ = rotation[0] /*->m[0][0]*/ * rotation[4] /*->m[1][1]*/ -
230 rotation[1] /*->m[0][1]*/ * rotation[3] /*->m[1][0]*/;
231
232 scale = (inorm + jnorm) / 2.0f;
233 inv_Z = scale * inv_focalLength;
234
235 count++;
236 converged = ((criteria.type & CV_TERMCRIT_EPS) && (diff < criteria.epsilon));
237 converged |= ((criteria.type & CV_TERMCRIT_ITER) && (count == criteria.max_iter));
238 }
239 const float invScale = 1 / scale;
240 translation[0] = imagePoints[0].x * invScale;
241 translation[1] = imagePoints[0].y * invScale;
242 translation[2] = 1 / inv_Z;
243
244 return CV_NO_ERR;
245 }
246
247
icvReleasePOSITObject(CvPOSITObject ** ppObject)248 static CvStatus icvReleasePOSITObject( CvPOSITObject ** ppObject )
249 {
250 cvFree( ppObject );
251 return CV_NO_ERR;
252 }
253
254 /*F///////////////////////////////////////////////////////////////////////////////////////
255 // Name: icvPseudoInverse3D
256 // Purpose: Pseudoinverse N x 3 matrix N >= 3
257 // Context:
258 // Parameters:
259 // a - input matrix
260 // b - pseudoinversed a
261 // n - number of rows in a
262 // method - if 0, then b = inv(transpose(a)*a) * transpose(a)
263 // if 1, then SVD used.
264 // Returns:
265 // Notes: Both matrix are stored by n-dimensional vectors.
266 // Now only method == 0 supported.
267 //F*/
268 void
icvPseudoInverse3D(float * a,float * b,int n,int method)269 icvPseudoInverse3D( float *a, float *b, int n, int method )
270 {
271 if( method == 0 )
272 {
273 float ata00 = 0;
274 float ata11 = 0;
275 float ata22 = 0;
276 float ata01 = 0;
277 float ata02 = 0;
278 float ata12 = 0;
279
280 int k;
281 /* compute matrix ata = transpose(a) * a */
282 for( k = 0; k < n; k++ )
283 {
284 float a0 = a[k];
285 float a1 = a[n + k];
286 float a2 = a[2 * n + k];
287
288 ata00 += a0 * a0;
289 ata11 += a1 * a1;
290 ata22 += a2 * a2;
291
292 ata01 += a0 * a1;
293 ata02 += a0 * a2;
294 ata12 += a1 * a2;
295 }
296 /* inverse matrix ata */
297 {
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 float det = 0;
307 det += ata00 * p00;
308 det += ata01 * p01;
309 det += ata02 * p02;
310
311 const float inv_det = 1 / det;
312
313 /* compute resultant matrix */
314 for( k = 0; k < n; k++ )
315 {
316 float a0 = a[k];
317 float a1 = a[n + k];
318 float a2 = a[2 * n + k];
319
320 b[k] = (p00 * a0 + p01 * a1 + p02 * a2) * inv_det;
321 b[n + k] = (p01 * a0 + p11 * a1 + p12 * a2) * inv_det;
322 b[2 * n + k] = (p02 * a0 + p12 * a1 + p22 * a2) * inv_det;
323 }
324 }
325 }
326
327 /*if ( method == 1 )
328 {
329 }
330 */
331
332 return;
333 }
334
335 CV_IMPL CvPOSITObject *
cvCreatePOSITObject(CvPoint3D32f * points,int numPoints)336 cvCreatePOSITObject( CvPoint3D32f * points, int numPoints )
337 {
338 CvPOSITObject *pObject = 0;
339 IPPI_CALL( icvCreatePOSITObject( points, numPoints, &pObject ));
340 return pObject;
341 }
342
343
344 CV_IMPL void
cvPOSIT(CvPOSITObject * pObject,CvPoint2D32f * imagePoints,double focalLength,CvTermCriteria criteria,float * rotation,float * translation)345 cvPOSIT( CvPOSITObject * pObject, CvPoint2D32f * imagePoints,
346 double focalLength, CvTermCriteria criteria,
347 float* rotation, float* translation )
348 {
349 IPPI_CALL( icvPOSIT( pObject, imagePoints,(float) focalLength, criteria,
350 rotation, translation ));
351 }
352
353 CV_IMPL void
cvReleasePOSITObject(CvPOSITObject ** ppObject)354 cvReleasePOSITObject( CvPOSITObject ** ppObject )
355 {
356 IPPI_CALL( icvReleasePOSITObject( ppObject ));
357 }
358
359 /* End of file. */
360