/// <summary> /// 骨のデータを蓄積する /// </summary> /// <param name="joints"></param> public void StoreBoneLength(Dictionary <JointType, Joint> joints) { Dictionary <JointType, Joint> validJoints = Utility.GetValidJoints(joints); Joint firstJoint, secondJoint; Bone boneKey; List <double> lengthVal; foreach (Bone bone in bones) { if (validJoints.TryGetValue(bone.Item1, out firstJoint) && validJoints.TryGetValue(bone.Item2, out secondJoint)) { double lengthSq = CvEx.GetDistanceSq(firstJoint.Position.ToCvPoint3D(), secondJoint.Position.ToCvPoint3D()); boneKey = Tuple.Create(firstJoint.JointType, secondJoint.JointType); if (boneLengthSqLog.TryGetValue(boneKey, out lengthVal)) { lengthVal.Add(lengthSq); } else { boneLengthSqLog[boneKey] = new List <double>() { lengthSq }; } } } }
/// <summary> /// Jointから絶対座標を引くDictionary二つの間の距離を求めます /// </summary> /// <param name="jointWithAbsPosition1">骨格ごとの絶対座標1</param> /// <param name="jointWithAbsPosition2">骨格ごとの絶対座標2</param> /// <param name="mirrored">一方の骨格を左右反転した結果で距離を求めるかどうか</param> /// <returns></returns> static double getDistance(IDictionary <JointType, CvPoint3D64f> jointWithAbsPosition1, IDictionary <JointType, CvPoint3D64f> jointWithAbsPosition2, bool mirrored) { IEnumerable <JointType> joints2Alt = jointWithAbsPosition2.Keys.Select(j => mirrored ? CalcEx.GetMirroredJoint(j) : j); List <JointType> intersect = jointWithAbsPosition1.Keys.Intersect(joints2Alt).ToList(); List <double> distanceSqList = new List <double>(); foreach (JointType joint in intersect) { JointType jointAlt = mirrored ? CalcEx.GetMirroredJoint(joint) : joint; distanceSqList.Add(CvEx.GetDistanceSq(jointWithAbsPosition1[joint], jointWithAbsPosition2[jointAlt])); } // 中央値の平方根 return(Math.Sqrt(CalcEx.GetMedian(distanceSqList))); }
/// <summary> /// 2つのJointsの距離を計算する /// </summary> /// <param name="jointsOne"></param> /// <param name="jointsTwo"></param> /// <returns></returns> public static Dictionary <JointType, double> CalcBodyDistanceSq(Dictionary <JointType, Joint> jointsOne, Dictionary <JointType, Joint> jointsTwo) { Dictionary <JointType, double> res = new Dictionary <JointType, double>(); IEnumerable <JointType> keys = jointsOne.Keys.Intersect(jointsTwo.Keys); foreach (JointType key in keys) { CvPoint3D64f one = jointsOne[key].Position.ToCvPoint3D(); CvPoint3D64f two = jointsTwo[key].Position.ToCvPoint3D(); double distanceSq = CvEx.GetDistanceSq(one, two); res[key] = distanceSq; } return(res); }
/// <summary> /// 統計値から有用な範囲の骨のみを残す /// </summary> /// <param name="joints"></param> /// <returns></returns> public Dictionary <JointType, Joint> FilterBonesByStatistics(Dictionary <JointType, Joint> joints) { // TODO crossから腕とか削除するやつ Dictionary <JointType, Joint> validJoints = Utility.GetValidJoints(joints); Dictionary <JointType, Joint> result = validJoints.CloneDeep(); Dictionary <JointType, bool> adaptJoints = new Dictionary <JointType, bool>(); Joint firstJoint, secondJoint; Bone boneKey; foreach (Bone bone in bones) { if (validJoints.TryGetValue(bone.Item1, out firstJoint) && validJoints.TryGetValue(bone.Item2, out secondJoint)) { boneKey = Tuple.Create(firstJoint.JointType, secondJoint.JointType); double lengthSq = CvEx.GetDistanceSq(firstJoint.Position.ToCvPoint3D(), secondJoint.Position.ToCvPoint3D()); if (boneLengthSqStatistics[bone].IsValidBone(lengthSq)) { adaptJoints[firstJoint.JointType] = true; adaptJoints[secondJoint.JointType] = true; } } } bool armDup = this.CheckArmDuplication(validJoints); if (armDup) { foreach (JointType jointType in this.arms) { adaptJoints[jointType] = false; } } foreach (JointType jointType in adaptJoints.Keys) { if (adaptJoints[jointType] == false) { result.Remove(jointType); } } return(result); }
/// <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); }
/// <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); } } } } } } }
private void buttonScalingScore_Click(object sender, RoutedEventArgs e) { int cols, rows; double horizLength, vertLength; if (!parseChessboardParameters(out cols, out rows, out horizLength, out vertLength)) { return; } // 以下改造 MotionDataHandler handler; string path; if (openMotionData(out handler, out path)) { CvMat displayMat1 = null; CvMat displayMat3 = null; CvMat displayMat4 = null; CvMat gray = null; int length = handler.FrameCount; if (length == 0) { return; } CvSize boardSize = new CvSize(cols, rows); CvSize imageSize = new CvSize(); List <Tuple <double, double> > pairs = new List <Tuple <double, double> >(); CvPoint2D32f[] lastCorners = null; IEnumerable <CvMat> colorImages, depthImages; Utility.LoadImages(handler.GetColorImagePaths(), out colorImages); Utility.LoadImages(handler.GetDepthImagePaths(), out depthImages); var images = colorImages.Zip(depthImages, (first, second) => Tuple.Create(first, second)); foreach (Tuple <CvMat, CvMat> imagePair in images) { CvMat imageMat = imagePair.Item1; CvMat depthMat = imagePair.Item2; if (displayMat4 == null) { displayMat4 = CvEx.InitCvMat(imageMat); } imageSize = new CvSize(imageMat.Cols, imageMat.Rows); CvPoint2D32f[] corners; int count; CvEx.InitCvMat(ref gray, imageMat, MatrixType.U8C1); imageMat.CvtColor(gray, ColorConversion.RgbToGray); if (gray.FindChessboardCorners(boardSize, out corners, out count, ChessboardFlag.AdaptiveThresh)) { CvEx.CloneCvMat(ref displayMat1, imageMat); CvTermCriteria criteria = new CvTermCriteria(50, 0.01); gray.FindCornerSubPix(corners, count, new CvSize(3, 3), new CvSize(-1, -1), criteria); CvPoint3D32f?[] cornerPoints = new CvPoint3D32f?[corners.Length]; for (int j = 0; j < corners.Length; j++) { CvPoint2D32f corner = corners[j]; double? value = CalcEx.BilateralFilterDepthMatSinglePixel(corner, depthMat, 100, 4, 9); if (value.HasValue) { cornerPoints[j] = new CvPoint3D32f(corner.X, corner.Y, value.Value); } } for (int x = 0; x < cols; x++) { for (int y = 0; y < rows; y++) { if (!cornerPoints[x + y * cols].HasValue) { continue; } CvPoint3D32f point1 = cornerPoints[x + y * cols].Value; CvPoint3D64f undistortPoint1 = this.UndistortionData.GetRealFromScreenPos(point1, imageSize); foreach (var offset in new[] { new { X = 1, Y = 0, D = horizLength }, new { X = 0, Y = 1, D = vertLength } }) { int dx = x + offset.X; int dy = y + offset.Y; if (dx >= cols || dy >= rows) { continue; } if (!cornerPoints[dx + dy * cols].HasValue) { continue; } CvPoint3D32f point2 = cornerPoints[dx + dy * cols].Value; CvPoint3D64f undistortPoint2 = this.UndistortionData.GetRealFromScreenPos(point2, imageSize); double distance = Math.Sqrt(CvEx.GetDistanceSq(undistortPoint1, undistortPoint2)); double scale = distance / offset.D; CvColor color = CalcEx.HSVtoRGB(Math.Max(0, Math.Min(300, scale * 600 - 450)), scale, 2 - scale); displayMat4.DrawLine((int)point1.X, (int)point1.Y, (int)point2.X, (int)point2.Y, new CvScalar(color.R, color.G, color.B), 1, LineType.AntiAlias); pairs.Add(new Tuple <double, double>(distance, offset.D)); } } } CvEx.DrawChessboardCornerFrame(displayMat1, boardSize, corners, new CvScalar(64, 128, 64)); displayMat1.DrawChessboardCorners(boardSize, corners, true); lastCorners = corners; putImage(displayMat1, PixelFormats.Rgb24); } else { CvEx.CloneCvMat(ref displayMat3, imageMat); putImage(displayMat3, PixelFormats.Rgb24); } } CvMat displayMat2 = CvEx.InitCvMat(displayMat1); displayMat1.Undistort2(displayMat2, this.UndistortionData.CameraStruct.CreateCvMat(), this.UndistortionData.DistortStruct.CreateCvMat(true)); if (lastCorners != null) { drawUndistortedCornerFrame(displayMat2, lastCorners, boardSize); } displayMat2.PutText(string.Format("Min: {0}", pairs.Min(x => x.Item1 / x.Item2)), new CvPoint(20, 20), new CvFont(FontFace.HersheyPlain, 1, 1), new CvScalar(255, 255, 255)); displayMat2.PutText(string.Format("Max: {0}", pairs.Max(x => x.Item1 / x.Item2)), new CvPoint(20, 40), new CvFont(FontFace.HersheyPlain, 1, 1), new CvScalar(255, 255, 255)); displayMat2.PutText(string.Format("Avg: {0}", pairs.Average(x => x.Item1 / x.Item2)), new CvPoint(20, 60), new CvFont(FontFace.HersheyPlain, 1, 1), new CvScalar(255, 255, 255)); displayMat2.PutText(string.Format("Med: {0}", CalcEx.GetMedian(pairs.Select(x => x.Item1 / x.Item2).ToList())), new CvPoint(20, 80), new CvFont(FontFace.HersheyPlain, 1, 1), new CvScalar(255, 255, 255)); putImage(displayMat4, PixelFormats.Rgb24); displayLabels(); } }