Libmv: Fix strict compiler warnings, unused variables
[blender.git] / intern / libmv / libmv / autotrack / predict_tracks.cc
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
23 #include "libmv/autotrack/marker.h"
24 #include "libmv/autotrack/predict_tracks.h"
25 #include "libmv/autotrack/tracks.h"
26 #include "libmv/base/vector.h"
27 #include "libmv/logging/logging.h"
28 #include "libmv/tracking/kalman_filter.h"
29
30 namespace mv {
31
32 namespace {
33
34 using libmv::vector;
35 using libmv::Vec2;
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 const double velocity_state_transition_data[] = {
61   1, dt,       0,  0,  0,        0,
62   0,  1,       0,  0,  0,        0,
63   0,  0,       1,  0,  0,        0,
64   0,  0,       0,  1, dt,        0,
65   0,  0,       0,  0,  1,        0,
66   0,  0,       0,  0,  0,        1
67 };
68
69 #if 0
70 // This 3rd-order system also models acceleration. This makes for "jerky"
71 // predictions, but that tend to be more accurate.
72 const double acceleration_state_transition_data[] = {
73   1, dt, dt*dt/2,  0,  0,        0,
74   0,  1,      dt,  0,  0,        0,
75   0,  0,       1,  0,  0,        0,
76   0,  0,       0,  1, dt,  dt*dt/2,
77   0,  0,       0,  0,  1,       dt,
78   0,  0,       0,  0,  0,        1
79 };
80
81 // This system (attempts) to add an angular velocity component. However, it's
82 // total junk.
83 const double angular_state_transition_data[] = {
84   1, dt,     -dt,  0,  0,        0,   // Position x
85   0,  1,       0,  0,  0,        0,   // Velocity x
86   0,  0,       1,  0,  0,        0,   // Angular momentum
87   0,  0,      dt,  1, dt,        0,   // Position y
88   0,  0,       0,  0,  1,        0,   // Velocity y
89   0,  0,       0,  0,  0,        1    // Ignored
90 };
91 #endif
92
93 const double* state_transition_data = velocity_state_transition_data;
94
95 // Observation matrix.
96 const double observation_data[] = {
97   1., 0., 0., 0., 0., 0.,
98   0., 0., 0., 1., 0., 0.
99 };
100
101 // Process covariance.
102 const double process_covariance_data[] = {
103  35,  0,  0,  0,  0,  0,
104   0,  5,  0,  0,  0,  0,
105   0,  0,  5,  0,  0,  0,
106   0,  0,  0, 35,  0,  0,
107   0,  0,  0,  0,  5,  0,
108   0,  0,  0,  0,  0,  5
109 };
110
111 // Process covariance.
112 const double measurement_covariance_data[] = {
113   0.01,  0.00,
114   0.00,  0.01,
115 };
116
117 // Initial covariance.
118 const double initial_covariance_data[] = {
119  10,  0,  0,  0,  0,  0,
120   0,  1,  0,  0,  0,  0,
121   0,  0,  1,  0,  0,  0,
122   0,  0,  0, 10,  0,  0,
123   0,  0,  0,  0,  1,  0,
124   0,  0,  0,  0,  0,  1
125 };
126
127 typedef mv::KalmanFilter<double, 6, 2> TrackerKalman;
128
129 TrackerKalman filter(state_transition_data,
130                      observation_data,
131                      process_covariance_data,
132                      measurement_covariance_data);
133
134 bool OrderByFrameLessThan(const Marker* a, const Marker* b) {
135   if (a->frame == b->frame) {
136     if (a->clip == b->clip) {
137       return a->track < b->track;
138     }
139     return a->clip < b->clip;
140   }
141   return a->frame < b-> frame;
142 }
143
144 // Predicted must be after the previous markers (in the frame numbering sense).
145 void RunPrediction(const vector<Marker*> previous_markers,
146                    Marker* predicted_marker) {
147   TrackerKalman::State state;
148   state.mean << previous_markers[0]->center.x(), 0, 0,
149                 previous_markers[0]->center.y(), 0, 0;
150   state.covariance = Eigen::Matrix<double, 6, 6, Eigen::RowMajor>(
151       initial_covariance_data);
152
153   int current_frame = previous_markers[0]->frame;
154   int target_frame = predicted_marker->frame;
155
156   bool predict_forward = current_frame < target_frame;
157   int frame_delta = predict_forward ? 1 : -1;
158
159   for (int i = 1; i < previous_markers.size(); ++i) {
160     // Step forward predicting the state until it is on the current marker.
161     int predictions = 0;
162     for (;
163          current_frame != previous_markers[i]->frame;
164          current_frame += frame_delta) {
165       filter.Step(&state);
166       predictions++;
167       LG << "Predicted point (frame " << current_frame << "): "
168          << state.mean(0) << ", " << state.mean(3);
169     }
170     // Log the error -- not actually used, but interesting.
171     Vec2 error = previous_markers[i]->center.cast<double>() -
172                  Vec2(state.mean(0), state.mean(3));
173     LG << "Prediction error for " << predictions << " steps: ("
174        << error.x() << ", " << error.y() << "); norm: " << error.norm();
175     // Now that the state is predicted in the current frame, update the state
176     // based on the measurement from the current frame.
177     filter.Update(previous_markers[i]->center.cast<double>(),
178                   Eigen::Matrix<double, 2, 2, Eigen::RowMajor>(
179                       measurement_covariance_data),
180                   &state);
181     LG << "Updated point: " << state.mean(0) << ", " << state.mean(3);
182   }
183   // At this point as all the prediction that's possible is done. Finally
184   // predict until the target frame.
185   for (; current_frame != target_frame; current_frame += frame_delta) {
186     filter.Step(&state);
187     LG << "Final predicted point (frame " << current_frame << "): "
188        << state.mean(0) << ", " << state.mean(3);
189   }
190
191   // The x and y positions are at 0 and 3; ignore acceleration and velocity.
192   predicted_marker->center.x() = state.mean(0);
193   predicted_marker->center.y() = state.mean(3);
194
195   // Take the patch from the last marker then shift it to match the prediction.
196   const Marker& last_marker = *previous_markers[previous_markers.size() - 1];
197   predicted_marker->patch = last_marker.patch;
198   Vec2f delta = predicted_marker->center - last_marker.center;
199   for (int i = 0; i < 4; ++i) {
200     predicted_marker->patch.coordinates.row(i) += delta;
201   }
202
203   // Alter the search area as well so it always corresponds to the center.
204   predicted_marker->search_region = last_marker.search_region;
205   predicted_marker->search_region.Offset(delta);
206 }
207
208 }  // namespace
209
210 bool PredictMarkerPosition(const Tracks& tracks, Marker* marker) {
211   // Get all markers for this clip and track.
212   vector<Marker> markers;
213   tracks.GetMarkersForTrackInClip(marker->clip, marker->track, &markers);
214
215   if (markers.empty()) {
216     LG << "No markers to predict from for " << *marker;
217     return false;
218   }
219
220   // Order the markers by frame within the clip.
221   vector<Marker*> boxed_markers(markers.size());
222   for (int i = 0; i < markers.size(); ++i) {
223     boxed_markers[i] = &markers[i];
224   }
225   std::sort(boxed_markers.begin(), boxed_markers.end(), OrderByFrameLessThan);
226
227   // Find the insertion point for this marker among the returned ones.
228   int insert_at = -1;      // If we find the exact frame
229   int insert_before = -1;  // Otherwise...
230   for (int i = 0; i < boxed_markers.size(); ++i) {
231     if (boxed_markers[i]->frame == marker->frame) {
232       insert_at = i;
233       break;
234     }
235     if (boxed_markers[i]->frame > marker->frame) {
236       insert_before = i;
237       break;
238     }
239   }
240
241   // Forward starts at the marker or insertion point, and goes forward.
242   int forward_scan_begin, forward_scan_end;
243
244   // Backward scan starts at the marker or insertion point, and goes backward.
245   int backward_scan_begin, backward_scan_end;
246
247   // Determine the scanning ranges.
248   if (insert_at == -1 && insert_before == -1) {
249     // Didn't find an insertion point except the end.
250     forward_scan_begin = forward_scan_end = 0;
251     backward_scan_begin = markers.size() - 1;
252     backward_scan_end = 0;
253   } else if (insert_at != -1) {
254     // Found existing marker; scan before and after it.
255     forward_scan_begin = insert_at + 1;
256     forward_scan_end = markers.size() - 1;;
257     backward_scan_begin = insert_at - 1;
258     backward_scan_end = 0;
259   } else {
260     // Didn't find existing marker but found an insertion point.
261     forward_scan_begin = insert_before;
262     forward_scan_end = markers.size() - 1;;
263     backward_scan_begin = insert_before - 1;
264     backward_scan_end = 0;
265   }
266
267   const int num_consecutive_needed = 2;
268
269   if (forward_scan_begin <= forward_scan_end &&
270       forward_scan_end - forward_scan_begin > num_consecutive_needed) {
271     // TODO(keir): Finish this.
272   }
273
274   bool predict_forward = false;
275   if (backward_scan_end <= backward_scan_begin) {
276     // TODO(keir): Add smarter handling and detecting of consecutive frames!
277     predict_forward = true;
278   }
279
280   const int max_frames_to_predict_from = 20;
281   if (predict_forward) {
282     if (backward_scan_begin - backward_scan_end < num_consecutive_needed) {
283       // Not enough information to do a prediction.
284       LG << "Predicting forward impossible, not enough information";
285       return false;
286     }
287     LG << "Predicting forward";
288     int predict_begin =
289         std::max(backward_scan_begin - max_frames_to_predict_from, 0);
290     int predict_end = backward_scan_begin;
291     vector<Marker*> previous_markers;
292     for (int i = predict_begin; i <= predict_end; ++i) {
293       previous_markers.push_back(boxed_markers[i]);
294     }
295     RunPrediction(previous_markers, marker);
296     return true;
297   } else {
298     if (forward_scan_end - forward_scan_begin < num_consecutive_needed) {
299       // Not enough information to do a prediction.
300       LG << "Predicting backward impossible, not enough information";
301       return false;
302     }
303     LG << "Predicting backward";
304     int predict_begin =
305         std::min(forward_scan_begin + max_frames_to_predict_from,
306                  forward_scan_end);
307     int predict_end = forward_scan_begin;
308     vector<Marker*> previous_markers;
309     for (int i = predict_begin; i >= predict_end; --i) {
310       previous_markers.push_back(boxed_markers[i]);
311     }
312     RunPrediction(previous_markers, marker);
313     return false;
314   }
315
316 }
317
318 }  // namespace mv