Differential D8756 Diff 28314 libmv/autotrack/predict_tracks.cc

Changeset View

Standalone View

libmv/autotrack/predict_tracks.cc

Context not available.
	//	//
	// Author: mierle@gmail.com (Keir Mierle)	// Author: mierle@gmail.com (Keir Mierle)

		#include <cassert>

	#include "libmv/autotrack/marker.h"	#include "libmv/autotrack/marker.h"
	#include "libmv/autotrack/predict_tracks.h"	#include "libmv/autotrack/predict_tracks.h"
	#include "libmv/autotrack/tracks.h"	#include "libmv/autotrack/tracks.h"
	#include "libmv/base/vector.h"	#include "libmv/base/vector.h"
	#include "libmv/logging/logging.h"	#include "libmv/logging/logging.h"
	#include "libmv/tracking/kalman_filter.h"

	namespace mv {	namespace mv {

Context not available.
	0, 0, 0, 0, 0, 1	0, 0, 0, 0, 0, 1
	};	};

	typedef mv::KalmanFilter<double, 6, 2> TrackerKalman;

	TrackerKalman filter(state_transition_data,	TrackerKalman filter(state_transition_data,
	observation_data,	observation_data,
Context not available.

	}	}

		// Initialize the Kalman state.
		void KalmanFilterState::Init(const Marker& first_marker, int frameStep) {
		assert(!_hasInitializedState);
		_state.mean << first_marker.center.x(), 0, 0,
		first_marker.center.y(), 0, 0;
		_state.covariance = Eigen::Matrix<double, 6, 6, Eigen::RowMajor>(
		initial_covariance_data);
		_stateFrame = first_marker.frame;
		_previousMarker = first_marker;
		_frameStep = frameStep;
		_hasInitializedState = true;
		}

		// predict forward until target_frame is reached
		// previous_marker: previous marker position (predicted or measured)
		bool KalmanFilterState::PredictForward(const int target_frame,
		Marker* predicted_marker) {
		assert(_previousMarker.frame == _stateFrame);

		// Step forward predicting the state until target_frame.
		for (;
		_stateFrame != target_frame;
		_stateFrame += _frameStep) {
		filter.Step(&_state);
		LG << "Predicted point (frame " << _stateFrame << "): "
		<< _state.mean(0) << ", " << _state.mean(3);
		}
		predicted_marker->frame = target_frame;
		// The x and y positions are at 0 and 3; ignore acceleration and velocity.
		predicted_marker->center.x() = _state.mean(0);
		predicted_marker->center.y() = _state.mean(3);

		// Take the patch from the last marker then shift it to match the prediction.
		predicted_marker->patch = _previousMarker.patch;
		Vec2f delta = predicted_marker->center - _previousMarker.center;
		for (int i = 0; i < 4; ++i) {
		predicted_marker->patch.coordinates.row(i) += delta;
		}

		// Alter the search area as well so it always corresponds to the center.
		predicted_marker->search_region = _previousMarker.search_region;
		predicted_marker->search_region.Offset(delta);
		_previousMarker = *predicted_marker;
		return _hasInitializedState;
		} // PredictForward

		// returns true if update was succesful
		bool KalmanFilterState::Update(const Marker& measured_marker) {
		assert(measured_marker.frame == _stateFrame);
		// Log the error -- not actually used, but interesting.
		Vec2 error = measured_marker.center.cast<double>() -
		Vec2(_state.mean(0), _state.mean(3));
		LG << "Prediction error: ("
		<< error.x() << ", " << error.y() << "); norm: " << error.norm();
		// Now that the state is predicted in the current frame, update the state
		// based on the measurement from the current frame.
		filter.Update(measured_marker.center.cast<double>(),
		Eigen::Matrix<double, 2, 2, Eigen::RowMajor>(
		measurement_covariance_data),
		&_state);
		LG << "Updated point: " << _state.mean(0) << ", " << _state.mean(3);
		_previousMarker = measured_marker;
		return true;
		} // Update

	} // namespace mv	} // namespace mv
Context not available.