/// <summary>
/// 骨格座標の信頼性の値(分散以外)を求めます
/// </summary>
/// <param name="record"></param>
/// <param name="time"></param>
/// <param name="user"></param>
/// <returns></returns>
public double GetSkeletonReliability(MotionData prevFrame, MotionData nextFrame, Dictionary <JointType, Joint> prevJoints, Dictionary <JointType, Joint> nextJoints, DateTime time,
CameraIntrinsics cameraInfo)
{
double periodAfter = (time - prevFrame.TimeStamp).TotalSeconds;
double periodBefore = (nextFrame.TimeStamp - time).TotalSeconds;
double weightPeriod = Math.Exp(-periodAfter / 0.2) + Math.Exp(-periodBefore / 0.2);
if (prevJoints == null || prevJoints.Count == 0)
{
return(0);
}
if (nextJoints == null || nextJoints.Count == 0)
{
return(0);
}
double prevEdge = 1;
double nextEdge = 1;
if (prevJoints.Count > 0)
{
prevEdge = prevJoints.Values.Select(p => Utility.GetRatioSqOfPrincipalToFocal(cameraInfo, p.Position.X, p.Position.Y)).Select(v => 1.0 / (1.0 + Math.Pow(v, 4))).Average();
}
if (nextJoints.Count > 0)
{
nextEdge = nextJoints.Values.Select(p => Utility.GetRatioSqOfPrincipalToFocal(cameraInfo, p.Position.X, p.Position.Y)).Select(v => 1.0 / (1.0 + Math.Pow(v, 4))).Average();
}
return(weightPeriod * Math.Sqrt(prevEdge * nextEdge));
}
/// <summary>
/// 指定されたレコードの指定された時刻の指定されたユーザの骨格座標の水平垂直標準偏差積を求めます
/// </summary>
/// <param name="record"></param>
/// <param name="time"></param>
/// <param name="user"></param>
/// <returns></returns>
public double GetVarianceWeight(MotionData prevFrame, MotionData nextFrame, Dictionary<JointType, Joint> prevJoints, Dictionary<JointType, Joint> nextJoints, DateTime time)
{
if (prevJoints == null || prevJoints.Count == 0)
return 0;
if (nextJoints == null || nextJoints.Count == 0)
return 0;
double prevVariance = GetSkeletonCamPosVariance(prevJoints.Values.Select(p => (CvPoint3D64f)p.Position.ToCvPoint3D()).ToList());
double nextVariance = GetSkeletonCamPosVariance(prevJoints.Values.Select(p => (CvPoint3D64f)p.Position.ToCvPoint3D()).ToList());
return Math.Sqrt(prevVariance * nextVariance);
}
/// <summary>
/// データを破棄して既存のデータをロードします。
/// </summary>
/// <param name="dataDir"></param>
public void LoadAndSetData(string dataDir)
{
this.dataDir = dataDir;
this.recordPath = Path.Combine(dataDir, bodyInfoFilename);
this.motionDataList = this.GetMotionDataFromFile(this.recordPath);
MotionData md = this.motionDataList[0];
this.colorWidth = md.ColorWidth;
this.colorHeight = md.ColorHeight;
this.depthWidth = md.DepthUserWidth;
this.depthHeight = md.DepthUserHeight;
}
/// <summary>
/// 指定されたレコードの指定された時刻の指定されたユーザの骨格座標の水平垂直標準偏差積を求めます
/// </summary>
/// <param name="record"></param>
/// <param name="time"></param>
/// <param name="user"></param>
/// <returns></returns>
public double GetVarianceWeight(MotionData prevFrame, MotionData nextFrame, Dictionary <JointType, Joint> prevJoints, Dictionary <JointType, Joint> nextJoints, DateTime time)
{
if (prevJoints == null || prevJoints.Count == 0)
{
return(0);
}
if (nextJoints == null || nextJoints.Count == 0)
{
return(0);
}
double prevVariance = GetSkeletonCamPosVariance(prevJoints.Values.Select(p => (CvPoint3D64f)p.Position.ToCvPoint3D()).ToList());
double nextVariance = GetSkeletonCamPosVariance(prevJoints.Values.Select(p => (CvPoint3D64f)p.Position.ToCvPoint3D()).ToList());
return(Math.Sqrt(prevVariance * nextVariance));
}
private void ConvertOldToNew(string filepath)
{
List <MotionDataOld> mdos;
var serializer = MessagePackSerializer.Get <List <MotionDataOld> >();
using (FileStream fs = File.Open(filepath, FileMode.Open))
{
mdos = (List <MotionDataOld>)serializer.Unpack(fs);
}
//ここで変換する
List <MotionData> newMd = new List <MotionData>();
foreach (MotionDataOld mdo in mdos)
{
MotionData md = new MotionData();
md.FrameNo = mdo.FrameNo;
md.ImagePath = mdo.ImagePath;
md.DepthPath = mdo.DepthPath;
md.UserPath = mdo.UserPath;
md.bodies = mdo.bodies.CloneDeep();
md.TimeStamp = mdo.TimeStamp;
md.ColorWidth = mdo.ColorWidth;
md.ColorHeight = mdo.ColorHeight;
md.DepthUserWidth = mdo.DepthUserWidth;
md.DepthUserHeight = mdo.DepthUserHeight;
if (mdo.depthLUT != null)
{
md.depthLUT = mdo.depthLUT.CloneDeep();
}
else
{
md.depthLUT = null;
}
newMd.Add(md);
}
var serializer2 = MessagePackSerializer.Get <List <MotionData> >();
using (FileStream fs = File.Open(filepath, FileMode.OpenOrCreate, FileAccess.Write))
{
lock (newMd)
{
serializer2.Pack(fs, newMd);
}
}
}
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>
/// Bodyのidと代表点の座標を返す
/// </summary>
/// <param name="recordNo"></param>
/// <returns></returns>
public List <Tuple <ulong, Point> > GetIdAndPosition(int recordNo)
{
MotionData md = this.records[recordNo];
List <Tuple <ulong, Point> > ret = new List <Tuple <ulong, Point> >();
foreach (SerializableBody body in md.bodies)
{
if (body.colorSpacePoints != null && body.colorSpacePoints.Count != 0)
{
ret.Add(Tuple.Create(body.TrackingId, Utility.GetAveragePoint(body.colorSpacePoints.Values)));
}
else
{
ret.Add(null);
}
}
return(ret);
}
/// <summary>
/// Bodyの左右を反転する
/// </summary>
/// <param name="recordNo"></param>
/// <param name="UserIds"></param>
public void InverseBody(int recordNo, int integratedId = -1, ulong originalId = 0)
{
MotionData md = this.records[recordNo];
foreach (SerializableBody body in md.bodies)
{
if (integratedId != -1 && body.integratedId == integratedId)
{
body.InverseJoints();
body.mirrored = !body.mirrored;
}
else if (originalId != 0 && body.TrackingId == originalId)
{
body.InverseJoints();
body.mirrored = !body.mirrored;
}
}
}
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();
}
private void ConvertOldToNew(string filepath)
{
List<MotionDataOld> mdos;
var serializer = MessagePackSerializer.Get<List<MotionDataOld>>();
using (FileStream fs = File.Open(filepath, FileMode.Open))
{
mdos = (List<MotionDataOld>)serializer.Unpack(fs);
}
//ここで変換する
List<MotionData> newMd = new List<MotionData>();
foreach (MotionDataOld mdo in mdos)
{
MotionData md = new MotionData();
md.FrameNo = mdo.FrameNo;
md.ImagePath = mdo.ImagePath;
md.DepthPath = mdo.DepthPath;
md.UserPath = mdo.UserPath;
md.bodies = mdo.bodies.CloneDeep();
md.TimeStamp = mdo.TimeStamp;
md.ColorWidth = mdo.ColorWidth;
md.ColorHeight = mdo.ColorHeight;
md.DepthUserWidth = mdo.DepthUserWidth;
md.DepthUserHeight = mdo.DepthUserHeight;
if (mdo.depthLUT != null)
{
md.depthLUT = mdo.depthLUT.CloneDeep();
}
else
{
md.depthLUT = null;
}
newMd.Add(md);
}
var serializer2 = MessagePackSerializer.Get<List<MotionData>>();
using (FileStream fs = File.Open(filepath, FileMode.OpenOrCreate, FileAccess.Write))
{
lock (newMd)
{
serializer2.Pack(fs, newMd);
}
}
}
/// <summary>
/// データを追加
/// TODO : 2回目を同じファイル名でレコードすると確実に落ちる
/// </summary>
/// <param name="frameNo"></param>
/// <param name="dateTime"></param>
/// <param name="bodies"></param>
public void AddData(int frameNo, DateTime dateTime, Body[] bodies, ref byte[] colorPixels, ref ushort[] depthBuffer, ref byte[] bodyIndexBuffer, Dictionary <ulong, PointsPair> pointPairs)
{
this.SaveImages(frameNo, ref colorPixels, ref depthBuffer, ref bodyIndexBuffer);
MotionData motionData = new MotionData(frameNo, this.dataDir, dateTime, bodies, pointPairs);
motionData.ColorWidth = this.colorWidth;
motionData.ColorHeight = this.colorHeight;
motionData.DepthUserWidth = this.depthWidth;
motionData.DepthUserHeight = this.depthHeight;
if (frameNo == 0)
{
motionData.depthLUT = this.DepthLUT;
}
lock (this.motioDataQueue)
{
this.motioDataQueue.Enqueue(motionData);
}
}
/// <summary>
/// 骨格座標の信頼性の値(分散以外)を求めます
/// </summary>
/// <param name="record"></param>
/// <param name="time"></param>
/// <param name="user"></param>
/// <returns></returns>
public double GetSkeletonReliability(MotionData prevFrame, MotionData nextFrame, Dictionary<JointType, Joint> prevJoints, Dictionary<JointType, Joint> nextJoints, DateTime time,
CameraIntrinsics cameraInfo)
{
double periodAfter = (time - prevFrame.TimeStamp).TotalSeconds;
double periodBefore = (nextFrame.TimeStamp - time).TotalSeconds;
double weightPeriod = Math.Exp(-periodAfter / 0.2) + Math.Exp(-periodBefore / 0.2);
if (prevJoints == null || prevJoints.Count == 0)
return 0;
if (nextJoints == null || nextJoints.Count == 0)
return 0;
double prevEdge = 1;
double nextEdge = 1;
if (prevJoints.Count > 0)
{
prevEdge = prevJoints.Values.Select(p => Utility.GetRatioSqOfPrincipalToFocal(cameraInfo, p.Position.X, p.Position.Y)).Select(v => 1.0 / (1.0 + Math.Pow(v, 4))).Average();
}
if (nextJoints.Count > 0)
{
nextEdge = nextJoints.Values.Select(p => Utility.GetRatioSqOfPrincipalToFocal(cameraInfo, p.Position.X, p.Position.Y)).Select(v => 1.0 / (1.0 + Math.Pow(v, 4))).Average();
}
return weightPeriod * Math.Sqrt(prevEdge * nextEdge);
}
/// <summary>
/// バイナリから読み込む
/// </summary>
/// <param name="path"></param>
/// <returns></returns>
public static List <object> LoadFromSequentialBinary(string path)
{
IFormatter formatter = new BinaryFormatter();
List <object> res = new List <object>();
using (FileStream fs = new FileStream(path, FileMode.Open, FileAccess.Read, FileShare.None))
{
while (fs.Length != fs.Position)
{
MotionData md = (MotionData)formatter.Deserialize(fs);
md.isValid = true;
res.Add(md);
//if (md.TimeStamp.Minute < 20)
//{
// res.Add(md);
//}
//if (md.TimeStamp.Hour == 11 && md.TimeStamp.Minute >= 20)
//{
// break;
//}
}
}
return(res);
}
/// <summary>
/// recordNoのデータを使わないように設定する
/// </summary>
/// <param name="recordNo"></param>
public void SetDataNotValid(int recordNo)
{
MotionData md = this.records[recordNo];
md.isValid = false;
}
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;
}
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));
}
/// <summary>
/// データを追加
/// TODO : 2回目を同じファイル名でレコードすると確実に落ちる
/// </summary>
/// <param name="frameNo"></param>
/// <param name="dateTime"></param>
/// <param name="bodies"></param>
public void AddData(int frameNo, DateTime dateTime, Body[] bodies, ref byte[] colorPixels, ref ushort[] depthBuffer, ref byte[] bodyIndexBuffer, Dictionary<ulong, PointsPair> pointPairs)
{
this.SaveImages(frameNo, ref colorPixels, ref depthBuffer, ref bodyIndexBuffer);
MotionData motionData = new MotionData(frameNo, this.dataDir, dateTime, bodies, pointPairs);
motionData.ColorWidth = this.colorWidth;
motionData.ColorHeight = this.colorHeight;
motionData.DepthUserWidth = this.depthWidth;
motionData.DepthUserHeight = this.depthHeight;
if (frameNo == 0)
{
motionData.depthLUT = this.DepthLUT;
}
lock (this.motioDataQueue)
{
this.motioDataQueue.Enqueue(motionData);
}
}
