/// <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]);
}
}
}
///<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();
}
}
/// <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();
}
