Blender  V3.3
predict_tracks.cc
Go to the documentation of this file.
1 // Copyright (c) 2014 libmv authors.
2 //
3 // Permission is hereby granted, free of charge, to any person obtaining a copy
4 // of this software and associated documentation files (the "Software"), to
5 // deal in the Software without restriction, including without limitation the
6 // rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
7 // sell copies of the Software, and to permit persons to whom the Software is
8 // furnished to do so, subject to the following conditions:
9 //
10 // The above copyright notice and this permission notice shall be included in
11 // all copies or substantial portions of the Software.
12 //
13 // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
14 // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
15 // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
16 // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
17 // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
18 // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
19 // IN THE SOFTWARE.
20 //
21 // Author: mierle@gmail.com (Keir Mierle)
22 
24 #include "libmv/autotrack/marker.h"
25 #include "libmv/autotrack/tracks.h"
26 #include "libmv/base/vector.h"
27 #include "libmv/logging/logging.h"
29 
30 namespace mv {
31 
32 namespace {
33 
34 using libmv::Vec2;
35 using libmv::vector;
36 
37 // Implied time delta between steps. Set empirically by tweaking and seeing
38 // what numbers did best at prediction.
39 const double dt = 3.8;
40 
41 // State transition matrix.
42 
43 // The states for predicting a track are as follows:
44 //
45 // 0 - X position
46 // 1 - X velocity
47 // 2 - X acceleration
48 // 3 - Y position
49 // 4 - Y velocity
50 // 5 - Y acceleration
51 //
52 // Note that in the velocity-only state transition matrix, the acceleration
53 // component is ignored; so technically the system could be modelled with only
54 // 4 states instead of 6. For ease of implementation, this keeps order 6.
55 
56 // Choose one or the other model from below (velocity or acceleration).
57 
58 // For a typical system having constant velocity. This gives smooth-appearing
59 // predictions, but they are not always as accurate.
60 //
61 // clang-format off
62 const double velocity_state_transition_data[] = {
63  1, dt, 0, 0, 0, 0,
64  0, 1, 0, 0, 0, 0,
65  0, 0, 1, 0, 0, 0,
66  0, 0, 0, 1, dt, 0,
67  0, 0, 0, 0, 1, 0,
68  0, 0, 0, 0, 0, 1
69 };
70 // clang-format on
71 
72 #if 0
73 // This 3rd-order system also models acceleration. This makes for "jerky"
74 // predictions, but that tend to be more accurate.
75 //
76 // clang-format off
77 const double acceleration_state_transition_data[] = {
78  1, dt, dt*dt/2, 0, 0, 0,
79  0, 1, dt, 0, 0, 0,
80  0, 0, 1, 0, 0, 0,
81  0, 0, 0, 1, dt, dt*dt/2,
82  0, 0, 0, 0, 1, dt,
83  0, 0, 0, 0, 0, 1
84 };
85 // clang-format on
86 
87 // This system (attempts) to add an angular velocity component. However, it's
88 // total junk.
89 //
90 // clang-format off
91 const double angular_state_transition_data[] = {
92  1, dt, -dt, 0, 0, 0, // Position x
93  0, 1, 0, 0, 0, 0, // Velocity x
94  0, 0, 1, 0, 0, 0, // Angular momentum
95  0, 0, dt, 1, dt, 0, // Position y
96  0, 0, 0, 0, 1, 0, // Velocity y
97  0, 0, 0, 0, 0, 1 // Ignored
98 };
99 // clang-format on
100 #endif
101 
102 const double* state_transition_data = velocity_state_transition_data;
103 
104 // Observation matrix.
105 // clang-format off
106 const double observation_data[] = {
107  1., 0., 0., 0., 0., 0.,
108  0., 0., 0., 1., 0., 0.
109 };
110 // clang-format on
111 
112 // Process covariance.
113 //
114 // clang-format off
115 const double process_covariance_data[] = {
116  35, 0, 0, 0, 0, 0,
117  0, 5, 0, 0, 0, 0,
118  0, 0, 5, 0, 0, 0,
119  0, 0, 0, 35, 0, 0,
120  0, 0, 0, 0, 5, 0,
121  0, 0, 0, 0, 0, 5
122 };
123 // clang-format on
124 
125 // Process covariance.
126 const double measurement_covariance_data[] = {
127  0.01,
128  0.00,
129  0.00,
130  0.01,
131 };
132 
133 // Initial covariance.
134 //
135 // clang-format off
136 const double initial_covariance_data[] = {
137  10, 0, 0, 0, 0, 0,
138  0, 1, 0, 0, 0, 0,
139  0, 0, 1, 0, 0, 0,
140  0, 0, 0, 10, 0, 0,
141  0, 0, 0, 0, 1, 0,
142  0, 0, 0, 0, 0, 1
143 };
144 // clang-format on
145 
146 typedef mv::KalmanFilter<double, 6, 2> TrackerKalman;
147 
148 TrackerKalman filter(state_transition_data,
149  observation_data,
150  process_covariance_data,
151  measurement_covariance_data);
152 
153 bool OrderByFrameLessThan(const Marker* a, const Marker* b) {
154  if (a->frame == b->frame) {
155  if (a->clip == b->clip) {
156  return a->track < b->track;
157  }
158  return a->clip < b->clip;
159  }
160  return a->frame < b->frame;
161 }
162 
163 // Predicted must be after the previous markers (in the frame numbering sense).
164 void RunPrediction(const vector<Marker*> previous_markers,
165  Marker* predicted_marker) {
166  TrackerKalman::State state;
167  state.mean << previous_markers[0]->center.x(), 0, 0,
168  previous_markers[0]->center.y(), 0, 0;
169  state.covariance =
170  Eigen::Matrix<double, 6, 6, Eigen::RowMajor>(initial_covariance_data);
171 
172  int current_frame = previous_markers[0]->frame;
173  int target_frame = predicted_marker->frame;
174 
175  bool predict_forward = current_frame < target_frame;
176  int frame_delta = predict_forward ? 1 : -1;
177 
178  for (int i = 1; i < previous_markers.size(); ++i) {
179  // Step forward predicting the state until it is on the current marker.
180  int predictions = 0;
181  for (; current_frame != previous_markers[i]->frame;
182  current_frame += frame_delta) {
183  filter.Step(&state);
184  predictions++;
185  LG << "Predicted point (frame " << current_frame << "): " << state.mean(0)
186  << ", " << state.mean(3);
187  }
188  // Log the error -- not actually used, but interesting.
189  Vec2 error = previous_markers[i]->center.cast<double>() -
190  Vec2(state.mean(0), state.mean(3));
191  LG << "Prediction error for " << predictions << " steps: (" << error.x()
192  << ", " << error.y() << "); norm: " << error.norm();
193  // Now that the state is predicted in the current frame, update the state
194  // based on the measurement from the current frame.
195  filter.Update(previous_markers[i]->center.cast<double>(),
196  Eigen::Matrix<double, 2, 2, Eigen::RowMajor>(
197  measurement_covariance_data),
198  &state);
199  LG << "Updated point: " << state.mean(0) << ", " << state.mean(3);
200  }
201  // At this point as all the prediction that's possible is done. Finally
202  // predict until the target frame.
203  for (; current_frame != target_frame; current_frame += frame_delta) {
204  filter.Step(&state);
205  LG << "Final predicted point (frame " << current_frame
206  << "): " << state.mean(0) << ", " << state.mean(3);
207  }
208 
209  // The x and y positions are at 0 and 3; ignore acceleration and velocity.
210  predicted_marker->center.x() = state.mean(0);
211  predicted_marker->center.y() = state.mean(3);
212 
213  // Take the patch from the last marker then shift it to match the prediction.
214  const Marker& last_marker = *previous_markers[previous_markers.size() - 1];
215  predicted_marker->patch = last_marker.patch;
216  Vec2f delta = predicted_marker->center - last_marker.center;
217  for (int i = 0; i < 4; ++i) {
218  predicted_marker->patch.coordinates.row(i) += delta;
219  }
220 
221  // Alter the search area as well so it always corresponds to the center.
222  predicted_marker->search_region = last_marker.search_region;
223  predicted_marker->search_region.Offset(delta);
224 }
225 
226 } // namespace
227 
229  const PredictDirection direction,
230  Marker* marker) {
231  // Get all markers for this clip and track.
232  vector<Marker> markers;
233  tracks.GetMarkersForTrackInClip(marker->clip, marker->track, &markers);
234 
235  if (markers.empty()) {
236  LG << "No markers to predict from for " << *marker;
237  return false;
238  }
239 
240  // Order the markers by frame within the clip.
241  vector<Marker*> boxed_markers(markers.size());
242  for (int i = 0; i < markers.size(); ++i) {
243  boxed_markers[i] = &markers[i];
244  }
245  std::sort(boxed_markers.begin(), boxed_markers.end(), OrderByFrameLessThan);
246 
247  // Find the insertion point for this marker among the returned ones.
248  int insert_at = -1; // If we find the exact frame
249  int insert_before = -1; // Otherwise...
250  for (int i = 0; i < boxed_markers.size(); ++i) {
251  if (boxed_markers[i]->frame == marker->frame) {
252  insert_at = i;
253  break;
254  }
255  if (boxed_markers[i]->frame > marker->frame) {
256  insert_before = i;
257  break;
258  }
259  }
260 
261  // Forward starts at the marker or insertion point, and goes forward.
262  int forward_scan_begin, forward_scan_end;
263 
264  // Backward scan starts at the marker or insertion point, and goes backward.
265  int backward_scan_begin, backward_scan_end;
266 
267  // Determine the scanning ranges.
268  if (insert_at == -1 && insert_before == -1) {
269  // Didn't find an insertion point except the end.
270  forward_scan_begin = forward_scan_end = 0;
271  backward_scan_begin = markers.size() - 1;
272  backward_scan_end = 0;
273  } else if (insert_at != -1) {
274  // Found existing marker; scan before and after it.
275  forward_scan_begin = insert_at + 1;
276  forward_scan_end = markers.size() - 1;
277  backward_scan_begin = insert_at - 1;
278  backward_scan_end = 0;
279  } else {
280  // Didn't find existing marker but found an insertion point.
281  forward_scan_begin = insert_before;
282  forward_scan_end = markers.size() - 1;
283  backward_scan_begin = insert_before - 1;
284  backward_scan_end = 0;
285  }
286 
287  const int num_consecutive_needed = 2;
288 
289  if (forward_scan_begin <= forward_scan_end &&
290  forward_scan_end - forward_scan_begin > num_consecutive_needed) {
291  // TODO(keir): Finish this.
292  }
293 
294  bool predict_forward = false;
295  switch (direction) {
297  if (backward_scan_end <= backward_scan_begin) {
298  // TODO(keir): Add smarter handling and detecting of consecutive frames!
299  predict_forward = true;
300  }
301  break;
302 
303  case PredictDirection::FORWARD: predict_forward = true; break;
304 
305  case PredictDirection::BACKWARD: predict_forward = false; break;
306  }
307 
308  const int max_frames_to_predict_from = 20;
309  if (predict_forward) {
310  if (backward_scan_begin - backward_scan_end < num_consecutive_needed) {
311  // Not enough information to do a prediction.
312  LG << "Predicting forward impossible, not enough information";
313  return false;
314  }
315  LG << "Predicting forward";
316  int predict_begin =
317  std::max(backward_scan_begin - max_frames_to_predict_from, 0);
318  int predict_end = backward_scan_begin;
319  vector<Marker*> previous_markers;
320  for (int i = predict_begin; i <= predict_end; ++i) {
321  previous_markers.push_back(boxed_markers[i]);
322  }
323  RunPrediction(previous_markers, marker);
324  return true;
325  } else {
326  if (forward_scan_end - forward_scan_begin < num_consecutive_needed) {
327  // Not enough information to do a prediction.
328  LG << "Predicting backward impossible, not enough information";
329  return false;
330  }
331  LG << "Predicting backward";
332  int predict_begin = std::min(
333  forward_scan_begin + max_frames_to_predict_from, forward_scan_end);
334  int predict_end = forward_scan_begin;
335  vector<Marker*> previous_markers;
336  for (int i = predict_begin; i >= predict_end; --i) {
337  previous_markers.push_back(boxed_markers[i]);
338  }
339  RunPrediction(previous_markers, marker);
340  return true;
341  }
342 }
343 
344 } // namespace mv
NSNotificationCenter * center
void sort(btMatrix3x3 &U, btVector3 &sigma, btMatrix3x3 &V, int t)
Helper function of 3X3 SVD for sorting singular values.
DO_INLINE void filter(lfVector *V, fmatrix3x3 *S)
const vector< Marker > & markers
const int state
#define LG
static void error(const char *str)
Definition: meshlaplacian.c:51
VecMat::Vec2< float > Vec2f
Definition: Geom.h:20
static unsigned a[3]
Definition: RandGen.cpp:78
std::vector< ElementType, Eigen::aligned_allocator< ElementType > > vector
Eigen::Vector2d Vec2
Definition: numeric.h:105
bool PredictMarkerPosition(const Tracks &tracks, const PredictDirection direction, Marker *marker)
PredictDirection
static const pxr::TfToken b("b", pxr::TfToken::Immortal)
#define min(a, b)
Definition: sort.c:35
int frame
Definition: marker.h:42
int clip
Definition: marker.h:41
int track
Definition: marker.h:43
ListBase tracks
Definition: tracking.c:60
double Vec2[2]
float max