1a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp/* Copyright 2016 The TensorFlow Authors. All Rights Reserved. 2a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 3a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew HarpLicensed under the Apache License, Version 2.0 (the "License"); 4a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harpyou may not use this file except in compliance with the License. 5a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew HarpYou may obtain a copy of the License at 6a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 7a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp http://www.apache.org/licenses/LICENSE-2.0 8a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 9a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew HarpUnless required by applicable law or agreed to in writing, software 10a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harpdistributed under the License is distributed on an "AS IS" BASIS, 11a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew HarpWITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew HarpSee the License for the specific language governing permissions and 13a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harplimitations under the License. 14a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp==============================================================================*/ 15a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 16a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp#ifdef __RENDER_OPENGL__ 17a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp#include <GLES/gl.h> 18a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp#include <GLES/glext.h> 19a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp#endif 20a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 21a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp#include <string> 22a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp#include <map> 23a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 24a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp#include "tensorflow/examples/android/jni/object_tracking/geom.h" 25a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp#include "tensorflow/examples/android/jni/object_tracking/image-inl.h" 26a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp#include "tensorflow/examples/android/jni/object_tracking/image.h" 27a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp#include "tensorflow/examples/android/jni/object_tracking/integral_image.h" 286a5b2d4e5b4cdd82aa442ec423fee024e64cad2dAndrew Harp#include "tensorflow/examples/android/jni/object_tracking/logging.h" 29a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp#include "tensorflow/examples/android/jni/object_tracking/time_log.h" 30a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp#include "tensorflow/examples/android/jni/object_tracking/utils.h" 31a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 32a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp#include "tensorflow/examples/android/jni/object_tracking/config.h" 33a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp#include "tensorflow/examples/android/jni/object_tracking/flow_cache.h" 34a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp#include "tensorflow/examples/android/jni/object_tracking/keypoint_detector.h" 35a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp#include "tensorflow/examples/android/jni/object_tracking/object_detector.h" 36a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp#include "tensorflow/examples/android/jni/object_tracking/object_tracker.h" 37a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp#include "tensorflow/examples/android/jni/object_tracking/optical_flow.h" 38a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 39a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harpnamespace tf_tracking { 40a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 41a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew HarpObjectTracker::ObjectTracker(const TrackerConfig* const config, 42a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp ObjectDetectorBase* const detector) 43a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp : config_(config), 44a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp frame_width_(config->image_size.width), 45a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp frame_height_(config->image_size.height), 46a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp curr_time_(0), 47a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp num_frames_(0), 48a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp flow_cache_(&config->flow_config), 49a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp keypoint_detector_(&config->keypoint_detector_config), 50a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp curr_num_frame_pairs_(0), 51a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp first_frame_index_(0), 52a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp frame1_(new ImageData(frame_width_, frame_height_)), 53a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp frame2_(new ImageData(frame_width_, frame_height_)), 54a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp detector_(detector), 55a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp num_detected_(0) { 56a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp for (int i = 0; i < kNumFrames; ++i) { 57a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp frame_pairs_[i].Init(-1, -1); 58a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp } 59a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp} 60a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 61a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 62a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew HarpObjectTracker::~ObjectTracker() { 63a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp for (TrackedObjectMap::iterator iter = objects_.begin(); 64a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp iter != objects_.end(); iter++) { 65a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp TrackedObject* object = iter->second; 66a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp SAFE_DELETE(object); 67a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp } 68a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp} 69a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 70a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 71a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp// Finds the correspondences for all the points in the current pair of frames. 72a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp// Stores the results in the given FramePair. 73a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harpvoid ObjectTracker::FindCorrespondences(FramePair* const frame_pair) const { 74a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp // Keypoints aren't found until they're found. 75a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp memset(frame_pair->optical_flow_found_keypoint_, false, 76a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp sizeof(*frame_pair->optical_flow_found_keypoint_) * kMaxKeypoints); 77a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp TimeLog("Cleared old found keypoints"); 78a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 79a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp int num_keypoints_found = 0; 80a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 81a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp // For every keypoint... 82a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp for (int i_feat = 0; i_feat < frame_pair->number_of_keypoints_; ++i_feat) { 83a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp Keypoint* const keypoint1 = frame_pair->frame1_keypoints_ + i_feat; 84a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp Keypoint* const keypoint2 = frame_pair->frame2_keypoints_ + i_feat; 85a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 86a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp if (flow_cache_.FindNewPositionOfPoint( 87a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp keypoint1->pos_.x, keypoint1->pos_.y, 88a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp &keypoint2->pos_.x, &keypoint2->pos_.y)) { 89a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp frame_pair->optical_flow_found_keypoint_[i_feat] = true; 90a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp ++num_keypoints_found; 91a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp } 92a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp } 93a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 94a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp TimeLog("Found correspondences"); 95a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 96a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp LOGV("Found %d of %d keypoint correspondences", 97a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp num_keypoints_found, frame_pair->number_of_keypoints_); 98a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp} 99a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 100a99712b6229a39d6c6a6e0bb60b8ff6934216017A. Unique TensorFlowervoid ObjectTracker::NextFrame(const uint8_t* const new_frame, 101a99712b6229a39d6c6a6e0bb60b8ff6934216017A. Unique TensorFlower const uint8_t* const uv_frame, 102a99712b6229a39d6c6a6e0bb60b8ff6934216017A. Unique TensorFlower const int64_t timestamp, 103a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp const float* const alignment_matrix_2x3) { 104a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp IncrementFrameIndex(); 105a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp LOGV("Received frame %d", num_frames_); 106a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 107a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp FramePair* const curr_change = frame_pairs_ + GetNthIndexFromEnd(0); 108a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp curr_change->Init(curr_time_, timestamp); 109a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 110a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp CHECK_ALWAYS(curr_time_ < timestamp, 111a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp "Timestamp must monotonically increase! Went from %lld to %lld" 112a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp " on frame %d.", 113a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp curr_time_, timestamp, num_frames_); 114a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp curr_time_ = timestamp; 115a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 116a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp // Swap the frames. 117a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp frame1_.swap(frame2_); 118a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 119a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp frame2_->SetData(new_frame, uv_frame, frame_width_, timestamp, 1); 120a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 121a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp if (detector_.get() != NULL) { 122a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp detector_->SetImageData(frame2_.get()); 123a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp } 124a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 125a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp flow_cache_.NextFrame(frame2_.get(), alignment_matrix_2x3); 126a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 127a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp if (num_frames_ == 1) { 128a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp // This must be the first frame, so abort. 129a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp return; 130a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp } 131a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 132a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp if (config_->always_track || objects_.size() > 0) { 133a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp LOGV("Tracking %zu targets", objects_.size()); 134a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp ComputeKeypoints(true); 135a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp TimeLog("Keypoints computed!"); 136a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 137a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp FindCorrespondences(curr_change); 138a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp TimeLog("Flow computed!"); 139a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 140a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp TrackObjects(); 141a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp } 142a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp TimeLog("Targets tracked!"); 143a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 144a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp if (detector_.get() != NULL && num_frames_ % kDetectEveryNFrames == 0) { 145a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp DetectTargets(); 146a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp } 147a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp TimeLog("Detected objects."); 148a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp} 149a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 150a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew HarpTrackedObject* ObjectTracker::MaybeAddObject( 151a99712b6229a39d6c6a6e0bb60b8ff6934216017A. Unique TensorFlower const std::string& id, const Image<uint8_t>& source_image, 152a99712b6229a39d6c6a6e0bb60b8ff6934216017A. Unique TensorFlower const BoundingBox& bounding_box, const ObjectModelBase* object_model) { 153a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp // Train the detector if this is a new object. 154a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp if (objects_.find(id) != objects_.end()) { 155a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp return objects_[id]; 156a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp } 157a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 158a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp // Need to get a non-const version of the model, or create a new one if it 159a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp // wasn't given. 160a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp ObjectModelBase* model = NULL; 161a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp if (detector_ != NULL) { 162a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp // If a detector is registered, then this new object must have a model. 163a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp CHECK_ALWAYS(object_model != NULL, "No model given!"); 164a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp model = detector_->CreateObjectModel(object_model->GetName()); 165a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp } 166a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp TrackedObject* const object = 167a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp new TrackedObject(id, source_image, bounding_box, model); 168a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 169a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp objects_[id] = object; 170a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp return object; 171a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp} 172a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 173a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harpvoid ObjectTracker::RegisterNewObjectWithAppearance( 174a99712b6229a39d6c6a6e0bb60b8ff6934216017A. Unique TensorFlower const std::string& id, const uint8_t* const new_frame, 175a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp const BoundingBox& bounding_box) { 176a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp ObjectModelBase* object_model = NULL; 177a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 178a99712b6229a39d6c6a6e0bb60b8ff6934216017A. Unique TensorFlower Image<uint8_t> image(frame_width_, frame_height_); 179a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp image.FromArray(new_frame, frame_width_, 1); 180a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 181a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp if (detector_ != NULL) { 182a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp object_model = detector_->CreateObjectModel(id); 183a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp CHECK_ALWAYS(object_model != NULL, "Null object model!"); 184a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 185a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp const IntegralImage integral_image(image); 186a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp object_model->TrackStep(bounding_box, image, integral_image, true); 187a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp } 188a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 189a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp // Create an object at this position. 190a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp CHECK_ALWAYS(!HaveObject(id), "Already have this object!"); 191a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp if (objects_.find(id) == objects_.end()) { 192a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp TrackedObject* const object = 193a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp MaybeAddObject(id, image, bounding_box, object_model); 194a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp CHECK_ALWAYS(object != NULL, "Object not created!"); 195a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp } 196a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp} 197a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 198a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harpvoid ObjectTracker::SetPreviousPositionOfObject(const std::string& id, 199a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp const BoundingBox& bounding_box, 200a99712b6229a39d6c6a6e0bb60b8ff6934216017A. Unique TensorFlower const int64_t timestamp) { 201a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp CHECK_ALWAYS(timestamp > 0, "Timestamp too low! %lld", timestamp); 202a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp CHECK_ALWAYS(timestamp <= curr_time_, 203a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp "Timestamp too great! %lld vs %lld", timestamp, curr_time_); 204a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 205a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp TrackedObject* const object = GetObject(id); 206a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 207a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp // Track this bounding box from the past to the current time. 208a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp const BoundingBox current_position = TrackBox(bounding_box, timestamp); 209a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 210a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp object->UpdatePosition(current_position, curr_time_, *frame2_, false); 211a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 212a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp VLOG(2) << "Set tracked position for " << id << " to " << bounding_box 213a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp << std::endl; 214a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp} 215a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 216a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 217a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harpvoid ObjectTracker::SetCurrentPositionOfObject( 218a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp const std::string& id, const BoundingBox& bounding_box) { 219a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp SetPreviousPositionOfObject(id, bounding_box, curr_time_); 220a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp} 221a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 222a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 223a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harpvoid ObjectTracker::ForgetTarget(const std::string& id) { 224a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp LOGV("Forgetting object %s", id.c_str()); 225a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp TrackedObject* const object = GetObject(id); 226a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp delete object; 227a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp objects_.erase(id); 228a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 229a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp if (detector_ != NULL) { 230a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp detector_->DeleteObjectModel(id); 231a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp } 232a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp} 233a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 234a99712b6229a39d6c6a6e0bb60b8ff6934216017A. Unique TensorFlowerint ObjectTracker::GetKeypointsPacked(uint16_t* const out_data, 235a99712b6229a39d6c6a6e0bb60b8ff6934216017A. Unique TensorFlower const float scale) const { 236a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp const FramePair& change = frame_pairs_[GetNthIndexFromEnd(0)]; 237a99712b6229a39d6c6a6e0bb60b8ff6934216017A. Unique TensorFlower uint16_t* curr_data = out_data; 238a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp int num_keypoints = 0; 239a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 240a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp for (int i = 0; i < change.number_of_keypoints_; ++i) { 241a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp if (change.optical_flow_found_keypoint_[i]) { 242a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp ++num_keypoints; 243a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp const Point2f& point1 = change.frame1_keypoints_[i].pos_; 244a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp *curr_data++ = RealToFixed115(point1.x * scale); 245a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp *curr_data++ = RealToFixed115(point1.y * scale); 246a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 247a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp const Point2f& point2 = change.frame2_keypoints_[i].pos_; 248a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp *curr_data++ = RealToFixed115(point2.x * scale); 249a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp *curr_data++ = RealToFixed115(point2.y * scale); 250a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp } 251a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp } 252a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 253a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp return num_keypoints; 254a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp} 255a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 256a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 257a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harpint ObjectTracker::GetKeypoints(const bool only_found, 258a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp float* const out_data) const { 259a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp int curr_keypoint = 0; 260a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp const FramePair& change = frame_pairs_[GetNthIndexFromEnd(0)]; 261a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 262a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp for (int i = 0; i < change.number_of_keypoints_; ++i) { 263a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp if (!only_found || change.optical_flow_found_keypoint_[i]) { 264a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp const int base = curr_keypoint * kKeypointStep; 265a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp out_data[base + 0] = change.frame1_keypoints_[i].pos_.x; 266a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp out_data[base + 1] = change.frame1_keypoints_[i].pos_.y; 267a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 268a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp out_data[base + 2] = 269a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp change.optical_flow_found_keypoint_[i] ? 1.0f : -1.0f; 270a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp out_data[base + 3] = change.frame2_keypoints_[i].pos_.x; 271a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp out_data[base + 4] = change.frame2_keypoints_[i].pos_.y; 272a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 273a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp out_data[base + 5] = change.frame1_keypoints_[i].score_; 274a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp out_data[base + 6] = change.frame1_keypoints_[i].type_; 275a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp ++curr_keypoint; 276a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp } 277a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp } 278a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 279a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp LOGV("Got %d keypoints.", curr_keypoint); 280a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 281a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp return curr_keypoint; 282a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp} 283a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 284a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 285a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew HarpBoundingBox ObjectTracker::TrackBox(const BoundingBox& region, 286a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp const FramePair& frame_pair) const { 287a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp float translation_x; 288a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp float translation_y; 289a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 290a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp float scale_x; 291a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp float scale_y; 292a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 293a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp BoundingBox tracked_box(region); 294a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp frame_pair.AdjustBox( 295a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp tracked_box, &translation_x, &translation_y, &scale_x, &scale_y); 296a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 297a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp tracked_box.Shift(Point2f(translation_x, translation_y)); 298a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 299a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp if (scale_x > 0 && scale_y > 0) { 300a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp tracked_box.Scale(scale_x, scale_y); 301a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp } 302a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp return tracked_box; 303a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp} 304a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 305a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew HarpBoundingBox ObjectTracker::TrackBox(const BoundingBox& region, 306a99712b6229a39d6c6a6e0bb60b8ff6934216017A. Unique TensorFlower const int64_t timestamp) const { 307a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp CHECK_ALWAYS(timestamp > 0, "Timestamp too low! %lld", timestamp); 308a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp CHECK_ALWAYS(timestamp <= curr_time_, "Timestamp is in the future!"); 309a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 310a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp // Anything that ended before the requested timestamp is of no concern to us. 311a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp bool found_it = false; 312a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp int num_frames_back = -1; 313a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp for (int i = 0; i < curr_num_frame_pairs_; ++i) { 314a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp const FramePair& frame_pair = 315a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp frame_pairs_[GetNthIndexFromEnd(i)]; 316a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 317a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp if (frame_pair.end_time_ <= timestamp) { 318a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp num_frames_back = i - 1; 319a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 320a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp if (num_frames_back > 0) { 321a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp LOGV("Went %d out of %d frames before finding frame. (index: %d)", 322a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp num_frames_back, curr_num_frame_pairs_, GetNthIndexFromEnd(i)); 323a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp } 324a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 325a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp found_it = true; 326a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp break; 327a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp } 328a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp } 329a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 330a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp if (!found_it) { 331a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp LOGW("History did not go back far enough! %lld vs %lld", 332a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp frame_pairs_[GetNthIndexFromEnd(0)].end_time_ - 333a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp frame_pairs_[GetNthIndexFromStart(0)].end_time_, 334a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp frame_pairs_[GetNthIndexFromEnd(0)].end_time_ - timestamp); 335a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp } 336a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 337a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp // Loop over all the frames in the queue, tracking the accumulated delta 338a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp // of the point from frame to frame. It's possible the point could 339a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp // go out of frame, but keep tracking as best we can, using points near 340a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp // the edge of the screen where it went out of bounds. 341a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp BoundingBox tracked_box(region); 342a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp for (int i = num_frames_back; i >= 0; --i) { 343a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp const FramePair& frame_pair = frame_pairs_[GetNthIndexFromEnd(i)]; 344a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp SCHECK(frame_pair.end_time_ >= timestamp, "Frame timestamp was too early!"); 345a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp tracked_box = TrackBox(tracked_box, frame_pair); 346a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp } 347a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp return tracked_box; 348a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp} 349a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 350a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 351a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp// Converts a row-major 3x3 2d transformation matrix to a column-major 4x4 352a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp// 3d transformation matrix. 353a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harpinline void Convert3x3To4x4( 354a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp const float* const in_matrix, float* const out_matrix) { 355a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp // X 356a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp out_matrix[0] = in_matrix[0]; 357a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp out_matrix[1] = in_matrix[3]; 358a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp out_matrix[2] = 0.0f; 359a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp out_matrix[3] = 0.0f; 360a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 361a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp // Y 362a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp out_matrix[4] = in_matrix[1]; 363a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp out_matrix[5] = in_matrix[4]; 364a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp out_matrix[6] = 0.0f; 365a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp out_matrix[7] = 0.0f; 366a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 367a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp // Z 368a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp out_matrix[8] = 0.0f; 369a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp out_matrix[9] = 0.0f; 370a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp out_matrix[10] = 1.0f; 371a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp out_matrix[11] = 0.0f; 372a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 373a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp // Translation 374a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp out_matrix[12] = in_matrix[2]; 375a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp out_matrix[13] = in_matrix[5]; 376a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp out_matrix[14] = 0.0f; 377a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp out_matrix[15] = 1.0f; 378a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp} 379a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 380a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 381a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harpvoid ObjectTracker::Draw(const int canvas_width, const int canvas_height, 382a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp const float* const frame_to_canvas) const { 383a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp#ifdef __RENDER_OPENGL__ 384a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); 385a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 386a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp glMatrixMode(GL_PROJECTION); 387a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp glLoadIdentity(); 388a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 389a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp glOrthof(0.0f, canvas_width, 0.0f, canvas_height, 0.0f, 1.0f); 390a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 391a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp // To make Y go the right direction (0 at top of frame). 392a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp glScalef(1.0f, -1.0f, 1.0f); 393a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp glTranslatef(0.0f, -canvas_height, 0.0f); 394a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 395a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp glMatrixMode(GL_MODELVIEW); 396a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp glLoadIdentity(); 397a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 398a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp glPushMatrix(); 399a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 400a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp // Apply the frame to canvas transformation. 401a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp static GLfloat transformation[16]; 402a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp Convert3x3To4x4(frame_to_canvas, transformation); 403a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp glMultMatrixf(transformation); 404a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 405a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp // Draw tracked object bounding boxes. 406a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp for (TrackedObjectMap::const_iterator iter = objects_.begin(); 407a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp iter != objects_.end(); ++iter) { 408a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp TrackedObject* tracked_object = iter->second; 409a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp tracked_object->Draw(); 410a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp } 411a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 412a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp static const bool kRenderDebugPyramid = false; 413a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp if (kRenderDebugPyramid) { 414a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp glColor4f(1.0f, 1.0f, 1.0f, 1.0f); 415a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp for (int i = 0; i < kNumPyramidLevels * 2; ++i) { 416a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp Sprite(*frame1_->GetPyramidSqrt2Level(i)).Draw(); 417a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp } 418a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp } 419a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 420a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp static const bool kRenderDebugDerivative = false; 421a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp if (kRenderDebugDerivative) { 422a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp glColor4f(1.0f, 1.0f, 1.0f, 1.0f); 423a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp for (int i = 0; i < kNumPyramidLevels; ++i) { 424a99712b6229a39d6c6a6e0bb60b8ff6934216017A. Unique TensorFlower const Image<int32_t>& dx = *frame1_->GetSpatialX(i); 425a99712b6229a39d6c6a6e0bb60b8ff6934216017A. Unique TensorFlower Image<uint8_t> render_image(dx.GetWidth(), dx.GetHeight()); 426a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp for (int y = 0; y < dx.GetHeight(); ++y) { 427a99712b6229a39d6c6a6e0bb60b8ff6934216017A. Unique TensorFlower const int32_t* dx_ptr = dx[y]; 428a99712b6229a39d6c6a6e0bb60b8ff6934216017A. Unique TensorFlower uint8_t* dst_ptr = render_image[y]; 429a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp for (int x = 0; x < dx.GetWidth(); ++x) { 430a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp *dst_ptr++ = Clip(-(*dx_ptr++), 0, 255); 431a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp } 432a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp } 433a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 434a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp Sprite(render_image).Draw(); 435a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp } 436a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp } 437a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 438a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp if (detector_ != NULL) { 439a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp glDisable(GL_CULL_FACE); 440a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp detector_->Draw(); 441a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp } 442a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp glPopMatrix(); 443a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp#endif 444a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp} 445a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 446a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harpstatic void AddQuadrants(const BoundingBox& box, 447a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp std::vector<BoundingBox>* boxes) { 448a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp const Point2f center = box.GetCenter(); 449a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 450a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp float x1 = box.left_; 451a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp float x2 = center.x; 452a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp float x3 = box.right_; 453a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 454a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp float y1 = box.top_; 455a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp float y2 = center.y; 456a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp float y3 = box.bottom_; 457a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 458a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp // Upper left. 459a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp boxes->push_back(BoundingBox(x1, y1, x2, y2)); 460a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 461a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp // Upper right. 462a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp boxes->push_back(BoundingBox(x2, y1, x3, y2)); 463a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 464a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp // Bottom left. 465a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp boxes->push_back(BoundingBox(x1, y2, x2, y3)); 466a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 467a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp // Bottom right. 468a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp boxes->push_back(BoundingBox(x2, y2, x3, y3)); 469a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 470a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp // Whole thing. 471a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp boxes->push_back(box); 472a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp} 473a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 474a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harpvoid ObjectTracker::ComputeKeypoints(const bool cached_ok) { 475a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp const FramePair& prev_change = frame_pairs_[GetNthIndexFromEnd(1)]; 476a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp FramePair* const curr_change = &frame_pairs_[GetNthIndexFromEnd(0)]; 477a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 478a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp std::vector<BoundingBox> boxes; 479a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 480a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp for (TrackedObjectMap::iterator object_iter = objects_.begin(); 481a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp object_iter != objects_.end(); ++object_iter) { 482a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp BoundingBox box = object_iter->second->GetPosition(); 483a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp box.Scale(config_->object_box_scale_factor_for_features, 484a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp config_->object_box_scale_factor_for_features); 485a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp AddQuadrants(box, &boxes); 486a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp } 487a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 488a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp AddQuadrants(frame1_->GetImage()->GetContainingBox(), &boxes); 489a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 490a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp keypoint_detector_.FindKeypoints(*frame1_, boxes, prev_change, curr_change); 491a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp} 492a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 493a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 494a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp// Given a vector of detections and a model, simply returns the Detection for 495a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp// that model with the highest correlation. 496a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harpbool ObjectTracker::GetBestObjectForDetection( 497a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp const Detection& detection, TrackedObject** match) const { 498a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp TrackedObject* best_match = NULL; 499a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp float best_overlap = -FLT_MAX; 500a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 501a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp LOGV("Looking for matches in %zu objects!", objects_.size()); 502a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp for (TrackedObjectMap::const_iterator object_iter = objects_.begin(); 503a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp object_iter != objects_.end(); ++object_iter) { 504a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp TrackedObject* const tracked_object = object_iter->second; 505a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 506a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp const float overlap = tracked_object->GetPosition().PascalScore( 507a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp detection.GetObjectBoundingBox()); 508a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 509a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp if (!detector_->AllowSpontaneousDetections() && 510a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp (detection.GetObjectModel() != tracked_object->GetModel())) { 511a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp if (overlap > 0.0f) { 512a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp return false; 513a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp } 514a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp continue; 515a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp } 516a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 517a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp const float jump_distance = 518a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp (tracked_object->GetPosition().GetCenter() - 519a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp detection.GetObjectBoundingBox().GetCenter()).LengthSquared(); 520a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 521a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp const float allowed_distance = 522a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp tracked_object->GetAllowableDistanceSquared(); 523a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 524a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp LOGV("Distance: %.2f, Allowed distance %.2f, Overlap: %.2f", 525a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp jump_distance, allowed_distance, overlap); 526a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 527a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp // TODO(andrewharp): No need to do this verification twice, eliminate 528a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp // one of the score checks (the other being in OnDetection). 529a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp if (jump_distance < allowed_distance && 530a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp overlap > best_overlap && 531a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp tracked_object->GetMatchScore() + kMatchScoreBuffer < 532a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp detection.GetMatchScore()) { 533a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp best_match = tracked_object; 534a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp best_overlap = overlap; 535a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp } else if (overlap > 0.0f) { 536a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp return false; 537a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp } 538a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp } 539a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 540a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp *match = best_match; 541a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp return true; 542a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp} 543a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 544a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 545a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harpvoid ObjectTracker::ProcessDetections( 546a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp std::vector<Detection>* const detections) { 547a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp LOGV("Initial detection done, iterating over %zu detections now.", 548a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp detections->size()); 549a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 550a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp const bool spontaneous_detections_allowed = 551a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp detector_->AllowSpontaneousDetections(); 552a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp for (std::vector<Detection>::const_iterator it = detections->begin(); 553a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp it != detections->end(); ++it) { 554a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp const Detection& detection = *it; 555a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp SCHECK(frame2_->GetImage()->Contains(detection.GetObjectBoundingBox()), 556a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp "Frame does not contain bounding box!"); 557a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 558a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp TrackedObject* best_match = NULL; 559a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 560a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp const bool no_collisions = 561a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp GetBestObjectForDetection(detection, &best_match); 562a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 563a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp // Need to get a non-const version of the model, or create a new one if it 564a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp // wasn't given. 565a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp ObjectModelBase* model = 566a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp const_cast<ObjectModelBase*>(detection.GetObjectModel()); 567a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 568a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp if (best_match != NULL) { 569a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp if (model != best_match->GetModel()) { 570a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp CHECK_ALWAYS(detector_->AllowSpontaneousDetections(), 571a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp "Model for object changed but spontaneous detections not allowed!"); 572a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp } 573a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp best_match->OnDetection(model, 574a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp detection.GetObjectBoundingBox(), 575a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp detection.GetMatchScore(), 576a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp curr_time_, *frame2_); 577a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp } else if (no_collisions && spontaneous_detections_allowed) { 578a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp if (detection.GetMatchScore() > kMinimumMatchScore) { 579a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp LOGV("No match, adding it!"); 580a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp const ObjectModelBase* model = detection.GetObjectModel(); 581a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp std::ostringstream ss; 582a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp // TODO(andrewharp): Generate this in a more general fashion. 583a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp ss << "hand_" << num_detected_++; 584a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp std::string object_name = ss.str(); 585a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp MaybeAddObject(object_name, *frame2_->GetImage(), 586a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp detection.GetObjectBoundingBox(), model); 587a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp } 588a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp } 589a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp } 590a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp} 591a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 592a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 593a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harpvoid ObjectTracker::DetectTargets() { 594a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp // Detect all object model types that we're currently tracking. 595a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp std::vector<const ObjectModelBase*> object_models; 596a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp detector_->GetObjectModels(&object_models); 597a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp if (object_models.size() == 0) { 598a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp LOGV("No objects to search for, aborting."); 599a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp return; 600a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp } 601a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 602a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp LOGV("Trying to detect %zu models", object_models.size()); 603a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 604a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp LOGV("Creating test vector!"); 605a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp std::vector<BoundingSquare> positions; 606a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 607a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp for (TrackedObjectMap::iterator object_iter = objects_.begin(); 608a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp object_iter != objects_.end(); ++object_iter) { 609a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp TrackedObject* const tracked_object = object_iter->second; 610a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 611a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp#if DEBUG_PREDATOR 612a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp positions.push_back(GetCenteredSquare( 613a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp frame2_->GetImage()->GetContainingBox(), 32.0f)); 614a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp#else 615a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp const BoundingBox& position = tracked_object->GetPosition(); 616a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 617a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp const float square_size = MAX( 618a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp kScanMinSquareSize / (kLastKnownPositionScaleFactor * 619a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp kLastKnownPositionScaleFactor), 620a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp MIN(position.GetWidth(), 621a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp position.GetHeight())) / kLastKnownPositionScaleFactor; 622a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 623a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp FillWithSquares(frame2_->GetImage()->GetContainingBox(), 624a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp tracked_object->GetPosition(), 625a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp square_size, 626a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp kScanMinSquareSize, 627a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp kLastKnownPositionScaleFactor, 628a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp &positions); 629a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp } 630a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp#endif 631a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 632a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp LOGV("Created test vector!"); 633a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 634a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp std::vector<Detection> detections; 635a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp LOGV("Detecting!"); 636a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp detector_->Detect(positions, &detections); 637a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp LOGV("Found %zu detections", detections.size()); 638a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 639a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp TimeLog("Finished detection."); 640a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 641a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp ProcessDetections(&detections); 642a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 643a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp TimeLog("iterated over detections"); 644a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 645a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp LOGV("Done detecting!"); 646a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp} 647a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 648a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 649a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harpvoid ObjectTracker::TrackObjects() { 650a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp // TODO(andrewharp): Correlation should be allowed to remove objects too. 651a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp const bool automatic_removal_allowed = detector_.get() != NULL ? 652a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp detector_->AllowSpontaneousDetections() : false; 653a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 654a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp LOGV("Tracking %zu objects!", objects_.size()); 655a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp std::vector<std::string> dead_objects; 656a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp for (TrackedObjectMap::iterator iter = objects_.begin(); 657a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp iter != objects_.end(); iter++) { 658a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp TrackedObject* object = iter->second; 659a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp const BoundingBox tracked_position = TrackBox( 660a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp object->GetPosition(), frame_pairs_[GetNthIndexFromEnd(0)]); 661a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp object->UpdatePosition(tracked_position, curr_time_, *frame2_, false); 662a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 663a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp if (automatic_removal_allowed && 664a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp object->GetNumConsecutiveFramesBelowThreshold() > 665a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp kMaxNumDetectionFailures * 5) { 666a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp dead_objects.push_back(iter->first); 667a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp } 668a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp } 669a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 670a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp if (detector_ != NULL && automatic_removal_allowed) { 671a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp for (std::vector<std::string>::iterator iter = dead_objects.begin(); 672a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp iter != dead_objects.end(); iter++) { 673a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp LOGE("Removing object! %s", iter->c_str()); 674a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp ForgetTarget(*iter); 675a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp } 676a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp } 677a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp TimeLog("Tracked all objects."); 678a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 679a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp LOGV("%zu objects tracked!", objects_.size()); 680a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp} 681a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp 682a30b9926bd7d5276d6ff35af9428dee3e77b7dcbAndrew Harp} // namespace tf_tracking 683