public void CalcFilterCoeffs(MEMSReadingsSet3f readings, out double gyroCoeff, out double accMagnetCoeff, double initAccMagnet, double initGyro)
{
var gyroVect = readings.GyroVector3f.Values;
var gyroMagn = Math.Sqrt(gyroVect[0] * gyroVect[0] + gyroVect[1] * gyroVect[1] + gyroVect[2] * gyroVect[2]);
double gs = 2;
double gm = 1;
double gmin = 0.5;
double gmax = 0.8;
var gCoeff = gmin + (1 - 1 / (1 + gyroMagn * gm)) * (gmax - gmin);
gyroCoeff = gCoeff;
accMagnetCoeff = initAccMagnet;
Console.WriteLine("gyroCoeff: " + gyroCoeff.ToString() + " ; magnetCoeff: " + accMagnetCoeff.ToString());
}
//
protected void Init()
{
this._isStarted = false;
this._isPaused = false;
//this._is_videoSourceCap_ImageGrabbed_inUse = false;
//this._is_StereoMEMSDataProviderCVCapFromFile__nextAccValEvent_inUse = false;
//this._is_StereoMEMSDataProviderCVCapFromFile__nextMagnetValEvent_inUse = false;
//this._is_StereoMEMSDataProviderCVCapFromFile__nextGyroValEvent_inUse = false;
//this._is_StereoMEMSDataProviderCVCapFromFile__nextVideoFrameEvent_inUse = false;
this._isNewAccVal = false;
this._isNewMagnetVal = false;
this._isNewGyroVal = false;
_isVideoStreamExpired = false;
_isAccStreamExpired = false;
_isMagnetStreamExpired = false;
_isGyroStreamExpired = false;
this._currentMEMSSet = new MEMSReadingsSet3f();
if (this._useMEMS)
{
this._currentMEMSSet.AccVector3f = this.GetNextAccVector3f();
this._currentMEMSSet.MagnetVector3f = this.GetNextMagnetVector3f();
this._currentMEMSSet.GyroVector3f = this.GetNextGyroVector3f();
this._startMEMSTimeStampIOrig = this._currentMEMSSet.AccVector3f.TimeStampI;
this._nextAccValEvent += StereoMEMSDataProviderCVCapFromFile__nextAccValEvent;
this._nextMagnetValEvent += StereoMEMSDataProviderCVCapFromFile__nextMagnetValEvent;
this._nextGyroValEvent += StereoMEMSDataProviderCVCapFromFile__nextGyroValEvent;
}
if (this._useVideo)
{
var rawFrame = this._videoSourceCap.QueryFrame();
this._currentStereoFrame = this.ElementFromRawFrame(rawFrame);
rawFrame = null;
this._nextVideoFrameEvent += StereoMEMSDataProviderCVCapFromFile__nextVideoFrameEvent;
this._isFirstVideoFrame = true;
}
}
protected MEMSReadingsSet3f GetResultMEMSSet()
{
var res = new MEMSReadingsSet3f()
{
AccVector3f = new ReadingsVector3f(this._currentMEMSSet.AccVector3f),
MagnetVector3f = new ReadingsVector3f(this._currentMEMSSet.MagnetVector3f),
GyroVector3f = new ReadingsVector3f(this._currentMEMSSet.GyroVector3f)
};
res.AccVector3f.TimeStampI = this.RecalcMEMSTimeStamp(res.AccVector3f.TimeStampI);
res.MagnetVector3f.TimeStampI = this.RecalcMEMSTimeStamp(res.MagnetVector3f.TimeStampI);
res.GyroVector3f.TimeStampI = this.RecalcMEMSTimeStamp(res.GyroVector3f.TimeStampI);
return res;
}
public override MEMSReadingsSet3f GetNextReadingsSet()
{
MEMSReadingsSet3f res = new MEMSReadingsSet3f();
res.AccVector3f = this.GetNextAccVector3f();
res.MagnetVector3f = this.GetNextMagnetVector3f();
res.GyroVector3f = this.GetNextGyroVector3f();
this.CurrentReadingsSet3f = res;
return res;
}
protected MEMSReadingsSet3f ConvertToMEMSReadingsSet3f(byte[] buffer)
{
if (buffer.Length != RecMessageSize)
{
return null;
}
MEMSReadingsSet3f res = new MEMSReadingsSet3f();
//res.AccVector3f = this.GetNextAccVector3f();
//res.MagnetVector3f = this.GetNextMagnetVector3f();
//res.GyroVector3f = this.GetNextGyroVector3f();
return res;
}
public double[][] GetAccMagnetOrientationMatrix(MEMSReadingsSet3f newReadings, bool useAccMagnet, bool useGyroscope, bool useLowpassFilter, bool useAdaptiveFiltering, double accMagnetFilterCoeff, double gyroFilterCoeff)
{
if (useAdaptiveFiltering)
{
this.CalcFilterCoeffs(newReadings, out gyroFilterCoeff, out accMagnetFilterCoeff, accMagnetFilterCoeff, gyroFilterCoeff);
}
double[][] res = new double[3][];
for (int i = 0; i < 3; ++i)
{
res[i] = new double[3];
}
MCvPoint3D64f rawAccPoint = new MCvPoint3D64f(newReadings.AccVector3f.Values[0], newReadings.AccVector3f.Values[1], newReadings.AccVector3f.Values[2]);
MCvPoint3D64f rawMagnetPoint = new MCvPoint3D64f(newReadings.MagnetVector3f.Values[0], newReadings.MagnetVector3f.Values[1], newReadings.MagnetVector3f.Values[2]);
double gyroSpeedMul = 1;
if (useAdaptiveFiltering)
{
if(useAccMagnet)
{
gyroSpeedMul = this.GyroSpeedMul;
}
}
var gyroDiffMatr = this.GetGyroRotationMatrix(newReadings.GyroVector3f, gyroSpeedMul);
MCvPoint3D64f accFilteredPoint;
MCvPoint3D64f magnetFilteredPoint;
if (useLowpassFilter && this.OldAMGFusedAcc != null && this.OldAMGFusedMagnet != null)
{
if (useAccMagnet)
{
//this.FilterAccMagnet(ref resAccPoint, ref resMagnetPoint, accMagnetFilterCoeff);
accFilteredPoint = new MCvPoint3D64f(
rawAccPoint.x * accMagnetFilterCoeff + OldAMGFusedAcc.Value.x * (1 - accMagnetFilterCoeff),
rawAccPoint.y * accMagnetFilterCoeff + OldAMGFusedAcc.Value.y * (1 - accMagnetFilterCoeff),
rawAccPoint.z * accMagnetFilterCoeff + OldAMGFusedAcc.Value.z * (1 - accMagnetFilterCoeff)
);
magnetFilteredPoint = new MCvPoint3D64f(
rawMagnetPoint.x * accMagnetFilterCoeff + OldAMGFusedMagnet.Value.x * (1 - accMagnetFilterCoeff),
rawMagnetPoint.y * accMagnetFilterCoeff + OldAMGFusedMagnet.Value.y * (1 - accMagnetFilterCoeff),
rawMagnetPoint.z * accMagnetFilterCoeff + OldAMGFusedMagnet.Value.z * (1 - accMagnetFilterCoeff)
);
}
else
{
accFilteredPoint = new MCvPoint3D64f(this.OldAMGFusedAcc.Value.x, this.OldAMGFusedAcc.Value.y, this.OldAMGFusedAcc.Value.z);
magnetFilteredPoint = new MCvPoint3D64f(this.OldAMGFusedMagnet.Value.x, this.OldAMGFusedMagnet.Value.y, this.OldAMGFusedMagnet.Value.z);
}
}
else
{
accFilteredPoint = new MCvPoint3D64f(rawAccPoint.x, rawAccPoint.y, rawAccPoint.z);
magnetFilteredPoint = new MCvPoint3D64f(rawMagnetPoint.x, rawMagnetPoint.y, rawMagnetPoint.z);
}
MCvPoint3D64f resAccPoint = new MCvPoint3D64f(0, 9, 0);
MCvPoint3D64f resMagnetPoint = new MCvPoint3D64f(48, 2, 0);
if(this.OldAMGFusedAcc == null || this.OldAMGFusedMagnet == null)
{
resAccPoint = accFilteredPoint;
resMagnetPoint = magnetFilteredPoint;
}
else
{
if (useGyroscope && useAccMagnet)
{
resAccPoint = MatrixToMCvPoint3D64f(MCvPoint3D64fToMatrix(OldAMGFusedAcc.Value).Mul(gyroDiffMatr).Mul(gyroFilterCoeff).Add(MCvPoint3D64fToMatrix(accFilteredPoint).Mul(1 - gyroFilterCoeff)));
resMagnetPoint = MatrixToMCvPoint3D64f(MCvPoint3D64fToMatrix(OldAMGFusedMagnet.Value).Mul(gyroDiffMatr).Mul(gyroFilterCoeff).Add(MCvPoint3D64fToMatrix(magnetFilteredPoint).Mul(1 - gyroFilterCoeff)));
}
else if (useAccMagnet)
{
resAccPoint = accFilteredPoint;
resMagnetPoint = magnetFilteredPoint;
}
else if(useGyroscope)
{
resAccPoint = MatrixToMCvPoint3D64f(MCvPoint3D64fToMatrix(OldAMGFusedAcc.Value).Mul(gyroDiffMatr));
resMagnetPoint = MatrixToMCvPoint3D64f(MCvPoint3D64fToMatrix(OldAMGFusedMagnet.Value).Mul(gyroDiffMatr));
}
}
this.OldAMGFusedAcc = resAccPoint;
this.OldAMGFusedMagnet = resMagnetPoint;
this.OldRawAcc = rawAccPoint;
this.OldRawMagnet = rawMagnetPoint;
this.OldFilteredAcc = accFilteredPoint;
this.OldFilteredMagnet = magnetFilteredPoint;
this.OldGyroReadings = newReadings.GyroVector3f;
MCvPoint3D64f x;
MCvPoint3D64f y;
MCvPoint3D64f z;
this.GetOrthoNormalAccMagnetBasis(resAccPoint, resMagnetPoint, out x, out y, out z);
Matrix<double> accMagnetMatrix = new Matrix<double>(3, 3);
accMagnetMatrix[0, 0] = x.x; accMagnetMatrix[0, 1] = x.y; accMagnetMatrix[0, 2] = x.z;
accMagnetMatrix[1, 0] = y.x; accMagnetMatrix[1, 1] = y.y; accMagnetMatrix[1, 2] = y.z;
accMagnetMatrix[2, 0] = z.x; accMagnetMatrix[2, 1] = z.y; accMagnetMatrix[2, 2] = z.z;
//AccMagnetGyro fuse
Matrix<double> newGAMFusedMatrix = null;
//old amg fusion
//if (this.OldGAMFusedMatrix == null)
//{
// newGAMFusedMatrix = accMagnetMatrix;
//}
//else
//{
// var newGyroMatrix = this.OldGAMFusedMatrix.Mul(gyroDiffMatr);
// newGAMFusedMatrix = this.FuseAccMagnetGyro(accMagnetMatrix, newGyroMatrix, gyroFilterCoeff);
//}
newGAMFusedMatrix = accMagnetMatrix;
OldGAMFusedMatrix = newGAMFusedMatrix;
////
var resMatrix = newGAMFusedMatrix.Transpose();
//res = this.ConvertMatrix(m);
//gyro test
//m = this.OldGyroMatrix;
//m = m.Transpose();
//
//auto res set up
//for (int i = 0; i < 3; ++i)
//{
// for (int j = 0; j < 3; ++j)
// {
// res[i][j] = resMatrix[i, j];
// }
//}
////
//manual res set up
res[0][0] = -resMatrix[0, 0]; res[0][1] = -resMatrix[0, 1]; res[0][2] = resMatrix[0, 2];
res[1][0] = -resMatrix[1, 0]; res[1][1] = -resMatrix[1, 1]; res[1][2] = resMatrix[1, 2];
res[2][0] = -resMatrix[2, 0]; res[2][1] = -resMatrix[2, 1]; res[2][2] = resMatrix[2, 2];
////
return res;
}
private void ProcessMEMSReadings(MEMSReadingsSet3f nextReadings)
{
if (this.isOrientCalcInUse)
{
return;
}
this.isOrientCalcInUse = true;
if (this.IsActualReadingsSet(nextReadings))
{
double gyroCoeff = 0.8;
double accMagnetCoeff = (double)this.accMagnetFilterTrackBar.Value / this.accMagnetFilterTrackBar.Maximum;
bool useAccMagnet = this.accMagnetCheckBox.Checked;
bool useGyro = this.gyroCheckBox.Checked;
bool useAdoptiveFilter = this.adoptiveFilterCheckBox.Checked;
this.ReadingsTestOuptut(nextReadings);
var orientMatr3f = this.OrientationCalc.GetOrientationMatrix(
newReadings: nextReadings,
useAccMagnet: useAccMagnet,
useGyroscope: useGyro,
useLowpassFilter: true,
useAdaptiveFiltering: useAdoptiveFilter,
accMagnetFilterCoeff: accMagnetCoeff,
gyroFilterCoeff: gyroCoeff);
lock (this.memsOrientationMatrLock)
{
this.memsRotMatr = orientMatr3f;
}
//var res = this.MulReadingsVect(orientMatr3f, nextReadings.AccVector3f, true);
//log readings check
//this.MEMSRTBLogger.WriteLn("readings check");
//this.PrintVector(this.MEMSRTBLogger, res);
//this.MEMSRTBLogger.WriteLn("");
////
////log matrix
//this.MEMSRTBLogger.WriteLn("orientationMatrix:");
//this.PrintMatrix(this.MEMSRTBLogger, orientMatr3f);
//this.MEMSRTBLogger.WriteLn("");
////
}
this.isOrientCalcInUse = false;
}
private bool IsActualReadingsSet(MEMSReadingsSet3f set)
{
if (set == null)
{
return false;
}
if (!set.IsNotEmpty())
{
return false;
}
if (set.TimeStampOrigI == 0)
{
return false;
}
return true;
}
private void ReadingsTestOuptut(MEMSReadingsSet3f nextReadings)
{
//nextReadings = this.MemsProvider.GetNextReadingsSet();
//MEMS READINGS to enable - comment
return;
//
Console.WriteLine("timestamp: " + nextReadings.TimeStampOrigI.ToString());
Console.WriteLine("acc: "
+ " Xa=" + nextReadings.AccVector3f.Values[0]
+ " Ya=" + nextReadings.AccVector3f.Values[1]
+ " Za=" + nextReadings.AccVector3f.Values[2]
+ "\nmagnet: "
+ " Xm=" + nextReadings.MagnetVector3f.Values[0]
+ " Ym=" + nextReadings.MagnetVector3f.Values[1]
+ " Zm=" + nextReadings.MagnetVector3f.Values[2]
+ "\ngyro: "
+ " Xg=" + nextReadings.GyroVector3f.Values[0]
+ " Yg=" + nextReadings.GyroVector3f.Values[1]
+ " Zg=" + nextReadings.GyroVector3f.Values[2]
+ "\n\n");
}
public MEMSReadingsSet3f(MEMSReadingsSet3f original)
{
this.AccVector3f = new ReadingsVector3f(original.AccVector3f);
this.MagnetVector3f = new ReadingsVector3f(original.MagnetVector3f);
this.GyroVector3f = new ReadingsVector3f(original.GyroVector3f);
}