bool FrontBackCheckv(VNectModel.JointPoint jp1, VNectModel.JointPoint jp2, bool flag)
{
if (flag)
{
jp1.Error++;
if (jp1.Error == jp1.RattlingCheckFrame)
{
jp1.Error = 0;
jp2.Error = 0;
return(false);
}
jp2.Error++;;
return(true);
}
/*
* if (jp1.PrevScore3D > 0.5 && jp2.Score3D > 0.5)
* {
* jp1.Error = false;
* jp2.Error = false;
* return false;
* }
*/
//if (jp1.PrevScore3D > jp1.Score3D && jp2.PrevScore3D > jp2.Score3D)
{
var l1 = Vector3.Distance(jp1.PrevNow3D, jp1.Now3D);
var c1 = Vector3.Distance(jp2.PrevNow3D, jp1.Now3D);
var l2 = Vector3.Distance(jp2.PrevNow3D, jp2.Now3D);
var c2 = Vector3.Distance(jp1.PrevNow3D, jp2.Now3D);
if (l1 > c1 && l2 > c2)
{
jp1.Error++;
if (jp1.Error == jp1.RattlingCheckFrame)
{
jp1.Error = 0;
jp2.Error = 0;
return(false);
}
jp2.Error++;
return(true);
}
else
{
jp1.Error = 0;
jp2.Error = 0;
return(false);
}
}
/*else
* {
* jp1.Error = false;
* jp2.Error = false;
* }*/
}
/// <summary>
/// Kalman filter
/// </summary>
/// <param name="measurement">joint points</param>
void KalmanUpdate(VNectModel.JointPoint measurement)
{
measurementUpdate(measurement);
measurement.Pos3D.x = measurement.X.x + (measurement.Now3D.x - measurement.X.x) * measurement.K.x;
measurement.Pos3D.y = measurement.X.y + (measurement.Now3D.y - measurement.X.y) * measurement.K.y;
measurement.Pos3D.z = measurement.X.z + (measurement.Now3D.z - measurement.X.z) * measurement.K.z;
measurement.X = measurement.Pos3D;
}
void measurementUpdate(VNectModel.JointPoint measurement)
{
measurement.K.x = (measurement.P.x + KalmanParamQ) / (measurement.P.x + KalmanParamQ + KalmanParamR);
measurement.K.y = (measurement.P.y + KalmanParamQ) / (measurement.P.y + KalmanParamQ + KalmanParamR);
measurement.K.z = (measurement.P.z + KalmanParamQ) / (measurement.P.z + KalmanParamQ + KalmanParamR);
measurement.P.x = KalmanParamR * (measurement.P.x + KalmanParamQ) / (KalmanParamR + measurement.P.x + KalmanParamQ);
measurement.P.y = KalmanParamR * (measurement.P.y + KalmanParamQ) / (KalmanParamR + measurement.P.y + KalmanParamQ);
measurement.P.z = KalmanParamR * (measurement.P.z + KalmanParamQ) / (KalmanParamR + measurement.P.z + KalmanParamQ);
}
void measurementUpdate(VNectModel.JointPoint measurement)
{
measurement.K.x = (measurement.P.x + this.KalmanParamQ) / (measurement.P.x + this.KalmanParamQ + this.KalmanParamR);
measurement.K.y = (measurement.P.y + this.KalmanParamQ) / (measurement.P.y + this.KalmanParamQ + this.KalmanParamR);
measurement.K.z = (measurement.P.z + this.KalmanParamQ) / (measurement.P.z + this.KalmanParamQ + this.KalmanParamR);
measurement.P.x = this.KalmanParamR * (measurement.P.x + this.KalmanParamQ) / (this.KalmanParamR + measurement.P.x + this.KalmanParamQ);
measurement.P.y = this.KalmanParamR * (measurement.P.y + this.KalmanParamQ) / (this.KalmanParamR + measurement.P.y + this.KalmanParamQ);
measurement.P.z = this.KalmanParamR * (measurement.P.z + this.KalmanParamQ) / (this.KalmanParamR + measurement.P.z + this.KalmanParamQ);
}
// Start is called before the first frame update
void Start()
{
jointPoints = VNectModel.Init();
if (jointPoints == null)
{
yse = true;
}
// jointPoints = otherObject.GetComponent<VNectModel>().JointPoints;
thisjointPoints = new VNectModel.JointPoint[PositionIndex.Count.Int()];
for (var i = 0; i < PositionIndex.Count.Int(); i++)
{
thisjointPoints[i] = new VNectModel.JointPoint();
}
anim = this.gameObject.GetComponent <Animator>();
thisjointPoints[PositionIndex.rShldrBend.Int()].Transform = anim.GetBoneTransform(HumanBodyBones.RightUpperArm);
thisjointPoints[PositionIndex.rForearmBend.Int()].Transform = anim.GetBoneTransform(HumanBodyBones.RightLowerArm);
thisjointPoints[PositionIndex.rHand.Int()].Transform = anim.GetBoneTransform(HumanBodyBones.RightHand);
thisjointPoints[PositionIndex.rThumb2.Int()].Transform = anim.GetBoneTransform(HumanBodyBones.RightThumbIntermediate);
thisjointPoints[PositionIndex.rMid1.Int()].Transform = anim.GetBoneTransform(HumanBodyBones.RightMiddleProximal);
// Left Arm
thisjointPoints[PositionIndex.lShldrBend.Int()].Transform = anim.GetBoneTransform(HumanBodyBones.LeftUpperArm);
thisjointPoints[PositionIndex.lForearmBend.Int()].Transform = anim.GetBoneTransform(HumanBodyBones.LeftLowerArm);
thisjointPoints[PositionIndex.lHand.Int()].Transform = anim.GetBoneTransform(HumanBodyBones.LeftHand);
thisjointPoints[PositionIndex.lThumb2.Int()].Transform = anim.GetBoneTransform(HumanBodyBones.LeftThumbIntermediate);
thisjointPoints[PositionIndex.lMid1.Int()].Transform = anim.GetBoneTransform(HumanBodyBones.LeftMiddleProximal);
// Face
thisjointPoints[PositionIndex.lEar.Int()].Transform = anim.GetBoneTransform(HumanBodyBones.Head);
thisjointPoints[PositionIndex.lEye.Int()].Transform = anim.GetBoneTransform(HumanBodyBones.LeftEye);
thisjointPoints[PositionIndex.rEar.Int()].Transform = anim.GetBoneTransform(HumanBodyBones.Head);
thisjointPoints[PositionIndex.rEye.Int()].Transform = anim.GetBoneTransform(HumanBodyBones.RightEye);
thisjointPoints[PositionIndex.Nose.Int()].Transform = Nose.transform;
// Right Leg
thisjointPoints[PositionIndex.rThighBend.Int()].Transform = anim.GetBoneTransform(HumanBodyBones.RightUpperLeg);
thisjointPoints[PositionIndex.rShin.Int()].Transform = anim.GetBoneTransform(HumanBodyBones.RightLowerLeg);
thisjointPoints[PositionIndex.rFoot.Int()].Transform = anim.GetBoneTransform(HumanBodyBones.RightFoot);
thisjointPoints[PositionIndex.rToe.Int()].Transform = anim.GetBoneTransform(HumanBodyBones.RightToes);
// Left Leg
thisjointPoints[PositionIndex.lThighBend.Int()].Transform = anim.GetBoneTransform(HumanBodyBones.LeftUpperLeg);
thisjointPoints[PositionIndex.lShin.Int()].Transform = anim.GetBoneTransform(HumanBodyBones.LeftLowerLeg);
thisjointPoints[PositionIndex.lFoot.Int()].Transform = anim.GetBoneTransform(HumanBodyBones.LeftFoot);
thisjointPoints[PositionIndex.lToe.Int()].Transform = anim.GetBoneTransform(HumanBodyBones.LeftToes);
// etc
thisjointPoints[PositionIndex.abdomenUpper.Int()].Transform = anim.GetBoneTransform(HumanBodyBones.Spine);
thisjointPoints[PositionIndex.hip.Int()].Transform = anim.GetBoneTransform(HumanBodyBones.Hips);
thisjointPoints[PositionIndex.head.Int()].Transform = anim.GetBoneTransform(HumanBodyBones.Head);
thisjointPoints[PositionIndex.neck.Int()].Transform = anim.GetBoneTransform(HumanBodyBones.Neck);
thisjointPoints[PositionIndex.spine.Int()].Transform = anim.GetBoneTransform(HumanBodyBones.Spine);
}
