auto import from //depot/cupcake/@135843
[android/platform/external/neven.git] / Embedded / common / src / b_BitFeatureEm / LocalScanDetector.c
1 /*
2  * Copyright (C) 2008 The Android Open Source Project
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  *      http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  */
16
17 /* ---- includes ----------------------------------------------------------- */
18
19 #include "b_BasicEm/Functions.h"
20 #include "b_BasicEm/Math.h"
21 #include "b_ImageEm/Functions.h"
22 #include "b_BitFeatureEm/LocalScanDetector.h"
23
24 /* ------------------------------------------------------------------------- */
25
26 /* ========================================================================= */
27 /*                                                                           */
28 /* ---- \ghd{ auxiliary functions } ---------------------------------------- */
29 /*                                                                           */
30 /* ========================================================================= */
31
32 /* ------------------------------------------------------------------------- */
33
34 /** applies PCA mapping 
35  *  Input and output clusters may be identical
36  */
37 void bbf_LocalScanDetector_pcaMap( struct bbs_Context* cpA,
38                                                                    const struct bbf_LocalScanDetector* ptrA, 
39                                                                    const struct bts_IdCluster2D* inClusterPtrA,
40                                                                    struct bts_IdCluster2D* outClusterPtrA )
41 {
42         bbs_DEF_fNameL( "bbf_LocalScanDetector_pcaMap" )
43
44         struct bts_Cluster2D* tmpCl1PtrL  = ( struct bts_Cluster2D* )&ptrA->tmpCluster1E;
45         struct bts_Cluster2D* tmpCl2PtrL  = ( struct bts_Cluster2D* )&ptrA->tmpCluster2E;
46         struct bts_RBFMap2D*  rbfPtrL     = ( struct bts_RBFMap2D* )&ptrA->rbfMapE;
47         struct bts_Flt16Alt2D altL;
48         uint32 outBbpL = inClusterPtrA->clusterE.bbpE;
49         uint32 iL, jL;
50
51         /* setup two equivalent clusters holding the essential (alt-free) moves to be handled by PCA */
52         bts_IdCluster2D_convertToEqivalentClusters( cpA, 
53                                                                                                 inClusterPtrA,
54                                                                                                 &ptrA->pcaClusterE,
55                                                                                                 tmpCl1PtrL,
56                                                                                                 tmpCl2PtrL );
57
58         altL = bts_Cluster2D_alt( cpA, tmpCl1PtrL, tmpCl2PtrL, bts_ALT_RIGID );
59         bts_Cluster2D_transform( cpA, tmpCl1PtrL, altL );
60         bts_RBFMap2D_compute( cpA, rbfPtrL, tmpCl2PtrL, tmpCl1PtrL );
61         bts_RBFMap2D_mapCluster( cpA, rbfPtrL, &ptrA->pcaClusterE.clusterE, tmpCl1PtrL, 6/* ! */ );
62
63         /* PCA projection: cluster1 -> cluster1 */
64         {
65                 /* mat elements: 8.8 */
66                 const int16* matPtrL = ptrA->pcaMatE.arrPtrE;
67                 
68                 /* same bbp as pca cluster */
69                 const int16* avgPtrL = ptrA->pcaAvgE.arrPtrE;
70
71                 struct bts_Int16Vec2D* vecArrL = tmpCl1PtrL->vecArrE;
72
73                 /* projected vector */
74                 int32 prjVecL[ bpi_LOCAL_SCAN_DETECTOR_MAX_PCA_DIM ];
75
76                 /* width of matrix */
77                 uint16 matWidthL = tmpCl1PtrL->sizeE * 2;
78
79                 if( ptrA->pcaDimSubSpaceE > bpi_LOCAL_SCAN_DETECTOR_MAX_PCA_DIM )
80                 {
81                         bbs_ERROR1( "%s:\nbpi_RF_LANDMARKER_MAX_PCA_DIM exceeded", fNameL );
82                         return;
83                 }
84
85                 /* forward trafo */
86                 for( iL = 0; iL < ptrA->pcaDimSubSpaceE; iL++ )
87                 {
88                         int32 sumL = 0;
89                         avgPtrL = ptrA->pcaAvgE.arrPtrE;
90                         for( jL = 0; jL < tmpCl1PtrL->sizeE; jL++ )
91                         {
92                                 sumL += matPtrL[ 0 ] * ( vecArrL[ jL ].xE - avgPtrL[ 0 ] );
93                                 sumL += matPtrL[ 1 ] * ( vecArrL[ jL ].yE - avgPtrL[ 1 ] );
94                                 avgPtrL += 2;
95                                 matPtrL += 2;
96                         }
97                         prjVecL[ iL ] = ( sumL + 128 ) >> 8;
98                 }
99
100                 matPtrL = ptrA->pcaMatE.arrPtrE;
101                 avgPtrL = ptrA->pcaAvgE.arrPtrE;
102                 vecArrL = tmpCl1PtrL->vecArrE;
103
104                 /* backward trafo */
105                 for( jL = 0; jL < tmpCl1PtrL->sizeE; jL++ )
106                 {
107                         int32 sumL = 0;
108                         for( iL = 0; iL < ptrA->pcaDimSubSpaceE; iL++ )
109                         {
110                                 sumL += matPtrL[ iL * matWidthL + 0 ] * prjVecL[ iL ];
111                         }
112
113                         vecArrL[ jL ].xE = ( ( sumL + 128 ) >> 8 ) + avgPtrL[ 0 ];
114
115                         sumL = 0;
116                         for( iL = 0; iL < ptrA->pcaDimSubSpaceE; iL++ )
117                         {
118                                 sumL += matPtrL[ iL * matWidthL + 1 ] * prjVecL[ iL ];
119                         }
120
121                         vecArrL[ jL ].yE = ( ( sumL + 128 ) >> 8 ) + avgPtrL[ 1 ];
122
123                         matPtrL += 2;
124                         avgPtrL += 2;
125                 }
126         }
127
128         /* ALT backtransformation */
129         bts_IdCluster2D_copy( cpA, outClusterPtrA, &ptrA->pcaClusterE ); 
130         bts_Cluster2D_copyTransform( cpA, &outClusterPtrA->clusterE, tmpCl1PtrL, bts_Flt16Alt2D_inverted( &altL ), outBbpL );
131 }
132
133 /* ------------------------------------------------------------------------- */
134
135 /* ========================================================================= */
136 /*                                                                           */
137 /* ---- \ghd{ constructor / destructor } ----------------------------------- */
138 /*                                                                           */
139 /* ========================================================================= */
140
141 /* ------------------------------------------------------------------------- */
142
143 void bbf_LocalScanDetector_init( struct bbs_Context* cpA,
144                                                              struct bbf_LocalScanDetector* ptrA )
145 {
146         bbs_memset16( ptrA->ftrPtrArrE, 0, bbs_SIZEOF16( ptrA->ftrPtrArrE ) );
147         bts_RBFMap2D_init( cpA, &ptrA->rbfMapE );
148         bts_Cluster2D_init( cpA, &ptrA->tmpCluster1E ); 
149         bts_Cluster2D_init( cpA, &ptrA->tmpCluster2E ); 
150         bts_Cluster2D_init( cpA, &ptrA->tmpCluster3E ); 
151         bts_Cluster2D_init( cpA, &ptrA->tmpCluster4E ); 
152         bbf_LocalScanner_init( cpA, &ptrA->scannerE );
153         bbs_Int32Arr_init( cpA, &ptrA->actArrE );
154         bbs_Int16Arr_init( cpA, &ptrA->idxArrE );
155         bbs_UInt8Arr_init( cpA, &ptrA->workImageBufE );
156         ptrA->maxImageWidthE = 0;
157         ptrA->maxImageHeightE = 0;
158
159         ptrA->patchWidthE = 0;
160         ptrA->patchHeightE = 0;
161         ptrA->scanWidthE = 0;
162         ptrA->scanHeightE = 0;
163         ptrA->scaleExpE = 0;
164         ptrA->interpolatedWarpingE = TRUE;
165         ptrA->warpScaleThresholdE = 0;
166         bts_IdCluster2D_init( cpA, &ptrA->refClusterE );
167         bts_Cluster2D_init( cpA, &ptrA->scanClusterE );
168         bbs_UInt16Arr_init( cpA, &ptrA->ftrDataArrE );
169         bbf_BitParam_init( cpA, &ptrA->bitParamE );
170         ptrA->outlierDistanceE = 0;
171         bts_IdCluster2D_init( cpA, &ptrA->pcaClusterE );
172         bbs_Int16Arr_init( cpA, &ptrA->pcaAvgE );
173         bbs_Int16Arr_init( cpA, &ptrA->pcaMatE );
174         ptrA->pcaDimSubSpaceE = 0;
175         ptrA->maxImageWidthE = 0;
176         ptrA->maxImageHeightE = 0;
177 }
178
179 /* ------------------------------------------------------------------------- */
180
181 void bbf_LocalScanDetector_exit( struct bbs_Context* cpA,
182                                                              struct bbf_LocalScanDetector* ptrA )
183 {
184         uint16 iL;
185         for( iL = 0; iL < ptrA->scanClusterE.sizeE; iL++ ) bbf_featureExit( cpA, ptrA->ftrPtrArrE[ iL ] );
186         bbs_memset16( ptrA->ftrPtrArrE, 0, bbs_SIZEOF16( ptrA->ftrPtrArrE ) );
187
188         bts_RBFMap2D_exit( cpA, &ptrA->rbfMapE );
189         bts_Cluster2D_exit( cpA, &ptrA->tmpCluster1E ); 
190         bts_Cluster2D_exit( cpA, &ptrA->tmpCluster2E ); 
191         bts_Cluster2D_exit( cpA, &ptrA->tmpCluster3E ); 
192         bts_Cluster2D_exit( cpA, &ptrA->tmpCluster4E ); 
193         bbf_LocalScanner_exit( cpA, &ptrA->scannerE );
194         bbs_Int32Arr_exit( cpA, &ptrA->actArrE );
195         bbs_Int16Arr_exit( cpA, &ptrA->idxArrE );
196         bbs_UInt8Arr_exit( cpA, &ptrA->workImageBufE );
197         ptrA->maxImageWidthE = 0;
198         ptrA->maxImageHeightE = 0;
199
200         ptrA->patchWidthE = 0;
201         ptrA->patchHeightE = 0;
202         ptrA->scanWidthE = 0;
203         ptrA->scanHeightE = 0;
204         ptrA->scaleExpE = 0;
205         ptrA->interpolatedWarpingE = TRUE;
206         ptrA->warpScaleThresholdE = 0;
207         bts_IdCluster2D_exit( cpA, &ptrA->refClusterE );
208         bts_Cluster2D_exit( cpA, &ptrA->scanClusterE );
209         bbs_UInt16Arr_exit( cpA, &ptrA->ftrDataArrE );
210         bbf_BitParam_exit( cpA, &ptrA->bitParamE );
211         ptrA->outlierDistanceE = 0;
212         bts_IdCluster2D_exit( cpA, &ptrA->pcaClusterE );
213         bbs_Int16Arr_exit( cpA, &ptrA->pcaAvgE );
214         bbs_Int16Arr_exit( cpA, &ptrA->pcaMatE );
215         ptrA->pcaDimSubSpaceE = 0;
216         ptrA->maxImageWidthE = 0;
217         ptrA->maxImageHeightE = 0;
218 }
219
220 /* ------------------------------------------------------------------------- */
221
222 /* ========================================================================= */
223 /*                                                                           */
224 /* ---- \ghd{ operators } -------------------------------------------------- */
225 /*                                                                           */
226 /* ========================================================================= */
227
228 /* ------------------------------------------------------------------------- */
229
230 void bbf_LocalScanDetector_copy( struct bbs_Context* cpA,
231                                                     struct bbf_LocalScanDetector* ptrA, 
232                                                     const struct bbf_LocalScanDetector* srcPtrA )
233 {
234         bbs_ERROR0( "bbf_LocalScanDetector_copy:\n Function is not available" );
235 }
236
237 /* ------------------------------------------------------------------------- */
238
239 flag bbf_LocalScanDetector_equal( struct bbs_Context* cpA,
240                                                      const struct bbf_LocalScanDetector* ptrA, 
241                                                      const struct bbf_LocalScanDetector* srcPtrA )
242 {
243         bbs_ERROR0( "bbf_LocalScanDetector_equal:\n Function is not available" );
244         return TRUE;
245 }
246
247 /* ------------------------------------------------------------------------- */
248
249 /* ========================================================================= */
250 /*                                                                           */
251 /* ---- \ghd{ query functions } -------------------------------------------- */
252 /*                                                                           */
253 /* ========================================================================= */
254
255 /* ------------------------------------------------------------------------- */
256
257 /* ========================================================================= */
258 /*                                                                           */
259 /* ---- \ghd{ modify functions } ------------------------------------------- */
260 /*                                                                           */
261 /* ========================================================================= */
262
263 /* ------------------------------------------------------------------------- */
264         
265 /* ========================================================================= */
266 /*                                                                           */
267 /* ---- \ghd{ I/O } -------------------------------------------------------- */
268 /*                                                                           */
269 /* ========================================================================= */
270
271 /* ------------------------------------------------------------------------- */
272         
273 uint32 bbf_LocalScanDetector_memSize( struct bbs_Context* cpA,
274                                                                       const struct bbf_LocalScanDetector* ptrA )
275 {
276         uint32 iL;
277         uint32 memSizeL = bbs_SIZEOF16( uint32 ) +
278                                           bbs_SIZEOF16( uint32 ); /* version */
279
280         memSizeL += bbs_SIZEOF16( ptrA->patchWidthE );
281         memSizeL += bbs_SIZEOF16( ptrA->patchHeightE );
282         memSizeL += bbs_SIZEOF16( ptrA->scanWidthE );
283         memSizeL += bbs_SIZEOF16( ptrA->scanHeightE );
284         memSizeL += bbs_SIZEOF16( ptrA->scaleExpE );
285         memSizeL += bbs_SIZEOF16( ptrA->interpolatedWarpingE );
286         memSizeL += bbs_SIZEOF16( ptrA->warpScaleThresholdE );
287         memSizeL += bts_IdCluster2D_memSize( cpA, &ptrA->refClusterE );
288         memSizeL += bts_Cluster2D_memSize( cpA, &ptrA->scanClusterE );
289         memSizeL += bbf_BitParam_memSize( cpA, &ptrA->bitParamE );
290         memSizeL += bbs_SIZEOF16( ptrA->outlierDistanceE );
291         memSizeL += bts_IdCluster2D_memSize( cpA, &ptrA->pcaClusterE );
292         memSizeL += bbs_Int16Arr_memSize( cpA, &ptrA->pcaAvgE );
293         memSizeL += bbs_Int16Arr_memSize( cpA, &ptrA->pcaMatE );
294         memSizeL += bbs_SIZEOF16( ptrA->pcaDimSubSpaceE );
295         memSizeL += bbs_SIZEOF16( ptrA->maxImageWidthE );
296         memSizeL += bbs_SIZEOF16( ptrA->maxImageHeightE );
297         for( iL = 0; iL < ptrA->scanClusterE.sizeE; iL++ ) memSizeL += bbf_featureMemSize( cpA, ptrA->ftrPtrArrE[ iL ] );
298
299         return memSizeL; 
300 }
301
302 /* ------------------------------------------------------------------------- */
303         
304 uint32 bbf_LocalScanDetector_memWrite( struct bbs_Context* cpA,
305                                                                            const struct bbf_LocalScanDetector* ptrA, 
306                                                                            uint16* memPtrA )
307 {
308         uint32 iL;
309         uint32 memSizeL = bbf_LocalScanDetector_memSize( cpA, ptrA );
310         memPtrA += bbs_memWrite32( &memSizeL, memPtrA );
311         memPtrA += bbs_memWriteUInt32( bbf_LOCAL_SCAN_DETECTOR_VERSION, memPtrA );
312
313         memPtrA += bbs_memWrite32( &ptrA->patchWidthE, memPtrA );
314         memPtrA += bbs_memWrite32( &ptrA->patchHeightE, memPtrA );
315         memPtrA += bbs_memWrite32( &ptrA->scanWidthE, memPtrA );
316         memPtrA += bbs_memWrite32( &ptrA->scanHeightE, memPtrA );
317         memPtrA += bbs_memWrite32( &ptrA->scaleExpE, memPtrA );
318         memPtrA += bbs_memWrite32( &ptrA->interpolatedWarpingE, memPtrA );
319         memPtrA += bbs_memWrite32( &ptrA->warpScaleThresholdE, memPtrA );
320         memPtrA += bts_IdCluster2D_memWrite( cpA, &ptrA->refClusterE, memPtrA );
321         memPtrA += bts_Cluster2D_memWrite( cpA, &ptrA->scanClusterE, memPtrA );
322         memPtrA += bbf_BitParam_memWrite( cpA, &ptrA->bitParamE, memPtrA );
323         memPtrA += bbs_memWrite32( &ptrA->outlierDistanceE, memPtrA );
324         memPtrA += bts_IdCluster2D_memWrite( cpA, &ptrA->pcaClusterE, memPtrA );
325         memPtrA += bbs_Int16Arr_memWrite( cpA, &ptrA->pcaAvgE, memPtrA );
326         memPtrA += bbs_Int16Arr_memWrite( cpA, &ptrA->pcaMatE, memPtrA );
327         memPtrA += bbs_memWrite32( &ptrA->pcaDimSubSpaceE, memPtrA );
328         memPtrA += bbs_memWrite32( &ptrA->maxImageWidthE, memPtrA );
329         memPtrA += bbs_memWrite32( &ptrA->maxImageHeightE, memPtrA );
330
331         for( iL = 0; iL < ptrA->scanClusterE.sizeE; iL++ ) memPtrA += bbf_featureMemWrite( cpA, ptrA->ftrPtrArrE[ iL ], memPtrA );
332
333         return memSizeL;
334 }
335
336 /* ------------------------------------------------------------------------- */
337
338 uint32 bbf_LocalScanDetector_memRead( struct bbs_Context* cpA,
339                                                                           struct bbf_LocalScanDetector* ptrA, 
340                                                                           const uint16* memPtrA, 
341                                                                           struct bbs_MemTbl* mtpA )
342 {
343         uint32 iL;
344         uint32 memSizeL, versionL;
345         struct bbs_MemTbl memTblL = *mtpA;
346         struct bbs_MemSeg* espL = bbs_MemTbl_segPtr( cpA, &memTblL, 0 );
347         struct bbs_MemSeg* sspL = bbs_MemTbl_sharedSegPtr( cpA, &memTblL, 0 );
348         if( bbs_Context_error( cpA ) ) return 0;
349
350         memPtrA += bbs_memRead32( &memSizeL, memPtrA );
351         memPtrA += bbs_memReadVersion32( cpA, &versionL, bbf_LOCAL_SCAN_DETECTOR_VERSION, memPtrA );
352
353
354         memPtrA += bbs_memRead32( &ptrA->patchWidthE, memPtrA );
355         memPtrA += bbs_memRead32( &ptrA->patchHeightE, memPtrA );
356         memPtrA += bbs_memRead32( &ptrA->scanWidthE, memPtrA );
357         memPtrA += bbs_memRead32( &ptrA->scanHeightE, memPtrA );
358         memPtrA += bbs_memRead32( &ptrA->scaleExpE, memPtrA );
359         memPtrA += bbs_memRead32( &ptrA->interpolatedWarpingE, memPtrA );
360         memPtrA += bbs_memRead32( &ptrA->warpScaleThresholdE, memPtrA );
361         memPtrA += bts_IdCluster2D_memRead( cpA, &ptrA->refClusterE, memPtrA, espL );
362         memPtrA += bts_Cluster2D_memRead( cpA, &ptrA->scanClusterE, memPtrA, espL );
363         memPtrA += bbf_BitParam_memRead( cpA, &ptrA->bitParamE, memPtrA );
364         memPtrA += bbs_memRead32( &ptrA->outlierDistanceE, memPtrA );
365         memPtrA += bts_IdCluster2D_memRead( cpA, &ptrA->pcaClusterE, memPtrA, espL );
366         memPtrA += bbs_Int16Arr_memRead( cpA, &ptrA->pcaAvgE, memPtrA, espL );
367         memPtrA += bbs_Int16Arr_memRead( cpA, &ptrA->pcaMatE, memPtrA, espL );
368         memPtrA += bbs_memRead32( &ptrA->pcaDimSubSpaceE, memPtrA );
369         memPtrA += bbs_memRead32( &ptrA->maxImageWidthE, memPtrA );
370         memPtrA += bbs_memRead32( &ptrA->maxImageHeightE, memPtrA );
371
372         /* check features & allocate data buffer */
373         {
374                 const uint16* memPtrL = memPtrA;
375                 uint32 dataSizeL = 0;
376                 for( iL = 0; iL < ptrA->scanClusterE.sizeE; iL++ )
377                 {
378                         enum bbf_FeatureType typeL = ( enum bbf_FeatureType )bbs_memPeek32( memPtrL + 4 );
379                         dataSizeL += bbf_featureSizeOf16( cpA, typeL );
380                         memPtrL += bbs_memPeek32( memPtrL );
381                 }
382                 bbs_UInt16Arr_create( cpA, &ptrA->ftrDataArrE, dataSizeL, espL );
383         }
384
385         /* load features & initialize pointers */
386         {
387                 uint16* dataPtrL = ptrA->ftrDataArrE.arrPtrE;
388                 for( iL = 0; iL < ptrA->scanClusterE.sizeE; iL++ )
389                 {
390                         enum bbf_FeatureType typeL = ( enum bbf_FeatureType )bbs_memPeek32( memPtrA + 4 );
391                         ptrA->ftrPtrArrE[ iL ] = ( struct bbf_Feature* )dataPtrL;
392                         bbf_featureInit( cpA, ptrA->ftrPtrArrE[ iL ], typeL );
393                         memPtrA += bbf_featureMemRead( cpA, ptrA->ftrPtrArrE[ iL ], memPtrA, &memTblL );
394                         dataPtrL += bbf_featureSizeOf16( cpA, typeL );
395                 }
396         }
397
398         if( memSizeL != bbf_LocalScanDetector_memSize( cpA, ptrA ) )
399         {
400                 bbs_ERR0( bbs_ERR_CORRUPT_DATA, "uint32 bbf_LocalScanDetector_memRead( struct bem_ScanGradientMove* ptrA, const uint16* memPtrA ):\n"
401                                 "size mismatch" );
402                 return 0;
403         }
404
405         if( ptrA->maxImageWidthE * ptrA->maxImageHeightE == 0 )
406         {
407                 bbs_ERR0( bbs_ERR_CORRUPT_DATA, "uint32 bbf_LocalScanDetector_memRead( struct bem_ScanGradientMove* ptrA, const uint16* memPtrA ):\n"
408                                                                         "maximum image width/height not set" );
409                 return 0;
410         }
411
412         /* initialize internal data */
413
414         /* ought to be placed on shared memory later */
415         bts_RBFMap2D_create( cpA, &ptrA->rbfMapE, bpi_LOCAL_SCAN_DETECTOR_MAX_CLUSTER_SIZE, sspL );
416         ptrA->rbfMapE.RBFTypeE = bts_RBF_LINEAR;
417         ptrA->rbfMapE.altTypeE = bts_ALT_RIGID;
418
419         bts_Cluster2D_create( cpA, &ptrA->tmpCluster1E, bpi_LOCAL_SCAN_DETECTOR_MAX_CLUSTER_SIZE, sspL ); 
420         bts_Cluster2D_create( cpA, &ptrA->tmpCluster2E, bpi_LOCAL_SCAN_DETECTOR_MAX_CLUSTER_SIZE, sspL );
421         bts_Cluster2D_create( cpA, &ptrA->tmpCluster3E, bpi_LOCAL_SCAN_DETECTOR_MAX_CLUSTER_SIZE, sspL );
422         bts_Cluster2D_create( cpA, &ptrA->tmpCluster4E, bpi_LOCAL_SCAN_DETECTOR_MAX_CLUSTER_SIZE, sspL );
423
424         bbs_Int32Arr_create( cpA, &ptrA->actArrE, bpi_LOCAL_SCAN_DETECTOR_MAX_CLUSTER_SIZE, sspL );
425         bbs_Int16Arr_create( cpA, &ptrA->idxArrE, bpi_LOCAL_SCAN_DETECTOR_MAX_CLUSTER_SIZE, sspL );
426
427         /* working image memory */
428         /* ought to be placed on shared memory later */
429         bbs_UInt8Arr_create( cpA, &ptrA->workImageBufE, ptrA->maxImageWidthE * ptrA->maxImageHeightE, sspL );
430
431         /* initialize local scanner (be aware of shared memory usage when moving this create function) */
432         bbf_LocalScanner_create( cpA, &ptrA->scannerE,
433                                                          ptrA->patchWidthE,
434                                                          ptrA->patchHeightE,
435                                                          ptrA->scaleExpE,
436                                                          ptrA->maxImageWidthE,
437                                                          ptrA->maxImageHeightE,
438                                                          ptrA->scaleExpE,
439                                                          ptrA->bitParamE.outerRadiusE,
440                                                          &memTblL );
441
442         return memSizeL;
443 }
444
445 /* ------------------------------------------------------------------------- */
446         
447 /* ========================================================================= */
448 /*                                                                           */
449 /* ---- \ghd{ exec functions } --------------------------------------------- */
450 /*                                                                           */
451 /* ========================================================================= */
452         
453 /* ------------------------------------------------------------------------- */
454
455 int32 bbf_LocalScanDetector_process( struct bbs_Context* cpA,
456                                                                          const struct bbf_LocalScanDetector* ptrA, 
457                                      uint8* imagePtrA, 
458                                                                          uint32 imageWidthA,
459                                                                          uint32 imageHeightA,
460                                                                          const struct bts_Int16Vec2D*  offsPtrA,
461                                                                          const struct bts_IdCluster2D* inClusterPtrA,
462                                                                          struct bts_IdCluster2D* outClusterPtrA )
463 {
464         bbs_DEF_fNameL( "bbf_LocalScanDetector_process" )
465
466         int32 pw0L = ptrA->patchWidthE;
467         int32 ph0L = ptrA->patchHeightE;
468         int32 pw1L = pw0L << ptrA->scaleExpE;
469         int32 ph1L = ph0L << ptrA->scaleExpE;
470
471         struct bts_Cluster2D* wrkClPtrL  = ( struct bts_Cluster2D* )&ptrA->tmpCluster1E;
472         struct bts_Cluster2D* refClPtrL  = ( struct bts_Cluster2D* )&ptrA->tmpCluster2E;
473         struct bts_Cluster2D* dstClPtrL  = ( struct bts_Cluster2D* )&ptrA->tmpCluster3E;
474         struct bts_Cluster2D* tmpClPtrL  = ( struct bts_Cluster2D* )&ptrA->tmpCluster4E;
475         struct bts_RBFMap2D*  rbfPtrL    = ( struct bts_RBFMap2D* )&ptrA->rbfMapE;
476         struct bbf_LocalScanner* scnPtrL = ( struct bbf_LocalScanner* )&ptrA->scannerE;
477
478         int32* actArrL = ( int32* )ptrA->actArrE.arrPtrE;
479         int16* idxArrL = ( int16* )ptrA->idxArrE.arrPtrE;
480
481         uint32 workImageWidthL, workImageHeightL;
482
483         struct bts_Flt16Alt2D altL;
484
485         int32 confidenceL;
486         uint32 iL;
487         uint32 sizeL = ptrA->scanClusterE.sizeE;
488
489         if( sizeL > bpi_LOCAL_SCAN_DETECTOR_MAX_CLUSTER_SIZE )
490         {
491                 bbs_ERROR1( "%s:\nScan cluster size exceeds bpi_LOCAL_SCAN_DETECTOR_MAX_CLUSTER_SIZE", fNameL );
492                 return 0;
493         }
494
495         /* compute equivalent clusters (matching ids) from input and reference cluster */
496         bts_IdCluster2D_convertToEqivalentClusters( cpA, inClusterPtrA, &ptrA->refClusterE, wrkClPtrL, refClPtrL );
497
498         /* altL: orig image -> normalized image */
499         altL = bts_Cluster2D_alt( cpA, wrkClPtrL, refClPtrL, bts_ALT_RIGID );
500
501         /* transorm work cluster to normalized image */
502         bts_Cluster2D_transformBbp( cpA, wrkClPtrL, altL, 6 );
503
504         /* map: ref cluster -> work cluster */
505         bts_RBFMap2D_compute( cpA, rbfPtrL, refClPtrL, wrkClPtrL );
506
507         /* copy: scanClusterE -> work cluster */
508         bts_Cluster2D_copy( cpA, wrkClPtrL, &ptrA->scanClusterE );
509
510         /* copy: refClusterE -> ref cluster */
511         bts_Cluster2D_copy( cpA, refClPtrL, &ptrA->refClusterE.clusterE );
512
513         /* apply map to work cluster */
514         bts_Cluster2D_rbfTransform( cpA, wrkClPtrL, rbfPtrL );
515
516         /* apply map to ref cluster */
517         bts_Cluster2D_rbfTransform( cpA, refClPtrL, rbfPtrL );
518
519         {
520                 /* analyze boundaries; get exact dimensions of working image */
521                 int32 workBorderWL = ( ( ptrA->scanWidthE  + pw1L + 1 ) >> 1 ) + 1; /* add a pixel to ensure full search area */
522                 int32 workBorderHL = ( ( ptrA->scanHeightE + ph1L + 1 ) >> 1 ) + 1; /* add a pixel to ensure full search area */
523                 struct bts_Int16Rect workAreaL = bts_Cluster2D_boundingBox( cpA, wrkClPtrL );
524                 workAreaL.x1E = workAreaL.x1E >> wrkClPtrL->bbpE;
525                 workAreaL.y1E = workAreaL.y1E >> wrkClPtrL->bbpE;
526                 workAreaL.x2E = workAreaL.x2E >> wrkClPtrL->bbpE;
527                 workAreaL.y2E = workAreaL.y2E >> wrkClPtrL->bbpE;
528                 workAreaL.x1E -= workBorderWL;
529                 workAreaL.y1E -= workBorderHL;
530                 workAreaL.x2E += workBorderWL;
531                 workAreaL.y2E += workBorderHL;
532
533                 workImageWidthL  = workAreaL.x2E - workAreaL.x1E;
534                 workImageHeightL = workAreaL.y2E - workAreaL.y1E;
535
536                 /* truncate if necessary (should not occur in normal operation) */
537                 workImageWidthL = workImageWidthL > ptrA->maxImageWidthE ? ptrA->maxImageWidthE : workImageWidthL;
538                 workImageHeightL = workImageHeightL > ptrA->maxImageHeightE ? ptrA->maxImageHeightE : workImageHeightL;
539
540                 /* adjust ALT */
541                 altL.vecE.xE -= workAreaL.x1E << altL.vecE.bbpE;
542                 altL.vecE.yE -= workAreaL.y1E << altL.vecE.bbpE;
543
544                 /* adjust work cluster */
545                 for( iL = 0; iL < wrkClPtrL->sizeE; iL++ )
546                 {
547                         wrkClPtrL->vecArrE[ iL ].xE -= workAreaL.x1E << wrkClPtrL->bbpE;
548                         wrkClPtrL->vecArrE[ iL ].yE -= workAreaL.y1E << wrkClPtrL->bbpE;
549                 }
550
551                 /* adjust ref cluster */
552                 for( iL = 0; iL < wrkClPtrL->sizeE; iL++ )
553                 {
554                         refClPtrL->vecArrE[ iL ].xE -= workAreaL.x1E << refClPtrL->bbpE;
555                         refClPtrL->vecArrE[ iL ].yE -= workAreaL.y1E << refClPtrL->bbpE;
556                 }
557
558                 /* transform image */
559                 bim_filterWarp( cpA, 
560                                             ptrA->workImageBufE.arrPtrE, 
561                                                 imagePtrA, imageWidthA, imageHeightA, 
562                                                 offsPtrA,
563                                                 &altL, 
564                                                 workImageWidthL, workImageHeightL, 
565                                                 NULL, 
566                                                 ptrA->warpScaleThresholdE, 
567                                                 ptrA->interpolatedWarpingE );
568
569         }
570
571         /* scan over all positions of work cluster; target positions are stored in *dstClPtrL*/
572         {
573                 int32 regionWHL = ( ptrA->scanWidthE  + pw1L + 1 ) >> 1;
574                 int32 regionHHL = ( ptrA->scanHeightE + ph1L + 1 ) >> 1;
575                 struct bts_Int16Vec2D* srcVecArrL = wrkClPtrL->vecArrE;
576                 struct bts_Int16Vec2D* dstVecArrL = dstClPtrL->vecArrE;
577                 int32 vecBbpL = wrkClPtrL->bbpE;
578                 bts_Cluster2D_size( cpA, dstClPtrL, sizeL );
579                 dstClPtrL->bbpE = vecBbpL;
580
581                 /* initialize scanner */
582                 scnPtrL->patchWidthE = ptrA->patchWidthE;
583                 scnPtrL->patchHeightE = ptrA->patchWidthE;
584                 scnPtrL->scaleExpE = ptrA->scaleExpE;
585
586                 bbf_LocalScanner_assign( cpA, scnPtrL, ptrA->workImageBufE.arrPtrE, workImageWidthL, workImageHeightL, &ptrA->bitParamE );
587
588                 bbs_memset32( actArrL, 0x80000000, sizeL );
589
590                 do
591                 {
592                         for( iL = 0; iL < sizeL; iL++ )
593                         {
594                                 int32 bestActL = 0x80000000;
595                                 uint32 bestIdxL = 0;
596                                 struct bbf_Feature* ftrPtrL = ptrA->ftrPtrArrE[ iL ];
597
598                                 /* set scan region */
599                                 {
600                                         int32 x0L = ( ( wrkClPtrL->vecArrE[ iL ].xE >> ( wrkClPtrL->bbpE - 1 ) ) + 1 ) >> 1;
601                                         int32 y0L = ( ( wrkClPtrL->vecArrE[ iL ].yE >> ( wrkClPtrL->bbpE - 1 ) ) + 1 ) >> 1;
602                                         struct bts_Int16Rect scanRegionL = bts_Int16Rect_create( x0L - regionWHL, y0L - regionHHL, x0L + regionWHL, y0L + regionHHL );
603                                         bbf_LocalScanner_origScanRegion( cpA, scnPtrL, &scanRegionL );
604                                 }
605
606                                 do
607                                 {
608                                         int32 actL = ftrPtrL->vpActivityE( ftrPtrL, bbf_LocalScanner_getPatch( scnPtrL ) );
609
610                                         if( actL > bestActL )
611                                         {
612                                                 bestActL = actL;
613                                                 bestIdxL = bbf_LocalScanner_scanIndex( scnPtrL );
614                                         }
615                                 }
616                                 while( bbf_LocalScanner_next( cpA, scnPtrL ) );
617
618                                 {
619                                         int32 xL, yL; /* 16.16 */
620                                         bbf_LocalScanner_idxPos( scnPtrL, bestIdxL, &xL, &yL );
621                                         xL += pw1L << 15;
622                                         yL += ph1L << 15;
623                                         if( bestActL > actArrL[ iL ] )
624                                         {
625                                                 dstVecArrL[ iL ].xE = ( ( xL >> ( 15 - vecBbpL ) ) + 1 ) >> 1;
626                                                 dstVecArrL[ iL ].yE = ( ( yL >> ( 15 - vecBbpL ) ) + 1 ) >> 1;
627                                                 actArrL[ iL ] = bestActL;
628                                         }
629                                 }
630                         }
631                 }
632                 while( bbf_LocalScanner_nextOffset( cpA, scnPtrL ) );
633
634                 /* outlier analysis: outliers are disabled by setting their similarity to -1 */
635                 if( ptrA->outlierDistanceE > 0 )
636                 {
637                         /* altL: work cluster -> ref cluster */
638                         struct bts_Flt16Alt2D localAltL = bts_Cluster2D_alt( cpA, wrkClPtrL, dstClPtrL, bts_ALT_RIGID );
639
640                         /* squared distance 16.16 */
641                         uint32 dist2L = ( ptrA->outlierDistanceE >> 8 ) * ( ptrA->outlierDistanceE >> 8 );
642
643                         /* analyze deviations */
644                         for( iL = 0; iL < sizeL; iL++ )
645                         {
646                                 struct bts_Flt16Vec2D vecL = bts_Flt16Vec2D_create32( srcVecArrL[ iL ].xE, srcVecArrL[ iL ].yE, vecBbpL );
647                                 uint32 dev2L; /* squared deviation 16.16 */
648                                 vecL = bts_Flt16Alt2D_mapFlt( &localAltL, &vecL );
649                                 vecL = bts_Flt16Vec2D_sub( vecL, bts_Flt16Vec2D_create32( dstVecArrL[ iL ].xE, dstVecArrL[ iL ].yE, vecBbpL ) );
650                                 dev2L = bbs_convertU32( bts_Flt16Vec2D_norm2( &vecL ), vecL.bbpE << 1, 16 );
651                                 if( dev2L > dist2L ) actArrL[ iL ] = 0xF0000000;
652                         }
653                 }
654
655                 /* remove undetected positions but keep at least 1/2 best positions */
656                 {
657                         flag sortedL;
658
659                         /* bubble sort (no speed issue in this case) */
660                         for( iL = 0; iL < sizeL; iL++ ) idxArrL[ iL ] = iL;
661
662                         do
663                         {
664                                 sortedL = TRUE;
665                                 for( iL = 1; iL < sizeL; iL++ )
666                                 {
667                                         if( actArrL[ idxArrL[ iL - 1 ] ] < actArrL[ idxArrL[ iL ] ] )
668                                         {
669                                                 int16 tmpL = idxArrL[ iL - 1 ];
670                                                 idxArrL[ iL - 1 ] = idxArrL[ iL ];
671                                                 idxArrL[ iL ] = tmpL;
672                                                 sortedL = FALSE;
673                                         }
674                                 }
675                         }
676                         while( !sortedL );
677
678                         for( iL = ( sizeL >> 1 ); iL < sizeL && actArrL[ idxArrL[ iL ] ] >= 0; iL++ );
679
680                         {
681                                 uint32 subSizeL = iL;
682
683                                 /* reorder clusters */
684                                 bts_Cluster2D_size( cpA, tmpClPtrL, subSizeL );
685                                 {
686                                         struct bts_Int16Vec2D* tmpVecArrL = tmpClPtrL->vecArrE;
687                                         for( iL = 0; iL < subSizeL; iL++ ) tmpVecArrL[ iL ] = srcVecArrL[ idxArrL[ iL ] ];
688                                         for( iL = 0; iL < subSizeL; iL++ ) srcVecArrL[ iL ] = tmpVecArrL[ iL ];
689                                         for( iL = 0; iL < subSizeL; iL++ ) tmpVecArrL[ iL ] = dstVecArrL[ idxArrL[ iL ] ];
690                                         for( iL = 0; iL < subSizeL; iL++ ) dstVecArrL[ iL ] = tmpVecArrL[ iL ];
691                                 }
692                                 bts_Cluster2D_size( cpA, wrkClPtrL, subSizeL );
693                                 bts_Cluster2D_size( cpA, dstClPtrL, subSizeL );
694                         }
695                 }
696
697                 /* compute confidence */
698                 {
699                         int16* idxArrL = ptrA->idxArrE.arrPtrE;
700                         int32* actArrL = ptrA->actArrE.arrPtrE;
701                         int32 actSumL = 0; /* .20 */
702                         for( iL = 0; iL < sizeL; iL++ )
703                         {
704                                 float actL = ( actArrL[ idxArrL[ iL ] ] + 128 ) >> 8;
705                                 if( actL < 0 ) break;
706                                 actSumL += actL;
707                         }
708
709                         /* actSumL = average positive activity */
710                         actSumL = ( iL > 0 ) ? actSumL / iL : 0;
711
712                         confidenceL = ( ( ( int32 )iL << 20 ) - ( ( ( int32 )1 << 20 ) - actSumL ) ) / sizeL;
713
714                         /* adjust to 4.28 */
715                         confidenceL <<= 8;
716                 }
717
718         }
719
720         /* map: wrkCluster -> dstCluster */
721         bts_RBFMap2D_compute( cpA, rbfPtrL, wrkClPtrL, dstClPtrL );
722
723         /* apply map to ref cluster */
724         bts_Cluster2D_rbfTransform( cpA, refClPtrL, rbfPtrL );
725
726         /* copy ref cluster to outCluster */
727         bts_Cluster2D_copy( cpA, &outClusterPtrA->clusterE, refClPtrL );
728         bbs_Int16Arr_copy( cpA, &outClusterPtrA->idArrE, &ptrA->refClusterE.idArrE );
729
730         /* PCA Mapping */
731         if( ptrA->pcaDimSubSpaceE > 0 )
732         {
733                 bbf_LocalScanDetector_pcaMap( cpA, ptrA, outClusterPtrA, outClusterPtrA );
734         }
735
736         /* backtransform out cluster to original image */
737         bts_Cluster2D_transformBbp( cpA, &outClusterPtrA->clusterE, bts_Flt16Alt2D_inverted( &altL ), inClusterPtrA->clusterE.bbpE );
738
739         return confidenceL;
740 }
741
742 /* ------------------------------------------------------------------------- */
743
744 /* ========================================================================= */
745