Example #1
0
        /// <summary>
        /// 找到最大点的值或者位置
        /// </summary>
        /// <param name="data">数据</param>
        /// <param name="valuesorpositions">最大点的值或者位置</param>
        /// <param name="vorp">添加值或者位置(0为值,1为位置)</param>
        public static void FindMaxValuesOrPosition(double[] data, List <double> valuesorpositions, int vorp)
        {
            //高斯滤波平滑曲线
            double[] leftLGuass = Guass.GuassFitter(data.ToArray());

            //SMT得到0,1折线
            double[] leftLSMT = SMT.SMTFilter(leftLGuass);

            //确定0,1折线的开始结束点
            List <int> pointleftLSMT = SMT.pointSMT(leftLSMT);

            //找到几个步长最大点
            for (int i = 0; i < pointleftLSMT.Count; i = i + 2)
            {
                if (i == pointleftLSMT.Count - 1)
                {
                    if (vorp == 0)
                    {
                        double[] m = SMT.findMax(data, pointleftLSMT[i], data.Length - 1);
                        valuesorpositions.Add(m[vorp]);
                    }
                }
                else
                {
                    double[] m = SMT.findMax(data.ToArray(), pointleftLSMT[i], pointleftLSMT[i + 1]);
                    valuesorpositions.Add(m[vorp]);
                }
            }
        }
Example #2
0
        ///<summary>
        ///calculate the velocity and acceleration
        ///</summary>

        public static void CalculateNSUResult()
        {
            try
            {
                double[] currentHeight = new double[NSUheight.Count];
                for (int i = 0; i < NSUheight.Count; ++i)
                {
                    currentHeight[i] = NSUheight[i];
                }
                //Guass Filter
                double[] GuassianHeight = Guass.GuassFitter(currentHeight);
                //SMT Filter
                double[] SMTHeight = SMT.SMTFilter(GuassianHeight);
                //find the startpoint and endpoint
                List <int>      pointSMTHeight = SMT.pointSMT(SMTHeight);
                List <double[]> startPoint     = new List <double[]>();
                List <double[]> endPoint       = new List <double[]>();
                for (int i = 0; i < pointSMTHeight.Count; i = i + 2)
                {
                    if (i == pointSMTHeight.Count - 1)
                    {
                        double[] max = SMT.findMax(GuassianHeight, pointSMTHeight[i], currentHeight.Length - 1);
                        double[] min = SMT.findMin(GuassianHeight, pointSMTHeight[i], currentHeight.Length - 1);
                        startPoint.Add(max);
                        endPoint.Add(min);
                    }
                    else
                    {
                        double[] max = SMT.findMax(GuassianHeight, pointSMTHeight[i], pointSMTHeight[i + 1]);
                        double[] min = SMT.findMin(GuassianHeight, pointSMTHeight[i], pointSMTHeight[i + 1]);
                        startPoint.Add(max);
                        endPoint.Add(min);
                    }
                }
                double[] deltaHeightUp       = new double[startPoint.Count];
                double[] deltaTimeUp         = new double[startPoint.Count];
                double[] NSUVelocityUp       = new double[startPoint.Count];
                double[] NSUAccelerationUp   = new double[startPoint.Count];
                double[] deltaHeightDown     = new double[startPoint.Count];
                double[] deltaTimeDown       = new double[startPoint.Count];
                double[] NSUVelocityDown     = new double[startPoint.Count];
                double[] NSUAccelerationDown = new double[startPoint.Count];
                for (int i = 0; i < startPoint.Count; ++i)
                {
                    deltaHeightUp[i]     = startPoint[i][0] - endPoint[i][0];
                    deltaTimeUp[i]       = (NSUtime[Convert.ToInt32(startPoint[i][1])] - NSUtime[Convert.ToInt32(endPoint[i][1])]).TotalSeconds;
                    NSUVelocityUp[i]     = deltaHeightUp[i] / deltaTimeUp[i];
                    NSUAccelerationUp[i] = deltaHeightUp[i] / Math.Pow(deltaTimeUp[i], 2);
                }
                BingRen.NSUVelocityUp     = NSUVelocityUp.Average();
                BingRen.NSUAccelerationUp = NSUAccelerationUp.Average();
            }
            catch
            {
                ClearNSUData();
                System.Windows.MessageBox.Show("No Skeleton Data!");
                return;
            }
            finally
            {
                Debug.WriteLine(BingRen.NSUVelocityUp + " " + BingRen.NSUVelocityDown + " " + BingRen.NSUAccelerationUp + " " + BingRen.NSUAccelerationDown);
                ClearNSUData();
            }
        }
Example #3
0
        /// <summary>
        /// 计算结果并出报告
        /// </summary>
        public void calResult()
        {
            try
            {
                //实时返回的数据
                List <double>   stepWidthList       = new List <double>();
                List <double>   leftstepLengthList  = new List <double>();
                List <double>   rightstepLengthList = new List <double>();
                List <double>   leftstepHeightList  = new List <double>();
                List <double>   rightstepHeightList = new List <double>();
                List <DateTime> timeList            = new List <DateTime>();

                //实时返回的6个骨骼坐标
                List <CameraSpacePoint> AnkleLeftList  = new List <CameraSpacePoint>();
                List <CameraSpacePoint> AnkleRightList = new List <CameraSpacePoint>();
                List <CameraSpacePoint> FootLeftList   = new List <CameraSpacePoint>();
                List <CameraSpacePoint> FootRightList  = new List <CameraSpacePoint>();
                List <CameraSpacePoint> KneeLeftList   = new List <CameraSpacePoint>();
                List <CameraSpacePoint> KneeRightList  = new List <CameraSpacePoint>();

                //实时返回的数据
                for (int i = 0; i < AllResultElement.Count; i++)
                {
                    stepWidthList.Add((double)AllResultElement[i].Dequeue());
                    leftstepLengthList.Add((double)AllResultElement[i].Dequeue());
                    rightstepLengthList.Add((double)AllResultElement[i].Dequeue());
                    leftstepHeightList.Add((double)AllResultElement[i].Dequeue());
                    rightstepHeightList.Add((double)AllResultElement[i].Dequeue());
                    timeList.Add((DateTime)AllResultElement[i].Dequeue());
                }

                //实时返回的6个骨骼坐标
                for (int i = 0; i < AllBuTaiElement.Count; i++)
                {
                    AnkleLeftList.Add((CameraSpacePoint)AllBuTaiElement[i].Dequeue());
                    AnkleRightList.Add((CameraSpacePoint)AllBuTaiElement[i].Dequeue());
                    FootLeftList.Add((CameraSpacePoint)AllBuTaiElement[i].Dequeue());
                    FootRightList.Add((CameraSpacePoint)AllBuTaiElement[i].Dequeue());
                    KneeLeftList.Add((CameraSpacePoint)AllBuTaiElement[i].Dequeue());
                    KneeRightList.Add((CameraSpacePoint)AllBuTaiElement[i].Dequeue());
                }

                //计算走了多少步
                //膝盖左右的距离
                //double[] xP = new double[FootLeftList.Count];
                double[] yP = new double[FootLeftList.Count];
                for (int i = 0; i < FootLeftList.Count; i++)
                {
                    //xP[i] = AnkleLeftList[i].Z;
                    yP[i] = Math.Sqrt(Math.Pow(AnkleLeftList[i].X - AnkleRightList[i].X, 2) +
                                      Math.Pow(AnkleLeftList[i].Y - AnkleRightList[i].Y, 2) +
                                      Math.Pow(AnkleLeftList[i].Z - AnkleRightList[i].Z, 2));
                }
                //高斯滤波
                double[] GuassyP = Guass.GuassFitter(yP);

                //SMT滤波
                double[] SMTyP = SMT.SMTFilter(GuassyP);

                //找范围确定步数
                List <int> pointSMTyp = SMT.pointSMT(SMTyP);
                BingRen.StepCount = (pointSMTyp.Count + 1) / 2;

                //找每一步的起始点
                List <double[]> startendPoint = new List <double[]>();
                for (int i = 0; i < pointSMTyp.Count; i = i + 2)
                {
                    if (i == pointSMTyp.Count - 1)
                    {
                        double[] m = SMT.findMax(GuassyP, pointSMTyp[i], yP.Length - 1);
                        startendPoint.Add(m);
                    }
                    else
                    {
                        double[] m = SMT.findMax(GuassyP, pointSMTyp[i], pointSMTyp[i + 1]);
                        startendPoint.Add(m);
                    }
                }


                //计算步速
                DateTime startTime = timeList[0];
                DateTime endTime   = timeList[timeList.Count - 1];
                TimeSpan useTime   = endTime.Subtract(startTime);
                double   ut        = useTime.TotalSeconds;        //TotalSeconds 属性表示整数和小数秒,而 Seconds 属性表示整秒数。
                double   distance  = Math.Abs(AnkleLeftList[AnkleLeftList.Count - 1].Z - AnkleLeftList[0].Z);
                BingRen.StepSpeed = Math.Round(distance / ut, 4); //距离除以时间求步速


                //计算左步速,右歩速,周期歩速
                double mLeftSpeed  = 0;
                double dLeftSpeed  = 0;
                double mRightSpeed = 0;
                double dRightSpeed = 0;
                double mCycleSpeed = 0;
                double dCycleSpeed = 0;

                ///记录落地点
                List <double> leftFootLowPoints  = new List <double>(); //左脚抬起到落地
                List <double> rightFootLowPoints = new List <double>(); //右脚抬起到落地
                List <double> cycleFootLowPoints = new List <double>(); //右脚抬起落地到左脚抬起落地


                List <double> AbsMaxPoints = new List <double>();
                //步长绝对值最大点位置
                BuTai.FindMaxValuesOrPosition(LeftSubRightFootZ.ToArray(), AbsMaxPoints, 1);
                for (int i = 0; i < AbsMaxPoints.Count; i++)
                {
                    if (i == 0)
                    {
                        if (FootFlag[(int)AbsMaxPoints[i]])
                        {
                            leftFootLowPoints.Add((int)AbsMaxPoints[i]);
                        }
                        else
                        {
                            rightFootLowPoints.Add((int)AbsMaxPoints[i]);
                        }
                    }
                    else if (i == AbsMaxPoints.Count)
                    {
                        if (FootFlag[(int)AbsMaxPoints[i]])
                        {
                            rightFootLowPoints.Add((int)AbsMaxPoints[i]);
                        }
                        else
                        {
                            leftFootLowPoints.Add((int)AbsMaxPoints[i]);
                        }
                    }
                    else
                    {
                        leftFootLowPoints.Add((int)AbsMaxPoints[i]);
                        rightFootLowPoints.Add((int)AbsMaxPoints[i]);
                    }
                }

                for (int i = 0; i < AbsMaxPoints.Count; i++)
                {
                    if (!FootFlag[(int)AbsMaxPoints[i]])
                    {
                        cycleFootLowPoints.Add((int)AbsMaxPoints[i]);
                    }
                }

                List <double> LeftSpeed = new List <double>();
                for (int i = 1; i < leftFootLowPoints.Count; i = i + 2)
                {
                    TimeSpan t = FootTime[(int)leftFootLowPoints[i]].Subtract(FootTime[(int)leftFootLowPoints[i - 1]]);

                    LeftSpeed.Add(Math.Abs(LeftFootZ[(int)leftFootLowPoints[i]] - LeftFootZ[(int)leftFootLowPoints[i - 1]])
                                  / t.TotalSeconds);
                }
                BuTai.CalCulateMD(LeftSpeed, ref mLeftSpeed, ref dLeftSpeed);

                List <double> RightSpeed = new List <double>();
                for (int i = 1; i < rightFootLowPoints.Count; i = i + 2)
                {
                    TimeSpan t = FootTime[(int)rightFootLowPoints[i]].Subtract(FootTime[(int)rightFootLowPoints[i - 1]]);

                    RightSpeed.Add(Math.Abs(RightFootZ[(int)rightFootLowPoints[i]] - RightFootZ[(int)rightFootLowPoints[i - 1]])
                                   / t.TotalSeconds);
                }
                BuTai.CalCulateMD(RightSpeed, ref mRightSpeed, ref dRightSpeed);


                List <double> CycleSpeed = new List <double>();
                for (int i = 1; i < cycleFootLowPoints.Count; i++)
                {
                    TimeSpan t = FootTime[(int)cycleFootLowPoints[i]].Subtract(FootTime[(int)cycleFootLowPoints[i - 1]]);

                    CycleSpeed.Add(Math.Abs(LeftFootZ[(int)cycleFootLowPoints[i]] - RightFootZ[(int)cycleFootLowPoints[i - 1]])
                                   / t.TotalSeconds);
                }
                BuTai.CalCulateMD(CycleSpeed, ref mCycleSpeed, ref dCycleSpeed);

                /***
                 * List<double> leftFootLowPoints = new List<double>();
                 * List<double> rightFootLowPoints = new List<double>();
                 * List<double> cycleFootLowPoints = new List<double>();
                 * for (int i = 1; i < LeftSubRightFootZ.Count; i++)
                 * {
                 *  if (i == 1)
                 *  {
                 *      if (LeftSubRightFootZ[i] >= 0)
                 *      {
                 *          rightFootLowPoints.Add(i);
                 *          cycleFootLowPoints.Add(i);
                 *      }
                 *      else
                 *      {
                 *          leftFootLowPoints.Add(i);
                 *      }
                 *  }
                 *  else
                 *  {
                 *      if (LeftSubRightFootZ[i] >= 0 && LeftSubRightFootZ[i - 1] <= 0)
                 *      {
                 *          rightFootLowPoints.Add(i);
                 *          cycleFootLowPoints.Add(i);
                 *      }
                 *      if (LeftSubRightFootZ[i] <= 0 && LeftSubRightFootZ[i - 1] >= 0)
                 *      {
                 *          leftFootLowPoints.Add(i);
                 *          cycleFootLowPoints.Add(i);
                 *      }
                 *  }
                 * }
                 *
                 * List<double> LeftSpeed = new List<double>();
                 * for (int i = 1; i < leftFootLowPoints.Count; i++)
                 * {
                 *  TimeSpan t = FootTime[(int)leftFootLowPoints[i]].Subtract(FootTime[(int)leftFootLowPoints[i - 1]]);
                 *
                 *  LeftSpeed.Add(Math.Abs(LeftFootZ[(int)leftFootLowPoints[i]] - LeftFootZ[(int)leftFootLowPoints[i - 1]])
                 *      / t.TotalSeconds);
                 * }
                 * BuTai.CalCulateMD(LeftSpeed, ref mLeftSpeed, ref dLeftSpeed);
                 *
                 * List<double> RightSpeed = new List<double>();
                 * for (int i = 1; i < rightFootLowPoints.Count; i++)
                 * {
                 *  TimeSpan t = FootTime[(int)rightFootLowPoints[i]].Subtract(FootTime[(int)rightFootLowPoints[i - 1]]);
                 *
                 *  RightSpeed.Add(Math.Abs(RightFootZ[(int)rightFootLowPoints[i]] - RightFootZ[(int)rightFootLowPoints[i - 1]])
                 *      / t.TotalSeconds);
                 * }
                 * BuTai.CalCulateMD(RightSpeed, ref mRightSpeed, ref dRightSpeed);
                 *
                 *
                 * List<double> CycleSpeed = new List<double>();
                 * for (int i = 2; i < cycleFootLowPoints.Count; i++)
                 * {
                 *  TimeSpan t = FootTime[(int)cycleFootLowPoints[i]].Subtract(FootTime[(int)cycleFootLowPoints[i - 2]]);
                 *
                 *  CycleSpeed.Add(Math.Abs(RightFootZ[(int)cycleFootLowPoints[i]] - RightFootZ[(int)cycleFootLowPoints[i - 2]])
                 *      / t.TotalSeconds);
                 * }
                 * BuTai.CalCulateMD(CycleSpeed, ref mCycleSpeed, ref dCycleSpeed);
                 **/

                //赋值
                BingRen.LeftSpeedM  = Math.Round(mLeftSpeed, 4);
                BingRen.LeftSpeedD  = Math.Round(dLeftSpeed, 4);
                BingRen.RightSpeedM = Math.Round(mRightSpeed, 4);
                BingRen.RightSpeedD = Math.Round(dRightSpeed, 4);
                BingRen.CyleSpeedM  = Math.Round(mCycleSpeed, 4);
                BingRen.CyleSpeedD  = Math.Round(dCycleSpeed, 4);

                ///计算左右步长均值,左右步长方差,左右步长协调性
                //计算左步长
                double        mLeftStepLength = 0;
                double        dLeftStepLength = 0;
                List <double> realLeftLength  = new List <double>();
                for (int i = 0; i < leftstepLengthList.Count; i++)
                {
                    if (leftstepLengthList[i] != 0)
                    {
                        realLeftLength.Add(Math.Abs(leftstepLengthList[i]));
                    }
                }
                //存储几个最大点的值
                List <double> leftStepLengthMax = new List <double>();
                BuTai.FindMaxValuesOrPosition(realLeftLength.ToArray(), leftStepLengthMax, 0);
                BuTai.CalCulateMD(leftStepLengthMax, ref mLeftStepLength, ref dLeftStepLength);


                //计算右步长
                double        mRightStepLength = 0;
                double        dRightStepLength = 0;
                List <double> realRightength   = new List <double>();
                for (int i = 0; i < rightstepLengthList.Count; i++)
                {
                    if (rightstepLengthList[i] != 0)
                    {
                        realRightength.Add(rightstepLengthList[i]);
                    }
                }
                //存储几个最大点的值
                List <double> rightStepLengthMax = new List <double>();
                BuTai.FindMaxValuesOrPosition(realRightength.ToArray(), rightStepLengthMax, 0);
                BuTai.CalCulateMD(rightStepLengthMax, ref mRightStepLength, ref dRightStepLength);

                //赋值和求左右步长的协调性
                BingRen.RightStepLengthM  = Math.Round(mRightStepLength, 4);
                BingRen.RightStepLengthD  = Math.Round(dRightStepLength, 4);
                BingRen.LeftStepLengthM   = Math.Round(mLeftStepLength, 4);
                BingRen.LeftStepLengthD   = Math.Round(dLeftStepLength, 4);
                BingRen.StepLengthBalance = Math.Round(Math.Abs(mRightStepLength - mLeftStepLength), 4);


                //求左右高均值,左右步高方差,左右步高的协调性
                //计算左步高均值和方差
                double        mLeftStepHeight = 0;
                double        dLeftStepHeight = 0;
                List <double> realeftHeight   = new List <double>();
                for (int i = 0; i < leftstepHeightList.Count; i++)
                {
                    if (leftstepHeightList[i] != 0)
                    {
                        realeftHeight.Add(leftstepHeightList[i]);
                    }
                }
                //存储几个最大点
                List <double> leftMaxHeight = new List <double>();
                BuTai.FindMaxValuesOrPosition(realeftHeight.ToArray(), leftMaxHeight, 0);
                BuTai.CalCulateMD(leftMaxHeight, ref mLeftStepHeight, ref dLeftStepHeight);

                //计算右步高均值和方差
                double        mRightStepHeight = 0;
                double        dRightStepHeight = 0;
                List <double> realrightHeight  = new List <double>();
                for (int i = 0; i < rightstepHeightList.Count; i++)
                {
                    if (rightstepHeightList[i] != 0)
                    {
                        realrightHeight.Add(rightstepHeightList[i]);
                    }
                }
                //存储几个最大点
                List <double> rightMaxHeight = new List <double>();
                BuTai.FindMaxValuesOrPosition(realrightHeight.ToArray(), rightMaxHeight, 0);
                BuTai.CalCulateMD(rightMaxHeight, ref mRightStepHeight, ref dRightStepHeight);

                //赋值和计算左右步高协调性
                BingRen.RightStepHeightM  = Math.Round(mRightStepHeight, 4);
                BingRen.RightStepHeightD  = Math.Round(dRightStepHeight, 4);
                BingRen.LeftStepHeightM   = Math.Round(mLeftStepHeight, 4);
                BingRen.LeftStepHeightD   = Math.Round(dLeftStepHeight, 4);
                BingRen.StepHeightBalance = Math.Round(Math.Abs(mRightStepHeight - mLeftStepHeight), 4);

                ///计算步宽平均方差,步距平均方差
                //计算步宽(平均)
                double mstepWidth = 0;
                double dstepWidth = 0;
                BuTai.CalCulateMD(stepWidthList, ref mstepWidth, ref dstepWidth);

                //赋值
                BingRen.StepWidthM = Math.Round(mstepWidth, 4);
                BingRen.StepWidthD = Math.Round(dstepWidth, 4);

                //计算步距
                double mStepDistance = 0;
                double dStepDistance = 0;
                //存储几个最大点的值
                List <double> stepdistanceMax = new List <double>();
                BuTai.FindMaxValuesOrPosition(StepDistance.ToArray(), stepdistanceMax, 0);
                BuTai.CalCulateMD(stepdistanceMax, ref mStepDistance, ref dStepDistance);

                //赋值
                BingRen.StepDistanceM = Math.Round(mStepDistance, 4);
                BingRen.StepDistanceD = Math.Round(dStepDistance, 4);


                //算z值
                double mZ = 0;
                double dZ = 0;
                BuTai.CalCulateMD(ValueZ, ref mZ, ref dZ);

                BingRen.ZM = Math.Round(mZ, 4);
                BingRen.ZD = Math.Round(dZ, 4);

                Console.WriteLine(BingRen.StepSpeed + " " + BingRen.RightSpeedM + " " + BingRen.RightSpeedD + " " + BingRen.LeftSpeedM + " " + BingRen.LeftSpeedD + "\n"
                                  + BingRen.CyleSpeedM + " " + BingRen.CyleSpeedD + " " + BingRen.RightStepLengthM + " " + BingRen.RightStepLengthD + " " + BingRen.LeftStepLengthM + " " + BingRen.LeftStepLengthD + "\n"
                                  + BingRen.StepLengthBalance + " " + BingRen.RightStepHeightD + " " + BingRen.RightStepHeightM + " " + BingRen.LeftStepHeightD + " " + BingRen.LeftStepHeightM + "\n"
                                  + BingRen.StepHeightBalance + " " + BingRen.StepWidthM + " " + BingRen.StepWidthD + " " + BingRen.StepDistanceM + " " + BingRen.StepDistanceD + "\n"
                                  + BingRen.ZM + " " + BingRen.ZD);
            }
            catch
            {
                clearData();
                System.Windows.MessageBox.Show("没有收集到骨骼数据!");
                return;
            }


            //暂时不用

            /*
             * //生成报告
             * try
             * {
             *  //如果没有文件生成表头
             *  if (!File.Exists(savePath))
             *  {
             *      createExcel();
             *  }
             *  //添加内容
             *  addExcel(BingRen.StepSpeed, BingRen.StepWidth, BingRen.LeftstepHeight, BingRen.RightstepHeight, BingRen.LeftstepLength, BingRen.RightstepLength, BingRen.StepCount);
             *
             * }
             * catch (Exception ex)
             * {
             *  Console.WriteLine(ex.Message);
             * }
             **/
            //清理步态数据
            clearData();
        }