• Home
  • Line#
  • Scopes#
  • Navigate#
  • Raw
  • Download
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 // Various keypoint detecting functions.
17 
18 #include "tensorflow/tools/android/test/jni/object_tracking/keypoint_detector.h"
19 
20 #include <float.h>
21 
22 #include "tensorflow/tools/android/test/jni/object_tracking/config.h"
23 #include "tensorflow/tools/android/test/jni/object_tracking/image-inl.h"
24 #include "tensorflow/tools/android/test/jni/object_tracking/image.h"
25 #include "tensorflow/tools/android/test/jni/object_tracking/keypoint.h"
26 #include "tensorflow/tools/android/test/jni/object_tracking/time_log.h"
27 #include "tensorflow/tools/android/test/jni/object_tracking/utils.h"
28 
29 namespace tf_tracking {
30 
GetDistSquaredBetween(const int * vec1,const int * vec2)31 static inline int GetDistSquaredBetween(const int* vec1, const int* vec2) {
32   return Square(vec1[0] - vec2[0]) + Square(vec1[1] - vec2[1]);
33 }
34 
ScoreKeypoints(const ImageData & image_data,const int num_candidates,Keypoint * const candidate_keypoints)35 void KeypointDetector::ScoreKeypoints(const ImageData& image_data,
36                                       const int num_candidates,
37                                       Keypoint* const candidate_keypoints) {
38   const Image<int>& I_x = *image_data.GetSpatialX(0);
39   const Image<int>& I_y = *image_data.GetSpatialY(0);
40 
41   if (config_->detect_skin) {
42     const Image<uint8_t>& u_data = *image_data.GetU();
43     const Image<uint8_t>& v_data = *image_data.GetV();
44 
45     static const int reference[] = {111, 155};
46 
47     // Score all the keypoints.
48     for (int i = 0; i < num_candidates; ++i) {
49       Keypoint* const keypoint = candidate_keypoints + i;
50 
51       const int x_pos = keypoint->pos_.x * 2;
52       const int y_pos = keypoint->pos_.y * 2;
53 
54       const int curr_color[] = {u_data[y_pos][x_pos], v_data[y_pos][x_pos]};
55       keypoint->score_ =
56           HarrisFilter(I_x, I_y, keypoint->pos_.x, keypoint->pos_.y) /
57           GetDistSquaredBetween(reference, curr_color);
58     }
59   } else {
60     // Score all the keypoints.
61     for (int i = 0; i < num_candidates; ++i) {
62       Keypoint* const keypoint = candidate_keypoints + i;
63       keypoint->score_ =
64           HarrisFilter(I_x, I_y, keypoint->pos_.x, keypoint->pos_.y);
65     }
66   }
67 }
68 
69 
KeypointCompare(const void * const a,const void * const b)70 inline int KeypointCompare(const void* const a, const void* const b) {
71   return (reinterpret_cast<const Keypoint*>(a)->score_ -
72           reinterpret_cast<const Keypoint*>(b)->score_) <= 0 ? 1 : -1;
73 }
74 
75 
76 // Quicksorts detected keypoints by score.
SortKeypoints(const int num_candidates,Keypoint * const candidate_keypoints) const77 void KeypointDetector::SortKeypoints(const int num_candidates,
78                                    Keypoint* const candidate_keypoints) const {
79   qsort(candidate_keypoints, num_candidates, sizeof(Keypoint), KeypointCompare);
80 
81 #ifdef SANITY_CHECKS
82   // Verify that the array got sorted.
83   float last_score = FLT_MAX;
84   for (int i = 0; i < num_candidates; ++i) {
85     const float curr_score = candidate_keypoints[i].score_;
86 
87     // Scores should be monotonically increasing.
88     SCHECK(last_score >= curr_score,
89           "Quicksort failure! %d: %.5f > %d: %.5f (%d total)",
90           i - 1, last_score, i, curr_score, num_candidates);
91 
92     last_score = curr_score;
93   }
94 #endif
95 }
96 
97 
SelectKeypointsInBox(const BoundingBox & box,const Keypoint * const candidate_keypoints,const int num_candidates,const int max_keypoints,const int num_existing_keypoints,const Keypoint * const existing_keypoints,Keypoint * const final_keypoints) const98 int KeypointDetector::SelectKeypointsInBox(
99     const BoundingBox& box,
100     const Keypoint* const candidate_keypoints,
101     const int num_candidates,
102     const int max_keypoints,
103     const int num_existing_keypoints,
104     const Keypoint* const existing_keypoints,
105     Keypoint* const final_keypoints) const {
106   if (max_keypoints <= 0) {
107     return 0;
108   }
109 
110   // This is the distance within which keypoints may be placed to each other
111   // within this box, roughly based on the box dimensions.
112   const int distance =
113       MAX(1, MIN(box.GetWidth(), box.GetHeight()) * kClosestPercent / 2.0f);
114 
115   // First, mark keypoints that already happen to be inside this region. Ignore
116   // keypoints that are outside it, however close they might be.
117   interest_map_->Clear(false);
118   for (int i = 0; i < num_existing_keypoints; ++i) {
119     const Keypoint& candidate = existing_keypoints[i];
120 
121     const int x_pos = candidate.pos_.x;
122     const int y_pos = candidate.pos_.y;
123     if (box.Contains(candidate.pos_)) {
124       MarkImage(x_pos, y_pos, distance, interest_map_.get());
125     }
126   }
127 
128   // Now, go through and check which keypoints will still fit in the box.
129   int num_keypoints_selected = 0;
130   for (int i = 0; i < num_candidates; ++i) {
131     const Keypoint& candidate = candidate_keypoints[i];
132 
133     const int x_pos = candidate.pos_.x;
134     const int y_pos = candidate.pos_.y;
135 
136     if (!box.Contains(candidate.pos_) ||
137         !interest_map_->ValidPixel(x_pos, y_pos)) {
138       continue;
139     }
140 
141     if (!(*interest_map_)[y_pos][x_pos]) {
142       final_keypoints[num_keypoints_selected++] = candidate;
143       if (num_keypoints_selected >= max_keypoints) {
144         break;
145       }
146       MarkImage(x_pos, y_pos, distance, interest_map_.get());
147     }
148   }
149   return num_keypoints_selected;
150 }
151 
152 
SelectKeypoints(const std::vector<BoundingBox> & boxes,const Keypoint * const candidate_keypoints,const int num_candidates,FramePair * const curr_change) const153 void KeypointDetector::SelectKeypoints(
154     const std::vector<BoundingBox>& boxes,
155     const Keypoint* const candidate_keypoints,
156     const int num_candidates,
157     FramePair* const curr_change) const {
158   // Now select all the interesting keypoints that fall insider our boxes.
159   curr_change->number_of_keypoints_ = 0;
160   for (std::vector<BoundingBox>::const_iterator iter = boxes.begin();
161       iter != boxes.end(); ++iter) {
162     const BoundingBox bounding_box = *iter;
163 
164     // Count up keypoints that have already been selected, and fall within our
165     // box.
166     int num_keypoints_already_in_box = 0;
167     for (int i = 0; i < curr_change->number_of_keypoints_; ++i) {
168       if (bounding_box.Contains(curr_change->frame1_keypoints_[i].pos_)) {
169         ++num_keypoints_already_in_box;
170       }
171     }
172 
173     const int max_keypoints_to_find_in_box =
174         MIN(kMaxKeypointsForObject - num_keypoints_already_in_box,
175             kMaxKeypoints - curr_change->number_of_keypoints_);
176 
177     const int num_new_keypoints_in_box = SelectKeypointsInBox(
178         bounding_box,
179         candidate_keypoints,
180         num_candidates,
181         max_keypoints_to_find_in_box,
182         curr_change->number_of_keypoints_,
183         curr_change->frame1_keypoints_,
184         curr_change->frame1_keypoints_ + curr_change->number_of_keypoints_);
185 
186     curr_change->number_of_keypoints_ += num_new_keypoints_in_box;
187 
188     LOGV("Selected %d keypoints!", curr_change->number_of_keypoints_);
189   }
190 }
191 
192 
193 // Walks along the given circle checking for pixels above or below the center.
194 // Returns a score, or 0 if the keypoint did not pass the criteria.
195 //
196 // Parameters:
197 //  circle_perimeter: the circumference in pixels of the circle.
198 //  threshold: the minimum number of contiguous pixels that must be above or
199 //             below the center value.
200 //  center_ptr: the location of the center pixel in memory
201 //  offsets: the relative offsets from the center pixel of the edge pixels.
TestCircle(const int circle_perimeter,const int threshold,const uint8_t * const center_ptr,const int * offsets)202 inline int TestCircle(const int circle_perimeter, const int threshold,
203                       const uint8_t* const center_ptr, const int* offsets) {
204   // Get the actual value of the center pixel for easier reference later on.
205   const int center_value = static_cast<int>(*center_ptr);
206 
207   // Number of total pixels to check.  Have to wrap around some in case
208   // the contiguous section is split by the array edges.
209   const int num_total = circle_perimeter + threshold - 1;
210 
211   int num_above = 0;
212   int above_diff = 0;
213 
214   int num_below = 0;
215   int below_diff = 0;
216 
217   // Used to tell when this is definitely not going to meet the threshold so we
218   // can early abort.
219   int minimum_by_now = threshold - num_total + 1;
220 
221   // Go through every pixel along the perimeter of the circle, and then around
222   // again a little bit.
223   for (int i = 0; i < num_total; ++i) {
224     // This should be faster than mod.
225     const int perim_index = i < circle_perimeter ? i : i - circle_perimeter;
226 
227     // This gets the value of the current pixel along the perimeter by using
228     // a precomputed offset.
229     const int curr_value =
230         static_cast<int>(center_ptr[offsets[perim_index]]);
231 
232     const int difference = curr_value - center_value;
233 
234     if (difference > kFastDiffAmount) {
235       above_diff += difference;
236       ++num_above;
237 
238       num_below = 0;
239       below_diff = 0;
240 
241       if (num_above >= threshold) {
242         return above_diff;
243       }
244     } else if (difference < -kFastDiffAmount) {
245       below_diff += difference;
246       ++num_below;
247 
248       num_above = 0;
249       above_diff = 0;
250 
251       if (num_below >= threshold) {
252         return below_diff;
253       }
254     } else {
255       num_above = 0;
256       num_below = 0;
257       above_diff = 0;
258       below_diff = 0;
259     }
260 
261     // See if there's any chance of making the threshold.
262     if (MAX(num_above, num_below) < minimum_by_now) {
263       // Didn't pass.
264       return 0;
265     }
266     ++minimum_by_now;
267   }
268 
269   // Didn't pass.
270   return 0;
271 }
272 
273 
274 // Returns a score in the range [0.0, positive infinity) which represents the
275 // relative likelihood of a point being a corner.
HarrisFilter(const Image<int32_t> & I_x,const Image<int32_t> & I_y,const float x,const float y) const276 float KeypointDetector::HarrisFilter(const Image<int32_t>& I_x,
277                                      const Image<int32_t>& I_y, const float x,
278                                      const float y) const {
279   if (I_x.ValidInterpPixel(x - kHarrisWindowSize, y - kHarrisWindowSize) &&
280       I_x.ValidInterpPixel(x + kHarrisWindowSize, y + kHarrisWindowSize)) {
281     // Image gradient matrix.
282     float G[] = { 0, 0, 0, 0 };
283     CalculateG(kHarrisWindowSize, x, y, I_x, I_y, G);
284 
285     const float dx = G[0];
286     const float dy = G[3];
287     const float dxy = G[1];
288 
289     // Harris-Nobel corner score.
290     return (dx * dy - Square(dxy)) / (dx + dy + FLT_MIN);
291   }
292 
293   return 0.0f;
294 }
295 
296 
AddExtraCandidatesForBoxes(const std::vector<BoundingBox> & boxes,const int max_num_keypoints,Keypoint * const keypoints) const297 int KeypointDetector::AddExtraCandidatesForBoxes(
298     const std::vector<BoundingBox>& boxes,
299     const int max_num_keypoints,
300     Keypoint* const keypoints) const {
301   int num_keypoints_added = 0;
302 
303   for (std::vector<BoundingBox>::const_iterator iter = boxes.begin();
304       iter != boxes.end(); ++iter) {
305     const BoundingBox box = *iter;
306 
307     for (int i = 0; i < kNumToAddAsCandidates; ++i) {
308       for (int j = 0; j < kNumToAddAsCandidates; ++j) {
309         if (num_keypoints_added >= max_num_keypoints) {
310           LOGW("Hit cap of %d for temporary keypoints!", max_num_keypoints);
311           return num_keypoints_added;
312         }
313 
314         Keypoint& curr_keypoint = keypoints[num_keypoints_added++];
315         curr_keypoint.pos_ = Point2f(
316             box.left_ + box.GetWidth() * (i + 0.5f) / kNumToAddAsCandidates,
317             box.top_ + box.GetHeight() * (j + 0.5f) / kNumToAddAsCandidates);
318         curr_keypoint.type_ = KEYPOINT_TYPE_INTEREST;
319       }
320     }
321   }
322 
323   return num_keypoints_added;
324 }
325 
326 
FindKeypoints(const ImageData & image_data,const std::vector<BoundingBox> & rois,const FramePair & prev_change,FramePair * const curr_change)327 void KeypointDetector::FindKeypoints(const ImageData& image_data,
328                                    const std::vector<BoundingBox>& rois,
329                                    const FramePair& prev_change,
330                                    FramePair* const curr_change) {
331   // Copy keypoints from second frame of last pass to temp keypoints of this
332   // pass.
333   int number_of_tmp_keypoints = CopyKeypoints(prev_change, tmp_keypoints_);
334 
335   const int max_num_fast = kMaxTempKeypoints - number_of_tmp_keypoints;
336   number_of_tmp_keypoints +=
337       FindFastKeypoints(image_data, max_num_fast,
338                        tmp_keypoints_ + number_of_tmp_keypoints);
339 
340   TimeLog("Found FAST keypoints");
341 
342   if (number_of_tmp_keypoints >= kMaxTempKeypoints) {
343     LOGW("Hit cap of %d for temporary keypoints (FAST)! %d keypoints",
344          kMaxTempKeypoints, number_of_tmp_keypoints);
345   }
346 
347   if (kAddArbitraryKeypoints) {
348     // Add some for each object prior to scoring.
349     const int max_num_box_keypoints =
350         kMaxTempKeypoints - number_of_tmp_keypoints;
351     number_of_tmp_keypoints +=
352         AddExtraCandidatesForBoxes(rois, max_num_box_keypoints,
353                                    tmp_keypoints_ + number_of_tmp_keypoints);
354     TimeLog("Added box keypoints");
355 
356     if (number_of_tmp_keypoints >= kMaxTempKeypoints) {
357       LOGW("Hit cap of %d for temporary keypoints (boxes)! %d keypoints",
358            kMaxTempKeypoints, number_of_tmp_keypoints);
359     }
360   }
361 
362   // Score them...
363   LOGV("Scoring %d keypoints!", number_of_tmp_keypoints);
364   ScoreKeypoints(image_data, number_of_tmp_keypoints, tmp_keypoints_);
365   TimeLog("Scored keypoints");
366 
367   // Now pare it down a bit.
368   SortKeypoints(number_of_tmp_keypoints, tmp_keypoints_);
369   TimeLog("Sorted keypoints");
370 
371   LOGV("%d keypoints to select from!", number_of_tmp_keypoints);
372 
373   SelectKeypoints(rois, tmp_keypoints_, number_of_tmp_keypoints, curr_change);
374   TimeLog("Selected keypoints");
375 
376   LOGV("Picked %d (%d max) final keypoints out of %d potential.",
377        curr_change->number_of_keypoints_,
378        kMaxKeypoints, number_of_tmp_keypoints);
379 }
380 
381 
CopyKeypoints(const FramePair & prev_change,Keypoint * const new_keypoints)382 int KeypointDetector::CopyKeypoints(const FramePair& prev_change,
383                                   Keypoint* const new_keypoints) {
384   int number_of_keypoints = 0;
385 
386   // Caching values from last pass, just copy and compact.
387   for (int i = 0; i < prev_change.number_of_keypoints_; ++i) {
388     if (prev_change.optical_flow_found_keypoint_[i]) {
389       new_keypoints[number_of_keypoints] =
390           prev_change.frame2_keypoints_[i];
391 
392       new_keypoints[number_of_keypoints].score_ =
393           prev_change.frame1_keypoints_[i].score_;
394 
395       ++number_of_keypoints;
396     }
397   }
398 
399   TimeLog("Copied keypoints");
400   return number_of_keypoints;
401 }
402 
403 
404 // FAST keypoint detector.
FindFastKeypoints(const Image<uint8_t> & frame,const int quadrant,const int downsample_factor,const int max_num_keypoints,Keypoint * const keypoints)405 int KeypointDetector::FindFastKeypoints(const Image<uint8_t>& frame,
406                                         const int quadrant,
407                                         const int downsample_factor,
408                                         const int max_num_keypoints,
409                                         Keypoint* const keypoints) {
410   /*
411    // Reference for a circle of diameter 7.
412    const int circle[] = {0, 0, 1, 1, 1, 0, 0,
413                          0, 1, 0, 0, 0, 1, 0,
414                          1, 0, 0, 0, 0, 0, 1,
415                          1, 0, 0, 0, 0, 0, 1,
416                          1, 0, 0, 0, 0, 0, 1,
417                          0, 1, 0, 0, 0, 1, 0,
418                          0, 0, 1, 1, 1, 0, 0};
419    const int circle_offset[] =
420        {2, 3, 4, 8, 12, 14, 20, 21, 27, 28, 34, 36, 40, 44, 45, 46};
421    */
422 
423   // Quick test of compass directions.  Any length 16 circle with a break of up
424   // to 4 pixels will have at least 3 of these 4 pixels active.
425   static const int short_circle_perimeter = 4;
426   static const int short_threshold = 3;
427   static const int short_circle_x[] = { -3,  0, +3,  0 };
428   static const int short_circle_y[] = {  0, -3,  0, +3 };
429 
430   // Precompute image offsets.
431   int short_offsets[short_circle_perimeter];
432   for (int i = 0; i < short_circle_perimeter; ++i) {
433     short_offsets[i] = short_circle_x[i] + short_circle_y[i] * frame.GetWidth();
434   }
435 
436   // Large circle values.
437   static const int full_circle_perimeter = 16;
438   static const int full_threshold = 12;
439   static const int full_circle_x[] =
440       { -1,  0, +1, +2, +3, +3, +3, +2, +1, +0, -1, -2, -3, -3, -3, -2 };
441   static const int full_circle_y[] =
442       { -3, -3, -3, -2, -1,  0, +1, +2, +3, +3, +3, +2, +1, +0, -1, -2 };
443 
444   // Precompute image offsets.
445   int full_offsets[full_circle_perimeter];
446   for (int i = 0; i < full_circle_perimeter; ++i) {
447     full_offsets[i] = full_circle_x[i] + full_circle_y[i] * frame.GetWidth();
448   }
449 
450   const int scratch_stride = frame.stride();
451 
452   keypoint_scratch_->Clear(0);
453 
454   // Set up the bounds on the region to test based on the passed-in quadrant.
455   const int quadrant_width = (frame.GetWidth() / 2) - kFastBorderBuffer;
456   const int quadrant_height = (frame.GetHeight() / 2) - kFastBorderBuffer;
457   const int start_x =
458       kFastBorderBuffer + ((quadrant % 2 == 0) ? 0 : quadrant_width);
459   const int start_y =
460       kFastBorderBuffer + ((quadrant < 2) ? 0 : quadrant_height);
461   const int end_x = start_x + quadrant_width;
462   const int end_y = start_y + quadrant_height;
463 
464   // Loop through once to find FAST keypoint clumps.
465   for (int img_y = start_y; img_y < end_y; ++img_y) {
466     const uint8_t* curr_pixel_ptr = frame[img_y] + start_x;
467 
468     for (int img_x = start_x; img_x < end_x; ++img_x) {
469       // Only insert it if it meets the quick minimum requirements test.
470       if (TestCircle(short_circle_perimeter, short_threshold,
471                      curr_pixel_ptr, short_offsets) != 0) {
472         // Longer test for actual keypoint score..
473         const int fast_score = TestCircle(full_circle_perimeter,
474                                           full_threshold,
475                                           curr_pixel_ptr,
476                                           full_offsets);
477 
478         // Non-zero score means the keypoint was found.
479         if (fast_score != 0) {
480           uint8_t* const center_ptr = (*keypoint_scratch_)[img_y] + img_x;
481 
482           // Increase the keypoint count on this pixel and the pixels in all
483           // 4 cardinal directions.
484           *center_ptr += 5;
485           *(center_ptr - 1) += 1;
486           *(center_ptr + 1) += 1;
487           *(center_ptr - scratch_stride) += 1;
488           *(center_ptr + scratch_stride) += 1;
489         }
490       }
491 
492       ++curr_pixel_ptr;
493     }  // x
494   }  // y
495 
496   TimeLog("Found FAST keypoints.");
497 
498   int num_keypoints = 0;
499   // Loop through again and Harris filter pixels in the center of clumps.
500   // We can shrink the window by 1 pixel on every side.
501   for (int img_y = start_y + 1; img_y < end_y - 1; ++img_y) {
502     const uint8_t* curr_pixel_ptr = (*keypoint_scratch_)[img_y] + start_x;
503 
504     for (int img_x = start_x + 1; img_x < end_x - 1; ++img_x) {
505       if (*curr_pixel_ptr >= kMinNumConnectedForFastKeypoint) {
506        Keypoint* const keypoint = keypoints + num_keypoints;
507         keypoint->pos_ = Point2f(
508             img_x * downsample_factor, img_y * downsample_factor);
509         keypoint->score_ = 0;
510         keypoint->type_ = KEYPOINT_TYPE_FAST;
511 
512         ++num_keypoints;
513         if (num_keypoints >= max_num_keypoints) {
514           return num_keypoints;
515         }
516       }
517 
518       ++curr_pixel_ptr;
519     }  // x
520   }  // y
521 
522   TimeLog("Picked FAST keypoints.");
523 
524   return num_keypoints;
525 }
526 
FindFastKeypoints(const ImageData & image_data,const int max_num_keypoints,Keypoint * const keypoints)527 int KeypointDetector::FindFastKeypoints(const ImageData& image_data,
528                                         const int max_num_keypoints,
529                                         Keypoint* const keypoints) {
530   int downsample_factor = 1;
531   int num_found = 0;
532 
533   // TODO(andrewharp): Get this working for multiple image scales.
534   for (int i = 0; i < 1; ++i) {
535     const Image<uint8_t>& frame = *image_data.GetPyramidSqrt2Level(i);
536     num_found += FindFastKeypoints(
537         frame, fast_quadrant_,
538         downsample_factor, max_num_keypoints, keypoints + num_found);
539     downsample_factor *= 2;
540   }
541 
542   // Increment the current quadrant.
543   fast_quadrant_ = (fast_quadrant_ + 1) % 4;
544 
545   return num_found;
546 }
547 
548 }  // namespace tf_tracking
549