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