/// <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="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);
}
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));
}
/// <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;
}
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);
}
