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