/// <summary>
/// 计算下肢灵活度结果
/// </summary>
public static void CalculateLowerLimbResult()
{
try
{
//计算左脚抬脚高度均值方差
double mLeftHeight = 0;
double dLeftHeight = 0;
List <double> realLeftHight = new List <double>();
for (int i = 0; i < LeftFootHeight.Count; i++)
{
if (LeftFootHeight[i] != 0)
{
realLeftHight.Add(Math.Abs(LeftFootHeight[i]));
}
}
//存储几个最大点的值
List <double> leftHeightMax = new List <double>();
BuTai.FindMaxValuesOrPosition(realLeftHight.ToArray(), leftHeightMax, 0);
BuTai.CalCulateMD(leftHeightMax, ref mLeftHeight, ref dLeftHeight);
//计算右脚抬脚高度均值方差
double mRightHeight = 0;
double dRightHeight = 0;
List <double> realRigthHight = new List <double>();
for (int i = 0; i < RightFootHeight.Count; i++)
{
if (RightFootHeight[i] != 0)
{
realRigthHight.Add(Math.Abs(RightFootHeight[i]));
}
}
//存储几个最大点的值
List <double> rightHeightMax = new List <double>();
BuTai.FindMaxValuesOrPosition(realRigthHight.ToArray(), rightHeightMax, 0);
BuTai.CalCulateMD(rightHeightMax, ref mRightHeight, ref dRightHeight);
//左右脚变异度
BingRen.RightFootAberrance = Math.Round(dRightHeight / mRightHeight, 4);
BingRen.LeftFootAberrance = Math.Round(dLeftHeight / mLeftHeight, 4);
}
catch
{
ClearLowerLimbData();
System.Windows.MessageBox.Show("没有收集到骨骼数据!");
return;
}
finally
{
Console.WriteLine(BingRen.LeftFootAberrance + " " + BingRen.RightFootAberrance + "\n");
ClearLowerLimbData();
}
}
/// <summary>
/// 搜集抬脚高度
/// </summary>
/// <param name="body">当前身体</param>
/// <param name="bf">当前骨骼帧</param>
public static void CollectFootHeight(Body body, BodyFrame bf)
{
//收集左脚与右脚点数
IReadOnlyDictionary <JointType, Joint> joints = body.Joints;
CameraSpacePoint LeftFoot = joints[JointType.AnkleLeft].Position;
CameraSpacePoint RightFoot = joints[JointType.AnkleRight].Position;
//计算离地高度添加到队列
LeftFootHeight.Add(BuTai.distancetoFloor(LeftFoot, bf));
RightFootHeight.Add(BuTai.distancetoFloor(RightFoot, bf));
}
///<summary>
///collect the height of NSU
///<param name = "body"/>current body</param>
///<param neme = "bf"/>current skeleton</param>
///</summary>
public static void CollectNSUheight(Body body, BodyFrame bf)
{
//whether it is the first time
d1 = DateTime.Now;
IReadOnlyDictionary <JointType, Joint> joints = body.Joints;
CameraSpacePoint Neck = joints[JointType.Neck].Position;
//calculate the height of Neck and add it to the queue
NSUheight.Add(BuTai.distancetoFloor(Neck, bf));
NSUtime.Add(d1);
}
/// <summary>
/// 实时显示步态参数
/// </summary>
/// <param name="body">当前人体</param>
/// <param name="fe">当前骨骼帧</param>
public void changBuTaiLabels(Body body, BodyFrame fe)
{
//取得6个骨骼点坐标和时间
Queue <Object> e = butai.getBuTaiData(body);
CameraSpacePoint bonePoint1 = (CameraSpacePoint)e.Dequeue();
CameraSpacePoint bonePoint2 = (CameraSpacePoint)e.Dequeue();
CameraSpacePoint bonePoint3 = (CameraSpacePoint)e.Dequeue();
CameraSpacePoint bonePoint4 = (CameraSpacePoint)e.Dequeue();
CameraSpacePoint bonePoint5 = (CameraSpacePoint)e.Dequeue();
CameraSpacePoint bonePoint6 = (CameraSpacePoint)e.Dequeue();
DateTime dt = (DateTime)e.Dequeue();
TimeSpan tp = (TimeSpan)e.Dequeue();
//显示实时左步长和右步长
double StepLength = bonePoint1.Z - bonePoint2.Z;
double leftstepLength = 0;
double rightstepLength = 0;
if (StepLength > 0)
{
this.RightStepLengthLabel.Content = "右步长:" + Math.Round(Math.Abs(StepLength), 4);
rightstepLength = StepLength;
leftstepLength = 0;
}
else
{
this.LeftStepLengthLabel.Content = "左步长:" + Math.Round(Math.Abs(StepLength), 4);
leftstepLength = StepLength;
rightstepLength = 0;
}
//实时显示左步高和右步高
double leftstepHeight = BuTai.distancetoFloor(bonePoint3, fe);
double rightstepHeight = BuTai.distancetoFloor(bonePoint4, fe);
this.LeftStepHeightLabel.Content = "左步高:" + Math.Round(leftstepHeight, 4);
this.RightStepHeightLabel.Content = "右步高:" + Math.Round(rightstepHeight, 4);
//实时显示步宽
double stepwidth = Math.Sqrt(Math.Abs(bonePoint1.X - bonePoint2.X));
this.StepWidthLabel.Content = "步宽:" + Math.Round(stepwidth, 4);
//将结果储存起来
butai.setResultData(stepwidth, leftstepLength, rightstepLength, leftstepHeight, rightstepHeight, dt);
butai.setElementData(bonePoint1, bonePoint2, bonePoint3, bonePoint4, bonePoint5, bonePoint6);
}
/// <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();
}