/// <summary>
/// FrameSequenceの構築, そのうちダイアログとか出したい
/// </summary>
private void LoadFrames()
{
string[] datadir = new string[] {
@"C:\Users\non\Desktop\1222\1226_kinect1\calib",
@"C:\Users\non\Desktop\1222\1226_kinect2\calib",
@"C:\Users\non\Desktop\1222\1226_kinect3\calib",
@"C:\Users\non\Desktop\1222\1226_kinect4\calib",
@"C:\Users\non\Desktop\1222\1226_kinect5\calib",
};
List <string> mapdir = new List <string>()
{
@"C:\Users\non\Desktop\1222\1226_kinect1\coordmap.dump",
@"C:\Users\non\Desktop\1222\1226_kinect2\coordmap.dump",
@"C:\Users\non\Desktop\1222\1226_kinect3\coordmap.dump",
@"C:\Users\non\Desktop\1222\1226_kinect4\coordmap.dump",
@"C:\Users\non\Desktop\1222\1226_kinect5\coordmap.dump",
};
List <string> cameradir = new List <string>()
{
@"C:\Users\non\Desktop\1222\1226_kinect1\CameraInfo.dump",
@"C:\Users\non\Desktop\1222\1226_kinect2\CameraInfo.dump",
@"C:\Users\non\Desktop\1222\1226_kinect3\CameraInfo.dump",
@"C:\Users\non\Desktop\1222\1226_kinect4\CameraInfo.dump",
@"C:\Users\non\Desktop\1222\1226_kinect5\CameraInfo.dump",
};
this.frameSequence = new FrameSequence(datadir);
this.frameSequence.LocalCoordinateMappers = mapdir.Select(s => (LocalCoordinateMapper)Utility.LoadFromBinary(s)).ToList();
this.frameSequence.CameraInfo = cameradir.Select(s => (CameraIntrinsics)Utility.LoadFromBinary(s)).ToList();
}
/// <summary>
/// フレーム範囲における座標変換行列を深度情報から計算する
/// </summary>
/// <param name="frames"></param>
public static List <CvMat> GetConvMatrixFromDepthFrameSequence(FrameSequence frameSeq, int startIndex, int endIndex)
{
List <CvMat> conversions = frameSeq.ToWorldConversions;
IEnumerable <Frame> frames = frameSeq.Slice(startIndex, endIndex);
foreach (Frame frame in frames)
{
Func <float, double> distance2weight = x => 1.0 / (x * 0 + 400 / 1000f);
using (ColoredIterativePointMatching sipm = new ColoredIterativePointMatching(frame, frameSeq.LocalCoordinateMappers, conversions, distance2weight, 200))
{
conversions = sipm.CalculateTransformSequntially(0.1, 1);
}
}
return(conversions);
}
/// <summary>
/// 時系列データの前後を比較して左右反転をなおす
/// </summary>
/// <param name="frameSeq"></param>
public static void SequentialCorrect(FrameSequence frameSeq)
{
CvPoint3D64f[] prevShoulderVectors = new CvPoint3D64f[frameSeq.recordNum];
CvPoint3D64f[] prevHipVectors = new CvPoint3D64f[frameSeq.recordNum];
foreach (Frame frame in frameSeq.Frames)
{
List <SerializableBody> bodies = frame.GetSelectedBodyList(integratedIds: frameSeq.selecteedIntegretedIdList);
if (bodies.Count == 0)
{
continue;
}
CvPoint3D64f[] shoulderVectors = bodies.Select(b =>
(CvPoint3D64f)(b.Joints[JointType.ShoulderLeft].Position.ToCvPoint3D() - b.Joints[JointType.ShoulderRight].Position.ToCvPoint3D())).ToArray();
CvPoint3D64f[] hipVectors = bodies.Select(b =>
(CvPoint3D64f)(b.Joints[JointType.HipLeft].Position.ToCvPoint3D() - b.Joints[JointType.HipRight].Position.ToCvPoint3D())).ToArray();
// 0回目の処理
if (prevShoulderVectors == default(CvPoint3D64f[]) || prevHipVectors == default(CvPoint3D64f[]))
{
prevShoulderVectors = shoulderVectors;
prevHipVectors = hipVectors;
continue;
}
double[] shoulderCosArray = shoulderVectors.Zip(prevShoulderVectors, (cur, prev) => CvEx.Cos(cur, prev)).ToArray();
double[] hipCosArray = hipVectors.Zip(prevHipVectors, (cur, prev) => CvEx.Cos(cur, prev)).ToArray();
for (int no = 0; no < frame.recordNum; no++)
{
if (shoulderCosArray[no] <= -0.8 || hipCosArray[no] <= -0.8)
{
frame.InverseBody(no, integratedId: frameSeq.selecteedIntegretedIdList[no]);
}
}
// Inverseされていた場合には骨の位置関係が変わっているため
prevShoulderVectors = bodies.Select(b =>
(CvPoint3D64f)(b.Joints[JointType.ShoulderLeft].Position.ToCvPoint3D() - b.Joints[JointType.ShoulderRight].Position.ToCvPoint3D())).ToArray();
prevHipVectors = bodies.Select(b =>
(CvPoint3D64f)(b.Joints[JointType.HipLeft].Position.ToCvPoint3D() - b.Joints[JointType.HipRight].Position.ToCvPoint3D())).ToArray();
}
}
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];
}
}
}
/// <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="frameSeq"></param>
public static void SequentialCorrect(FrameSequence frameSeq)
{
CvPoint3D64f[] prevShoulderVectors = new CvPoint3D64f[frameSeq.recordNum];
CvPoint3D64f[] prevHipVectors = new CvPoint3D64f[frameSeq.recordNum];
foreach (Frame frame in frameSeq.Frames)
{
List<SerializableBody> bodies = frame.GetSelectedBodyList(integratedIds: frameSeq.selecteedIntegretedIdList);
if (bodies.Count == 0)
{
continue;
}
CvPoint3D64f[] shoulderVectors = bodies.Select(b =>
(CvPoint3D64f)(b.Joints[JointType.ShoulderLeft].Position.ToCvPoint3D() - b.Joints[JointType.ShoulderRight].Position.ToCvPoint3D())).ToArray();
CvPoint3D64f[] hipVectors = bodies.Select(b =>
(CvPoint3D64f)(b.Joints[JointType.HipLeft].Position.ToCvPoint3D() - b.Joints[JointType.HipRight].Position.ToCvPoint3D())).ToArray();
// 0回目の処理
if (prevShoulderVectors == default(CvPoint3D64f[]) || prevHipVectors == default(CvPoint3D64f[]))
{
prevShoulderVectors = shoulderVectors;
prevHipVectors = hipVectors;
continue;
}
double[] shoulderCosArray = shoulderVectors.Zip(prevShoulderVectors, (cur, prev) => CvEx.Cos(cur, prev)).ToArray();
double[] hipCosArray = hipVectors.Zip(prevHipVectors, (cur, prev) => CvEx.Cos(cur, prev)).ToArray();
for (int no = 0; no < frame.recordNum; no++)
{
if (shoulderCosArray[no] <= -0.8 || hipCosArray[no] <= -0.8)
{
frame.InverseBody(no, integratedId: frameSeq.selecteedIntegretedIdList[no]);
}
}
// Inverseされていた場合には骨の位置関係が変わっているため
prevShoulderVectors = bodies.Select(b =>
(CvPoint3D64f)(b.Joints[JointType.ShoulderLeft].Position.ToCvPoint3D() - b.Joints[JointType.ShoulderRight].Position.ToCvPoint3D())).ToArray();
prevHipVectors = bodies.Select(b =>
(CvPoint3D64f)(b.Joints[JointType.HipLeft].Position.ToCvPoint3D() - b.Joints[JointType.HipRight].Position.ToCvPoint3D())).ToArray();
}
}
/// <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);
}
}
}
}
}
}
}
public void CleanAndNormalize(FrameSequence frameSeq)
{
// TODO :
//body.Joints = this.NormalizeLegJoints(body.Joints, frameSeq.BodyStat.boneLengthSqStatistics);
}
/// <summary>
/// フレーム範囲における座標変換行列を深度情報から計算する
/// </summary>
/// <param name="frames"></param>
public static List<CvMat> GetConvMatrixFromDepthFrameSequence(FrameSequence frameSeq, int startIndex, int endIndex)
{
List<CvMat> conversions = frameSeq.ToWorldConversions;
IEnumerable<Frame> frames = frameSeq.Slice(startIndex, endIndex);
foreach (Frame frame in frames)
{
Func<float, double> distance2weight = x => 1.0 / (x * 0 + 400 / 1000f);
using (ColoredIterativePointMatching sipm = new ColoredIterativePointMatching(frame, frameSeq.LocalCoordinateMappers, conversions, distance2weight, 200))
{
conversions = sipm.CalculateTransformSequntially(0.1, 1);
}
}
return conversions;
}
/// <summary>
/// FrameSequenceの構築, そのうちダイアログとか出したい
/// </summary>
private void LoadFrames()
{
string[] datadir = new string[] {
@"C:\Users\non\Desktop\1222\1226_kinect1\calib",
@"C:\Users\non\Desktop\1222\1226_kinect2\calib",
@"C:\Users\non\Desktop\1222\1226_kinect3\calib",
@"C:\Users\non\Desktop\1222\1226_kinect4\calib",
@"C:\Users\non\Desktop\1222\1226_kinect5\calib",
};
List<string> mapdir = new List<string>() {
@"C:\Users\non\Desktop\1222\1226_kinect1\coordmap.dump",
@"C:\Users\non\Desktop\1222\1226_kinect2\coordmap.dump",
@"C:\Users\non\Desktop\1222\1226_kinect3\coordmap.dump",
@"C:\Users\non\Desktop\1222\1226_kinect4\coordmap.dump",
@"C:\Users\non\Desktop\1222\1226_kinect5\coordmap.dump",
};
List<string> cameradir = new List<string>() {
@"C:\Users\non\Desktop\1222\1226_kinect1\CameraInfo.dump",
@"C:\Users\non\Desktop\1222\1226_kinect2\CameraInfo.dump",
@"C:\Users\non\Desktop\1222\1226_kinect3\CameraInfo.dump",
@"C:\Users\non\Desktop\1222\1226_kinect4\CameraInfo.dump",
@"C:\Users\non\Desktop\1222\1226_kinect5\CameraInfo.dump",
};
this.frameSequence = new FrameSequence(datadir);
this.frameSequence.LocalCoordinateMappers = mapdir.Select(s => (LocalCoordinateMapper)Utility.LoadFromBinary(s)).ToList();
this.frameSequence.CameraInfo = cameradir.Select(s => (CameraIntrinsics)Utility.LoadFromBinary(s)).ToList();
}
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 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 CleanAndNormalize(FrameSequence frameSeq)
{
// TODO :
//body.Joints = this.NormalizeLegJoints(body.Joints, frameSeq.BodyStat.boneLengthSqStatistics);
}
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);
}
/// <summary>
/// フレーム範囲における座標変換行列を骨格情報から計算する
/// </summary>
/// <param name="frames"></param>
public static List<CvMat> GetConvMatrixFromBoneFrameSequence(FrameSequence frameSeq, int startIndex, int endIndex)
{
Dictionary<Tuple<int, int>, int> cooccurenceCount = new Dictionary<Tuple<int, int>, int>();
IEnumerable<Frame> frames = frameSeq.Frames.Skip(startIndex).Take(endIndex);
List<SerializableBody> bodies;
foreach (Frame frame in frames)
{
if (frameSeq.Segmentations == null)
{
bodies = frame.GetSelectedBodyList(originalIds:frameSeq.selectedOriginalIdList);
}
else
{
bodies = frame.GetSelectedBodyList(integratedIds:frameSeq.selecteedIntegretedIdList);
}
if (bodies.Count() != frame.recordNum)
{
continue;
}
bool[] validFlags = frame.GetValidFlags();
for (int i = 0; i < frame.recordNum; i++)
{
for (int j = i + 1; j < frame.recordNum; j++)
{
if (validFlags[i] && validFlags[j] == false)
{
continue;
}
Dictionary<JointType, Joint> joint1 = Utility.GetValidJoints(bodies[i].Joints);
Dictionary<JointType, Joint> joint2 = Utility.GetValidJoints(bodies[j].Joints);
foreach (JointType jointType in joint1.Keys.Intersect(joint2.Keys))
{
Tuple<int, int> key = new Tuple<int, int>(i, j);
int count;
if (!cooccurenceCount.TryGetValue(key, out count))
{
count = 0;
}
cooccurenceCount[key] = count + 1;
}
}
}
}
// 依存関係のツリーを作る
int baseRecordIndex;
Dictionary<int, int> dependencies;
// とりあえず先頭フレームのレコード数にしてるけど、プロジェクトとかが持つべき値
if (!CalcEx.GetDependencyTree(frameSeq.recordNum, cooccurenceCount, list => list.Sum(), out baseRecordIndex, out dependencies))
{
System.Windows.MessageBox.Show("骨格が他のレコードと同時に映っているフレームがないレコードがあるため計算できませんでした");
return frameSeq.ToWorldConversions;
}
else
{
Dictionary<int, ICoordConversion3D> conversionsPerDependencyKey = new Dictionary<int, ICoordConversion3D>();
foreach (Frame frame in frameSeq.Frames)
{
if (frameSeq.Segmentations == null)
{
bodies = frame.GetSelectedBodyList(originalIds:frameSeq.selectedOriginalIdList);
}
else
{
bodies = frame.GetSelectedBodyList(integratedIds:frameSeq.selecteedIntegretedIdList);
}
if (bodies.Count() != frame.recordNum)
continue;
List<CvSize> depthUsersizeList = frame.DepthUserSize;
bool[] validFlags = frame.GetValidFlags();
foreach (KeyValuePair<int, int> dependencyPair in CalcEx.EnumerateDependencyPairs(baseRecordIndex, dependencies))
{
if (validFlags[dependencyPair.Key] && validFlags[dependencyPair.Value] == false)
{
continue;
}
// 変換計算用オブジェクトを拾ってくる
ICoordConversion3D conv;
if (!conversionsPerDependencyKey.TryGetValue(dependencyPair.Key, out conv))
{
conversionsPerDependencyKey[dependencyPair.Key] = conv = new CoordRotTransConversion();
}
Dictionary<JointType, Joint> joint1 = Utility.GetValidJoints(bodies[dependencyPair.Key].Joints);
Dictionary<JointType, Joint> joint2 = Utility.GetValidJoints(bodies[dependencyPair.Value].Joints);
foreach (JointType jointType in joint1.Keys.Intersect(joint2.Keys))
{
CvPoint3D64f camPoint1 = joint1[jointType].Position.ToCvPoint3D();
CvPoint3D64f camPoint2 = joint2[jointType].Position.ToCvPoint3D();
// それぞれのカメラ座標系におけるそれぞれの対応点をセットに入れる
conv.PutPoint(camPoint1, camPoint2, 1);
}
}
}
List<CvMat> convList = frameSeq.ToWorldConversions;
foreach (KeyValuePair<int, int> dependencyPair in CalcEx.EnumerateDependencyPairs(baseRecordIndex, dependencies))
{
CvMat relConv = conversionsPerDependencyKey[dependencyPair.Key].Solve();
CvMat baseConversion = convList[dependencyPair.Value];
convList[dependencyPair.Key] = baseConversion * relConv;
}
return convList;
}
}
/// <summary>
/// フレーム範囲における座標変換行列を骨格情報から計算する
/// </summary>
/// <param name="frames"></param>
public static List <CvMat> GetConvMatrixFromBoneFrameSequence(FrameSequence frameSeq, int startIndex, int endIndex)
{
Dictionary <Tuple <int, int>, int> cooccurenceCount = new Dictionary <Tuple <int, int>, int>();
IEnumerable <Frame> frames = frameSeq.Frames.Skip(startIndex).Take(endIndex);
List <SerializableBody> bodies;
foreach (Frame frame in frames)
{
if (frameSeq.Segmentations == null)
{
bodies = frame.GetSelectedBodyList(originalIds: frameSeq.selectedOriginalIdList);
}
else
{
bodies = frame.GetSelectedBodyList(integratedIds: frameSeq.selecteedIntegretedIdList);
}
if (bodies.Count() != frame.recordNum)
{
continue;
}
bool[] validFlags = frame.GetValidFlags();
for (int i = 0; i < frame.recordNum; i++)
{
for (int j = i + 1; j < frame.recordNum; j++)
{
if (validFlags[i] && validFlags[j] == false)
{
continue;
}
Dictionary <JointType, Joint> joint1 = Utility.GetValidJoints(bodies[i].Joints);
Dictionary <JointType, Joint> joint2 = Utility.GetValidJoints(bodies[j].Joints);
foreach (JointType jointType in joint1.Keys.Intersect(joint2.Keys))
{
Tuple <int, int> key = new Tuple <int, int>(i, j);
int count;
if (!cooccurenceCount.TryGetValue(key, out count))
{
count = 0;
}
cooccurenceCount[key] = count + 1;
}
}
}
}
// 依存関係のツリーを作る
int baseRecordIndex;
Dictionary <int, int> dependencies;
// とりあえず先頭フレームのレコード数にしてるけど、プロジェクトとかが持つべき値
if (!CalcEx.GetDependencyTree(frameSeq.recordNum, cooccurenceCount, list => list.Sum(), out baseRecordIndex, out dependencies))
{
System.Windows.MessageBox.Show("骨格が他のレコードと同時に映っているフレームがないレコードがあるため計算できませんでした");
return(frameSeq.ToWorldConversions);
}
else
{
Dictionary <int, ICoordConversion3D> conversionsPerDependencyKey = new Dictionary <int, ICoordConversion3D>();
foreach (Frame frame in frameSeq.Frames)
{
if (frameSeq.Segmentations == null)
{
bodies = frame.GetSelectedBodyList(originalIds: frameSeq.selectedOriginalIdList);
}
else
{
bodies = frame.GetSelectedBodyList(integratedIds: frameSeq.selecteedIntegretedIdList);
}
if (bodies.Count() != frame.recordNum)
{
continue;
}
List <CvSize> depthUsersizeList = frame.DepthUserSize;
bool[] validFlags = frame.GetValidFlags();
foreach (KeyValuePair <int, int> dependencyPair in CalcEx.EnumerateDependencyPairs(baseRecordIndex, dependencies))
{
if (validFlags[dependencyPair.Key] && validFlags[dependencyPair.Value] == false)
{
continue;
}
// 変換計算用オブジェクトを拾ってくる
ICoordConversion3D conv;
if (!conversionsPerDependencyKey.TryGetValue(dependencyPair.Key, out conv))
{
conversionsPerDependencyKey[dependencyPair.Key] = conv = new CoordRotTransConversion();
}
Dictionary <JointType, Joint> joint1 = Utility.GetValidJoints(bodies[dependencyPair.Key].Joints);
Dictionary <JointType, Joint> joint2 = Utility.GetValidJoints(bodies[dependencyPair.Value].Joints);
foreach (JointType jointType in joint1.Keys.Intersect(joint2.Keys))
{
CvPoint3D64f camPoint1 = joint1[jointType].Position.ToCvPoint3D();
CvPoint3D64f camPoint2 = joint2[jointType].Position.ToCvPoint3D();
// それぞれのカメラ座標系におけるそれぞれの対応点をセットに入れる
conv.PutPoint(camPoint1, camPoint2, 1);
}
}
}
List <CvMat> convList = frameSeq.ToWorldConversions;
foreach (KeyValuePair <int, int> dependencyPair in CalcEx.EnumerateDependencyPairs(baseRecordIndex, dependencies))
{
CvMat relConv = conversionsPerDependencyKey[dependencyPair.Key].Solve();
CvMat baseConversion = convList[dependencyPair.Value];
convList[dependencyPair.Key] = baseConversion * relConv;
}
return(convList);
}
}
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];
}
}
}
