1 /* Copyright 2016 The TensorFlow Authors. All Rights Reserved.
2
3 Licensed under the Apache License, Version 2.0 (the "License");
4 you may not use this file except in compliance with the License.
5 You may obtain a copy of the License at
6
7 http://www.apache.org/licenses/LICENSE-2.0
8
9 Unless required by applicable law or agreed to in writing, software
10 distributed under the License is distributed on an "AS IS" BASIS,
11 WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 See the License for the specific language governing permissions and
13 limitations under the License.
14 ==============================================================================*/
15
16 #ifndef TENSORFLOW_EXAMPLES_ANDROID_JNI_OBJECT_TRACKING_UTILS_H_
17 #define TENSORFLOW_EXAMPLES_ANDROID_JNI_OBJECT_TRACKING_UTILS_H_
18
19 #include <math.h>
20 #include <stdint.h>
21 #include <stdlib.h>
22 #include <time.h>
23
24 #include <cmath> // for std::abs(float)
25
26 #ifndef HAVE_CLOCK_GETTIME
27 // Use gettimeofday() instead of clock_gettime().
28 #include <sys/time.h>
29 #endif // ifdef HAVE_CLOCK_GETTIME
30
31 #include "tensorflow/examples/android/jni/object_tracking/logging.h"
32
33 // TODO(andrewharp): clean up these macros to use the codebase statndard.
34
35 // A very small number, generally used as the tolerance for accumulated
36 // floating point errors in bounds-checks.
37 #define EPSILON 0.00001f
38
39 #define SAFE_DELETE(pointer) {\
40 if ((pointer) != NULL) {\
41 LOGV("Safe deleting pointer: %s", #pointer);\
42 delete (pointer);\
43 (pointer) = NULL;\
44 } else {\
45 LOGV("Pointer already null: %s", #pointer);\
46 }\
47 }
48
49
50 #ifdef __GOOGLE__
51
52 #define CHECK_ALWAYS(condition, format, ...) {\
53 CHECK(condition) << StringPrintf(format, ##__VA_ARGS__);\
54 }
55
56 #define SCHECK(condition, format, ...) {\
57 DCHECK(condition) << StringPrintf(format, ##__VA_ARGS__);\
58 }
59
60 #else
61
62 #define CHECK_ALWAYS(condition, format, ...) {\
63 if (!(condition)) {\
64 LOGE("CHECK FAILED (%s): " format, #condition, ##__VA_ARGS__);\
65 abort();\
66 }\
67 }
68
69 #ifdef SANITY_CHECKS
70 #define SCHECK(condition, format, ...) {\
71 CHECK_ALWAYS(condition, format, ##__VA_ARGS__);\
72 }
73 #else
74 #define SCHECK(condition, format, ...) {}
75 #endif // SANITY_CHECKS
76
77 #endif // __GOOGLE__
78
79
80 #ifndef MAX
81 #define MAX(a, b) (((a) > (b)) ? (a) : (b))
82 #endif
83 #ifndef MIN
84 #define MIN(a, b) (((a) > (b)) ? (b) : (a))
85 #endif
86
CurrentThreadTimeNanos()87 inline static int64_t CurrentThreadTimeNanos() {
88 #ifdef HAVE_CLOCK_GETTIME
89 struct timespec tm;
90 clock_gettime(CLOCK_THREAD_CPUTIME_ID, &tm);
91 return tm.tv_sec * 1000000000LL + tm.tv_nsec;
92 #else
93 struct timeval tv;
94 gettimeofday(&tv, NULL);
95 return tv.tv_sec * 1000000000 + tv.tv_usec * 1000;
96 #endif
97 }
98
CurrentRealTimeMillis()99 inline static int64_t CurrentRealTimeMillis() {
100 #ifdef HAVE_CLOCK_GETTIME
101 struct timespec tm;
102 clock_gettime(CLOCK_MONOTONIC, &tm);
103 return tm.tv_sec * 1000LL + tm.tv_nsec / 1000000LL;
104 #else
105 struct timeval tv;
106 gettimeofday(&tv, NULL);
107 return tv.tv_sec * 1000 + tv.tv_usec / 1000;
108 #endif
109 }
110
111
112 template<typename T>
Square(const T a)113 inline static T Square(const T a) {
114 return a * a;
115 }
116
117
118 template<typename T>
Clip(const T a,const T floor,const T ceil)119 inline static T Clip(const T a, const T floor, const T ceil) {
120 SCHECK(ceil >= floor, "Bounds mismatch!");
121 return (a <= floor) ? floor : ((a >= ceil) ? ceil : a);
122 }
123
124
125 template<typename T>
Floor(const T a)126 inline static int Floor(const T a) {
127 return static_cast<int>(a);
128 }
129
130
131 template<typename T>
Ceil(const T a)132 inline static int Ceil(const T a) {
133 return Floor(a) + 1;
134 }
135
136
137 template<typename T>
InRange(const T a,const T min,const T max)138 inline static bool InRange(const T a, const T min, const T max) {
139 return (a >= min) && (a <= max);
140 }
141
142
ValidIndex(const int a,const int max)143 inline static bool ValidIndex(const int a, const int max) {
144 return (a >= 0) && (a < max);
145 }
146
147
NearlyEqual(const float a,const float b,const float tolerance)148 inline bool NearlyEqual(const float a, const float b, const float tolerance) {
149 return std::abs(a - b) < tolerance;
150 }
151
152
NearlyEqual(const float a,const float b)153 inline bool NearlyEqual(const float a, const float b) {
154 return NearlyEqual(a, b, EPSILON);
155 }
156
157
158 template<typename T>
Round(const float a)159 inline static int Round(const float a) {
160 return (a - static_cast<float>(floor(a) > 0.5f) ? ceil(a) : floor(a));
161 }
162
163
164 template<typename T>
Swap(T * const a,T * const b)165 inline static void Swap(T* const a, T* const b) {
166 // Cache out the VALUE of what's at a.
167 T tmp = *a;
168 *a = *b;
169
170 *b = tmp;
171 }
172
173
randf()174 static inline float randf() {
175 return rand() / static_cast<float>(RAND_MAX);
176 }
177
randf(const float min_value,const float max_value)178 static inline float randf(const float min_value, const float max_value) {
179 return randf() * (max_value - min_value) + min_value;
180 }
181
RealToFixed115(const float real_number)182 static inline uint16_t RealToFixed115(const float real_number) {
183 SCHECK(InRange(real_number, 0.0f, 2048.0f),
184 "Value out of range! %.2f", real_number);
185
186 static const float kMult = 32.0f;
187 const float round_add = (real_number > 0.0f) ? 0.5f : -0.5f;
188 return static_cast<uint16_t>(real_number * kMult + round_add);
189 }
190
FixedToFloat115(const uint16_t fp_number)191 static inline float FixedToFloat115(const uint16_t fp_number) {
192 const float kDiv = 32.0f;
193 return (static_cast<float>(fp_number) / kDiv);
194 }
195
RealToFixed1616(const float real_number)196 static inline int RealToFixed1616(const float real_number) {
197 static const float kMult = 65536.0f;
198 SCHECK(InRange(real_number, -kMult, kMult),
199 "Value out of range! %.2f", real_number);
200
201 const float round_add = (real_number > 0.0f) ? 0.5f : -0.5f;
202 return static_cast<int>(real_number * kMult + round_add);
203 }
204
FixedToFloat1616(const int fp_number)205 static inline float FixedToFloat1616(const int fp_number) {
206 const float kDiv = 65536.0f;
207 return (static_cast<float>(fp_number) / kDiv);
208 }
209
210 template<typename T>
211 // produces numbers in range [0,2*M_PI] (rather than -PI,PI)
FastAtan2(const T y,const T x)212 inline T FastAtan2(const T y, const T x) {
213 static const T coeff_1 = (T)(M_PI / 4.0);
214 static const T coeff_2 = (T)(3.0 * coeff_1);
215 const T abs_y = fabs(y);
216 T angle;
217 if (x >= 0) {
218 T r = (x - abs_y) / (x + abs_y);
219 angle = coeff_1 - coeff_1 * r;
220 } else {
221 T r = (x + abs_y) / (abs_y - x);
222 angle = coeff_2 - coeff_1 * r;
223 }
224 static const T PI_2 = 2.0 * M_PI;
225 return y < 0 ? PI_2 - angle : angle;
226 }
227
228 #define NELEMS(X) (sizeof(X) / sizeof(X[0]))
229
230 namespace tf_tracking {
231
232 #ifdef __ARM_NEON
233 float ComputeMeanNeon(const float* const values, const int num_vals);
234
235 float ComputeStdDevNeon(const float* const values, const int num_vals,
236 const float mean);
237
238 float ComputeWeightedMeanNeon(const float* const values,
239 const float* const weights, const int num_vals);
240
241 float ComputeCrossCorrelationNeon(const float* const values1,
242 const float* const values2,
243 const int num_vals);
244 #endif
245
ComputeMeanCpu(const float * const values,const int num_vals)246 inline float ComputeMeanCpu(const float* const values, const int num_vals) {
247 // Get mean.
248 float sum = values[0];
249 for (int i = 1; i < num_vals; ++i) {
250 sum += values[i];
251 }
252 return sum / static_cast<float>(num_vals);
253 }
254
255
ComputeMean(const float * const values,const int num_vals)256 inline float ComputeMean(const float* const values, const int num_vals) {
257 return
258 #ifdef __ARM_NEON
259 (num_vals >= 8) ? ComputeMeanNeon(values, num_vals) :
260 #endif
261 ComputeMeanCpu(values, num_vals);
262 }
263
264
ComputeStdDevCpu(const float * const values,const int num_vals,const float mean)265 inline float ComputeStdDevCpu(const float* const values,
266 const int num_vals,
267 const float mean) {
268 // Get Std dev.
269 float squared_sum = 0.0f;
270 for (int i = 0; i < num_vals; ++i) {
271 squared_sum += Square(values[i] - mean);
272 }
273 return sqrt(squared_sum / static_cast<float>(num_vals));
274 }
275
276
ComputeStdDev(const float * const values,const int num_vals,const float mean)277 inline float ComputeStdDev(const float* const values,
278 const int num_vals,
279 const float mean) {
280 return
281 #ifdef __ARM_NEON
282 (num_vals >= 8) ? ComputeStdDevNeon(values, num_vals, mean) :
283 #endif
284 ComputeStdDevCpu(values, num_vals, mean);
285 }
286
287
288 // TODO(andrewharp): Accelerate with NEON.
ComputeWeightedMean(const float * const values,const float * const weights,const int num_vals)289 inline float ComputeWeightedMean(const float* const values,
290 const float* const weights,
291 const int num_vals) {
292 float sum = 0.0f;
293 float total_weight = 0.0f;
294 for (int i = 0; i < num_vals; ++i) {
295 sum += values[i] * weights[i];
296 total_weight += weights[i];
297 }
298 return sum / num_vals;
299 }
300
301
ComputeCrossCorrelationCpu(const float * const values1,const float * const values2,const int num_vals)302 inline float ComputeCrossCorrelationCpu(const float* const values1,
303 const float* const values2,
304 const int num_vals) {
305 float sxy = 0.0f;
306 for (int offset = 0; offset < num_vals; ++offset) {
307 sxy += values1[offset] * values2[offset];
308 }
309
310 const float cross_correlation = sxy / num_vals;
311
312 return cross_correlation;
313 }
314
315
ComputeCrossCorrelation(const float * const values1,const float * const values2,const int num_vals)316 inline float ComputeCrossCorrelation(const float* const values1,
317 const float* const values2,
318 const int num_vals) {
319 return
320 #ifdef __ARM_NEON
321 (num_vals >= 8) ? ComputeCrossCorrelationNeon(values1, values2, num_vals)
322 :
323 #endif
324 ComputeCrossCorrelationCpu(values1, values2, num_vals);
325 }
326
327
NormalizeNumbers(float * const values,const int num_vals)328 inline void NormalizeNumbers(float* const values, const int num_vals) {
329 // Find the mean and then subtract so that the new mean is 0.0.
330 const float mean = ComputeMean(values, num_vals);
331 VLOG(2) << "Mean is " << mean;
332 float* curr_data = values;
333 for (int i = 0; i < num_vals; ++i) {
334 *curr_data -= mean;
335 curr_data++;
336 }
337
338 // Now divide by the std deviation so the new standard deviation is 1.0.
339 // The numbers might all be identical (and thus shifted to 0.0 now),
340 // so only scale by the standard deviation if this is not the case.
341 const float std_dev = ComputeStdDev(values, num_vals, 0.0f);
342 if (std_dev > 0.0f) {
343 VLOG(2) << "Std dev is " << std_dev;
344 curr_data = values;
345 for (int i = 0; i < num_vals; ++i) {
346 *curr_data /= std_dev;
347 curr_data++;
348 }
349 }
350 }
351
352
353 // Returns the determinant of a 2x2 matrix.
354 template<class T>
FindDeterminant2x2(const T * const a)355 inline T FindDeterminant2x2(const T* const a) {
356 // Determinant: (ad - bc)
357 return a[0] * a[3] - a[1] * a[2];
358 }
359
360
361 // Finds the inverse of a 2x2 matrix.
362 // Returns true upon success, false if the matrix is not invertible.
363 template<class T>
Invert2x2(const T * const a,float * const a_inv)364 inline bool Invert2x2(const T* const a, float* const a_inv) {
365 const float det = static_cast<float>(FindDeterminant2x2(a));
366 if (fabs(det) < EPSILON) {
367 return false;
368 }
369 const float inv_det = 1.0f / det;
370
371 a_inv[0] = inv_det * static_cast<float>(a[3]); // d
372 a_inv[1] = inv_det * static_cast<float>(-a[1]); // -b
373 a_inv[2] = inv_det * static_cast<float>(-a[2]); // -c
374 a_inv[3] = inv_det * static_cast<float>(a[0]); // a
375
376 return true;
377 }
378
379 } // namespace tf_tracking
380
381 #endif // TENSORFLOW_EXAMPLES_ANDROID_JNI_OBJECT_TRACKING_UTILS_H_
382