function err = cale_e( features, trajectory, test_out ) % CALC_E calculate the error between a trajectory and a set of feature pts % and the output of a motion tracker. % J. Watlington, 11/30/95 % The format of the trajectory data is : % [ tx ty tz wx wy wz B ] per row % The format of the state data is : % [ tx ty tz*B B X0 Y0 Z0 X1 Y1 Z1 ... ] per row sfm_state_size = 4; arbitrary_scaling_constant = 1; % First, determine some input size parameters [ num_time, num_traj ] = size( trajectory ); [ num_features, feature_dim ] = size( features ); [ num_test_outs, test_out_dim ] = size( test_out ); if num_test_outs < num_time num_time = num_test_outs; end state_size = sfm_state_size + (num_features * feature_dim); if state_size ~= test_out_dim 'We got a problem here, dimensions arent equal !' return end % Allocate the result matrix err = zeros( [ num_time, state_size ] ); % preallocate results comp_vec = zeros([ 1, state_size ]); % One of the harder parts of the comparison is that everything in % the solution is relative. We relate the Z parameter of the first % state feature point, which has a system noise cov of 0.0, to the equiv. % feature point parameter. fix_param = feature_dim; % Z component of first feature arb_scale = features(1, fix_param) / (test_out(1, sfm_state_size+fix_param)+1) % Iterate over all the frames of data for time = 1:num_time % Create a vector containing both camera params and projected feature pts % Start with the camera parameters comp_vec( 1:3 ) = trajectory( time, 1:3 ); % translational comps comp_vec( sfm_state_size ) = trajectory( time, 7 ); % beta for ft = 1:num_features index = sfm_state_size + 1 + (feature_dim*(ft - 1)); comp_vec( index:(index+(feature_dim-2))) = features( ft,1:feature_dim-1); comp_vec( index+(feature_dim-1) ) = features( ft,feature_dim) - 1; end % Calculate the error diff = (test_out( time,:) .* arb_scale) - comp_vec; err( time, : ) = (diff .* 100.0) ./ comp_vec end