/// <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);
}
/// <summary>
/// 2つのベクトルの角度をラジアンで返す
/// </summary>
/// <param name="one"></param>
/// <param name="two"></param>
/// <returns></returns>
public static double GetVectorRadian(CvPoint3D64f one, CvPoint3D64f two)
{
// masterにslaveを回転させるコード
//CvPoint3D64f slave = new CvPoint3D64f(0.7071, 0, 0.7071);
//CvPoint3D64f master = new CvPoint3D64f(0, 0, 1);
//double rad = Utility.GetVectorRadian(master, slave);
//CvMat mat = CvEx.GetRotation3D(new CvPoint3D64f(0, 1, 0), rad);
//CvPoint3D64f to = CvEx.RotatePoint3D(slave, mat);
one.Y = 0;
two.Y = 0;
CvPoint3D64f norm = CvEx.Cross(two, one);
double cos = CvEx.Cos(two, one);
double rad = Math.Acos(cos);
if (norm.Y < 0)
{
rad = -1 * rad;
}
return(rad);
}
/// <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];
}
}
}