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); }