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