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]
% gradients : gradient at first and last point 
%
% 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
    
    if isempty(gradients) || numel(gradients) ~= 2
        error('gradients must have 2 elements');
    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
            pStart = gradients(1);
        else
            pStart = (points(i,2) - points(i-2,2))/2;
        end
        
        if i == rows
            pEnd = gradients(2);
        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);
gradient = [0 0];
[yy xx] = cubicSpline(N,[x' y'],gradient);
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]
% gradients : gradient at first and last point 
%
% 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
    
    if isempty(gradients) || numel(gradients) ~= 4
        error('gradients must have 4 elements');
    end
    %%
    % get number of points
    [rows ~] = size(points);
    
    % get total length of points
    u = [1 : rows]';
    x = [points(:,1)];
    y = [points(:,2)];
    
    [xx,~] = cubicSpline(N, [u x],gradients(:,1));
    [yy,~] = cubicSpline(N, [u y],gradients(:,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];
[yy xx] = cubicSpline2d(N,[x' y'],gradient);
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 :

You can download the code here.

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.

Download file here

        /// <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);
        }