static IPrivateKey DecodePrivateKey(string fname)
{
byte[] buf = File.ReadAllBytes(fname);
PEMObject[] fpo = AsnIO.DecodePEM(buf);
if (fpo.Length == 0)
{
buf = AsnIO.FindBER(buf);
}
else
{
buf = null;
foreach (PEMObject po in fpo)
{
string tt = po.type.ToUpperInvariant();
if (tt.IndexOf("PRIVATE KEY") >= 0)
{
if (buf != null)
{
throw new Exception(
"Multiple keys in '"
+ fname + "'");
}
buf = po.data;
}
}
}
if (buf == null)
{
throw new Exception(string.Format(
"No private key in file '{0}'", fname));
}
return(KF.DecodePrivateKey(buf));
}
public static KF BuildKf(double[] num, double[] den, DenseMatrix measCov, DenseMatrix procCov)
{
var format = new System.Globalization.NumberFormatInfo
{
NumberDecimalSeparator = ".",
NegativeSign = "-"
};
var numStr = new StringBuilder();
foreach (var par in num)
{
numStr.Append(" " + par.ToString(format));
}
var denStr = new StringBuilder();
foreach (var par in den)
{
denStr.Append(" " + par.ToString(format));
}
var ssf = GenerateSensorModel(numStr.ToString(), denStr.ToString(), 0.01);
var kf = new KF(ssf, measCov, procCov);
return(kf);
}
//метод очистки
private void ClearMethod()
{
InputCodeTextBox.Clear();
OutputCodeTextBox.Clear();
InputDecodeTextBox.Clear();
OutputDecodeTextBox.Clear();
KF.Clear();
}
public override void Serialize(StreamWriter SW) //get the transform in deserialization
{
SW.WriteLine(ToString());
base.Serialize(SW);
SW.WriteLine("KeyFramesCount:\t" + keyFrames.Count.ToString());
foreach (KeyFrame KF in keyFrames)
{
KF.Serialize(SW);
}
SW.WriteLine("End Of " + ToString());
}
public static FedKF ToFedKf(this Specimen spec)
{
int totalLength = SensorsCount * (NumOrder + DenOrder);
if (totalLength > spec.Genes.Length)
{
throw new ArgumentException("Genome of the specimen is too short;");
}
var filters = new KF[SensorsCount];
var numDenLength = NumOrder + DenOrder;
var partitioner = Partitioner.Create(0, totalLength, numDenLength);
Parallel.ForEach(
partitioner,
(range, loopstate) =>
{
int sensorIdx = range.Item1 / numDenLength;
var numParams = new double[NumOrder];
var denParams = new double[DenOrder];
for (int i = range.Item1, j = 0; i < range.Item2; i++, j++)
{
if (j < NumOrder)
{
numParams[j] = spec.Genes[i];
}
else
{
denParams[j - NumOrder] = spec.Genes[i];
}
}
filters[sensorIdx] = KFBuilder.BuildKf(numParams, denParams, new DenseMatrix(1, 1, SensorsOutputCovariances[sensorIdx, sensorIdx]), ProcessCovariances);
});
var fkf = new FedKF(filters, DirectCosinsMatrix, SensorsOutputCovariances);
return(fkf);
}
public static KF BuildKf(double[] num, double[] den, DenseMatrix measCov, DenseMatrix procCov)
{
var format = new System.Globalization.NumberFormatInfo
{
NumberDecimalSeparator = ".",
NegativeSign = "-"
};
var numStr = new StringBuilder();
foreach (var par in num)
{
numStr.Append(" " + par.ToString(format));
}
var denStr = new StringBuilder();
foreach (var par in den)
{
denStr.Append(" " + par.ToString(format));
}
var ssf = GenerateSensorModel(numStr.ToString(), denStr.ToString(), 0.01);
var kf = new KF(ssf, measCov, procCov);
return kf;
}
void RunUpdate()
{
var openSeeData = openSee.GetOpenSeeData(faceId);
if (openSeeData == null || openSeeData.fit3DError > openSee.maxFit3DError)
{
return;
}
if (openSeeData.time > updated)
{
frameTime = (float)(openSeeData.time - updated);
updated = openSeeData.time;
trackingFrames++;
if (trackingFrames < skipFirst)
{
return;
}
}
else
{
Interpolate();
return;
}
if (!kalmanInit)
{
kalmanPVX = new KF(smooth);
kalmanPVY = new KF(smooth);
kalmanPVZ = new KF(smooth);
kalmanRVX = new KF(smooth);
kalmanRVY = new KF(smooth);
kalmanRVZ = new KF(smooth);
kalmanInit = true;
}
if (smooth != kalmanSmooth)
{
kalmanPVX.SetSmoothness(smooth);
kalmanPVY.SetSmoothness(smooth);
kalmanPVZ.SetSmoothness(smooth);
kalmanRVX.SetSmoothness(smooth);
kalmanRVY.SetSmoothness(smooth);
kalmanRVZ.SetSmoothness(smooth);
kalmanSmooth = smooth;
}
Quaternion convertedQuaternion = new Quaternion(-openSeeData.rawQuaternion.y, -openSeeData.rawQuaternion.x, openSeeData.rawQuaternion.z, openSeeData.rawQuaternion.w);
Vector3 t = openSeeData.translation;
t.x = -t.x;
t.z = -t.z;
Vector3 convertedTranslation = new Vector3(t.x, t.y, t.z);
// Check for angular outliers
if (gotAccepted)
{
float angularDifference = Quaternion.Angle(lastAccepted, convertedQuaternion);
if (angularDifference >= outlierThresholdAngle)
{
if (!skipped)
{
if (outlierSkipPeriod >= 0.0000001f)
{
skipStartTime = Time.time;
skipped = true;
}
}
else
{
if (Time.time > skipStartTime + outlierSkipPeriod)
{
lastAccepted = convertedQuaternion;
skipped = false;
}
}
}
else
{
skipped = false;
lastAccepted = convertedQuaternion;
}
}
else
{
lastAccepted = convertedQuaternion;
skipped = false;
}
gotAccepted = true;
if (skipped)
{
outlierSkips += 1;
return;
}
// Check for positional outliers
bool keepOffset = false;
if (gotAcceptedPosition)
{
float distance = Vector3.Distance(t, lastAcceptedPosition);
if (distance > outlierThresholdDistance)
{
if (!skippedPosition)
{
if (outlierSkipPeriodDistance >= 0.0000001f)
{
skipStartTimePosition = Time.time;
skippedPosition = true;
}
}
else
{
if (Time.time > skipStartTimePosition + outlierSkipPeriodDistance)
{
lastAcceptedPosition = t;
skippedPosition = false;
if (outlierRecalibrate)
{
calibrate = true;
keepOffset = outlierKeepOffset;
outlierCalibrations += 1;
}
}
}
}
else
{
skippedPosition = false;
lastAcceptedPosition = t;
}
}
else
{
skippedPosition = false;
lastAcceptedPosition = t;
}
gotAcceptedPosition = true;
if (skippedPosition)
{
outlierSkipsDistance += 1;
return;
}
if (calibrate)
{
dR = convertedQuaternion;
dR = Quaternion.Inverse(dR);
dT = t;
rotationOffset = new Vector3(dR.eulerAngles.x, dR.eulerAngles.y, dR.eulerAngles.z);
translationOffset = new Vector3(dT.x, dT.y, dT.z);
lastAcceptedPosition = translationOffset;
skippedPosition = false;
}
if (mirrorMotion != lastMirror || (mirrorMotion && calibrate))
{
dR = Quaternion.Inverse(MirrorQuaternion(Quaternion.Inverse(dR)));
dT = MirrorTranslation(dT);
lastMirror = mirrorMotion;
}
if (calibrate && keepOffset)
{
dR = Quaternion.Inverse(Quaternion.Inverse(transform.localRotation) * Quaternion.Inverse(dR));
dT -= transform.localPosition;
}
calibrate = false;
if (mirrorMotion)
{
convertedQuaternion = MirrorQuaternion(convertedQuaternion);
t = MirrorTranslation(t);
}
if (interpolateState > 1)
{
avgInterps = Mathf.Lerp(avgInterps, (float)interpolationCount, 0.15f);
}
interpolationCount = 0;
averageInterpolations = avgInterps;
lastT = transform.localPosition;
lastR = transform.localRotation;
if (interpolateState > 0)
{
currentT = FilterPos(currentT, (t - dT) * translationScale);
currentR = FilterRot(transform.localRotation, convertedQuaternion * dR);
}
else
{
currentT = (t - dT) * translationScale;
currentR = convertedQuaternion * dR;
}
if (interpolateState < 2)
{
interpolateState++;
}
if (interpolate)
{
Interpolate();
}
else
{
transform.localPosition = FilterPos(transform.localPosition, (t - dT) * translationScale);
transform.localRotation = FilterRot(transform.localRotation, convertedQuaternion * dR);
}
if (kinematicInterpolation != null && kinematicInterpolation.gameObject != gameObject)
{
if (interpolate)
{
kinematicInterpolation.UpdateKI(updated, currentT, currentR);
}
else
{
kinematicInterpolation.UpdateKI(updated, transform.localPosition, transform.localRotation);
}
}
if (driftBack)
{
if (mirrorMotion)
{
dT = Vector3.Lerp(dT, MirrorTranslation(convertedTranslation), driftFactor);
dR = Quaternion.Lerp(dR, Quaternion.Inverse(convertedQuaternion), driftFactor);
}
else
{
dT = Vector3.Lerp(dT, convertedTranslation, driftFactor);
dR = Quaternion.Lerp(dR, Quaternion.Inverse(convertedQuaternion), driftFactor);
}
rotationOffset = new Vector3(dR.eulerAngles.x, dR.eulerAngles.y, dR.eulerAngles.z);
translationOffset = new Vector3(dT.x, dT.y, dT.z);
}
}
