/// <summary>
/// 現在のフレームのレコード・ユーザを信頼できるデータとしてタグ付けする
/// TODO : UI上で特別なデータは色とかで反映する
/// </summary>
/// <param name="sender"></param>
/// <param name="e"></param>
private void CheckTrustDataFrame_Click(object sender, RoutedEventArgs e)
{
// record, userを選択中のやつ
for (int recordNo = 0; recordNo < frameSequence.recordNum; recordNo++)
{
if (this.isRecordSelected[recordNo])
{
int record = recordNo;
int user = this.frameSequence.selecteedIntegretedIdList[recordNo];
int frame = this.playingIndex;
TrustData td = new TrustData(frame, record, user);
// こいつをUI上で反映する手がかりにする
SerializableBody body = td.GetBody(this.frameSequence.Frames);
body.trustData = true;
this.frameSequence.trustData.Add(td);
}
}
}
public void DeleteBody(int recordNo, int integratedId = -1, ulong originalId = 0)
{
MotionData md = this.records[recordNo];
for (int i = 0; i < md.bodies.Length; i++)
{
SerializableBody body = md.bodies[i];
if (integratedId != -1 && body.integratedId == integratedId)
{
Array.Clear(md.bodies, i, 1);
}
else if (originalId != 0 && body.TrackingId == originalId)
{
Array.Clear(md.bodies, i, 1);
}
}
md.bodies = md.bodies.Where(b => b != null).ToArray();
}
/// <summary>
/// idの一致するBodyを返す
/// </summary>
/// <param name="recordNo"></param>
/// <param name="integratedId"></param>
/// <param name="originalId"></param>
/// <returns></returns>
public SerializableBody GetSelectedBody(int recordNo, int integratedId = -1, ulong originalId = 0)
{
SerializableBody body = null;
if (integratedId != -1)
{
body = this.records[recordNo].bodies.Where((b) => b.integratedId == integratedId).FirstOrDefault();
}
else if (originalId != 0)
{
body = this.records[recordNo].bodies.Where((b) => b.TrackingId == originalId).FirstOrDefault();
}
/// idが違うときの場合. 本来はセグメンテーションすべき.
if (body == null || body.Equals(default(SerializableBody)))
{
return(null);
}
return(body);
}
/// <summary>
/// idの一致するBody[]を返す
/// </summary>
/// <param name="selectedUserIdList"></param>
/// <returns></returns>
public List <SerializableBody> GetSelectedBodyList(int[] integratedIds = null, ulong[] originalIds = null)
{
List <SerializableBody> bodies = new List <SerializableBody>();
for (int recordNo = 0; recordNo < this.recordNum; recordNo++)
{
SerializableBody body = null;
if (integratedIds != null && integratedIds.Count() == this.recordNum)
{
body = this.GetSelectedBody(recordNo, integratedId: integratedIds[recordNo]);
}
else if (originalIds != null && originalIds.Count() == this.recordNum)
{
body = this.GetSelectedBody(recordNo, originalId: originalIds[recordNo]);
}
if (body == null)
{
return(new List <SerializableBody>());
}
bodies.Add(body);
}
return(bodies);
}
/// <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 Window_KeyUp(object sender, KeyEventArgs e)
{
// select
if (e.Key == Key.S)
{
var focusedElement = FocusManager.GetFocusedElement(this);
// 上のスライダー
if (IsSeeking == true && focusedElement == this.PlaySlider)
{
this.endIndex = (int)this.PlaySlider.Value;
this.PlaySlider.SelectionEnd = this.endIndex;
foreach (Slider slider in this.recordSliders)
{
slider.Maximum = this.endIndex;
}
IsSeeking = false;
}
// レコードスライダー
else if (IsSeeking == true && focusedElement is Slider)
{
rect = null;
Slider slider = (Slider)focusedElement;
int index = slider.Name[slider.Name.Length - 1] - '0' - 1;
// フレーム範囲を無効に
IEnumerable <Frame> frames = frameSequence.Slice(this.recordInvalidStart, (int)slider.Value);
foreach (Frame frame in frames)
{
frame.SetDataNotValid(index);
}
// 初期化
this.recordInvalidStart = this.frameSequence.Frames.Count;
IsSeeking = false;
}
}
// reverse
else if (e.Key == Key.R)
{
IEnumerable <int> range = Enumerable.Range(frameInverseList.Min(), (frameInverseList.Max() - frameInverseList.Min() + 1));
foreach (int frameIndex in range)
{
Frame frame = this.frameSequence.Frames[frameIndex];
this.InverseRecordUser(frame);
}
frameInverseList.Clear();
}
// delete upper body or legs
else if (e.Key == Key.U || e.Key == Key.D)
{
IEnumerable <int> range = Enumerable.Range(frameRemoveList.Min(), (frameRemoveList.Max() - frameRemoveList.Min() + 1));
foreach (int frameIndex in range)
{
for (int recordNo = 0; recordNo < frameSequence.recordNum; recordNo++)
{
if (this.isRecordSelected[recordNo])
{
SerializableBody body = this.frameSequence.Frames[frameIndex].GetSelectedBody(recordNo, frameSequence.selecteedIntegretedIdList[recordNo]);
if (body != null)
{
if (e.Key == Key.U)
{
body.RemoveJoints(Utility.UpperBody);
}
else if (e.Key == Key.D)
{
body.RemoveJoints(Utility.Legs);
}
}
}
}
}
frameRemoveList.Clear();
}
}
public static Tuple <Dictionary <int, List <Dictionary <JointType, CvPoint3D64f> > >, Dictionary <int, List <DateTime> > > ExportFromProject(FrameSequence frameseq, int startIndex, int endIndex)
{
TimeSpan period = new TimeSpan((long)(10000000 / frameseq.frameRate));
List <DateTime> timestamps = new List <DateTime>();
for (DateTime time = frameseq.Frames[startIndex].Time; time < frameseq.Frames[endIndex].Time; time += period)
{
timestamps.Add(time);
}
HashSet <Tuple <int, JointType> > uniqueUserJoint = new HashSet <Tuple <int, JointType> >();
List <Frame> frames = frameseq.Slice(startIndex, endIndex);
foreach (Frame frame in frames)
{
for (int i = 0; i < frameseq.recordNum; i++)
{
foreach (SerializableBody body in frame.GetBodyList(i))
{
foreach (JointType jointType in Utility.GetValidJoints(body.Joints).Keys)
{
// TrackingIdと勝手に作ったuser idの対応辞書が必要. 以下の処理はとりあえず
Tuple <int, JointType> userJoint = new Tuple <int, JointType>(body.integratedId, jointType);
if (!uniqueUserJoint.Contains(userJoint))
{
uniqueUserJoint.Add(userJoint);
}
}
}
}
}
List <Tuple <int, JointType> > userJointPairs = (
from pair in uniqueUserJoint
orderby pair.Item1, pair.Item2
select pair
).ToList();
Dictionary <int, List <Dictionary <JointType, CvPoint3D64f> > > allBodies = new Dictionary <int, List <Dictionary <JointType, CvPoint3D64f> > >();
Dictionary <int, List <DateTime> > allTimes = new Dictionary <int, List <DateTime> >();
SkeletonInterpolator skeletonInterpolator = new SkeletonInterpolator(0.5, true);
foreach (int user in userJointPairs.Select(p => p.Item1).Distinct())
{
skeletonInterpolator.pivot = new Dictionary <JointType, CvPoint3D64f>();
// pivot初期化処理
Dictionary <JointType, Joint>[] firstJoints = new Dictionary <JointType, Joint> [frameseq.recordNum];
for (int no = 0; no < frameseq.recordNum; no++)
{
var bodies = frameseq.GetNextData(no, timestamps[0]).bodies;
if (bodies.Count() > 0)
{
SerializableBody body = bodies.Where(b => b.integratedId == user).FirstOrDefault();
if (body != null)
{
var validJoints = Utility.GetValidJoints(body.Joints).ToDictionary(p => p.Key, p => (CvPoint3D64f)p.Value.Position.ToCvPoint3D());
if (skeletonInterpolator.pivot.Count < validJoints.Count)
{
skeletonInterpolator.pivot = validJoints;
}
}
}
}
List <Dictionary <JointType, CvPoint3D64f> > jointsSeq = new List <Dictionary <JointType, CvPoint3D64f> >();
List <DateTime> times = new List <DateTime>();
foreach (DateTime time in timestamps)
{
Dictionary <JointType, CvPoint3D64f> joints = skeletonInterpolator.IntegrateSkeleton(time, user, frameseq);
// jointsがnullの場合にはダミーデータを突っ込んで長さを稼ぐ
if (joints == null)
{
jointsSeq.Add(new Dictionary <JointType, CvPoint3D64f>()
{
{ JointType.SpineBase, new CvPoint3D64f(float.MaxValue, float.MaxValue, float.MaxValue) }
});
}
else
{
jointsSeq.Add(joints);
}
times.Add(time);
}
allBodies[user] = jointsSeq;
allTimes[user] = times;
}
return(Tuple.Create(allBodies, allTimes));
}
public Dictionary <JointType, CvPoint3D64f> IntegrateSkeleton(DateTime time, int userInt, FrameSequence frameSeq)
{
List <CvMat> ToWorldConversions = frameSeq.ToWorldConversions;
List <CameraIntrinsics> cameraInfo = frameSeq.CameraInfo;
List <UserSegmentation> segm = frameSeq.Segmentations;
Dictionary <JointType, CvPoint3D64f>[] jointsArr = new Dictionary <JointType, CvPoint3D64f> [frameSeq.recordNum];
Dictionary <JointType, CvPoint3D64f>[] pivotCandidate = new Dictionary <JointType, CvPoint3D64f> [frameSeq.recordNum];
double[] reliabilityArr = new double[frameSeq.recordNum];
double[] weightArr = new double[frameSeq.recordNum];
for (int recordNo = 0; recordNo < frameSeq.recordNum; recordNo++)
{
MotionData prevData = frameSeq.GetPrevData(recordNo, time);
MotionData nextData = frameSeq.GetNextData(recordNo, time);
if (prevData == null || nextData == null || prevData.bodies.Length * nextData.bodies.Length == 0 || !prevData.isValid || !nextData.isValid)
{
jointsArr[recordNo] = null;
reliabilityArr[recordNo] = 0;
weightArr[recordNo] = 0;
continue;
}
SerializableBody prevBody = prevData.bodies.Where(b => b.integratedId == userInt).FirstOrDefault();
SerializableBody nextBody = nextData.bodies.Where(b => b.integratedId == userInt).FirstOrDefault();
if (prevBody == null || nextBody == null || prevBody.Equals(default(SerializableBody)) || nextBody.Equals(default(SerializableBody)))
{
jointsArr[recordNo] = null;
reliabilityArr[recordNo] = 0;
weightArr[recordNo] = 0;
continue;
}
// 統計情報によるフィルタリング
Dictionary <JointType, Joint> prevJoints;
Dictionary <JointType, Joint> nextJoints;
if (prevBody.Joints == null || nextBody.Joints == null)
{
continue;
}
// pivotが設定されてるとき、つまり、本番統合のとき
if (pivot != null)
{
// mirror矯正
prevJoints = this.CorrectMirrorJoint(prevBody.Joints, ToWorldConversions[recordNo]);
nextJoints = this.CorrectMirrorJoint(nextBody.Joints, ToWorldConversions[recordNo]);
// next pivot候補つめつめ
pivotCandidate[recordNo] = Utility.GetValidJoints(nextJoints).ToDictionary(p => p.Key, p => (CvPoint3D64f)p.Value.Position.ToCvPoint3D());
}
else
{
prevJoints = prevBody.Joints;
nextJoints = nextBody.Joints;
}
// 統計情報があるとき
if (frameSeq.BodyStat != null)
{
prevJoints = frameSeq.BodyStat.FilterBonesByStatistics(prevJoints);
nextJoints = frameSeq.BodyStat.FilterBonesByStatistics(nextJoints);
}
else
{
prevJoints = Utility.GetValidJoints(prevBody.Joints);
nextJoints = Utility.GetValidJoints(nextBody.Joints);
}
jointsArr[recordNo] = this.InterpolateSkeleton(prevData, nextData, prevJoints, nextJoints, time, ToWorldConversions[recordNo]);
reliabilityArr[recordNo] = this.GetSkeletonReliability(prevData, nextData, prevJoints, nextJoints, time, cameraInfo[recordNo]);
weightArr[recordNo] = this.GetVarianceWeight(prevData, nextData, prevJoints, nextJoints, time);
}
// pivot更新
pivot = new Dictionary <JointType, CvPoint3D64f>();
foreach (var candidate in pivotCandidate)
{
if (candidate != null && pivot.Count <= candidate.Count)
{
pivot = candidate;
}
}
double maxWeight = weightArr.Max();
double[] modifiedReliabilityList = weightArr.Select(w => Math.Max(0, (w / maxWeight) - _weightBase)).Zip(reliabilityArr, (a, b) => a * b).ToArray();
var keys = jointsArr.Where(j => j != null).SelectMany(j => j.Keys).Distinct().ToList();
if (maxWeight == 0)
{
return(null);
}
return(CalcEx.LinearMedianSkeletons(jointsArr, modifiedReliabilityList));
}
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];
}
}
}
