## Sunday, March 25, 2012

### Cubic spline interpolation

It's been a long since I actually coded any interpolation method.
Matlab is notorious for making you lazy since it's so easy to get things done and you tend to stop looking under the hood. A friend asked me for help on a cubic interpolation problem and since that was too easy I expanded it so I can use it on my projects.

The math behind cubic spline is really simple. You piecewise fit cubic polynomials using 4 data values (two points and two tangents) in order to create a smooth spline that passes from all given points. The wikipedia sources are really good so I won't dive into the math. Instead I'll provide some matlab code for doing the dirty deed. Matlab (as always) has a command for this (spline) but we wont be using it because I like getting my hands dirty.
function [ yy, xx] = cubicSpline(N,points,gradients)
% CUBICSPLINE - returns N interpolation points using cubic spline
%   interpolation method, this method cannot be used for space curves
%
% Input
% N         : number of interpolation points
% points    : given points [x y]
%
% Output
% xx        : uniform spaced function interpolation at N points
% yy        : uniform spaced N points
%
%-------------------------------------------------------------------------
%% Validate input arguments
if isempty(N) || N < 1
error('N must be >= 1');
end

if isempty(points) || size(points,1) < 1
error('point must have >= 1 rows');
end

if isempty(points) || size(points,2) ~= 2
error('point must have 2 collumns');
end

end
%% coefficient calculation part

% get number of points
[rows ~] = size(points);

% compute inverse matrix to be used
matrix = inv([1 0 0 0 ; 0 1 0 0 ; 1 1 1 1; 0 1 2 3]);

% initialize coefficients structure
coefficients = zeros(rows-1,4);

% given n points we must calculate n-1 polynomials
for i = 2 : rows
pEnd = [];
pStart = [];

% calculate gradient using finite central differences
if (i-1) == 1
else
pStart = (points(i,2) - points(i-2,2))/2;
end

if i == rows
else
pEnd = (points(i+1,2) - points(i-1,2))/2;
end

% create vector [Pi P'i Pi+1 P'i+1]'
vector = [points(i-1,2);pStart;points(i,2);pEnd];

% calculate polynomial coefficients
coefficients(i-1,:) = (matrix * vector)';
end

%% interpolation part

% get max X and min X and interval
minX = points(1,1);
maxX = points(end,1);
intervalX = (maxX - minX) / (N - 1);
xx = minX : intervalX : maxX;

% interpolate at given locations
yy = zeros(1,N);
splineIndex = 1;
for i = 2 : N-1
x = xx(i);
% find the index of the used spline
for j = splineIndex : rows
if x >= points(j,1) && x < points(j+1,1)
splineIndex = j;
break;
end
end
splineCoeffs = coefficients(splineIndex,:);
% compute m
m = (xx(i) - points(splineIndex,1))/...
(points(splineIndex+1,1) - points(splineIndex,1));
% compute value with given spline and m
yy(i) = splineCoeffs(1) + splineCoeffs(2) * m + ...
splineCoeffs(3) * m^2 + splineCoeffs(4) * m^3;
end
yy(1) = points(1,2);
yy(end) = points(end,2);
end



This code can be used to interpolate y=f(x) functions. For example :

%% Demonstration of cubic splines
N = 100;
x = [0:1:10];
y = sin(x);
xOriginal = [0:0.1:10];
yOriginal = sin(xOriginal);
figure;
plot(xOriginal,yOriginal,xx,yy,'--',x,y,'o','LineWidth',2);
legend('Original function','Interpolation spline','Given points');
%%



gives us the following graph :

any errors at the beginning and the end are due to the fact that I entered zero gradient at those points but provided the correct gradients the result should be much more precise.

To to make it somewhat useful in my projects I should use this function as a basis for calculating space curves. This excellent source explains that space curves are functions of u such as y = f(u) and x = f(u).

function [ yy,xx ] = cubicSpline2d(N, points, gradients )
% CUBICSPLINE - returns N interpolation points using cubic spline
%   interpolation method, this method cann be used for space curves
%
% Input
% N         : number of interpolation points
% points    : given points [x y]
%
% Output
% xx        : uniform spaced function interpolation at N points
% yy        : uniform spaced N points
%
%-------------------------------------------------------------------------
%% Validate input arguments
if isempty(N) || N < 1
error('N must be >= 1');
end

if isempty(points) || size(points,1) < 1
error('point must have >= 1 rows');
end

if isempty(points) || size(points,2) ~= 2
error('point must have 2 collumns');
end

end
%%
% get number of points
[rows ~] = size(points);

% get total length of points
u = [1 : rows]';
x = [points(:,1)];
y = [points(:,2)];

end



and the test script

%% Demonstration of cubic splines 2d
u = 0:0.5:2*pi;
N = numel(u)*10;
y = sin(u);
x = sin(u) + cos(u);
gradient = [0 0; 0 0];
plot(xx,yy,'--r',x,y,'ob','LineWidth',2);
legend('Interpolation spline','Given points');
title('Cubic space curve interpolation')
xlabel('x-axis');
ylabel('y-axis');
%%



gives us the following plot :

## Friday, March 2, 2012

### Latest results

As part of my hand tracking project I post these last videos. I believe I reached the top of the performance of the particle filter algorithm.

Unfortunately I want even better results so I should move to more complex algorithms. The problem is that real time performance is going to be much more difficult to achieve.

### Contour Refinement

In my hand tracking project I've used color segmentation to get the hand contour.
The problem is that usually the contour is not perfect, there are parts missing and/or there is some noise, for that reason I coded this general purpose contour refining function that fits the contour on a feature map moving each point along its normal.

        /// <summary>
/// Find better positions for each better at contour by searching on the edge normals on
/// the feature map
/// </summary>
/// <param name="_objectContour">The contour to be refined</param>
/// <param name="_featureMap">The feature map to be refined unto</param>
/// <param name="_normalOffset">The maximum number of pixels to offset</param>
/// <param name="_featureThreshold">The minimum feature value acceptable</param>
/// <returns>Refined Contour</returns>
public static Seq<Point> ContourRefine(
Seq<Point> _objectContour,
Image<Gray, float> _featureMap,
int _normalOffset = 5,
float _featureThreshold = float.MaxValue,
float _inertiaCoeff = 1.0f,
float _multiplierCoeff = -1.0f)
{
List<Point> pointsFitted = new List<Point>();
Point[] pointsArray = _objectContour.ToArray();
for (int i = 0; i < pointsArray.Length; i++)
{
int noPoints = pointsArray.Length,
ki = (i + 1) % noPoints, ik = (i >= 1) ? (i - 1) : (noPoints - 1 + i) % noPoints;
Point pointCurrent = pointsArray[i],
pointNext = pointsArray[ki],
pointPrev = pointsArray[ik];
// get normals pointing in and out
PointF pointNormalOut = NormalAtPoint(pointPrev, pointCurrent, pointNext, false),
pointNormalIn = NormalAtPoint(pointPrev, pointCurrent, pointNext, true);
// get points away from normal
Point pointOut = new Point(
(int)Math.Round(pointNormalOut.X * _normalOffset) + pointCurrent.X,
(int)Math.Round(pointNormalOut.Y * _normalOffset) + pointCurrent.Y),
pointIn = new Point(
(int)Math.Round(pointNormalIn.X * _normalOffset) + pointCurrent.X,
(int)Math.Round(pointNormalIn.Y * _normalOffset) + pointCurrent.Y);
LineSegment2D lineOut = new LineSegment2D(pointCurrent, pointOut),
lineIn = new LineSegment2D(pointCurrent, pointIn);

// sample along the normals
float[,] sampleIn = _featureMap.Sample(lineIn);
float[,] sampleOut = _featureMap.Sample(lineOut);
float maxByte = 0.0f, sample = 0.0f;
int j = 0;
bool inOut = false;
// run through the normal pointing out to find the best fit
for (int k = 0; k < sampleOut.Length; k++)
{
sample = sampleOut[k, 0] + _multiplierCoeff * (float)Math.Pow(_inertiaCoeff, k);
if (sample > maxByte)
{
maxByte = sample;
j = k;
inOut = false;
}
}

// run through the normal pointing in to find the best fit
for (int k = 0; k < sampleIn.Length; k++)
{
sample = sampleIn[k, 0] + _multiplierCoeff * (float)Math.Pow(_inertiaCoeff, k);
if (sample > maxByte)
{
maxByte = sample;
j = k;
inOut = true;
}
}

// if feature on point found exceeds a threshold add it to the contour
if (maxByte >= _featureThreshold)
{
int x, y;
double length, xLength, yLength;
if (!inOut)
{
xLength = lineOut.P1.X - lineOut.P2.X;
yLength = lineOut.P1.Y - lineOut.P2.Y;
length = lineOut.Length;
x = (int)Math.Round((float)j / (float)sampleOut.Length * pointNormalOut.X * _normalOffset);
y = (int)Math.Round((float)j / (float)sampleOut.Length * pointNormalOut.Y * _normalOffset);
}
else
{
xLength = lineIn.P1.X - lineIn.P2.X;
yLength = lineIn.P1.Y - lineIn.P2.Y;
length = lineIn.Length;
x = (int)Math.Round((float)j / (float)sampleIn.Length * pointNormalIn.X * _normalOffset);
y = (int)Math.Round((float)j / (float)sampleIn.Length * pointNormalIn.Y * _normalOffset);
}
pointsFitted.Add(new Point(pointCurrent.X + x, pointCurrent.Y + y));
}
}
_objectContour.Clear();
_objectContour.PushMulti(pointsFitted.ToArray(), BACK_OR_FRONT.BACK);
return _objectContour;
}

/// <summary>
/// Calulcate the normal at given point
/// </summary>
/// <param name="_prevPoint">Previous point</param>
/// <param name="_currentPoint">Current point</param>
/// <param name="_nextPoint">Next point</param>
/// <param name="_inOut">In or out flag</param>
/// <returns>Normal at point</returns>
public static PointF NormalAtPoint(
Point _prevPoint,
Point _currentPoint,
Point _nextPoint,
bool _inOut = true)
{
PointF normal;
float dx1 = _currentPoint.X - _prevPoint.X,
dx2 = _nextPoint.X - _currentPoint.X,
dy1 = _currentPoint.Y - _prevPoint.Y,
dy2 = _nextPoint.Y - _currentPoint.Y;
if (_inOut)
normal = new PointF((dy1 + dy2) * 0.5f, -(dx1 + dx2) * 0.5f);
else
normal = new PointF(-(dy1 + dy2) * 0.5f, (dx1 + dx2) * 0.5f);
return NormalizePoint(normal);
}

/// <summary>
/// Normalize a given point so its _noBinsAngle equals to one
/// </summary>
/// <param name="_point">Point to normalize</param>
/// <returns>Normalized point</returns>
public static PointF NormalizePoint(PointF _point)
{
float length = (float)Math.Sqrt(_point.X * _point.X + _point.Y * _point.Y);
if (length > 0.0f)
return new PointF(_point.X / length, _point.Y / length);
return new PointF(0.0f, 0.0f);
}