/// <summary>
/// Bodyの関節を反転する
/// </summary>
public void InverseJoints()
{
Dictionary <JointType, Joint> newJoints = new Dictionary <JointType, Joint>();
Dictionary <JointType, Point> newColorPoints = new Dictionary <JointType, Point>();
Dictionary <JointType, Point> newDepthPoints = new Dictionary <JointType, Point>();
foreach (JointType key in Enum.GetValues(typeof(JointType)))
{
JointType newKey = CalcEx.GetMirroredJoint(key);
if (this.Joints.ContainsKey(key))
{
newJoints[newKey] = this.Joints[key];
}
if (this.colorSpacePoints.ContainsKey(key))
{
newColorPoints[newKey] = this.colorSpacePoints[key];
}
if (this.depthSpacePoints.ContainsKey(key))
{
newDepthPoints[newKey] = this.depthSpacePoints[key];
}
}
this.Joints = newJoints;
this.colorSpacePoints = newColorPoints;
this.depthSpacePoints = newDepthPoints;
}
/// <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>
/// 反転している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);
}
}
}
}
}
}
}
/// <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);
}
