public static void Test()
{
CoordRotTransConversion crtc = new CoordRotTransConversion();
Random rand = new Random();
CvMat cov = new CvMat(4, 4, MatrixType.F64C1);
cov.Zero();
cov[0, 3] = rand.NextDouble() * 200 - 500;
cov[1, 3] = rand.NextDouble() * 200 - 500;
cov[2, 3] = rand.NextDouble() * 200 - 500;
cov[3, 3] = 1.0;
CvMat rotateConversion;
cov.GetSubRect(out rotateConversion, new CvRect(0, 0, 3, 3));
CvMat rotVector = new CvMat(1, 3, MatrixType.F64C1);
rotVector[0, 0] = rand.NextDouble() * 10 - 5;
rotVector[0, 1] = rand.NextDouble() * 10 - 5;
rotVector[0, 2] = rand.NextDouble() * 10 - 5;
Cv.Rodrigues2(rotVector, rotateConversion);
for (int i = 0; i < 100000; i++)
{
CvPoint3D64f from = new CvPoint3D64f(rand.NextDouble() * rand.NextDouble() * rand.NextDouble() * 200 - 500, rand.NextDouble() * 200 - 500, rand.NextDouble() * 200 - 500);
CvMat fromMat = new CvMat(4, 1, MatrixType.F64C1);
CvEx.FillCvMat(fromMat, new double[] { from.X, from.Y, from.Z, 1.0 });
CvMat toMat = cov * fromMat;
CvPoint3D64f to = new CvPoint3D64f(toMat[0, 0], toMat[0, 1], toMat[0, 2]);
crtc.PutPoint(from, to, 1.0);
}
CvMat ret = crtc.Solve();
Func <CvMat, CvMat, string> show = (i, o) =>
{
StringBuilder str = new StringBuilder();
str.AppendFormat("{0} = {1} / {2}\n", "11", i[0, 0].ToString("0.000"), o[0, 0].ToString("0.000"));
str.AppendFormat("{0} = {1} / {2}\n", "12", i[0, 1].ToString("0.000"), o[0, 1].ToString("0.000"));
str.AppendFormat("{0} = {1} / {2}\n", "13", i[0, 2].ToString("0.000"), o[0, 2].ToString("0.000"));
str.AppendFormat("{0} = {1} / {2}\n", "14", i[0, 3].ToString("0.000"), o[0, 3].ToString("0.000"));
str.AppendFormat("{0} = {1} / {2}\n", "21", i[1, 0].ToString("0.000"), o[1, 0].ToString("0.000"));
str.AppendFormat("{0} = {1} / {2}\n", "22", i[1, 1].ToString("0.000"), o[1, 1].ToString("0.000"));
str.AppendFormat("{0} = {1} / {2}\n", "23", i[1, 2].ToString("0.000"), o[1, 2].ToString("0.000"));
str.AppendFormat("{0} = {1} / {2}\n", "24", i[1, 3].ToString("0.000"), o[1, 3].ToString("0.000"));
str.AppendFormat("{0} = {1} / {2}\n", "31", i[2, 0].ToString("0.000"), o[2, 0].ToString("0.000"));
str.AppendFormat("{0} = {1} / {2}\n", "32", i[2, 1].ToString("0.000"), o[2, 1].ToString("0.000"));
str.AppendFormat("{0} = {1} / {2}\n", "33", i[2, 2].ToString("0.000"), o[2, 2].ToString("0.000"));
str.AppendFormat("{0} = {1} / {2}\n", "34", i[2, 3].ToString("0.000"), o[2, 3].ToString("0.000"));
str.AppendFormat("{0} = {1} / {2}\n", "41", i[3, 0].ToString("0.000"), o[3, 0].ToString("0.000"));
str.AppendFormat("{0} = {1} / {2}\n", "42", i[3, 1].ToString("0.000"), o[3, 1].ToString("0.000"));
str.AppendFormat("{0} = {1} / {2}\n", "43", i[3, 2].ToString("0.000"), o[3, 2].ToString("0.000"));
str.AppendFormat("{0} = {1} / {2}\n", "44", i[3, 3].ToString("0.000"), o[3, 3].ToString("0.000"));
return(str.ToString());
};
MessageBox.Show(show(cov, ret));
}
public static void Test()
{
CoordRotTransConversion crtc = new CoordRotTransConversion();
Random rand = new Random();
CvMat cov = new CvMat(4, 4, MatrixType.F64C1);
cov.Zero();
cov[0, 3] = rand.NextDouble() * 200 - 500;
cov[1, 3] = rand.NextDouble() * 200 - 500;
cov[2, 3] = rand.NextDouble() * 200 - 500;
cov[3, 3] = 1.0;
CvMat rotateConversion;
cov.GetSubRect(out rotateConversion, new CvRect(0, 0, 3, 3));
CvMat rotVector = new CvMat(1, 3, MatrixType.F64C1);
rotVector[0, 0] = rand.NextDouble() * 10 - 5;
rotVector[0, 1] = rand.NextDouble() * 10 - 5;
rotVector[0, 2] = rand.NextDouble() * 10 - 5;
Cv.Rodrigues2(rotVector, rotateConversion);
for (int i = 0; i < 100000; i++)
{
CvPoint3D64f from = new CvPoint3D64f(rand.NextDouble() * rand.NextDouble() * rand.NextDouble() * 200 - 500, rand.NextDouble() * 200 - 500, rand.NextDouble() * 200 - 500);
CvMat fromMat = new CvMat(4, 1, MatrixType.F64C1);
CvEx.FillCvMat(fromMat, new double[] { from.X, from.Y, from.Z, 1.0 });
CvMat toMat = cov * fromMat;
CvPoint3D64f to = new CvPoint3D64f(toMat[0, 0], toMat[0, 1], toMat[0, 2]);
crtc.PutPoint(from, to, 1.0);
}
CvMat ret = crtc.Solve();
Func<CvMat, CvMat, string> show = (i, o) =>
{
StringBuilder str = new StringBuilder();
str.AppendFormat("{0} = {1} / {2}\n", "11", i[0, 0].ToString("0.000"), o[0, 0].ToString("0.000"));
str.AppendFormat("{0} = {1} / {2}\n", "12", i[0, 1].ToString("0.000"), o[0, 1].ToString("0.000"));
str.AppendFormat("{0} = {1} / {2}\n", "13", i[0, 2].ToString("0.000"), o[0, 2].ToString("0.000"));
str.AppendFormat("{0} = {1} / {2}\n", "14", i[0, 3].ToString("0.000"), o[0, 3].ToString("0.000"));
str.AppendFormat("{0} = {1} / {2}\n", "21", i[1, 0].ToString("0.000"), o[1, 0].ToString("0.000"));
str.AppendFormat("{0} = {1} / {2}\n", "22", i[1, 1].ToString("0.000"), o[1, 1].ToString("0.000"));
str.AppendFormat("{0} = {1} / {2}\n", "23", i[1, 2].ToString("0.000"), o[1, 2].ToString("0.000"));
str.AppendFormat("{0} = {1} / {2}\n", "24", i[1, 3].ToString("0.000"), o[1, 3].ToString("0.000"));
str.AppendFormat("{0} = {1} / {2}\n", "31", i[2, 0].ToString("0.000"), o[2, 0].ToString("0.000"));
str.AppendFormat("{0} = {1} / {2}\n", "32", i[2, 1].ToString("0.000"), o[2, 1].ToString("0.000"));
str.AppendFormat("{0} = {1} / {2}\n", "33", i[2, 2].ToString("0.000"), o[2, 2].ToString("0.000"));
str.AppendFormat("{0} = {1} / {2}\n", "34", i[2, 3].ToString("0.000"), o[2, 3].ToString("0.000"));
str.AppendFormat("{0} = {1} / {2}\n", "41", i[3, 0].ToString("0.000"), o[3, 0].ToString("0.000"));
str.AppendFormat("{0} = {1} / {2}\n", "42", i[3, 1].ToString("0.000"), o[3, 1].ToString("0.000"));
str.AppendFormat("{0} = {1} / {2}\n", "43", i[3, 2].ToString("0.000"), o[3, 2].ToString("0.000"));
str.AppendFormat("{0} = {1} / {2}\n", "44", i[3, 3].ToString("0.000"), o[3, 3].ToString("0.000"));
return str.ToString();
};
MessageBox.Show(show(cov, ret));
}
/// <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="frames"></param>
public static List <CvMat> GetConvMatrixFromBoneFrameSequence(FrameSequence frameSeq, int startIndex, int endIndex)
{
Dictionary <Tuple <int, int>, int> cooccurenceCount = new Dictionary <Tuple <int, int>, int>();
IEnumerable <Frame> frames = frameSeq.Frames.Skip(startIndex).Take(endIndex);
List <SerializableBody> bodies;
foreach (Frame frame in frames)
{
if (frameSeq.Segmentations == null)
{
bodies = frame.GetSelectedBodyList(originalIds: frameSeq.selectedOriginalIdList);
}
else
{
bodies = frame.GetSelectedBodyList(integratedIds: frameSeq.selecteedIntegretedIdList);
}
if (bodies.Count() != frame.recordNum)
{
continue;
}
bool[] validFlags = frame.GetValidFlags();
for (int i = 0; i < frame.recordNum; i++)
{
for (int j = i + 1; j < frame.recordNum; j++)
{
if (validFlags[i] && validFlags[j] == false)
{
continue;
}
Dictionary <JointType, Joint> joint1 = Utility.GetValidJoints(bodies[i].Joints);
Dictionary <JointType, Joint> joint2 = Utility.GetValidJoints(bodies[j].Joints);
foreach (JointType jointType in joint1.Keys.Intersect(joint2.Keys))
{
Tuple <int, int> key = new Tuple <int, int>(i, j);
int count;
if (!cooccurenceCount.TryGetValue(key, out count))
{
count = 0;
}
cooccurenceCount[key] = count + 1;
}
}
}
}
// 依存関係のツリーを作る
int baseRecordIndex;
Dictionary <int, int> dependencies;
// とりあえず先頭フレームのレコード数にしてるけど、プロジェクトとかが持つべき値
if (!CalcEx.GetDependencyTree(frameSeq.recordNum, cooccurenceCount, list => list.Sum(), out baseRecordIndex, out dependencies))
{
System.Windows.MessageBox.Show("骨格が他のレコードと同時に映っているフレームがないレコードがあるため計算できませんでした");
return(frameSeq.ToWorldConversions);
}
else
{
Dictionary <int, ICoordConversion3D> conversionsPerDependencyKey = new Dictionary <int, ICoordConversion3D>();
foreach (Frame frame in frameSeq.Frames)
{
if (frameSeq.Segmentations == null)
{
bodies = frame.GetSelectedBodyList(originalIds: frameSeq.selectedOriginalIdList);
}
else
{
bodies = frame.GetSelectedBodyList(integratedIds: frameSeq.selecteedIntegretedIdList);
}
if (bodies.Count() != frame.recordNum)
{
continue;
}
List <CvSize> depthUsersizeList = frame.DepthUserSize;
bool[] validFlags = frame.GetValidFlags();
foreach (KeyValuePair <int, int> dependencyPair in CalcEx.EnumerateDependencyPairs(baseRecordIndex, dependencies))
{
if (validFlags[dependencyPair.Key] && validFlags[dependencyPair.Value] == false)
{
continue;
}
// 変換計算用オブジェクトを拾ってくる
ICoordConversion3D conv;
if (!conversionsPerDependencyKey.TryGetValue(dependencyPair.Key, out conv))
{
conversionsPerDependencyKey[dependencyPair.Key] = conv = new CoordRotTransConversion();
}
Dictionary <JointType, Joint> joint1 = Utility.GetValidJoints(bodies[dependencyPair.Key].Joints);
Dictionary <JointType, Joint> joint2 = Utility.GetValidJoints(bodies[dependencyPair.Value].Joints);
foreach (JointType jointType in joint1.Keys.Intersect(joint2.Keys))
{
CvPoint3D64f camPoint1 = joint1[jointType].Position.ToCvPoint3D();
CvPoint3D64f camPoint2 = joint2[jointType].Position.ToCvPoint3D();
// それぞれのカメラ座標系におけるそれぞれの対応点をセットに入れる
conv.PutPoint(camPoint1, camPoint2, 1);
}
}
}
List <CvMat> convList = frameSeq.ToWorldConversions;
foreach (KeyValuePair <int, int> dependencyPair in CalcEx.EnumerateDependencyPairs(baseRecordIndex, dependencies))
{
CvMat relConv = conversionsPerDependencyKey[dependencyPair.Key].Solve();
CvMat baseConversion = convList[dependencyPair.Value];
convList[dependencyPair.Key] = baseConversion * relConv;
}
return(convList);
}
}
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;
}
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="frames"></param>
public static List<CvMat> GetConvMatrixFromBoneFrameSequence(FrameSequence frameSeq, int startIndex, int endIndex)
{
Dictionary<Tuple<int, int>, int> cooccurenceCount = new Dictionary<Tuple<int, int>, int>();
IEnumerable<Frame> frames = frameSeq.Frames.Skip(startIndex).Take(endIndex);
List<SerializableBody> bodies;
foreach (Frame frame in frames)
{
if (frameSeq.Segmentations == null)
{
bodies = frame.GetSelectedBodyList(originalIds:frameSeq.selectedOriginalIdList);
}
else
{
bodies = frame.GetSelectedBodyList(integratedIds:frameSeq.selecteedIntegretedIdList);
}
if (bodies.Count() != frame.recordNum)
{
continue;
}
bool[] validFlags = frame.GetValidFlags();
for (int i = 0; i < frame.recordNum; i++)
{
for (int j = i + 1; j < frame.recordNum; j++)
{
if (validFlags[i] && validFlags[j] == false)
{
continue;
}
Dictionary<JointType, Joint> joint1 = Utility.GetValidJoints(bodies[i].Joints);
Dictionary<JointType, Joint> joint2 = Utility.GetValidJoints(bodies[j].Joints);
foreach (JointType jointType in joint1.Keys.Intersect(joint2.Keys))
{
Tuple<int, int> key = new Tuple<int, int>(i, j);
int count;
if (!cooccurenceCount.TryGetValue(key, out count))
{
count = 0;
}
cooccurenceCount[key] = count + 1;
}
}
}
}
// 依存関係のツリーを作る
int baseRecordIndex;
Dictionary<int, int> dependencies;
// とりあえず先頭フレームのレコード数にしてるけど、プロジェクトとかが持つべき値
if (!CalcEx.GetDependencyTree(frameSeq.recordNum, cooccurenceCount, list => list.Sum(), out baseRecordIndex, out dependencies))
{
System.Windows.MessageBox.Show("骨格が他のレコードと同時に映っているフレームがないレコードがあるため計算できませんでした");
return frameSeq.ToWorldConversions;
}
else
{
Dictionary<int, ICoordConversion3D> conversionsPerDependencyKey = new Dictionary<int, ICoordConversion3D>();
foreach (Frame frame in frameSeq.Frames)
{
if (frameSeq.Segmentations == null)
{
bodies = frame.GetSelectedBodyList(originalIds:frameSeq.selectedOriginalIdList);
}
else
{
bodies = frame.GetSelectedBodyList(integratedIds:frameSeq.selecteedIntegretedIdList);
}
if (bodies.Count() != frame.recordNum)
continue;
List<CvSize> depthUsersizeList = frame.DepthUserSize;
bool[] validFlags = frame.GetValidFlags();
foreach (KeyValuePair<int, int> dependencyPair in CalcEx.EnumerateDependencyPairs(baseRecordIndex, dependencies))
{
if (validFlags[dependencyPair.Key] && validFlags[dependencyPair.Value] == false)
{
continue;
}
// 変換計算用オブジェクトを拾ってくる
ICoordConversion3D conv;
if (!conversionsPerDependencyKey.TryGetValue(dependencyPair.Key, out conv))
{
conversionsPerDependencyKey[dependencyPair.Key] = conv = new CoordRotTransConversion();
}
Dictionary<JointType, Joint> joint1 = Utility.GetValidJoints(bodies[dependencyPair.Key].Joints);
Dictionary<JointType, Joint> joint2 = Utility.GetValidJoints(bodies[dependencyPair.Value].Joints);
foreach (JointType jointType in joint1.Keys.Intersect(joint2.Keys))
{
CvPoint3D64f camPoint1 = joint1[jointType].Position.ToCvPoint3D();
CvPoint3D64f camPoint2 = joint2[jointType].Position.ToCvPoint3D();
// それぞれのカメラ座標系におけるそれぞれの対応点をセットに入れる
conv.PutPoint(camPoint1, camPoint2, 1);
}
}
}
List<CvMat> convList = frameSeq.ToWorldConversions;
foreach (KeyValuePair<int, int> dependencyPair in CalcEx.EnumerateDependencyPairs(baseRecordIndex, dependencies))
{
CvMat relConv = conversionsPerDependencyKey[dependencyPair.Key].Solve();
CvMat baseConversion = convList[dependencyPair.Value];
convList[dependencyPair.Key] = baseConversion * relConv;
}
return convList;
}
}
/// <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;
}
