/// <summary> /// あるフレーム範囲の全ユーザの点群を出力する /// </summary> /// <param name="sender"></param> /// <param name="e"></param> private void ExportFrameRangeUserPointClouds_Click(object sender, RoutedEventArgs e) { List <Frame> frames = frameSequence.Slice(startIndex, endIndex); for (int i = 0; i < frameSequence.recordNum; i++) { List <List <float[]> > pointsSequence = new List <List <float[]> >(); string path = System.IO.Path.Combine(Environment.GetFolderPath(Environment.SpecialFolder.DesktopDirectory), i.ToString() + "_UserPointsCloud.dump"); for (int frameNo = 0; frameNo < frames.Count(); frameNo++) { // フレームレート半分 if (frameNo % 2 == 0) { continue; } Frame frame = frames[frameNo]; List <Tuple <CvPoint3D64f, CvColor> > colors = frameSequence.LocalCoordinateMappers[i].GetUserColorPoints(frame.DepthMatList[i], frame.ColorMatList[i], frame.UserMatList[i]); colors = colors.Select(t => Tuple.Create(CvEx.ConvertPoint3D(t.Item1, frameSequence.ToWorldConversions[i]), t.Item2)).ToList(); List <float[]> dumpColors = colors.Select(t => new float[] { (float)t.Item1.X, (float)t.Item1.Y, (float)t.Item1.Z, t.Item2.R, t.Item2.G, t.Item2.B }).ToList(); // 点の数1/10 dumpColors = dumpColors.Where((fs, index) => index % 10 == 0).ToList(); pointsSequence.Add(dumpColors); } Utility.SaveToBinary(pointsSequence.ToArray(), path); } }
/// <summary> /// Jointsに座標変換を適用する /// </summary> /// <param name="joints"></param> /// <param name="conversion"></param> /// <returns></returns> public static Dictionary <JointType, Joint> ApplyConversions(Dictionary <JointType, Joint> joints, CvMat conversion) { Dictionary <JointType, Joint> newJoints = new Dictionary <JointType, Joint>(); foreach (JointType jointType in joints.Keys) { Joint originalJoint = joints[jointType]; CvPoint3D64f fromPoint = originalJoint.Position.ToCvPoint3D(); CameraSpacePoint newPoint = CvEx.ConvertPoint3D(fromPoint, conversion).ToCameraSpacePoint(); originalJoint.Position = newPoint; newJoints[jointType] = originalJoint; } return(newJoints); }
public Dictionary <JointType, CvPoint3D64f> InterpolateSkeleton(MotionData prevFrame, MotionData nextFrame, Dictionary <JointType, Joint> prevJoints, Dictionary <JointType, Joint> nextJoints, DateTime time, CvMat ToWorldConversion) { double prevWeight; if (prevFrame.TimeStamp >= nextFrame.TimeStamp) { prevWeight = 1; } else { prevWeight = (time - prevFrame.TimeStamp).TotalSeconds / (nextFrame.TimeStamp - prevFrame.TimeStamp).TotalSeconds; } double nextWeight = 1.0 - prevWeight; if (prevJoints == null || nextJoints == null) { return(null); } Dictionary <JointType, CvPoint3D64f> prevData = prevJoints.ToDictionary(p => p.Key, p => (CvPoint3D64f)p.Value.Position.ToCvPoint3D()); Dictionary <JointType, CvPoint3D64f> nextData = nextJoints.ToDictionary(p => p.Key, p => (CvPoint3D64f)p.Value.Position.ToCvPoint3D()); List <JointType> joints = prevData.Keys.Union(nextData.Keys).ToList(); Dictionary <JointType, CvPoint3D64f> ret = new Dictionary <JointType, CvPoint3D64f>(); foreach (JointType joint in joints) { CvPoint3D64f prevPos, nextPos; bool prevFound = prevData.TryGetValue(joint, out prevPos); bool nextFound = nextData.TryGetValue(joint, out nextPos); if ((prevFound && nextFound)) { ret[joint] = CvEx.ConvertPoint3D(prevPos * prevWeight + nextPos * nextWeight, ToWorldConversion); } else if (_omitWhenDataLack) { if (prevFound) { ret[joint] = CvEx.ConvertPoint3D(prevPos, ToWorldConversion); } else if (nextFound) { ret[joint] = CvEx.ConvertPoint3D(nextPos, ToWorldConversion); } } } return(ret); }
/// <summary> /// あるフレームの点群を出力する /// </summary> /// <param name="sender"></param> /// <param name="e"></param> private void ExportFrameRangePointClouds_Click(object sender, RoutedEventArgs e) { List <Frame> frames = frameSequence.Slice(startIndex, endIndex); for (int i = 0; i < frameSequence.recordNum; i++) { List <float[]>[] pointsSequence = new List <float[]> [frames.Count()]; string path = System.IO.Path.Combine(Environment.GetFolderPath(Environment.SpecialFolder.DesktopDirectory), i.ToString() + "_PointsCloud.dump"); for (int frameNo = 0; frameNo < frames.Count(); frameNo++) { Frame frame = frames[frameNo]; List <Tuple <CvPoint3D64f, CvColor> > colors = frameSequence.LocalCoordinateMappers[i].DepthColorMatToRealPoints(frame.DepthMatList[i], frame.ColorMatList[i]); colors = colors.Select(t => Tuple.Create(CvEx.ConvertPoint3D(t.Item1, frameSequence.ToWorldConversions[i]), t.Item2)).ToList(); List <float[]> dumpColors = colors.Select(t => new float[] { (float)t.Item1.X, (float)t.Item1.Y, (float)t.Item1.Z, t.Item2.R, t.Item2.G, t.Item2.B }).ToList(); pointsSequence[frameNo] = dumpColors; } Utility.SaveToBinary(pointsSequence, path); } }
/// <summary> /// あるフレームにおける座標変換行列を骨格情報から計算する /// </summary> /// <param name="frame"></param> /// <param name="convList"></param> /// <param name="bodies"></param> /// <returns></returns> public static List <CvMat> GetConvMatrixFromBoneFrame(Frame frame, List <CvMat> convList, List <SerializableBody> bodies) { if (bodies.Count() != frame.recordNum) { System.Windows.MessageBox.Show("ユーザが選択されていないレコードがあります"); return(convList); } bool[] validFlags = frame.GetValidFlags(); for (int j = 1; j < frame.recordNum; j++) { Dictionary <JointType, Joint> joint1 = Utility.GetValidJoints(bodies[0].Joints); Dictionary <JointType, Joint> joint2 = Utility.GetValidJoints(bodies[j].Joints); if (validFlags[0] && validFlags[j] == false) { continue; } ICoordConversion3D crtc = new CoordRotTransConversion(); foreach (JointType jointType in Enum.GetValues(typeof(JointType))) { if (!joint1.ContainsKey(jointType)) { continue; } if (!joint2.ContainsKey(jointType)) { continue; } CvPoint3D64f from = joint2[jointType].Position.ToCvPoint3D(); CvPoint3D64f target = CvEx.ConvertPoint3D(joint1[jointType].Position.ToCvPoint3D(), convList[0]); // IsOriginlJointValid相当の処理を入れるかどうか crtc.PutPoint(from, target, 1); } convList[j] = crtc.Solve(); } return(convList); }
public static UserSegmentation[] Identification(FrameSequence frameseq, double maxDistance) { if (frameseq.Segmentations.Any(seg => seg == null)) { throw new InvalidOperationException("ユーザトラッキングデータがセグメンテーションされていません"); } HashSet <Tuple <RecordAndUser, RecordAndUser> > contemporaryList = new HashSet <Tuple <RecordAndUser, RecordAndUser> >(); for (int recordNo = 0; recordNo < frameseq.recordNum; recordNo++) { IEnumerable <MotionData> record = frameseq.GetMotionDataSequence(recordNo); int frameIndex = 0; foreach (MotionData motionData in record) { IList <ulong> users = motionData.bodies.ToList().Select(b => b.TrackingId).ToList(); foreach (var tuple in users.SelectMany(u => users.Select(v => new Tuple <RecordAndUser, RecordAndUser>(new RecordAndUser(recordNo, u), new RecordAndUser(recordNo, v))))) { contemporaryList.Add(tuple); } frameIndex++; } } DateTime beginTime = frameseq.startTime; DateTime endTime = frameseq.endTime; double frequency = frameseq.frameRate; TimeSpan increment = new TimeSpan((long)(10000000 / frequency)); long totalCount = (endTime.Ticks - beginTime.Ticks) / increment.Ticks; long totalIndex = 0; Dictionary <Tuple <RecordAndUser, RecordAndUser>, List <double> > distanceListMatrix = new Dictionary <Tuple <RecordAndUser, RecordAndUser>, List <double> >(); foreach (Frame frame in frameseq.Frames) { // 現在の時刻での各レコードの各ユーザの各骨格の絶対座標を求める Dictionary <ulong, Dictionary <JointType, CvPoint3D64f> >[] absPositions = new Dictionary <ulong, Dictionary <JointType, CvPoint3D64f> > [frameseq.recordNum]; for (int recordNo = 0; recordNo < frameseq.recordNum; recordNo++) { Dictionary <ulong, Dictionary <JointType, CvPoint3D64f> > recordUserPositions = new Dictionary <ulong, Dictionary <JointType, CvPoint3D64f> >(); foreach (SerializableBody body in frame.GetBodyList(recordNo)) { Dictionary <JointType, CvPoint3D64f> userPositions = new Dictionary <JointType, CvPoint3D64f>(); if (body.Joints == null) { continue; } foreach (var jointPair in body.Joints) { CvPoint3D64f posInCamera = jointPair.Value.Position.ToCvPoint3D(); CvPoint3D64f posInWorld = CvEx.ConvertPoint3D(posInCamera, frameseq.ToWorldConversions[recordNo]); userPositions[jointPair.Key] = posInWorld; } recordUserPositions[body.TrackingId] = userPositions; } absPositions[recordNo] = recordUserPositions; } // 現在の時刻で各レコード間のユーザ間の距離を求める for (int i = 0; i < frameseq.recordNum; i++) { if (absPositions[i] == null) { continue; } for (int j = i + 1; j < frameseq.recordNum; j++) { if (absPositions[j] == null) { continue; } foreach (var userJoint1 in absPositions[i]) { RecordAndUser recordUser1 = new RecordAndUser(i, userJoint1.Key); foreach (var userJoint2 in absPositions[j]) { RecordAndUser recordUser2 = new RecordAndUser(j, userJoint2.Key); double distanceNormal = getDistance(userJoint1.Value, userJoint2.Value, false); double distanceMirrored = getDistance(userJoint1.Value, userJoint2.Value, true); double distance = Math.Min(distanceNormal, distanceMirrored); Tuple <RecordAndUser, RecordAndUser> key = new Tuple <RecordAndUser, RecordAndUser>(recordUser1, recordUser2); List <double> distanceList; if (!distanceListMatrix.TryGetValue(key, out distanceList)) { distanceListMatrix[key] = distanceList = new List <double>(); } distanceList.Add(distance); } } } } totalIndex++; } // 中央値で集計して小さい順に並べる Dictionary <Tuple <RecordAndUser, RecordAndUser>, double> distanceMatrix = distanceListMatrix.ToDictionary(p => p.Key, p => CalcEx.GetMedian(p.Value)); List <Tuple <RecordAndUser, RecordAndUser, double> > neighborList = ( from x in distanceMatrix orderby x.Value select new Tuple <RecordAndUser, RecordAndUser, double>(x.Key.Item1, x.Key.Item2, x.Value) ).ToList(); IdentificationSet <RecordAndUser> identificationSet = new IdentificationSet <RecordAndUser>(); // 同一判定をする foreach (var neighbor in neighborList) { if (neighbor.Item3 > maxDistance) { identificationSet.Add(neighbor.Item1); identificationSet.Add(neighbor.Item2); continue; } IList <RecordAndUser> recordUsers1 = identificationSet.GetEquivalentElements(neighbor.Item1); IList <RecordAndUser> recordUsers2 = identificationSet.GetEquivalentElements(neighbor.Item2); // 同フレーム内にいるか判定 bool contemporary = ( from ru1 in recordUsers1 from ru2 in recordUsers2 select new Tuple <RecordAndUser, RecordAndUser>(ru1, ru2)).Any(pair => contemporaryList.Contains(pair)); if (!contemporary) { // 同フレーム内にいなければ同一視 identificationSet.MakeEquivalent(neighbor.Item1, neighbor.Item2); } } // 番号を圧縮 identificationSet.CompactIdentificationNumber(); // 新しいセグメンテーション番号を与える UserSegmentation[] ret = Enumerable.Range(0, frameseq.recordNum).Select(i => new UserSegmentation()).ToArray(); for (int recordNo = 0; recordNo < frameseq.recordNum; recordNo++) { foreach (var pair in frameseq.Segmentations[recordNo].Conversions) { int frameIndex = pair.Key; Dictionary <ulong, int> newConversions = new Dictionary <ulong, int>(); foreach (var conv in pair.Value) { int ident = identificationSet.ConvertToIdentificationNumber(new RecordAndUser(recordNo, conv.Key)); newConversions[conv.Key] = ident; } ret[recordNo].Conversions[frameIndex] = newConversions; } ret[recordNo].fixNumUsers(); } return(ret); }
public CvMat CalculateTransform(int targetModelIndex, bool updateInternalModelTransform, double randomSamplingRatio) { if (targetModelIndex < 0 || targetModelIndex >= _flannModels.Count) { throw new ArgumentOutOfRangeException("targetModelIndex"); } CoordRotTransConversion coordConverter = new CoordRotTransConversion(); //CoordConvertSpring coordConverter = new CoordConvertSpring(_modelTransforms[targetModelIndex]); //foreach (var point in dataPointListInWorldCoordinate) { List <Tuple <CvPoint3D64f, CvColor> > tuples = _flannModels[targetModelIndex].ModelPoints; if (randomSamplingRatio < 1) { Random rand = new Random(); tuples = tuples.Where(x => rand.NextDouble() < randomSamplingRatio).ToList(); } CvMat targetTransform = _modelTransforms[targetModelIndex]; List <CvMat> inverseTransforms = new List <CvMat>(); foreach (CvMat transform in _modelTransforms) { CvMat inv = CvEx.InitCvMat(transform); transform.Invert(inv); inverseTransforms.Add(inv); } float searchDistanceSq = this.SearchDistance * this.SearchDistance; Parallel.ForEach(tuples, tuple => { CvPoint3D64f point = tuple.Item1; CvColor color = tuple.Item2; //foreach (var point in points) { CvPoint3D64f worldPoint = CvEx.ConvertPoint3D(point, targetTransform); int minModelIndex = -1; int minPointIndex = -1; float minDistanceSq = float.MaxValue; for (int modelIndex = 0; modelIndex < _flannModels.Count; modelIndex++) { if (modelIndex == targetModelIndex) { continue; } CvPoint3D64f inversePoint = CvEx.ConvertPoint3D(worldPoint, inverseTransforms[modelIndex]); int[] indices; float[] distances; _flannModels[modelIndex].KnnSearch(inversePoint, color, out indices, out distances, 1); if (indices.Length >= 1) { float distanceSq = distances[0]; if (distanceSq <= searchDistanceSq) { if (distanceSq < minDistanceSq) { minModelIndex = modelIndex; minPointIndex = indices[0]; minDistanceSq = distanceSq; } } } } if (minModelIndex != -1) { Tuple <CvPoint3D64f, CvColor> bestModelPoint = _flannModels[minModelIndex].ModelPoints[minPointIndex]; double weightTo = 1.0 / (Math.Abs(bestModelPoint.Item1.Z - 1500 / 1000f) + 5000 / 1000f); double weightFrom = 1.0 / (Math.Abs(point.Z - 1500 / 1000f) + 5000 / 1000f); //weightFrom = weightTo = 1; double weight = _weightFromDistanceSq(minDistanceSq) * weightFrom * weightTo; CvPoint3D64f from = CvEx.ConvertPoint3D(point, targetTransform); CvPoint3D64f to = CvEx.ConvertPoint3D(bestModelPoint.Item1, _modelTransforms[minModelIndex]); coordConverter.PutPoint(from, to, weight); } }); CvMat ret = coordConverter.Solve() * targetTransform; if (updateInternalModelTransform) { _modelTransforms[targetModelIndex] = ret.Clone(); } return(ret); }
/// <summary> /// 修正してくれるやつ /// </summary> /// <param name="frameSeq"></param> public static void Correct(FrameSequence frameSeq) { List <OrderTuple> orders = new List <OrderTuple>(); foreach (var pair in frameSeq.Frames.Select((frame, index) => Tuple.Create(frame, index))) { for (int recordNo = 0; recordNo < pair.Item1.recordNum; recordNo++) { orders.Add(new OrderTuple(pair.Item1.Time, recordNo, pair.Item2)); } } // 前フレームのユーザと対応するBodyを保持する Dictionary <int, SerializableBody>[] prevUserBodyMapArray = new Dictionary <int, SerializableBody> [frameSeq.recordNum]; for (int i = 0; i < frameSeq.recordNum; i++) { prevUserBodyMapArray[i] = new Dictionary <int, SerializableBody>(); } SerializableBody prevUserBody; orders = orders.OrderBy(x => x.Timestamp.Ticks).ToList(); SkeletonInterpolator interp = new SkeletonInterpolator(0.5, true); foreach (OrderTuple tuple in orders) { Frame currFrame = frameSeq.Frames[tuple.FrameIndex]; const long maxInterval = (long)(10000000 * 0.1); DateTime prev = new DateTime(tuple.Timestamp.Ticks - maxInterval); IEnumerable <int> users = currFrame.GetBodyList(tuple.RecordIndex).Select(b => b.integratedId); foreach (int user in users) { SerializableBody currBody = currFrame.GetSelectedBody(tuple.RecordIndex, integratedId: user); // 前のBodyと比較して同じ場合は処理をスキップする if (prevUserBodyMapArray[tuple.RecordIndex].TryGetValue(user, out prevUserBody) && prevUserBody.Joints != null && currBody.Joints != null) { double avgDistanceSq = CalcBodyDistanceSq(prevUserBody.Joints, currBody.Joints).Values.Average(); if (avgDistanceSq == 0.0) { //currFrame.DeleteBody(tuple.RecordIndex, integratedId: user); continue; } } prevUserBodyMapArray[tuple.RecordIndex][user] = currBody; Dictionary <JointType, CvPoint3D64f> prevJoints = interp.IntegrateSkeleton(prev, user, frameSeq); if (prevJoints != null && currBody.Joints != null) { Dictionary <JointType, CvPoint3D64f> currJoints = currBody.Joints.ToDictionary(p => p.Key, p => (CvPoint3D64f)p.Value.Position.ToCvPoint3D()); HashSet <JointType> mirroredPrevKeys = new HashSet <JointType>(prevJoints.Keys.Select(j => CalcEx.GetMirroredJoint(j))); if (currJoints != null && prevJoints != null) { var absJoints = currJoints.ToDictionary(p => p.Key, p => CvEx.ConvertPoint3D(p.Value, frameSeq.ToWorldConversions[tuple.RecordIndex])); var absMirroredJoints = absJoints.ToDictionary(p => CalcEx.GetMirroredJoint(p.Key), p => p.Value); var availableKeys = prevJoints.Keys.Where(j => mirroredPrevKeys.Contains(j)).ToList(); var keysNormal = availableKeys.Intersect(absJoints.Keys).ToList(); var keysMirrored = availableKeys.Intersect(absMirroredJoints.Keys).ToList(); if (keysNormal.Count > 0 && keysMirrored.Count > 0) { double avg1 = keysNormal.Select(j => CvEx.GetDistanceSq(prevJoints[j], absJoints[j])).Average(); double avg2 = keysMirrored.Select(j => CvEx.GetDistanceSq(prevJoints[j], absMirroredJoints[j])).Average(); if (avg2 < avg1) { currFrame.InverseBody(tuple.RecordIndex, integratedId: user); } } } } } } }
/// <summary> /// 反転しているJointsをPivot基準に修正する /// </summary> /// <param name="joints"></param> /// <param name="conversion"></param> /// <returns></returns> public Dictionary <JointType, Joint> CorrectMirrorJoint(Dictionary <JointType, Joint> joints, CvMat conversion) { HashSet <JointType> mirroredPivotKeys = new HashSet <JointType>(pivot.Keys.Select(j => CalcEx.GetMirroredJoint(j))); List <JointType> availableKeys = pivot.Keys.Where(j => mirroredPivotKeys.Contains(j)).ToList(); // pivotの左右反転して共通なキー Dictionary <JointType, CvPoint3D64f> absJoints = joints.ToDictionary(p => p.Key, p => CvEx.ConvertPoint3D(p.Value.Position.ToCvPoint3D(), conversion)); Dictionary <JointType, CvPoint3D64f> absMirroredJoints = absJoints.ToDictionary(p => CalcEx.GetMirroredJoint(p.Key), p => p.Value); List <JointType> keysNormal = availableKeys.Intersect(absJoints.Keys).ToList(); List <JointType> keysMirrored = availableKeys.Intersect(absMirroredJoints.Keys).ToList(); if (keysNormal.Count > 0 && keysMirrored.Count > 0) { double avg1 = keysNormal.Select(j => CvEx.GetDistanceSq(pivot[j], absJoints[j])).Average(); double avg2 = keysMirrored.Select(j => CvEx.GetDistanceSq(pivot[j], absMirroredJoints[j])).Average(); // mirroredのほうが似てる場合 if (avg2 < avg1) { return(joints.ToDictionary(p => CalcEx.GetMirroredJoint(p.Key), p => p.Value)); } } return(joints); }
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]; } } }