Code cleanup: silence some -Wnarrowing warnings from C++11
[blender.git] / extern / libmv / libmv / simple_pipeline / detect.cc
1 /****************************************************************************
2 **
3 ** Copyright (c) 2011 libmv authors.
4 **
5 ** Permission is hereby granted, free of charge, to any person obtaining a copy
6 ** of this software and associated documentation files (the "Software"), to
7 ** deal in the Software without restriction, including without limitation the
8 ** rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
9 ** sell copies of the Software, and to permit persons to whom the Software is
10 ** furnished to do so, subject to the following conditions:
11 **
12 ** The above copyright notice and this permission notice shall be included in
13 ** all copies or substantial portions of the Software.
14 **
15 ** THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
16 ** IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
17 ** FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
18 ** AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
19 ** LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
20 ** FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
21 ** IN THE SOFTWARE.
22 **
23 ****************************************************************************/
24
25 #include "libmv/simple_pipeline/detect.h"
26 #include <third_party/fast/fast.h>
27 #include <stdlib.h>
28 #include <memory.h>
29
30 #ifdef __SSE2__
31 #include <emmintrin.h>
32 #endif
33
34 namespace libmv {
35
36 typedef unsigned int uint;
37
38 int featurecmp(const void *a_v, const void *b_v)
39 {
40   Feature *a = (Feature*)a_v;
41   Feature *b = (Feature*)b_v;
42
43   return b->score - a->score;
44 }
45
46 std::vector<Feature> DetectFAST(const unsigned char* data, int width, int height, int stride,
47                            int min_trackness, int min_distance) {
48   std::vector<Feature> features;
49   // TODO(MatthiasF): Support targetting a feature count (binary search trackness)
50   int num_features;
51   xy* all = fast9_detect(data, width, height,
52                          stride, min_trackness, &num_features);
53   if(num_features == 0) {
54     free(all);
55     return features;
56   }
57   int* scores = fast9_score(data, stride, all, num_features, min_trackness);
58   // TODO: merge with close feature suppression
59   xy* nonmax = nonmax_suppression(all, scores, num_features, &num_features);
60   free(all);
61   // Remove too close features
62   // TODO(MatthiasF): A resolution independent parameter would be better than distance
63   // e.g. a coefficient going from 0 (no minimal distance) to 1 (optimal circle packing)
64   // FIXME(MatthiasF): this method will not necessarily give all maximum markers
65   if(num_features) {
66     Feature *all_features = new Feature[num_features];
67
68     for(int i = 0; i < num_features; ++i) {
69       Feature a = { (float)nonmax[i].x, (float)nonmax[i].y, (float)scores[i], 0 };
70       all_features[i] = a;
71     }
72
73     qsort((void *)all_features, num_features, sizeof(Feature), featurecmp);
74
75     features.reserve(num_features);
76
77     int prev_score = all_features[0].score;
78     for(int i = 0; i < num_features; ++i) {
79       bool ok = true;
80       Feature a = all_features[i];
81       if(a.score>prev_score)
82         abort();
83       prev_score = a.score;
84
85       // compare each feature against filtered set
86       for(int j = 0; j < features.size(); j++) {
87         Feature& b = features[j];
88         if ( (a.x-b.x)*(a.x-b.x)+(a.y-b.y)*(a.y-b.y) < min_distance*min_distance ) {
89           // already a nearby feature
90           ok = false;
91           break;
92         }
93       }
94
95       if(ok) {
96         // add the new feature
97         features.push_back(a);
98       }
99     }
100
101     delete [] all_features;
102   }
103   free(scores);
104   free(nonmax);
105   return features;
106 }
107
108 #ifdef __SSE2__
109 static uint SAD(const ubyte* imageA, const ubyte* imageB, int strideA, int strideB) {
110   __m128i a = _mm_setzero_si128();
111   for(int i = 0; i < 16; i++) {
112     a = _mm_adds_epu16(a, _mm_sad_epu8( _mm_loadu_si128((__m128i*)(imageA+i*strideA)),
113                                         _mm_loadu_si128((__m128i*)(imageB+i*strideB))));
114   }
115   return _mm_extract_epi16(a,0) + _mm_extract_epi16(a,4);
116 }
117 #else
118 static uint SAD(const ubyte* imageA, const ubyte* imageB, int strideA, int strideB) {
119   uint sad=0;
120   for(int i = 0; i < 16; i++) {
121     for(int j = 0; j < 16; j++) {
122       sad += abs((int)imageA[i*strideA+j] - imageB[i*strideB+j]);
123     }
124   }
125   return sad;
126 }
127 #endif
128
129 void DetectMORAVEC(ubyte* image, int stride, int width, int height, Feature* detected, int* count, int distance, ubyte* pattern) {
130   unsigned short histogram[256];
131   memset(histogram,0,sizeof(histogram));
132   ubyte* scores = new ubyte[width*height];
133   memset(scores,0,width*height);
134   const int r = 1; //radius for self similarity comparison
135   for(int y=distance; y<height-distance; y++) {
136     for(int x=distance; x<width-distance; x++) {
137       ubyte* s = &image[y*stride+x];
138       int score = // low self-similarity with overlapping patterns //OPTI: load pattern once
139           SAD(s, s-r*stride-r, stride, stride)+SAD(s, s-r*stride, stride, stride)+SAD(s, s-r*stride+r, stride, stride)+
140           SAD(s, s         -r, stride, stride)+                                   SAD(s, s         +r, stride, stride)+
141           SAD(s, s+r*stride-r, stride, stride)+SAD(s, s+r*stride, stride, stride)+SAD(s, s+r*stride+r, stride, stride);
142       score /= 256; // normalize
143       if(pattern) score -= SAD(s, pattern, stride, 16); // find only features similar to pattern
144       if(score<=16) continue; // filter very self-similar features
145       score -= 16; // translate to score/histogram values
146       if(score>255) score=255; // clip
147       ubyte* c = &scores[y*width+x];
148       for(int i=-distance; i<0; i++) {
149         for(int j=-distance; j<distance; j++) {
150           int s = c[i*width+j];
151           if(s == 0) continue;
152           if(s >= score) goto nonmax;
153           c[i*width+j]=0, histogram[s]--;
154         }
155       }
156       for(int i=0, j=-distance; j<0; j++) {
157         int s = c[i*width+j];
158         if(s == 0) continue;
159         if(s >= score) goto nonmax;
160         c[i*width+j]=0, histogram[s]--;
161       }
162       c[0] = score, histogram[score]++;
163       nonmax:;
164     }
165   }
166   int min=255, total=0;
167   for(; min>0; min--) {
168     int h = histogram[min];
169     if(total+h > *count) break;
170     total += h;
171   }
172   int i=0;
173   for(int y=16; y<height-16; y++) {
174     for(int x=16; x<width-16; x++) {
175       int s = scores[y*width+x];
176       Feature f = { (float)x+8.0f, (float)y+8.0f, (float)s, 16 };
177       if(s>min) detected[i++] = f;
178     }
179   }
180   *count = i;
181   delete[] scores;
182 }
183
184 }