Пример #1
0
        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;
 }
Пример #4
0
        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;
        }
Пример #5
0
        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;
        }
Пример #6
0
        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;
        }
Пример #7
0
        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;
        }
Пример #8
0
 private bool IsActualReadingsSet(MEMSReadingsSet3f set)
 {
     if (set == null)
     {
         return false;
     }
     if (!set.IsNotEmpty())
     {
         return false;
     }
     if (set.TimeStampOrigI == 0)
     {
         return false;
     }
     return true;
 }
Пример #9
0
 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");
 }
Пример #10
0
 public MEMSReadingsSet3f(MEMSReadingsSet3f original)
 {
     this.AccVector3f = new ReadingsVector3f(original.AccVector3f);
     this.MagnetVector3f = new ReadingsVector3f(original.MagnetVector3f);
     this.GyroVector3f = new ReadingsVector3f(original.GyroVector3f);
 }