public ColoredIterativePointMatching(Frame frame, List<LocalCoordinateMapper> localCoordinateMappers, IList<CvMat> initialModelTransforms, Func<float, double> weightFromDistanceSq, double colorScale)
{
if (weightFromDistanceSq == null)
{
throw new ArgumentNullException("weightFromDistanceSq");
}
for (int i = 0; i < frame.recordNum; i++)
{
List<Tuple<CvPoint3D64f, CvColor>> modelPoints = localCoordinateMappers[i].GetUserColorPoints(frame.DepthMatList[i], frame.ColorMatList[i], frame.UserMatList[i]);
FlannColoredModelPoints model = new FlannColoredModelPoints(modelPoints, new KDTreeIndexParams(), new SearchParams(), colorScale);
_flannModels.Add(model);
}
if (_flannModels.Count != initialModelTransforms.Count)
{
throw new ArgumentException("transform list length must be same as model list length");
}
_modelTransforms = initialModelTransforms.ToList();
_weightFromDistanceSq = weightFromDistanceSq;
}
/// <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;
}
/// <summary>
/// あるフレームにおける座標変換行列を骨格情報から計算する
/// </summary>
/// <param name="frame"></param>
/// <param name="convList"></param>
/// <param name="selectedUserIdList"></param>
/// <returns></returns>
public static List<CvMat> GetConvMatrixFromBoneFrame(Frame frame, List<CvMat> convList, int[] integratedIs)
{
List<SerializableBody> bodies = frame.GetSelectedBodyList(integratedIds:integratedIs);
return GetConvMatrixFromBoneFrame(frame, convList, bodies);
}
/// <summary>
/// あるフレームにおける座標変換行列を骨格情報から計算する
/// </summary>
/// <param name="frame"></param>
/// <param name="convList"></param>
/// <param name="selectedUserIdList"></param>
/// <returns></returns>
public static List<CvMat> GetConvMatrixFromBoneFrame(Frame frame, List<CvMat> convList, ulong[] originalIds)
{
List<SerializableBody> bodies = frame.GetSelectedBodyList(originalIds:originalIds);
return GetConvMatrixFromBoneFrame(frame, convList, bodies);
}
/// <summary>
/// あるフレームにおける座標変換行列を深度情報から計算する
/// </summary>
/// <param name="frame"></param>
public static List<CvMat> GetConvMatrixFromDepthFrame(Frame frame, List<CvMat> convList, List<LocalCoordinateMapper> localCoordinateMappers)
{
Func<float, double> distance2weight = x => 1.0 / (x * 0 + 400 / 1000f);
using (ColoredIterativePointMatching sipm = new ColoredIterativePointMatching(frame, localCoordinateMappers, convList, distance2weight, 200))
{
List<CvMat> conversions = sipm.CalculateTransformSequntially(0.2, 3);
return conversions;
}
}
/// <summary>
/// 時刻を少しずつ進めながらフレームを作っていく
/// </summary>
/// <param name="records"></param>
/// <returns></returns>
private List<Frame> GenerateFrames()
{
List<Frame> frames = new List<Frame>();
List<Tuple<List<DateTime>, int[]>> timeInfos = new List<Tuple<List<DateTime>, int[]>>();
foreach (List<MotionData> record in this.originalRecords)
{
DateTime[] dateTimes = record.Select(m => m.TimeStamp).ToArray();
int[] indexes = record.Select((m, i) => i).ToArray();
Array.Sort(dateTimes, indexes);
timeInfos.Add(Tuple.Create(dateTimes.ToList(), indexes));
}
this.timeInfos = timeInfos;
for (DateTime time = this.startTime; time <= this.endTime; time += this.timePeriod)
{
// 同時刻のフレーム集合. Kinectの数だけ入るはず.
List<MotionData> currentRecords = new List<MotionData>();
List<MotionData> nextRecords = new List<MotionData>();
for (int i = 0; i < this.recordNum; i++)
{
List<MotionData> record = this.originalRecords[i];
List<DateTime> dateTimes = timeInfos[i].Item1;
int[] indexes = timeInfos[i].Item2;
int frameIndex = ListEx.GetMaxLessEqualIndexFromBinarySearch(dateTimes.BinarySearch(time));
int nextIndex = ListEx.GetMinGreaterEqualIndexFromBinarySearch(dateTimes.BinarySearch(time));
if (frameIndex < 0)
frameIndex = 0;
if (frameIndex >= record.Count())
frameIndex = record.Count() - 1;
if (nextIndex < 0)
nextIndex = 0;
if (nextIndex >= record.Count())
nextIndex = record.Count() - 1;
currentRecords.Add(record[indexes[frameIndex]]);
nextRecords.Add(record[indexes[nextIndex]]);
}
Frame frame = new Frame(currentRecords, nextRecords);
frame.Time = time;
frames.Add(frame);
}
return frames;
}