/// <summary> /// 統合後のデータに対して瞬間的な左右反転をなおす /// </summary> /// <param name="seqJoints"></param> public static List <Dictionary <JointType, CvPoint3D64f> > SequentialCorrect(List <Dictionary <JointType, CvPoint3D64f> > seqJoints) { CvPoint3D64f prevShoulderVector = new CvPoint3D64f(); CvPoint3D64f prevHipVector = new CvPoint3D64f(); List <Dictionary <JointType, CvPoint3D64f> > res = new List <Dictionary <JointType, CvPoint3D64f> >(); foreach (Dictionary <JointType, CvPoint3D64f> joints in seqJoints) { bool reverse = false; if (joints.ContainsKey(JointType.ShoulderLeft) && joints.ContainsKey(JointType.ShoulderRight)) { CvPoint3D64f shoulderVector = joints[JointType.ShoulderLeft] - joints[JointType.ShoulderRight]; if (prevShoulderVector == default(CvPoint3D64f)) { prevShoulderVector = shoulderVector; } if (CvEx.Cos(shoulderVector, prevShoulderVector) <= -0.8) { reverse = true; } } if (joints.ContainsKey(JointType.HipLeft) && joints.ContainsKey(JointType.HipRight)) { CvPoint3D64f hipVector = joints[JointType.HipLeft] - joints[JointType.HipRight]; if (prevHipVector == default(CvPoint3D64f)) { prevHipVector = hipVector; } if (CvEx.Cos(hipVector, prevHipVector) <= -0.8) { reverse = true; } } if (reverse) { Dictionary <JointType, CvPoint3D64f> newJoints = joints.ToDictionary(p => CalcEx.GetMirroredJoint(p.Key), p => p.Value); res.Add(newJoints); // reverseした後のただしいベクトルを入れなおしておく if (joints.ContainsKey(JointType.ShoulderLeft) && joints.ContainsKey(JointType.ShoulderRight)) { prevShoulderVector = newJoints[JointType.ShoulderLeft] - newJoints[JointType.ShoulderRight]; } if (joints.ContainsKey(JointType.HipLeft) && joints.ContainsKey(JointType.HipRight)) { prevHipVector = newJoints[JointType.HipLeft] - newJoints[JointType.HipRight]; } } else { res.Add(joints); } } return(res); }
/// <summary> /// 2つのベクトルの角度をラジアンで返す /// </summary> /// <param name="one"></param> /// <param name="two"></param> /// <returns></returns> public static double GetVectorRadian(CvPoint3D64f one, CvPoint3D64f two) { // masterにslaveを回転させるコード //CvPoint3D64f slave = new CvPoint3D64f(0.7071, 0, 0.7071); //CvPoint3D64f master = new CvPoint3D64f(0, 0, 1); //double rad = Utility.GetVectorRadian(master, slave); //CvMat mat = CvEx.GetRotation3D(new CvPoint3D64f(0, 1, 0), rad); //CvPoint3D64f to = CvEx.RotatePoint3D(slave, mat); one.Y = 0; two.Y = 0; CvPoint3D64f norm = CvEx.Cross(two, one); double cos = CvEx.Cos(two, one); double rad = Math.Acos(cos); if (norm.Y < 0) { rad = -1 * rad; } return(rad); }
/// <summary> /// 時系列データの前後を比較して左右反転をなおす /// </summary> /// <param name="frameSeq"></param> public static void SequentialCorrect(FrameSequence frameSeq) { CvPoint3D64f[] prevShoulderVectors = new CvPoint3D64f[frameSeq.recordNum]; CvPoint3D64f[] prevHipVectors = new CvPoint3D64f[frameSeq.recordNum]; foreach (Frame frame in frameSeq.Frames) { List <SerializableBody> bodies = frame.GetSelectedBodyList(integratedIds: frameSeq.selecteedIntegretedIdList); if (bodies.Count == 0) { continue; } CvPoint3D64f[] shoulderVectors = bodies.Select(b => (CvPoint3D64f)(b.Joints[JointType.ShoulderLeft].Position.ToCvPoint3D() - b.Joints[JointType.ShoulderRight].Position.ToCvPoint3D())).ToArray(); CvPoint3D64f[] hipVectors = bodies.Select(b => (CvPoint3D64f)(b.Joints[JointType.HipLeft].Position.ToCvPoint3D() - b.Joints[JointType.HipRight].Position.ToCvPoint3D())).ToArray(); // 0回目の処理 if (prevShoulderVectors == default(CvPoint3D64f[]) || prevHipVectors == default(CvPoint3D64f[])) { prevShoulderVectors = shoulderVectors; prevHipVectors = hipVectors; continue; } double[] shoulderCosArray = shoulderVectors.Zip(prevShoulderVectors, (cur, prev) => CvEx.Cos(cur, prev)).ToArray(); double[] hipCosArray = hipVectors.Zip(prevHipVectors, (cur, prev) => CvEx.Cos(cur, prev)).ToArray(); for (int no = 0; no < frame.recordNum; no++) { if (shoulderCosArray[no] <= -0.8 || hipCosArray[no] <= -0.8) { frame.InverseBody(no, integratedId: frameSeq.selecteedIntegretedIdList[no]); } } // Inverseされていた場合には骨の位置関係が変わっているため prevShoulderVectors = bodies.Select(b => (CvPoint3D64f)(b.Joints[JointType.ShoulderLeft].Position.ToCvPoint3D() - b.Joints[JointType.ShoulderRight].Position.ToCvPoint3D())).ToArray(); prevHipVectors = bodies.Select(b => (CvPoint3D64f)(b.Joints[JointType.HipLeft].Position.ToCvPoint3D() - b.Joints[JointType.HipRight].Position.ToCvPoint3D())).ToArray(); } }
public void Correct(FrameSequence frameSeq) { // 選択中の統合IDをチェック List <TrustData> trustDatas = frameSeq.trustData.Where(t => t.integratedBodyId == frameSeq.selecteedIntegretedIdList[0]).ToList(); // generate iteration List <Tuple <TrustData, int> > iterations = this.GenerateIterationRanges(frameSeq.Frames.Count(), frameSeq.trustData); foreach (Tuple <TrustData, int> iterationRange in iterations) { // set and calcurate pivot TrustData trustData = iterationRange.Item1; SerializableBody pivotBody = trustData.GetBody(frameSeq.Frames); // translate to world coordinate Dictionary <JointType, CvPoint3D64f> pivotJoints = pivotBody.Joints.ToDictionary(p => p.Key, p => CvEx.ConvertPoint3D(p.Value.Position.ToCvPoint3D(), frameSeq.ToWorldConversions[trustData.recordIndex])); CvPoint3D64f pivotBodyRightVector = this.CalcRightChestCrossVector(pivotJoints); CvPoint3D64f pivotBodyLeftVector = this.CalcLeftChestCrossVector(pivotJoints); CvPoint3D64f pivotBodyCrossVector = CvEx.Normalize(pivotBodyRightVector + pivotBodyLeftVector); // z軸との角度 +だったらあっち向いてる double pivotCrossZCos = CvEx.Cos(pivotBodyCrossVector, new CvPoint3D64f(0, 0, 1)); // ので、反転してたら修正する if (pivotCrossZCos > 0) { pivotBody.InverseJoints(); pivotJoints = pivotBody.Joints.ToDictionary(p => p.Key, p => CvEx.ConvertPoint3D(p.Value.Position.ToCvPoint3D(), frameSeq.ToWorldConversions[trustData.recordIndex])); pivotBodyRightVector = this.CalcRightChestCrossVector(pivotJoints); pivotBodyLeftVector = this.CalcLeftChestCrossVector(pivotJoints); pivotBodyCrossVector = CvEx.Normalize(pivotBodyRightVector + pivotBodyLeftVector); } // 繰り返し範囲の連続indexを生成して回す IEnumerable <int> continuousRange = this.GenerateContinuousRange(trustData.frameIndex, iterationRange.Item2); foreach (int frameIndex in continuousRange) { // 前のpivotとのベクトルの差が小さいやつを選んでいく投票空間 double[] bodyCos = new double[frameSeq.recordNum]; CvPoint3D64f[] bodyCrosses = new CvPoint3D64f[frameSeq.recordNum]; bool[] validFlags = frameSeq.Frames[frameIndex].GetValidFlags(); for (int recordNo = 0; recordNo < frameSeq.recordNum; recordNo++) { // pivotと一致した場合 if (trustData.recordIndex == recordNo && trustData.frameIndex == frameIndex) { bodyCos[recordNo] = 1; bodyCrosses[recordNo] = pivotBodyCrossVector; continue; } SerializableBody body = frameSeq.Frames[frameIndex].GetSelectedBody(recordNo, integratedId: trustData.integratedBodyId); if (body == null || body == default(SerializableBody) || body.Joints.Count == 0 || validFlags[recordNo] == false) { bodyCos[recordNo] = -1; continue; } Dictionary <JointType, CvPoint3D64f> joints = body.Joints.ToDictionary(p => p.Key, p => CvEx.ConvertPoint3D(p.Value.Position.ToCvPoint3D(), frameSeq.ToWorldConversions[recordNo])); // 右胸、左胸の外積ベクトル(正規化済み) CvPoint3D64f rightVector = this.CalcRightChestCrossVector(joints); CvPoint3D64f leftVector = this.CalcLeftChestCrossVector(joints); // 前フレームの基準ベクトルとの角度(cos) double bothCrossAngle = CvEx.Cos(rightVector, leftVector); double rightPivotAngle = CvEx.Cos(rightVector, pivotBodyCrossVector); double leftPivotAngle = CvEx.Cos(leftVector, pivotBodyCrossVector); bool removedFlag = false; // そもそも骨がなかった場合 if (rightVector == default(CvPoint3D64f)) { body.RemoveJoints(Utility.RightBody); removedFlag = true; } if (leftVector == default(CvPoint3D64f)) { body.RemoveJoints(Utility.LeftBody); removedFlag = true; } // 右と左のベクトルが離れすぎてる場合 if (bothCrossAngle <= 0) { body.RemoveJoints(Utility.UpperBody); removedFlag = true; } if (removedFlag) { bodyCos[recordNo] = -1; continue; } CvPoint3D64f bodyCrossVector = CvEx.Normalize(rightVector + leftVector); double bodyCrossdiff = CvEx.Cos(bodyCrossVector, pivotBodyCrossVector); // reverse check if (bodyCrossdiff <= -0.8) { // reverse and update body.InverseJoints(); joints = body.Joints.ToDictionary(p => p.Key, p => CvEx.ConvertPoint3D(p.Value.Position.ToCvPoint3D(), frameSeq.ToWorldConversions[recordNo])); rightVector = this.CalcRightChestCrossVector(joints); leftVector = this.CalcLeftChestCrossVector(joints); bodyCrossVector = CvEx.Normalize(rightVector + leftVector); bodyCrossdiff = CvEx.Cos(bodyCrossVector, pivotBodyCrossVector); } // update body angle bodyCos[recordNo] = bodyCrossdiff; bodyCrosses[recordNo] = bodyCrossVector; } // 前のpivotVectorと似ているほどよい. つまり1に近いほど int pivotRecordNo = bodyCos.ToList().IndexOf(bodyCos.Max()); // update pivot body vector pivotBodyCrossVector = bodyCrosses[pivotRecordNo]; } } }