39 const double dt = 3.8;
62 const double velocity_state_transition_data[] = {
77 const double acceleration_state_transition_data[] = {
78 1, dt, dt*dt/2, 0, 0, 0,
81 0, 0, 0, 1, dt, dt*dt/2,
91 const double angular_state_transition_data[] = {
102 const double* state_transition_data = velocity_state_transition_data;
106 const double observation_data[] = {
107 1., 0., 0., 0., 0., 0.,
108 0., 0., 0., 1., 0., 0.
115 const double process_covariance_data[] = {
126 const double measurement_covariance_data[] = {
136 const double initial_covariance_data[] = {
148 TrackerKalman
filter(state_transition_data,
150 process_covariance_data,
151 measurement_covariance_data);
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;
158 return a->clip <
b->clip;
160 return a->frame <
b->frame;
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;
170 Eigen::Matrix<double, 6, 6, Eigen::RowMajor>(initial_covariance_data);
172 int current_frame = previous_markers[0]->frame;
173 int target_frame = predicted_marker->frame;
175 bool predict_forward = current_frame < target_frame;
176 int frame_delta = predict_forward ? 1 : -1;
178 for (
int i = 1; i < previous_markers.size(); ++i) {
181 for (; current_frame != previous_markers[i]->frame;
182 current_frame += frame_delta) {
185 LG <<
"Predicted point (frame " << current_frame <<
"): " <<
state.mean(0)
186 <<
", " <<
state.mean(3);
189 Vec2 error = previous_markers[i]->center.cast<
double>() -
191 LG <<
"Prediction error for " << predictions <<
" steps: (" <<
error.x()
192 <<
", " <<
error.y() <<
"); norm: " <<
error.norm();
195 filter.Update(previous_markers[i]->
center.cast<
double>(),
196 Eigen::Matrix<double, 2, 2, Eigen::RowMajor>(
197 measurement_covariance_data),
199 LG <<
"Updated point: " <<
state.mean(0) <<
", " <<
state.mean(3);
203 for (; current_frame != target_frame; current_frame += frame_delta) {
205 LG <<
"Final predicted point (frame " << current_frame
206 <<
"): " <<
state.mean(0) <<
", " <<
state.mean(3);
210 predicted_marker->center.x() =
state.mean(0);
211 predicted_marker->center.y() =
state.mean(3);
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;
222 predicted_marker->search_region = last_marker.search_region;
223 predicted_marker->search_region.Offset(delta);
236 LG <<
"No markers to predict from for " << *marker;
241 vector<Marker*> boxed_markers(
markers.size());
242 for (
int i = 0; i <
markers.size(); ++i) {
243 boxed_markers[i] = &
markers[i];
245 std::sort(boxed_markers.begin(), boxed_markers.end(), OrderByFrameLessThan);
249 int insert_before = -1;
250 for (
int i = 0; i < boxed_markers.size(); ++i) {
251 if (boxed_markers[i]->frame == marker->
frame) {
255 if (boxed_markers[i]->frame > marker->
frame) {
262 int forward_scan_begin, forward_scan_end;
265 int backward_scan_begin, backward_scan_end;
268 if (insert_at == -1 && insert_before == -1) {
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) {
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;
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;
287 const int num_consecutive_needed = 2;
289 if (forward_scan_begin <= forward_scan_end &&
290 forward_scan_end - forward_scan_begin > num_consecutive_needed) {
294 bool predict_forward =
false;
297 if (backward_scan_end <= backward_scan_begin) {
299 predict_forward =
true;
308 const int max_frames_to_predict_from = 20;
309 if (predict_forward) {
310 if (backward_scan_begin - backward_scan_end < num_consecutive_needed) {
312 LG <<
"Predicting forward impossible, not enough information";
315 LG <<
"Predicting forward";
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]);
323 RunPrediction(previous_markers, marker);
326 if (forward_scan_end - forward_scan_begin < num_consecutive_needed) {
328 LG <<
"Predicting backward impossible, not enough information";
331 LG <<
"Predicting backward";
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]);
339 RunPrediction(previous_markers, marker);
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
static void error(const char *str)
VecMat::Vec2< float > Vec2f
std::vector< ElementType, Eigen::aligned_allocator< ElementType > > vector
bool PredictMarkerPosition(const Tracks &tracks, const PredictDirection direction, Marker *marker)
static const pxr::TfToken b("b", pxr::TfToken::Immortal)