/// <summary>
/// Computes instants speeds from a coordinate trace and stores the result
/// in the provided buffer
/// </summary>
/// <param name="path">The movement path</param>
/// <param name="frameRate">The framerate at acquisition</param>
/// <param name="isb">The buffer to store the speed in</param>
public void ComputeInstantSpeeds(CentroidBuffer path, int frameRate, InstantSpeedBuffer isb)
{
if (IsDisposed)
{
throw new ObjectDisposedException(this.ToString());
}
if (isb.Size.width != path.Size.width)
{
throw new ArgumentException("Path length and size of speed buffer must be the same!");
}
//Check that our buffers are present and match, otherwise fix
if (_calc1 == null)
{
_calc1 = new CentroidBuffer(path.Size.width);
_calc2 = new CentroidBuffer(path.Size.width);
}
else if (_calc1.Size.width != path.Size.width)
{
_calc1.Dispose();
_calc2.Dispose();
_calc1 = new CentroidBuffer(path.Size.width);
_calc2 = new CentroidBuffer(path.Size.width);
}
//compute difference trace subtracting the position at t from the position at t+1 by subtracting the unshifted input buffer from its shifted version
//the result gets stored in calc1 with the first value being 0
ip.ippiSet_32f_C1R(0, _calc1.Buffer, _calc1.Stride, _calc1.Size);
IppHelper.IppCheckCall(ip.ippiSub_32f_C1R(path.Buffer, path.Stride, (float *)((byte *)path.Buffer + 4), path.Stride, (float *)((byte *)_calc1.Buffer + 4), _calc1.Stride, new IppiSize(path.Size.width - 1, 2)));
//square the difference trace, storing the result in _calc2
IppHelper.IppCheckCall(ip.ippiSqr_32f_C1R(_calc1.Buffer, _calc1.Stride, _calc2.Buffer, _calc2.Stride, _calc1.Size));
//Add the values of the x and y coordinates, storing the sum of squares in the first row of _calc1
IppHelper.IppCheckCall(ip.ippiAdd_32f_C1R(_calc2.Buffer, _calc2.Stride, (float *)((byte *)_calc2.Buffer + _calc2.Stride), _calc2.Stride, _calc1.Buffer, _calc1.Stride, new IppiSize(_calc1.Size.width, 1)));
//Compute the square root of the sum-of-squares and copy the result to the first row of _calc2
IppHelper.IppCheckCall(ip.ippiSqrt_32f_C1R(_calc1.Buffer, _calc1.Stride, _calc2.Buffer, _calc2.Stride, new IppiSize(_calc1.Size.width, 1)));
//Multiply the distances by the framerate and store the result in our instant speed buffer
IppHelper.IppCheckCall(ip.ippiMulC_32f_C1R(_calc2.Buffer, _calc2.Stride, frameRate, isb.Buffer, isb.Stride, isb.Size));
}
/// <summary>
/// Implements coordinate smoothing analogous to using filtfilt in matlab with a step function kernel
/// (moving average with no peak displacement)
/// </summary>
/// <param name="src">The source track to smoothen</param>
/// <param name="dst">The destination buffer for the smoothened track</param>
/// <param name="windowSize">The windowsize for averaging</param>
public void SmoothenTrack(CentroidBuffer src, CentroidBuffer dst, int windowSize)
{
if (IsDisposed)
{
throw new ObjectDisposedException(this.ToString());
}
if (src.Size.width != dst.Size.width)
{
throw new ArgumentException("Source and destination buffers need to have the same size!");
}
//For the internal buffers we require a size that fits both the coordinate buffer we
//intend to filter as well as the border pixels required for filtering
int borderSize = (int)Math.Ceiling(windowSize / 2.0);
int reqSize = src.Size.width + borderSize * 2;
//Adjust internal buffers if necessary
if (_calc1 == null)
{
_calc1 = new CentroidBuffer(reqSize);
_calc2 = new CentroidBuffer(reqSize);
}
else if (_calc1.Size.width != reqSize)
{
_calc1.Dispose();
_calc2.Dispose();
_calc1 = new CentroidBuffer(reqSize);
_calc2 = new CentroidBuffer(reqSize);
}
if (_kernel == null)
{
_kernelSize = windowSize;
_kernel = (float *)Marshal.AllocHGlobal(_kernelSize * 4);
int i = 0;
while (i < _kernelSize)
{
_kernel[i++] = 1 / (float)_kernelSize;
}
}
else if (_kernelSize != windowSize)
{
Marshal.FreeHGlobal((IntPtr)_kernel);
_kernelSize = windowSize;
_kernel = (float *)Marshal.AllocHGlobal(_kernelSize * 4);
int i = 0;
while (i < _kernelSize)
{
_kernel[i++] = 1 / (float)_kernelSize;
}
}
//filter parameters
IppiSize regionSize = new IppiSize(src.Size.width, 1);
IppiPoint anchor = new IppiPoint(borderSize, 0);
IppiSize kernelSize = new IppiSize(_kernelSize, 1);
float * calc1XStart = (float *)((byte *)_calc1.Buffer + borderSize * 4);
float * calc1YStart = (float *)((byte *)_calc1.Buffer + borderSize * 4 + _calc1.Stride);
float * calc2XStart = (float *)((byte *)_calc2.Buffer + borderSize * 4);
float * calc2YStart = (float *)((byte *)_calc2.Buffer + borderSize * 4 + _calc2.Stride);
//Copy src buffer adding borders
IppHelper.IppCheckCall(ip.ippiCopyConstBorder_32f_C1R(src.Buffer, src.Stride, src.Size, _calc1.Buffer, _calc1.Stride, _calc1.Size, 0, borderSize, 0));
//Fill calc2 to have borders ready after filtering
IppHelper.IppCheckCall(ip.ippiSet_32f_C1R(0, _calc2.Buffer, _calc2.Stride, _calc2.Size));
//filter x-coordinates with our kernel
IppHelper.IppCheckCall(ip.ippiFilter_32f_C1R(calc1XStart, _calc1.Stride, calc2XStart, _calc2.Stride, regionSize, _kernel, kernelSize, anchor));
//filter y-coordinates with our kernel
IppHelper.IppCheckCall(ip.ippiFilter_32f_C1R(calc1YStart, _calc1.Stride, calc2YStart, _calc2.Stride, regionSize, _kernel, kernelSize, anchor));
//invert buffer - mirror on vertical axis
IppHelper.IppCheckCall(ip.ippiMirror_32f_C1R(_calc2.Buffer, _calc2.Stride, _calc1.Buffer, _calc1.Stride, _calc2.Size, IppiAxis.ippAxsVertical));
//filter x-coordinates with our kernel - now on inverted buffer
IppHelper.IppCheckCall(ip.ippiFilter_32f_C1R(calc1XStart, _calc1.Stride, calc2XStart, _calc2.Stride, regionSize, _kernel, kernelSize, anchor));
//filter y-coordinates with our kernel - now on inverted buffer
IppHelper.IppCheckCall(ip.ippiFilter_32f_C1R(calc1YStart, _calc1.Stride, calc2YStart, _calc2.Stride, regionSize, _kernel, kernelSize, anchor));
//flip buffer back
IppHelper.IppCheckCall(ip.ippiMirror_32f_C1R(_calc2.Buffer, _calc2.Stride, _calc1.Buffer, _calc1.Stride, _calc2.Size, IppiAxis.ippAxsVertical));
//copy to dest
IppHelper.IppCheckCall(ip.ippiCopy_32f_C1R(calc1XStart, _calc1.Stride, dst.Buffer, dst.Stride, dst.Size));
}
