cale_e.m

用ＭＡＴＬＡＢ实现ＬＭ算法库

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