/// <summary>
/// あるフレーム範囲の全ユーザの点群を出力する
/// </summary>
/// <param name="sender"></param>
/// <param name="e"></param>
private void ExportFrameRangeUserPointClouds_Click(object sender, RoutedEventArgs e)
{
List <Frame> frames = frameSequence.Slice(startIndex, endIndex);
for (int i = 0; i < frameSequence.recordNum; i++)
{
List <List <float[]> > pointsSequence = new List <List <float[]> >();
string path = System.IO.Path.Combine(Environment.GetFolderPath(Environment.SpecialFolder.DesktopDirectory), i.ToString() + "_UserPointsCloud.dump");
for (int frameNo = 0; frameNo < frames.Count(); frameNo++)
{
// フレームレート半分
if (frameNo % 2 == 0)
{
continue;
}
Frame frame = frames[frameNo];
List <Tuple <CvPoint3D64f, CvColor> > colors = frameSequence.LocalCoordinateMappers[i].GetUserColorPoints(frame.DepthMatList[i], frame.ColorMatList[i], frame.UserMatList[i]);
colors = colors.Select(t => Tuple.Create(CvEx.ConvertPoint3D(t.Item1, frameSequence.ToWorldConversions[i]), t.Item2)).ToList();
List <float[]> dumpColors = colors.Select(t => new float[] { (float)t.Item1.X, (float)t.Item1.Y, (float)t.Item1.Z, t.Item2.R, t.Item2.G, t.Item2.B }).ToList();
// 点の数1/10
dumpColors = dumpColors.Where((fs, index) => index % 10 == 0).ToList();
pointsSequence.Add(dumpColors);
}
Utility.SaveToBinary(pointsSequence.ToArray(), path);
}
}
/// <summary>
/// Jointsに座標変換を適用する
/// </summary>
/// <param name="joints"></param>
/// <param name="conversion"></param>
/// <returns></returns>
public static Dictionary <JointType, Joint> ApplyConversions(Dictionary <JointType, Joint> joints, CvMat conversion)
{
Dictionary <JointType, Joint> newJoints = new Dictionary <JointType, Joint>();
foreach (JointType jointType in joints.Keys)
{
Joint originalJoint = joints[jointType];
CvPoint3D64f fromPoint = originalJoint.Position.ToCvPoint3D();
CameraSpacePoint newPoint = CvEx.ConvertPoint3D(fromPoint, conversion).ToCameraSpacePoint();
originalJoint.Position = newPoint;
newJoints[jointType] = originalJoint;
}
return(newJoints);
}
public Dictionary <JointType, CvPoint3D64f> InterpolateSkeleton(MotionData prevFrame, MotionData nextFrame, Dictionary <JointType, Joint> prevJoints, Dictionary <JointType, Joint> nextJoints,
DateTime time, CvMat ToWorldConversion)
{
double prevWeight;
if (prevFrame.TimeStamp >= nextFrame.TimeStamp)
{
prevWeight = 1;
}
else
{
prevWeight = (time - prevFrame.TimeStamp).TotalSeconds / (nextFrame.TimeStamp - prevFrame.TimeStamp).TotalSeconds;
}
double nextWeight = 1.0 - prevWeight;
if (prevJoints == null || nextJoints == null)
{
return(null);
}
Dictionary <JointType, CvPoint3D64f> prevData = prevJoints.ToDictionary(p => p.Key, p => (CvPoint3D64f)p.Value.Position.ToCvPoint3D());
Dictionary <JointType, CvPoint3D64f> nextData = nextJoints.ToDictionary(p => p.Key, p => (CvPoint3D64f)p.Value.Position.ToCvPoint3D());
List <JointType> joints = prevData.Keys.Union(nextData.Keys).ToList();
Dictionary <JointType, CvPoint3D64f> ret = new Dictionary <JointType, CvPoint3D64f>();
foreach (JointType joint in joints)
{
CvPoint3D64f prevPos, nextPos;
bool prevFound = prevData.TryGetValue(joint, out prevPos);
bool nextFound = nextData.TryGetValue(joint, out nextPos);
if ((prevFound && nextFound))
{
ret[joint] = CvEx.ConvertPoint3D(prevPos * prevWeight + nextPos * nextWeight, ToWorldConversion);
}
else if (_omitWhenDataLack)
{
if (prevFound)
{
ret[joint] = CvEx.ConvertPoint3D(prevPos, ToWorldConversion);
}
else if (nextFound)
{
ret[joint] = CvEx.ConvertPoint3D(nextPos, ToWorldConversion);
}
}
}
return(ret);
}
/// <summary>
/// あるフレームの点群を出力する
/// </summary>
/// <param name="sender"></param>
/// <param name="e"></param>
private void ExportFrameRangePointClouds_Click(object sender, RoutedEventArgs e)
{
List <Frame> frames = frameSequence.Slice(startIndex, endIndex);
for (int i = 0; i < frameSequence.recordNum; i++)
{
List <float[]>[] pointsSequence = new List <float[]> [frames.Count()];
string path = System.IO.Path.Combine(Environment.GetFolderPath(Environment.SpecialFolder.DesktopDirectory), i.ToString() + "_PointsCloud.dump");
for (int frameNo = 0; frameNo < frames.Count(); frameNo++)
{
Frame frame = frames[frameNo];
List <Tuple <CvPoint3D64f, CvColor> > colors = frameSequence.LocalCoordinateMappers[i].DepthColorMatToRealPoints(frame.DepthMatList[i], frame.ColorMatList[i]);
colors = colors.Select(t => Tuple.Create(CvEx.ConvertPoint3D(t.Item1, frameSequence.ToWorldConversions[i]), t.Item2)).ToList();
List <float[]> dumpColors = colors.Select(t => new float[] { (float)t.Item1.X, (float)t.Item1.Y, (float)t.Item1.Z, t.Item2.R, t.Item2.G, t.Item2.B }).ToList();
pointsSequence[frameNo] = dumpColors;
}
Utility.SaveToBinary(pointsSequence, path);
}
}
/// <summary>
/// あるフレームにおける座標変換行列を骨格情報から計算する
/// </summary>
/// <param name="frame"></param>
/// <param name="convList"></param>
/// <param name="bodies"></param>
/// <returns></returns>
public static List <CvMat> GetConvMatrixFromBoneFrame(Frame frame, List <CvMat> convList, List <SerializableBody> bodies)
{
if (bodies.Count() != frame.recordNum)
{
System.Windows.MessageBox.Show("ユーザが選択されていないレコードがあります");
return(convList);
}
bool[] validFlags = frame.GetValidFlags();
for (int j = 1; j < frame.recordNum; j++)
{
Dictionary <JointType, Joint> joint1 = Utility.GetValidJoints(bodies[0].Joints);
Dictionary <JointType, Joint> joint2 = Utility.GetValidJoints(bodies[j].Joints);
if (validFlags[0] && validFlags[j] == false)
{
continue;
}
ICoordConversion3D crtc = new CoordRotTransConversion();
foreach (JointType jointType in Enum.GetValues(typeof(JointType)))
{
if (!joint1.ContainsKey(jointType))
{
continue;
}
if (!joint2.ContainsKey(jointType))
{
continue;
}
CvPoint3D64f from = joint2[jointType].Position.ToCvPoint3D();
CvPoint3D64f target = CvEx.ConvertPoint3D(joint1[jointType].Position.ToCvPoint3D(), convList[0]);
// IsOriginlJointValid相当の処理を入れるかどうか
crtc.PutPoint(from, target, 1);
}
convList[j] = crtc.Solve();
}
return(convList);
}
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);
}
public CvMat CalculateTransform(int targetModelIndex, bool updateInternalModelTransform, double randomSamplingRatio)
{
if (targetModelIndex < 0 || targetModelIndex >= _flannModels.Count)
{
throw new ArgumentOutOfRangeException("targetModelIndex");
}
CoordRotTransConversion coordConverter = new CoordRotTransConversion();
//CoordConvertSpring coordConverter = new CoordConvertSpring(_modelTransforms[targetModelIndex]);
//foreach (var point in dataPointListInWorldCoordinate) {
List <Tuple <CvPoint3D64f, CvColor> > tuples = _flannModels[targetModelIndex].ModelPoints;
if (randomSamplingRatio < 1)
{
Random rand = new Random();
tuples = tuples.Where(x => rand.NextDouble() < randomSamplingRatio).ToList();
}
CvMat targetTransform = _modelTransforms[targetModelIndex];
List <CvMat> inverseTransforms = new List <CvMat>();
foreach (CvMat transform in _modelTransforms)
{
CvMat inv = CvEx.InitCvMat(transform);
transform.Invert(inv);
inverseTransforms.Add(inv);
}
float searchDistanceSq = this.SearchDistance * this.SearchDistance;
Parallel.ForEach(tuples, tuple =>
{
CvPoint3D64f point = tuple.Item1;
CvColor color = tuple.Item2;
//foreach (var point in points) {
CvPoint3D64f worldPoint = CvEx.ConvertPoint3D(point, targetTransform);
int minModelIndex = -1;
int minPointIndex = -1;
float minDistanceSq = float.MaxValue;
for (int modelIndex = 0; modelIndex < _flannModels.Count; modelIndex++)
{
if (modelIndex == targetModelIndex)
{
continue;
}
CvPoint3D64f inversePoint = CvEx.ConvertPoint3D(worldPoint, inverseTransforms[modelIndex]);
int[] indices;
float[] distances;
_flannModels[modelIndex].KnnSearch(inversePoint, color, out indices, out distances, 1);
if (indices.Length >= 1)
{
float distanceSq = distances[0];
if (distanceSq <= searchDistanceSq)
{
if (distanceSq < minDistanceSq)
{
minModelIndex = modelIndex;
minPointIndex = indices[0];
minDistanceSq = distanceSq;
}
}
}
}
if (minModelIndex != -1)
{
Tuple <CvPoint3D64f, CvColor> bestModelPoint = _flannModels[minModelIndex].ModelPoints[minPointIndex];
double weightTo = 1.0 / (Math.Abs(bestModelPoint.Item1.Z - 1500 / 1000f) + 5000 / 1000f);
double weightFrom = 1.0 / (Math.Abs(point.Z - 1500 / 1000f) + 5000 / 1000f);
//weightFrom = weightTo = 1;
double weight = _weightFromDistanceSq(minDistanceSq) * weightFrom * weightTo;
CvPoint3D64f from = CvEx.ConvertPoint3D(point, targetTransform);
CvPoint3D64f to = CvEx.ConvertPoint3D(bestModelPoint.Item1, _modelTransforms[minModelIndex]);
coordConverter.PutPoint(from, to, weight);
}
});
CvMat ret = coordConverter.Solve() * targetTransform;
if (updateInternalModelTransform)
{
_modelTransforms[targetModelIndex] = ret.Clone();
}
return(ret);
}
/// <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>
/// 反転している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);
}
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];
}
}
}