%
% 1D function approximation example
% for estimating landmark range from sensed blob height
% using a (non-exclusive) set of function approximators (NN, RBF, spline)
%
% Chad Jenkins (cjenkins@cs.brown.edu)
% Modified: October 22, 2009
%
%

close all;
clear all;

% display variables
display_nn = 1; % display nearest neighbor result
display_rbf = 1; % display weighted average result
display_spline = 1; % display spline interpolation result

% width of influence of training data for weighted average
rbf_width = 50;
% scale influence of training datae by distance to nearest other training input 
variable_width_rbf = 1;

% ==========================================================
% initialize training data as input-output pairs
% inputs on 1st row: blob height (pixels)
% outputs on 2nd row: distance (centimeters)
% assume sorted by blob height
% ==========================================================
landmarkdistance_fiducialheight_examples = [
    [15	239];
    [20	219];
    [30	177];
    [40	145];
    [50	122];
    [60	107];
    [70	93];
    [80	84];
    [90	76];
    [100	67];
    [110	63];
    [120	58];
    [130	55];
    [140	51];
    [150	49];
    [160	45];
    [170	43];
    [180	41];
    [190	38];
    [200	37];
    [210	36];
    [220	33];
    [230	31];
    [240	30];
    [250	28];
    [260	27];
    [280	26];
    [300	25];
    [320	24];
    [330	22];
    [350	21];
    [370	20]
    ]';
landmarkdistance_fiducialheight_examples = flipud(landmarkdistance_fiducialheight_examples);

% ==========================================================
% initialize display
% ==========================================================
figure;
plot(landmarkdistance_fiducialheight_examples(1,:),landmarkdistance_fiducialheight_examples(2,:),'b.','MarkerSize',25);
hold on;

% ==========================================================
% generate test inputs to evaluate function
% ==========================================================
interval_start = min(landmarkdistance_fiducialheight_examples(1,:));
interval_end = max(landmarkdistance_fiducialheight_examples(1,:));
test_blobheights = [0:(interval_end-interval_start)/500:interval_end+150];
%test_blobheights = [interval_start:(interval_end-interval_start)/2000:interval_end]

% ==========================================================
% evaluate and display test inputs on function as the closest nearest neighbor (1-NN)
% ==========================================================
nnpredict_range = zeros(size(test_blobheights));
for i = 1:length(test_blobheights)
    [temp_min temp_index] = min((landmarkdistance_fiducialheight_examples(1,:)-repmat(test_blobheights(1,i),size(landmarkdistance_fiducialheight_examples(1,:)))).^2);
    nnpredict_range(1,i) = landmarkdistance_fiducialheight_examples(2,temp_index);
end
if (display_nn)
    plot(test_blobheights,nnpredict_range,'r.');
end

% ==========================================================
% evaluate and display test inputs on function as distance weighted average of training data (radial basis functions)
% ==========================================================

if (variable_width_rbf)
    landmarkdistance_fiducialheight_weights = abs(landmarkdistance_fiducialheight_examples(1,2:end) - landmarkdistance_fiducialheight_examples(1,1:end-1));
    landmarkdistance_fiducialheight_weights = rbf_width*min([[realmax landmarkdistance_fiducialheight_weights];[landmarkdistance_fiducialheight_weights realmax]],[],1);
end
for i = 1:length(test_blobheights)
    temp_weights = exp(-(landmarkdistance_fiducialheight_examples(1,:)-repmat(test_blobheights(1,i),size(landmarkdistance_fiducialheight_examples(1,:)))).^2/rbf_width);
    if (variable_width_rbf)
        temp_weights = exp(-((landmarkdistance_fiducialheight_examples(1,:)-repmat(test_blobheights(1,i),size(landmarkdistance_fiducialheight_examples(1,:)))).^2)./landmarkdistance_fiducialheight_weights);
    end
    temp_weights = temp_weights ./ sum(temp_weights);
    rbfpredict_range(1,i) = landmarkdistance_fiducialheight_examples(2,:)*temp_weights';
end
if (display_rbf)
    plot(test_blobheights,rbfpredict_range,'k.');
end

% ==========================================================
% evaluate and display test inputs on function through spline interpolation of training data
% ==========================================================
%splinepredict_range = spline(landmarkdistance_fiducialheight_examples(1,:),landmarkdistance_fiducialheight_examples(2,:),test_blobheights);
splinepredict_range = interp1(landmarkdistance_fiducialheight_examples(1,:),landmarkdistance_fiducialheight_examples(2,:),test_blobheights,'spline');
%splinepredict_range = interp1(landmarkdistance_fiducialheight_examples(1,:),landmarkdistance_fiducialheight_examples(2,:),test_blobheights,'cubic');
if (display_spline)
    plot(test_blobheights,splinepredict_range,'m.');
end

% label display axes
xlabel('input: blob height (pixels)');
ylabel('output: landmark range (cm)');