private Matrix CovarianceEstimation(Matrix kalmanGain)
{
var KH = (kalmanGain * H);
var khOrder = KH.ColumnCount;
return((Matrix.Identity(khOrder) - KH) * P);
}
public void Test_EstimatePolynomial()
{
// Polynomial a(x-r0)(x-r1)...
int n = 8;
float[] x = new float[] { -1.2f, 1.2f, 1.4f, 1.6f, 1.8f, 2.2f, -1.6f, -1.4f };
Matrix<float> estimationMatrix = new MathNet.Numerics.LinearAlgebra.Single.DenseMatrix(n, 2);
for(int r = 0; r < n; ++r)
{
estimationMatrix.At(r, 0, x[r]);
estimationMatrix.At(r, 1, (float)PolyValue(x[r]));
}
Polynomial poly = Polynomial.EstimatePolynomial(estimationMatrix, rank);
// Check if values are same for extimated coeffs and real poly
double[] testX = new double[] { 1.01, -10.0, 100.0 };
double real0 = PolyValue(testX[0]);
double est0 = poly.At((float)testX[0]);
double real1 = PolyValue(testX[1]);
double est1 = poly.At((float)testX[1]);
double real2 = PolyValue(testX[2]);
double est2 = poly.At((float)testX[2]);
Assert.IsTrue(Math.Abs(PolyValue(testX[0]) / poly.At((float)testX[0]) - 1.0) < 1e-3);
Assert.IsTrue(Math.Abs(PolyValue(testX[1]) / poly.At((float)testX[1]) - 1.0) < 1e-3);
Assert.IsTrue(Math.Abs(PolyValue(testX[2]) / poly.At((float)testX[2]) - 1.0) < 1e-3);
}
public void Test_FindRoots_EstimatedPoly()
{
int n = 8;
float[] x = new float[] { -1.2f, 1.2f, 1.4f, 1.6f, 1.8f, 2.2f, -1.6f, -1.4f };
Matrix<float> estimationMatrix = new MathNet.Numerics.LinearAlgebra.Single.DenseMatrix(n, 2);
for(int r = 0; r < n; ++r)
{
estimationMatrix.At(r, 0, x[r]);
estimationMatrix.At(r, 1, (float)PolyValue(x[r]));
}
Polynomial poly = Polynomial.EstimatePolynomial(estimationMatrix, rank);
PolynomialRootFinder rootFinder = new PolynomialRootFinder();
rootFinder.Poly = poly;
rootFinder.Process();
var roots = rootFinder.RealRoots;
Assert.IsTrue(roots.Count == rank);
Array.Sort(_r);
roots.Sort();
for(int i = 0; i < rank; ++i)
{
Assert.IsTrue( Math.Abs(roots[i] / _r[i] - 1.0f) < 1e-4f ||
Math.Abs(roots[i] - _r[i]) < 1e-4f);
}
}
static Matrix QGenerator(float dt, float stdDevProcessNoise)
{
// B * B^T * stdDev_w ^ 2
var dt2 = dt * dt;
var dt3 = dt2 * dt;
var BBT = Matrix.OfArray(new float[, ] {
{ 0.25f * dt2 * dt2, 0.5f * dt3 },
{ 0.5f * dt3, dt2 }
});
return(BBT * stdDevProcessNoise * stdDevProcessNoise);
}
public KalmanFilter(KalmanSetup setup)
{
this.stdDevProcess = setup.StdDevProcess;
this.stdDevMeasurement = setup.StdDevMeasurement;
this.X = setup.X0;
this.P = setup.P0;
this.H = setup.H;
this.F = setup.funcF;
this.B = setup.funcB;
this.Q = setup.funcQ;
this.R = setup.funcR(stdDevMeasurement);
}
public Vector Update(float dt, Vector measurement, Vector controlInput)
{
// Predict
this.X = PredictState(dt, controlInput);
this.P = PredictCovariance(dt);
// Update
var y = MeasurementResidual(measurement);
var S = CovarianceResidual();
var K = KalmanGain(S);
this.X = StateEstimation(K, y);
this.P = CovarianceEstimation(K);
return(this.X);
}
private Vector CrossProduct(Vector u, Vector v)
{
var iArr = new float[, ] {
{ u[1], u[2] }, { v[1], v[2] }
};
var jArr = new float[, ] {
{ u[2], u[0] }, { v[2], v[0] }
};
var kArr = new float[, ] {
{ u[0], u[1] }, { v[0], v[1] }
};
var i = DMatrix.OfArray(iArr);
var j = DMatrix.OfArray(jArr);
var k = DMatrix.OfArray(kArr);
var nArr = new float[] { i.Determinant(), j.Determinant(), k.Determinant() };
return(new DVector(nArr));
}
private ulong onePointKnn(
DenseColumnMajorMatrixStorage <float> query,
int i,
DenseColumnMajorMatrixStorage <int> indices,
DenseColumnMajorMatrixStorage <float> dists2,
Vector <float> maxRadii,
int k,
float epsilon,
SearchOptionFlags optionFlags)
{
var results = new ListPriorityQueue <int>(maxSize: k);
var queryMatrix = new MathNet.Numerics.LinearAlgebra.Single.DenseMatrix(query);
var q = queryMatrix.Column(i);
for (int j = 0; j < cloud.ColumnCount; j++)
{
var c = cloud.Column(j);
var diff = (c - q);
float l2 = diff * diff;
results.Enqueue(j, l2);
}
int kIdx = 0;
foreach (var j in results.Items)
{
indices.At(kIdx, i, j);
kIdx++;
}
kIdx = 0;
foreach (var d2 in results.Priorities)
{
dists2.At(kIdx, i, d2);
}
return(0);
}
private Vector StateEstimation(Matrix kalmanGain, Vector measurementResidual)
{
return(X + kalmanGain * measurementResidual);
}
private Matrix KalmanGain(Matrix covarianceResidual)
{
return(P * (Matrix)(H.Transpose() * covarianceResidual.Inverse()));
}
public Field4Drepresentation(
DenseMatrix dmListOfData,
Dictionary <string, object> properties,
Dictionary <string, int> valuesColumnsIndeces = null,
SlicingVariable varSliceBy = SlicingVariable.z,
string description = "")
{
InitializeComponent();
variableSliceBy = varSliceBy;
if (valuesColumnsIndeces == null)
{
valuesColumnsIndeces = new Dictionary <string, int>();
valuesColumnsIndeces.Add("x", 0);
valuesColumnsIndeces.Add("y", 1);
valuesColumnsIndeces.Add("z", 2);
valuesColumnsIndeces.Add("values", 3);
}
strDataDescription = description;
ThreadSafeOperations.SetText(lblDescription, strDataDescription, false);
defaultProperties = properties;
strOutputDirectory = (string)defaultProperties["DefaultDataFilesLocation"];
if (!ServiceTools.CheckIfDirectoryExists(strOutputDirectory))
{
strOutputDirectory = "";
}
dmDataList = dmListOfData.Copy();
double dataMaxVal = dmDataList.Column(3).Max();
double dataMinVal = dmDataList.Column(3).Min();
ILScene scene = new ILScene();
currSurfPlotCube = new ILPlotCube();
currSurfPlotCube.TwoDMode = false;
List <List <DenseMatrix> > lDataMatricesForSurfices = ReshapeDataToMatrices(dmDataList, variableSliceBy);
foreach (List <DenseMatrix> currSurfMatricesList in lDataMatricesForSurfices)
{
ILInArray <double> ilaXvalues = currSurfMatricesList[0].ToArray();
ILInArray <double> ilaYvalues = currSurfMatricesList[1].ToArray();
ILInArray <double> ilaZvalues = currSurfMatricesList[2].ToArray();
MathNet.Numerics.LinearAlgebra.Single.DenseMatrix floatDMcolors =
MathNet.Numerics.LinearAlgebra.Single.DenseMatrix.Create(currSurfMatricesList[0].RowCount,
currSurfMatricesList[0].ColumnCount, 0.0f);
currSurfMatricesList[3].MapConvert <Single>(dval => Convert.ToSingle(dval), floatDMcolors,
MathNet.Numerics.LinearAlgebra.Zeros.Include);
ILInArray <float> ilaForColorValues = floatDMcolors.ToArray();
ILSurface currSurf = new ILSurface(ilaZvalues, ilaXvalues, ilaYvalues);
currSurf.ColorMode = ILSurface.ColorModes.RBGA;
ColorScheme newCS = new ColorScheme("");
//ColorSchemeRuler newCSR = new ColorSchemeRuler(newCS, dataMinVal, dataMaxVal);
double[,] dmRvalues = DenseMatrix.Create(floatDMcolors.RowCount, floatDMcolors.ColumnCount,
(r, c) =>
{
Bgr currColor = newCS.GetColorByValueAndRange(currSurfMatricesList[3][r, c], dataMinVal, dataMaxVal);
return(currColor.Red / 255.0d);
}).ToArray();
double[,] dmGvalues = DenseMatrix.Create(floatDMcolors.RowCount, floatDMcolors.ColumnCount,
(r, c) =>
{
Bgr currColor = newCS.GetColorByValueAndRange(currSurfMatricesList[3][r, c], dataMinVal, dataMaxVal);
return(currColor.Green / 255.0d);
}).ToArray();
double[,] dmBvalues = DenseMatrix.Create(floatDMcolors.RowCount, floatDMcolors.ColumnCount,
(r, c) =>
{
Bgr currColor = newCS.GetColorByValueAndRange(currSurfMatricesList[3][r, c], dataMinVal, dataMaxVal);
return(currColor.Blue / 255.0d);
}).ToArray();
float[, ,] rgbaArrData = new float[4, dmRvalues.GetLength(0), dmRvalues.GetLength(1)];
for (int i = 0; i < dmRvalues.GetLength(0); i++)
{
for (int j = 0; j < dmRvalues.GetLength(1); j++)
{
rgbaArrData[0, i, j] = Convert.ToSingle(dmRvalues[i, j]);
rgbaArrData[1, i, j] = Convert.ToSingle(dmGvalues[i, j]);
rgbaArrData[2, i, j] = Convert.ToSingle(dmBvalues[i, j]);
rgbaArrData[3, i, j] = 0.3f;
}
}
currSurf.UpdateRGBA(null, rgbaArrData);
currSurf.UseLighting = true;
currSurfPlotCube.Children.Add(currSurf);
//if (currSurfPlotCube.Children.Count() > 4)
//{
// currSurfPlotCube.Children.Add(new ILColorbar());
//}
//if (currSurfPlotCube.Children.Count() > 4)
//{
// break;
//}
}
// surf.Children.Add(new ILColorbar());
//currSurfPlotCube.Children.Add(new ILColorbar());
//currSurfPlotCube.Rotation = Matrix4.Rotation(new Vector3(1, 0, 0), 1.2f) *
// Matrix4.Rotation(new Vector3(0, 0, 1), Math.PI);
currSurfPlotCube.Projection = Projection.Orthographic;
currSurfPlotCube.Plots.Reset();
scene.Add(currSurfPlotCube);
ilPanel1.Scene = scene;
}
public static async Task <PointCloud> GenerateObject(Recording recording, IProgress <int> progress, CancellationToken ct)
{
// Quick access image formats
var dFormat = recording.DepthFormat;
var cFormat = recording.ColourFormat;
#region Check for calibration data
if (!recording.IsCalibratedRecording)
{
throw new IOException("Pre-calibrated recording required!");
}
var calibration = recording.Calibration;
#endregion
#region Kalman Filter setup
var kSetup = new KalmanSetup()
{
funcF = new Func <float, Matrix>(FGenerator),
funcB = new Func <float, Matrix>(BGenerator),
H = Matrix.OfArray(new float[, ] {
{ 1, 0 }
}),
X0 = new Vector(new float[] { 0, 0 }),
P0 = Matrix.OfArray(new float[, ] {
{ 10, 0 }, { 0, 10 }
}),
funcQ = new Func <float, float, Matrix>(QGenerator),
funcR = new Func <float, Matrix>(RGenerator),
StdDevMeasurement = stdDevMeasurementNoise,
StdDevProcess = stdDevProcessNoise
};
float R = calibration.TurntableRadius;
float acc = -4 * mu * g / (3 * R);
var accelerationVector = new Vector(new float[] { acc });
#endregion
#region Initialise pipeline stages
var cv = new HueThresholder(calibration.Markers, calibration.MarkerSearchSpace);
var geom = new PointCloudBuilder(calibration, recording.MappingParameters);
var kf = new KalmanFilter(kSetup);
var opt = new ICPMinimiser(true);
#endregion
// start new pipeline thread
return(await Task.Run <PointCloud>(() =>
{
var returnCloud = new PointCloud();
PointCloud prevCloud = null;
float lastTime = 0;
double theta = 0;
Point3D lastx = new Point3D(0, 0, 0);
int advanceUnit = Processing.Settings.Default.ProcessEveryFrame ? 1 : 10;
#region KF Debug Logging
StreamWriter filterLog = null;
if (DebugSettings.Default.DebugMode)
{
filterLog = new StreamWriter(DebugSettings.Default.LogRoot + @"\filter.csv");
filterLog.AutoFlush = true;
filterLog.WriteLine("t,dt,dx,dtheta,theta,theta_KF,omega_KF");
}
#endregion
for (int i = 0; i < recording.NumberOfFrames; i += advanceUnit)
{
ct.ThrowIfCancellationRequested();
progress.Report(i);
#region Fetch
// Fetch frame from disk
float timestamp = recording.Timestamps[i];
var colour = recording.FetchColourFrame(i);
var depth = recording.FetchDepthFrame(i);
Frame f = new Frame(i, timestamp, depth, dFormat, colour, cFormat);
#endregion
#region Analyse (CV)
var markers2D = cv.IdentifyMarkers(f);
if (markers2D.Count == 0)
{
i -= (advanceUnit - 1);
continue;
}
#endregion
#region Build Geometry (GEOM)
PointCloud cloud;
KeyValuePair <TrackingMarker, Point3D> rotationMarker;
var geomSuccess = geom.ConstructCloud(f, markers2D, out cloud, out rotationMarker);
if (!geomSuccess)
{
i -= (advanceUnit - 1);
continue;
}
#endregion
#region Filter
// reposition marker based on offset
var polarX = CoordinateSystemConvertor.ToSpherical(rotationMarker.Value);
polarX.Azimuthal += (float)rotationMarker.Key.RotationalOffset;
var x = CoordinateSystemConvertor.ToCartesian(polarX);
// rotation approximation
var dx = ((Vector)x - lastx).Norm(3);
var dtheta = dx / calibration.TurntableRadius;
// update cumulative angle (with estimated angle)
theta += dtheta;
// calculate time delta
float dt = f.Timestamp - lastTime;
if (DebugSettings.Default.DebugMode)
{
filterLog.Write("{0},{1},{2},{3},{4},", f.Timestamp, dt, dx, dtheta, theta);
}
// calculate state vector
var stateVector = new Vector(new float[] { (float)theta });
// pass through kalman filter
var filteredStateVector = kf.Update(dt, stateVector, accelerationVector);
// extract angle from state vector
var filteredTheta = filteredStateVector[0];
var filteredOmega = filteredStateVector[1];
if (DebugSettings.Default.DebugMode)
{
filterLog.WriteLine("{0},{1}", filteredTheta, filteredOmega);
}
// update last time, last position and set filtered cumulative theta
theta = filteredTheta;
lastx = x;
lastTime = f.Timestamp;
#endregion
#region Optimise (OPT)
PointCloud optCloud = null;
if (prevCloud == null)
{
optCloud = cloud; // first frame; this is requires no optimising transformation
}
else if (prevCloud.Points.Count == 0)
{
optCloud = cloud; // no points in previous cloud (model); cannot align the current cloud to model
}
else
{
// Build best guess transformation matrix
float sinT = (float)Math.Sin(filteredTheta);
float cosT = (float)Math.Cos(filteredTheta);
var bestGuessTransformation = Matrix.Identity(4);
bestGuessTransformation[0, 0] = cosT;
bestGuessTransformation[0, 2] = -sinT;
bestGuessTransformation[2, 0] = sinT;
bestGuessTransformation[2, 2] = cosT;
// returned object is result of aligning cloud with prevCloud
optCloud = opt.Process(cloud, prevCloud, bestGuessTransformation);
}
prevCloud = cloud;
#endregion
#region Retire
returnCloud.Points.AddRange(optCloud.Points);
#endregion
}
if (DebugSettings.Default.DebugMode)
{
filterLog.Close();
}
return returnCloud;
}));
}
static Matrix RGenerator(float stdDevMeasurementNoise)
{
return(Matrix.OfArray(new float[, ] {
{ stdDevMeasurementNoise *stdDevMeasurementNoise }
}));
}
static Matrix BGenerator(float dt)
{
return(Matrix.OfArray(new float[, ] {
{ 0.5f * dt * dt }, { dt }
}));
}
static Matrix FGenerator(float dt)
{
return(Matrix.OfArray(new float[, ] {
{ 1, dt }, { 0, 1 }
}));
}
void Reader_FrameArrived(object sender, BodyFrameArrivedEventArgs e)
{
/*do nothing*/
//pull frame from skeleton -- display it
using (BodyFrame bf = e.FrameReference.AcquireFrame())
{
if (bf == null)
return; //do nothing if nothing found
bf.GetAndRefreshBodyData(bStorage); //copy given skeleton(s) frames (if multiple skeletons found) into buffer
//once the closest skeleton is determined draw THAT stickman
//pointer to closest skeleton frame
Body closestBody = null;
// int closestId = -1;
double closestDistance = double.MaxValue;
//out of detected skeletons in frame use the closest one -- determine closest one here
foreach (var body in bStorage)
{
//if valid skeleton
if (body.IsTracked)
{
if (body.Joints[JointType.SpineShoulder].Position.Z < closestDistance)
{
//update Id and distance
// closestId = 0;
closestBody = body; //set this skeleton as closest
closestDistance = body.Joints[JointType.SpineShoulder].Position.Z;
}
}
}
//draw the closest skeleton IF found
if (closestBody != null)
{
//draw next
//always clear cavnas
StickMen.Children.Clear();
DrawStickMan(closestBody, Brushes.Black, 3);
}
else
{
return; //nothing found!
}
//DrawStickMan(Skeleton skeleton, Brush brush, int thickness)
//if progress bar has filled
if (timerCount == 0)
{
//recording logic
if (currentState == ScreenStates.RECORDING_ENROLL || currentState == ScreenStates.RECORDING_LOGIN_GIVEN
|| currentState == ScreenStates.RECORDING_LOGIN_GUESS)
{
//quickly allocate space -- rely on garbage collection huehuehue
float[] currJoints = new float[numJoints * 3];
for (int i = 0; i < jointorder.Length; i++)
{
currJoints[i * 3] = closestBody.Joints[(JointType)jointorder[i]].Position.X;
currJoints[i * 3 + 1] = closestBody.Joints[(JointType)jointorder[i]].Position.Y;
currJoints[i * 3 + 2] = closestBody.Joints[(JointType)jointorder[i]].Position.Z;
}
if (rb.getBufferStatus() == true) //recording complete AND/OR buffer-size was reached
{
if ((currentState == ScreenStates.RECORDING_ENROLL))
{
float[,] lookupData, queryDataExpanded, queryData;
queryDataExpanded = (rb.getBufferData());
queryData = new float[rb.getBufferNumberFrames(), numJoints * 3];
for (int i = 0; i < rb.getBufferNumberFrames(); i++)
{
for (int j = 0; j < numJoints * 3; j++)
{
queryData[i, j] = queryDataExpanded[i, j];
}
}
queryData = normalizeData(queryData);
dataStore[currentUser][0][recordNumber] = new MathNet.Numerics.LinearAlgebra.Single.DenseMatrix(queryData);
if ((recordNumber != numRecords - 1))
{
recordNumber++;
rb.clearBuffer();
startTimer(3);
setProgramState(ScreenStates.RECORDING_ENROLL);
}
else
{
//store in g1, s1 slot
dataEnrolled = true;
dataStore[currentUser][0][recordNumber] = new MathNet.Numerics.LinearAlgebra.Single.DenseMatrix(queryData);
userHasData[currentUser] = true;
showPopup("Recorded " + rb.getBufferNumberFrames() + " frames for " + userNames[currentUser], Brushes.Blue);
//Code for saving dataStore as .csv file
//For each user
Directory.CreateDirectory(desktopDir + "KinectSamples\\User" + (currentUser + 1).ToString());
//For each Sample
for (int sNum = 0; sNum < numRecords; sNum++)
{
using (System.IO.StreamWriter file = new System.IO.StreamWriter(@desktopDir + "KinectSamples\\User" + (currentUser + 1).ToString() + "\\Sample" + sNum.ToString() + ".csv", false))
{
for (int j = 0; j < dataStore[currentUser][0][sNum].RowCount; j++)
{
for (int i = 0; i < numJoints * 3 - 1; i++)
{
file.Write(Convert.ToString(dataStore[currentUser][0][sNum][j, i]) + ",");
}
file.Write(Convert.ToString(dataStore[currentUser][0][sNum][j, numJoints * 3 - 1]) + "\n");
}
}
}
setProgramState(ScreenStates.HOME);
}
}
else
{
//bad "formatting"
float[,] lookupData, queryDataExpanded, queryData;
float bestScore = float.PositiveInfinity;
float dtwScore = float.PositiveInfinity;
float[][,] saveData = new float[5][,];
int userMatchIdx = -1;
//hide skeleton while processing
StickMen.Visibility = System.Windows.Visibility.Hidden;
queryDataExpanded = (rb.getBufferData());
queryData = new float[rb.getBufferNumberFrames(), numJoints * 3];
for (int i = 0; i < rb.getBufferNumberFrames(); i++)
{
for (int j = 0; j < numJoints * 3; j++)
{
queryData[i, j] = queryDataExpanded[i, j];
}
}
queryData = normalizeData(queryData);
//compute distances
switch (currentState)
{
case ScreenStates.RECORDING_LOGIN_GIVEN:
for (int i = 0; i < numRecords; i++)
{
lookupData = dataStore[currentUser][0][i].ToArray();
dtwScore = gestureDistance(queryData, lookupData, queryData.GetLength(0), lookupData.GetLength(0));
//TODO some analysis here ahahahahah
ResultText.Text = "User: "******"|Score: " + dtwScore;
if (dtwScore <= acceptThreshold)
{
showPopup("Welcome back, " + userNames[currentUser], Brushes.Green);
break;
}
else if (i == numRecords - 1)
{
showPopup("Sorry, I do not think you are " + userNames[currentUser], Brushes.Red);
}
}
setProgramState(ScreenStates.HOME);
//setProgramState(ScreenStates.RESULT);
break;
case ScreenStates.RECORDING_LOGIN_GUESS:
for (int uIdx = 0; uIdx < userHasData.Length; uIdx++)
{
if (userHasData[uIdx] == false) continue;
for (int i = 0; i < numRecords; i++)
{
lookupData = dataStore[uIdx][0][i].ToArray();
dtwScore = gestureDistance(queryData, lookupData, queryData.GetLength(0), lookupData.GetLength(0));
if (dtwScore < bestScore)
{
bestScore = dtwScore;
userMatchIdx = uIdx;
}
}
}
//after ALL comparisons then do some result feeding here
//TODO
ResultText.Text = "User:"******"|Score:" + bestScore;
if (bestScore <= acceptThreshold)
{
showPopup("Welcome back, " + userNames[userMatchIdx], Brushes.Green);
}
else
{
showPopup("Sorry, I do not think you are anyone I know.", Brushes.Red);
}
setProgramState(ScreenStates.HOME);
//ResultText.Text =
//setProgramState(ScreenStates.RESULT);
break;
default:
break;
}
}
}
else
{
rb.addFrame(currJoints, recordNumber);
StickMen.Children.Clear();
//update GUI to reflect recording or status here
if (rb.isMoving())
{
DrawStickMan(closestBody, Brushes.Green, 3);
}
else
{
DrawStickMan(closestBody, Brushes.Black, 3);
}
}
}
}
}
}
