public Plane(DVector pointA, DVector pointB, DVector pointC)
{
var u = pointC - pointA;
var v = pointC - pointB;
this.Normal = (DVector)CrossProduct(u, v).Normalize(3);
this.PlanarPoint = pointA;
}
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);
}
public static Vector<float> Solve(Matrix<float> A, Vector<float> b)
{
MathNet.Numerics.LinearAlgebra.Factorization.Svd<float> svd = A.Svd();
double dmax = svd.S[0];
Vector<float> y = new MathNet.Numerics.LinearAlgebra.Single.DenseVector(A.ColumnCount);
Vector<float> bp = svd.U.Transpose() * b;
// TODO: use the fact that S is sorted ( while S[i]/dmax < e then assume its greater )
int minSize = Math.Min(A.ColumnCount, A.RowCount);
for(int i = 0; i < minSize; ++i)
{
if(svd.S[i] / dmax < 1e-16)
{
y[i] = 0.0f;
}
else
{
y[i] = bp[i] / svd.S[i];
}
}
return svd.VT.Transpose() * y;
}
private Vector StateEstimation(Matrix kalmanGain, Vector measurementResidual)
{
return(X + kalmanGain * measurementResidual);
}
private Vector MeasurementResidual(Vector measurement)
{
return(measurement - H * X);
}
private Vector PredictState(float dt, Vector controlInput)
{
return(F(dt) * X + B(dt) * controlInput);
}
public MvaWacohForceVector(MathNet.Numerics.LinearAlgebra.Single.DenseVector vec)
: base(vec.ToArray())
{
}
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;
}));
}
public Plane(DVector normal, DVector pointInThePlane)
{
this.Normal = (DVector)normal.Normalize(3);
this.PlanarPoint = pointInThePlane;
}
