/// <summary>
/// CvPoint3D64fをOpenNI.Point3Dに変換します
/// </summary>
/// <param name="point"></param>
/// <returns></returns>
public static CameraSpacePoint ToCameraSpacePoint(this CvPoint3D64f point)
{
return(new CameraSpacePoint()
{
X = (float)point.X, Y = (float)point.Y, Z = (float)point.Z
});
}
/// <summary>
/// 縦ベクトル用の4x4平行移動行列を作成します
/// </summary>
/// <param name="translation"></param>
/// <returns></returns>
public static CvMat GetTranslation(CvPoint3D64f translation)
{
CvMat ret = CvMat.Identity(4, 4, MatrixType.F64C1);
ret[0, 3] = translation.X;
ret[1, 3] = translation.Y;
ret[2, 3] = translation.Z;
return(ret);
}
/// <summary>
/// 指定された軸に対して指定された角度だけ回転させる3x3行列を返します。縦ベクトルで右手系ならば軸の向きを奥の向きとした場合に時計回りに回転させます。
/// </summary>
/// <param name="axis"></param>
/// <param name="theta"></param>
/// <returns></returns>
public static CvMat GetRotation3D(CvPoint3D64f axis, double theta)
{
// sin theta / 2 ってどっちだ
axis = Normalize(axis);
double cos = Math.Cos(theta / 2.0);
double sin = Math.Sin(theta / 2.0);
return(QuaternionToMat3D(cos, axis * (sin)));
}
/// <summary>
/// ベクトルを正規化します
/// </summary>
/// <param name="vector"></param>
/// <returns></returns>
public static CvPoint3D64f Normalize(CvPoint3D64f vector)
{
double sqrt = Math.Sqrt(GetLengthSq(vector));
if (sqrt == 0)
{
return(new CvPoint3D64f(0, 0, 0));
}
return(vector * (1.0 / sqrt));
}
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="seqJoints"></param>
public static List <Dictionary <JointType, CvPoint3D64f> > SequentialCorrect(List <Dictionary <JointType, CvPoint3D64f> > seqJoints)
{
CvPoint3D64f prevShoulderVector = new CvPoint3D64f();
CvPoint3D64f prevHipVector = new CvPoint3D64f();
List <Dictionary <JointType, CvPoint3D64f> > res = new List <Dictionary <JointType, CvPoint3D64f> >();
foreach (Dictionary <JointType, CvPoint3D64f> joints in seqJoints)
{
bool reverse = false;
if (joints.ContainsKey(JointType.ShoulderLeft) && joints.ContainsKey(JointType.ShoulderRight))
{
CvPoint3D64f shoulderVector = joints[JointType.ShoulderLeft] - joints[JointType.ShoulderRight];
if (prevShoulderVector == default(CvPoint3D64f))
{
prevShoulderVector = shoulderVector;
}
if (CvEx.Cos(shoulderVector, prevShoulderVector) <= -0.8)
{
reverse = true;
}
}
if (joints.ContainsKey(JointType.HipLeft) && joints.ContainsKey(JointType.HipRight))
{
CvPoint3D64f hipVector = joints[JointType.HipLeft] - joints[JointType.HipRight];
if (prevHipVector == default(CvPoint3D64f))
{
prevHipVector = hipVector;
}
if (CvEx.Cos(hipVector, prevHipVector) <= -0.8)
{
reverse = true;
}
}
if (reverse)
{
Dictionary <JointType, CvPoint3D64f> newJoints = joints.ToDictionary(p => CalcEx.GetMirroredJoint(p.Key), p => p.Value);
res.Add(newJoints);
// reverseした後のただしいベクトルを入れなおしておく
if (joints.ContainsKey(JointType.ShoulderLeft) && joints.ContainsKey(JointType.ShoulderRight))
{
prevShoulderVector = newJoints[JointType.ShoulderLeft] - newJoints[JointType.ShoulderRight];
}
if (joints.ContainsKey(JointType.HipLeft) && joints.ContainsKey(JointType.HipRight))
{
prevHipVector = newJoints[JointType.HipLeft] - newJoints[JointType.HipRight];
}
}
else
{
res.Add(joints);
}
}
return(res);
}
public void KnnSearch(CvPoint3D64f point, CvColor color, out int[] indices, out float[] dists, int knn)
{
if (knn == 0)
{
indices = new int[0];
dists = new float[0];
return;
}
float[] input = new float[] { (float)point.X, (float)point.Y, (float)point.Z, (float)(color.R * _colorScale / 255), (float)(color.G * _colorScale / 255), (float)(color.B * _colorScale / 255) };
_flannIndex.KnnSearch(input, out indices, out dists, knn, _searchParam);
}
public void KnnSearch(CvPoint3D64f point, CvColor color, out int[] indices, out float[] dists, int knn)
{
if (knn == 0)
{
indices = new int[0];
dists = new float[0];
return;
}
float[] input = new float[] { (float)point.X, (float)point.Y, (float)point.Z, (float)(color.R * _colorScale / 255), (float)(color.G * _colorScale / 255), (float)(color.B * _colorScale / 255) };
_flannIndex.KnnSearch(input, out indices, out dists, knn, _searchParam);
}
/// <summary>
/// 左胸の外積ベクトル
/// </summary>
/// <param name="body"></param>
/// <returns></returns>
private CvPoint3D64f CalcLeftChestCrossVector(Dictionary <JointType, CvPoint3D64f> joints)
{
if (joints.ContainsKey(JointType.SpineBase) && joints.ContainsKey(JointType.ShoulderLeft))
{
CvPoint3D64f torsoToLeftShoulder = joints[JointType.ShoulderLeft] - joints[JointType.SpineMid];
CvPoint3D64f spine = joints[JointType.SpineShoulder] - joints[JointType.SpineMid];
CvPoint3D64f bodyCross = CvEx.Cross(torsoToLeftShoulder, spine);
return(CvEx.Normalize(bodyCross));
}
return(default(CvPoint3D64f));
}
/// <summary>
/// 3次元点(縦ベクトル)を3x3行列で(左掛けで)回転変換します。
/// </summary>
/// <param name="from"></param>
/// <param name="conversion"></param>
/// <returns></returns>
public static CvPoint3D64f RotatePoint3D(CvPoint3D64f from, CvMat conversion)
{
using (CvMat fromMat = new CvMat(3, 1, MatrixType.F64C1))
{
fromMat[0, 0] = from.X;
fromMat[1, 0] = from.Y;
fromMat[2, 0] = from.Z;
using (CvMat worldPoint = conversion * fromMat)
{
return(new CvPoint3D64f(worldPoint[0, 0], worldPoint[1, 0], worldPoint[2, 0]));
}
}
}
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>
/// Jointsに座標変換を適用する
/// </summary>
/// <param name="joints"></param>
/// <param name="conversion"></param>
/// <returns></returns>
public static Dictionary <JointType, Joint> ApplyConversions(Dictionary <JointType, Joint> joints, CvMat conversion)
{
Dictionary <JointType, Joint> newJoints = new Dictionary <JointType, Joint>();
foreach (JointType jointType in joints.Keys)
{
Joint originalJoint = joints[jointType];
CvPoint3D64f fromPoint = originalJoint.Position.ToCvPoint3D();
CameraSpacePoint newPoint = CvEx.ConvertPoint3D(fromPoint, conversion).ToCameraSpacePoint();
originalJoint.Position = newPoint;
newJoints[jointType] = originalJoint;
}
return(newJoints);
}
/// <summary>
/// 2つのJointsの距離を計算する
/// </summary>
/// <param name="jointsOne"></param>
/// <param name="jointsTwo"></param>
/// <returns></returns>
public static Dictionary <JointType, double> CalcBodyDistanceSq(Dictionary <JointType, Joint> jointsOne, Dictionary <JointType, Joint> jointsTwo)
{
Dictionary <JointType, double> res = new Dictionary <JointType, double>();
IEnumerable <JointType> keys = jointsOne.Keys.Intersect(jointsTwo.Keys);
foreach (JointType key in keys)
{
CvPoint3D64f one = jointsOne[key].Position.ToCvPoint3D();
CvPoint3D64f two = jointsTwo[key].Position.ToCvPoint3D();
double distanceSq = CvEx.GetDistanceSq(one, two);
res[key] = distanceSq;
}
return(res);
}
/// <summary>
/// Unityに吐くためのデータ変換
/// </summary>
/// <param name="originalJoints"></param>
/// <returns></returns>
public static List <Dictionary <int, float[]> > ConverToCompatibleJoint(List <Dictionary <JointType, CvPoint3D64f> > originalJoints)
{
List <Dictionary <int, float[]> > newJoints = new List <Dictionary <int, float[]> >();
foreach (Dictionary <JointType, CvPoint3D64f> joints in originalJoints)
{
Dictionary <int, float[]> body = new Dictionary <int, float[]>();
foreach (JointType jointType in joints.Keys)
{
CvPoint3D64f position = joints[jointType];
float[] points = new float[] { (float)position.X, (float)position.Y, (float)position.Z };
body.Add((int)jointType, points);
}
newJoints.Add(body);
}
return(newJoints);
}
public void KnnSearch(CvPoint3D64f point, CvColor color, out int[] indices, out float[] dists, float radius, int maxResult)
{
float[] input = new float[] { (float)point.X, (float)point.Y, (float)point.Z, (float)(color.R * _colorScale / 255), (float)(color.G * _colorScale / 255), (float)(color.B * _colorScale / 255) };
int count = 0;
const int divCount = 2;
int maxResult2 = maxResult * 4;
for (int k = divCount - 1; k >= 0; k--)
{
float coef = (float)Math.Pow(0.1, k);
int[] indices2 = new int[maxResult2];
float[] dists2 = new float[maxResult2];
_flannIndex.RadiusSearch(input, indices2, dists2, radius * coef, maxResult2, _searchParam);
for (count = 0; count < maxResult2; count++)
{
if (dists2[count] == 0 && dists2[count] == 0f && ModelPoints[0].Item1 != point)
{
break;
}
}
if (count >= maxResult)
{
indices = new int[maxResult];
dists = new float[maxResult];
for (int j = 0; j < maxResult; j++)
{
indices[j] = indices2[j];
dists[j] = dists2[j];
}
return;
}
if (k == 0)
{
indices = new int[count];
dists = new float[count];
for (int j = 0; j < count; j++)
{
indices[j] = indices2[j];
dists[j] = dists2[j];
}
return;
}
}
this.KnnSearch(point, color, out indices, out dists, count);
}
/// <summary>
/// 微分する. 無理やり値を突っ込むことで長さを変えない
/// </summary>
/// <param name="sequence"></param>
/// <param name="times"></param>
/// <returns></returns>
public static List <CvPoint3D64f> Difference(List <CvPoint3D64f> sequence, List <DateTime> times)
{
List <CvPoint3D64f> res = new List <CvPoint3D64f>();
for (int index = 1; index < sequence.Count() - 1; index++)
{
double h = 2 * (times[index + 1] - times[index - 1]).TotalSeconds;
CvPoint3D64f prev = sequence[index - 1];
CvPoint3D64f next = sequence[index + 1];
double x = (next.X - prev.X) / h;
double y = (next.Y - prev.Y) / h;
double z = (next.Z - prev.Z) / h;
res.Add(new CvPoint3D64f(x, y, z));
}
res.Insert(0, res.First());
res.Add(res.Last());
return(res);
}
/// <summary>
/// Unity用に吐かれたデータを変換しておく
/// </summary>
/// <param name="rawJointsSeq"></param>
/// <returns></returns>
public static List <Dictionary <JointType, CvPoint3D64f> > ConvertToCvPoint(List <Dictionary <int, float[]> > rawJointsSeq)
{
List <Dictionary <JointType, CvPoint3D64f> > newJointsSeq = new List <Dictionary <JointType, CvPoint3D64f> >();
foreach (Dictionary <int, float[]> joints in rawJointsSeq)
{
Dictionary <JointType, CvPoint3D64f> newJoints = new Dictionary <JointType, CvPoint3D64f>();
foreach (int jointNo in joints.Keys)
{
JointType jointType = ConvertIntToJointType(jointNo);
float[] jointAry = joints[jointNo];
CvPoint3D64f point = new CvPoint3D64f(jointAry[0], jointAry[1], jointAry[2]);
newJoints[jointType] = point;
}
newJointsSeq.Add(newJoints);
}
return(newJointsSeq);
}
public void KnnSearch(CvPoint3D64f point, CvColor color, out int[] indices, out float[] dists, float radius, int maxResult)
{
float[] input = new float[] { (float)point.X, (float)point.Y, (float)point.Z, (float)(color.R * _colorScale / 255), (float)(color.G * _colorScale / 255), (float)(color.B * _colorScale / 255) };
int count = 0;
const int divCount = 2;
int maxResult2 = maxResult * 4;
for (int k = divCount - 1; k >= 0; k--)
{
float coef = (float)Math.Pow(0.1, k);
int[] indices2 = new int[maxResult2];
float[] dists2 = new float[maxResult2];
_flannIndex.RadiusSearch(input, indices2, dists2, radius * coef, maxResult2, _searchParam);
for (count = 0; count < maxResult2; count++)
{
if (dists2[count] == 0 && dists2[count] == 0f && ModelPoints[0].Item1 != point)
break;
}
if (count >= maxResult)
{
indices = new int[maxResult];
dists = new float[maxResult];
for (int j = 0; j < maxResult; j++)
{
indices[j] = indices2[j];
dists[j] = dists2[j];
}
return;
}
if (k == 0)
{
indices = new int[count];
dists = new float[count];
for (int j = 0; j < count; j++)
{
indices[j] = indices2[j];
dists[j] = dists2[j];
}
return;
}
}
this.KnnSearch(point, color, out indices, out dists, count);
}
/// <summary>
/// 2つのベクトルの角度をラジアンで返す
/// </summary>
/// <param name="one"></param>
/// <param name="two"></param>
/// <returns></returns>
public static double GetVectorRadian(CvPoint3D64f one, CvPoint3D64f two)
{
// masterにslaveを回転させるコード
//CvPoint3D64f slave = new CvPoint3D64f(0.7071, 0, 0.7071);
//CvPoint3D64f master = new CvPoint3D64f(0, 0, 1);
//double rad = Utility.GetVectorRadian(master, slave);
//CvMat mat = CvEx.GetRotation3D(new CvPoint3D64f(0, 1, 0), rad);
//CvPoint3D64f to = CvEx.RotatePoint3D(slave, mat);
one.Y = 0;
two.Y = 0;
CvPoint3D64f norm = CvEx.Cross(two, one);
double cos = CvEx.Cos(two, one);
double rad = Math.Acos(cos);
if (norm.Y < 0)
{
rad = -1 * rad;
}
return(rad);
}
/// <summary>
/// 3次元点(縦ベクトル)を4x4行列で(左掛けで)変換します。
/// </summary>
/// <param name="from">変換前の三次元点の座標</param>
/// <param name="conversion">64bit浮動小数点1チャンネル4x4行列</param>
/// <returns></returns>
public static CvPoint3D64f ConvertPoint3D(CvPoint3D64f from, CvMat conversion)
{
double x = from.X * conversion[0, 0] + from.Y * conversion[0, 1] + from.Z * conversion[0, 2] + conversion[0, 3];
double y = from.X * conversion[1, 0] + from.Y * conversion[1, 1] + from.Z * conversion[1, 2] + conversion[1, 3];
double z = from.X * conversion[2, 0] + from.Y * conversion[2, 1] + from.Z * conversion[2, 2] + conversion[2, 3];
return(new CvPoint3D64f(x, y, z));
using (CvMat fromMat = new CvMat(4, 1, MatrixType.F64C1))
{
fromMat[0, 0] = from.X;
fromMat[1, 0] = from.Y;
fromMat[2, 0] = from.Z;
fromMat[3, 0] = 1;
using (CvMat worldPoint = conversion * fromMat)
{
return(new CvPoint3D64f(worldPoint[0, 0], worldPoint[1, 0], worldPoint[2, 0]));
}
}
}
/// <summary>
/// 足の長さを統計情報をもとに正規化する
/// </summary>
/// <param name="body"></param>
private Dictionary <JointType, Joint> NormalizeLegJoints(Dictionary <JointType, Joint> joints, Dictionary <Bone, BoneStatistics> boneStatistics)
{
List <Tuple <JointType, JointType> > legBones = Utility.GetLegBones();
foreach (Bone bone in legBones)
{
if (joints.ContainsKey(bone.Item1) && joints.ContainsKey(bone.Item2))
{
Joint joint1 = joints[bone.Item1];
Joint joint2 = joints[bone.Item2];
// 1の骨は動かさない. SpineBaseから順番に修正されることは保証されている.
double medianLength = Math.Sqrt(boneStatistics[bone].medianLengthSq);
CvPoint3D64f normalizedVector = CvEx.Normalize(joint1.Position.ToCvPoint3D() - joint2.Position.ToCvPoint3D());
CvPoint3D32f expandedVector = (CvPoint3D32f)(normalizedVector * medianLength);
joint2.Position = (joint1.Position.ToCvPoint3D() + expandedVector).ToCameraSpacePoint();
joints[bone.Item2] = joint2;
}
}
return(joints);
}
/// <summary>
/// CvPointを時間で線形補間
/// </summary>
/// <param name="data"></param>
/// <returns></returns>
public static List <CvPoint3D64f?> LinearInterpolate(List <DateTime> times, List <CvPoint3D64f?> points)
{
List <CvPoint3D64f?> res = new List <CvPoint3D64f?>();
DateTime start = times.First();
double t0 = 0;
double t1 = (times.Last() - times.First()).TotalMilliseconds;
CvPoint3D64f p0 = (CvPoint3D64f)points.First();
CvPoint3D64f p1 = (CvPoint3D64f)points.Last();
res.Add(p0);
for (int i = 1; i < times.Count() - 1; i++)
{
DateTime now = times[i];
double t = (now - start).TotalMilliseconds;
double x = LinearInterpolate(t0, p0.X, t1, p1.X, t);
double y = LinearInterpolate(t0, p0.Y, t1, p1.Y, t);
double z = LinearInterpolate(t0, p0.Z, t1, p1.Z, t);
res.Add(new CvPoint3D64f(x, y, z));
}
res.Add(p1);
return(res);
}
/// <summary>
/// 時系列データの前後を比較して左右反転をなおす
/// </summary>
/// <param name="frameSeq"></param>
public static void SequentialCorrect(FrameSequence frameSeq)
{
CvPoint3D64f[] prevShoulderVectors = new CvPoint3D64f[frameSeq.recordNum];
CvPoint3D64f[] prevHipVectors = new CvPoint3D64f[frameSeq.recordNum];
foreach (Frame frame in frameSeq.Frames)
{
List <SerializableBody> bodies = frame.GetSelectedBodyList(integratedIds: frameSeq.selecteedIntegretedIdList);
if (bodies.Count == 0)
{
continue;
}
CvPoint3D64f[] shoulderVectors = bodies.Select(b =>
(CvPoint3D64f)(b.Joints[JointType.ShoulderLeft].Position.ToCvPoint3D() - b.Joints[JointType.ShoulderRight].Position.ToCvPoint3D())).ToArray();
CvPoint3D64f[] hipVectors = bodies.Select(b =>
(CvPoint3D64f)(b.Joints[JointType.HipLeft].Position.ToCvPoint3D() - b.Joints[JointType.HipRight].Position.ToCvPoint3D())).ToArray();
// 0回目の処理
if (prevShoulderVectors == default(CvPoint3D64f[]) || prevHipVectors == default(CvPoint3D64f[]))
{
prevShoulderVectors = shoulderVectors;
prevHipVectors = hipVectors;
continue;
}
double[] shoulderCosArray = shoulderVectors.Zip(prevShoulderVectors, (cur, prev) => CvEx.Cos(cur, prev)).ToArray();
double[] hipCosArray = hipVectors.Zip(prevHipVectors, (cur, prev) => CvEx.Cos(cur, prev)).ToArray();
for (int no = 0; no < frame.recordNum; no++)
{
if (shoulderCosArray[no] <= -0.8 || hipCosArray[no] <= -0.8)
{
frame.InverseBody(no, integratedId: frameSeq.selecteedIntegretedIdList[no]);
}
}
// Inverseされていた場合には骨の位置関係が変わっているため
prevShoulderVectors = bodies.Select(b =>
(CvPoint3D64f)(b.Joints[JointType.ShoulderLeft].Position.ToCvPoint3D() - b.Joints[JointType.ShoulderRight].Position.ToCvPoint3D())).ToArray();
prevHipVectors = bodies.Select(b =>
(CvPoint3D64f)(b.Joints[JointType.HipLeft].Position.ToCvPoint3D() - b.Joints[JointType.HipRight].Position.ToCvPoint3D())).ToArray();
}
}
/// <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>
/// 3次元点(縦ベクトル)を3x3行列で(左掛けで)回転変換します。
/// </summary>
/// <param name="from"></param>
/// <param name="conversion"></param>
/// <returns></returns>
public static CvPoint3D64f RotatePoint3D(CvPoint3D64f from, CvMat conversion)
{
using (CvMat fromMat = new CvMat(3, 1, MatrixType.F64C1))
{
fromMat[0, 0] = from.X;
fromMat[1, 0] = from.Y;
fromMat[2, 0] = from.Z;
using (CvMat worldPoint = conversion * fromMat)
{
return new CvPoint3D64f(worldPoint[0, 0], worldPoint[1, 0], worldPoint[2, 0]);
}
}
}
/// <summary>
/// 2ベクトルの角度をcos(theta)として求めます
/// </summary>
/// <param name="vector1"></param>
/// <param name="vector2"></param>
/// <returns></returns>
public static double Cos(CvPoint3D64f vector1, CvPoint3D64f vector2)
{
return(Dot(vector1, vector2) / (GetLength(vector1) * GetLength(vector2)));
}
/// <summary>
/// 2ベクトルの内積を求めます
/// </summary>
/// <param name="vector1"></param>
/// <param name="vector2"></param>
/// <returns></returns>
public static double Dot(CvPoint3D64f vector1, CvPoint3D64f vector2)
{
return(vector1.X * vector2.X + vector1.Y * vector2.Y + vector1.Z * vector2.Z);
}
/// <summary>
/// CvPoint3D64fをdouble[]に変換する
/// </summary>
/// <param name="point"></param>
/// <returns></returns>
public static double[] ToArrayPoints(this CvPoint3D64f point)
{
return(new double[] { point.X, point.Y, point.Z });
}
/// <summary>
/// ベクトルを正規化します
/// </summary>
/// <param name="vector"></param>
/// <returns></returns>
public static CvPoint3D64f Normalize(CvPoint3D64f vector)
{
double sqrt = Math.Sqrt(GetLengthSq(vector));
if (sqrt == 0)
return new CvPoint3D64f(0, 0, 0);
return vector * (1.0 / sqrt);
}
/// <summary>
/// 2ベクトルの角度をcos(theta)として求めます
/// </summary>
/// <param name="vector1"></param>
/// <param name="vector2"></param>
/// <returns></returns>
public static double Cos(CvPoint3D64f vector1, CvPoint3D64f vector2)
{
return Dot(vector1, vector2) / (GetLength(vector1) * GetLength(vector2));
}
/// <summary>
/// ベクトルのユークリッド距離における長さを返します
/// </summary>
/// <param name="vector"></param>
/// <returns></returns>
public static double GetLengthSq(CvPoint3D64f vector)
{
return(vector.X * vector.X + vector.Y * vector.Y + vector.Z * vector.Z);
}
/// <summary>
/// 2ベクトルの内積を求めます
/// </summary>
/// <param name="vector1"></param>
/// <param name="vector2"></param>
/// <returns></returns>
public static double Dot(CvPoint3D64f vector1, CvPoint3D64f vector2)
{
return vector1.X * vector2.X + vector1.Y * vector2.Y + vector1.Z * vector2.Z;
}
public static extern void cvDecomposeProjectionMatrix(IntPtr projMatr, IntPtr calibMatr, IntPtr rotMatr, IntPtr posVect,
IntPtr rotMatrX, IntPtr rotMatrY, IntPtr rotMatrZ, ref CvPoint3D64f eulerAngles);
/// <summary>
/// 縦ベクトル用の4x4平行移動行列を作成します
/// </summary>
/// <param name="translation"></param>
/// <returns></returns>
public static CvMat GetTranslation(CvPoint3D64f translation)
{
CvMat ret = CvMat.Identity(4, 4, MatrixType.F64C1);
ret[0, 3] = translation.X;
ret[1, 3] = translation.Y;
ret[2, 3] = translation.Z;
return ret;
}
/// <summary>
/// 指定された軸に対して指定された角度だけ回転させる3x3行列を返します。縦ベクトルで右手系ならば軸の向きを奥の向きとした場合に時計回りに回転させます。
/// </summary>
/// <param name="axis"></param>
/// <param name="theta"></param>
/// <returns></returns>
public static CvMat GetRotation3D(CvPoint3D64f axis, double theta)
{
// sin theta / 2 ってどっちだ
axis = Normalize(axis);
double cos = Math.Cos(theta / 2.0);
double sin = Math.Sin(theta / 2.0);
return QuaternionToMat3D(cos, axis * (sin));
}
/// <summary>
/// クォータニオンの値から3x3回転行列を求めます．
/// </summary>
/// <param name="w"></param>
/// <param name="V"></param>
/// <returns></returns>
public static CvMat QuaternionToMat3D(double w, CvPoint3D64f V)
{
return QuaternionToMat3D(w, V.X, V.Y, V.Z);
}
/// <summary>
/// 統合後のデータに対して瞬間的な左右反転をなおす
/// </summary>
/// <param name="seqJoints"></param>
public static List<Dictionary<JointType, CvPoint3D64f>> SequentialCorrect(List<Dictionary<JointType, CvPoint3D64f>> seqJoints)
{
CvPoint3D64f prevShoulderVector = new CvPoint3D64f();
CvPoint3D64f prevHipVector = new CvPoint3D64f();
List<Dictionary<JointType, CvPoint3D64f>> res = new List<Dictionary<JointType, CvPoint3D64f>>();
foreach (Dictionary<JointType, CvPoint3D64f> joints in seqJoints)
{
bool reverse = false;
if (joints.ContainsKey(JointType.ShoulderLeft) && joints.ContainsKey(JointType.ShoulderRight))
{
CvPoint3D64f shoulderVector = joints[JointType.ShoulderLeft] - joints[JointType.ShoulderRight];
if (prevShoulderVector == default(CvPoint3D64f))
{
prevShoulderVector = shoulderVector;
}
if (CvEx.Cos(shoulderVector, prevShoulderVector) <= -0.8)
{
reverse = true;
}
}
if (joints.ContainsKey(JointType.HipLeft) && joints.ContainsKey(JointType.HipRight))
{
CvPoint3D64f hipVector = joints[JointType.HipLeft] - joints[JointType.HipRight];
if (prevHipVector == default(CvPoint3D64f))
{
prevHipVector = hipVector;
}
if (CvEx.Cos(hipVector, prevHipVector) <= -0.8)
{
reverse = true;
}
}
if (reverse)
{
Dictionary<JointType, CvPoint3D64f> newJoints = joints.ToDictionary(p => CalcEx.GetMirroredJoint(p.Key), p => p.Value);
res.Add(newJoints);
// reverseした後のただしいベクトルを入れなおしておく
if (joints.ContainsKey(JointType.ShoulderLeft) && joints.ContainsKey(JointType.ShoulderRight))
prevShoulderVector = newJoints[JointType.ShoulderLeft] - newJoints[JointType.ShoulderRight];
if (joints.ContainsKey(JointType.HipLeft) && joints.ContainsKey(JointType.HipRight))
prevHipVector = newJoints[JointType.HipLeft] - newJoints[JointType.HipRight];
}
else
{
res.Add(joints);
}
}
return res;
}
/// <summary>
/// 時系列データの前後を比較して左右反転をなおす
/// </summary>
/// <param name="frameSeq"></param>
public static void SequentialCorrect(FrameSequence frameSeq)
{
CvPoint3D64f[] prevShoulderVectors = new CvPoint3D64f[frameSeq.recordNum];
CvPoint3D64f[] prevHipVectors = new CvPoint3D64f[frameSeq.recordNum];
foreach (Frame frame in frameSeq.Frames)
{
List<SerializableBody> bodies = frame.GetSelectedBodyList(integratedIds: frameSeq.selecteedIntegretedIdList);
if (bodies.Count == 0)
{
continue;
}
CvPoint3D64f[] shoulderVectors = bodies.Select(b =>
(CvPoint3D64f)(b.Joints[JointType.ShoulderLeft].Position.ToCvPoint3D() - b.Joints[JointType.ShoulderRight].Position.ToCvPoint3D())).ToArray();
CvPoint3D64f[] hipVectors = bodies.Select(b =>
(CvPoint3D64f)(b.Joints[JointType.HipLeft].Position.ToCvPoint3D() - b.Joints[JointType.HipRight].Position.ToCvPoint3D())).ToArray();
// 0回目の処理
if (prevShoulderVectors == default(CvPoint3D64f[]) || prevHipVectors == default(CvPoint3D64f[]))
{
prevShoulderVectors = shoulderVectors;
prevHipVectors = hipVectors;
continue;
}
double[] shoulderCosArray = shoulderVectors.Zip(prevShoulderVectors, (cur, prev) => CvEx.Cos(cur, prev)).ToArray();
double[] hipCosArray = hipVectors.Zip(prevHipVectors, (cur, prev) => CvEx.Cos(cur, prev)).ToArray();
for (int no = 0; no < frame.recordNum; no++)
{
if (shoulderCosArray[no] <= -0.8 || hipCosArray[no] <= -0.8)
{
frame.InverseBody(no, integratedId: frameSeq.selecteedIntegretedIdList[no]);
}
}
// Inverseされていた場合には骨の位置関係が変わっているため
prevShoulderVectors = bodies.Select(b =>
(CvPoint3D64f)(b.Joints[JointType.ShoulderLeft].Position.ToCvPoint3D() - b.Joints[JointType.ShoulderRight].Position.ToCvPoint3D())).ToArray();
prevHipVectors = bodies.Select(b =>
(CvPoint3D64f)(b.Joints[JointType.HipLeft].Position.ToCvPoint3D() - b.Joints[JointType.HipRight].Position.ToCvPoint3D())).ToArray();
}
}
/// <summary>
/// 2ベクトルの外積を求めます
/// </summary>
/// <param name="v1"></param>
/// <param name="v2"></param>
/// <returns></returns>
public static CvPoint3D64f Cross(CvPoint3D64f v1, CvPoint3D64f v2)
{
return new CvPoint3D64f(v1.Y * v2.Z - v1.Z * v2.Y, v1.Z * v2.X - v1.X * v2.Z, v1.X * v2.Y - v1.Y * v2.X);
}
public void Correct(FrameSequence frameSeq)
{
// 選択中の統合IDをチェック
List<TrustData> trustDatas = frameSeq.trustData.Where(t => t.integratedBodyId == frameSeq.selecteedIntegretedIdList[0]).ToList();
// generate iteration
List<Tuple<TrustData, int>> iterations = this.GenerateIterationRanges(frameSeq.Frames.Count(), frameSeq.trustData);
foreach (Tuple<TrustData, int> iterationRange in iterations)
{
// set and calcurate pivot
TrustData trustData = iterationRange.Item1;
SerializableBody pivotBody = trustData.GetBody(frameSeq.Frames);
// translate to world coordinate
Dictionary<JointType, CvPoint3D64f> pivotJoints = pivotBody.Joints.ToDictionary(p => p.Key,
p => CvEx.ConvertPoint3D(p.Value.Position.ToCvPoint3D(), frameSeq.ToWorldConversions[trustData.recordIndex]));
CvPoint3D64f pivotBodyRightVector = this.CalcRightChestCrossVector(pivotJoints);
CvPoint3D64f pivotBodyLeftVector = this.CalcLeftChestCrossVector(pivotJoints);
CvPoint3D64f pivotBodyCrossVector = CvEx.Normalize(pivotBodyRightVector + pivotBodyLeftVector);
// z軸との角度 +だったらあっち向いてる
double pivotCrossZCos = CvEx.Cos(pivotBodyCrossVector, new CvPoint3D64f(0, 0, 1));
// ので、反転してたら修正する
if (pivotCrossZCos > 0)
{
pivotBody.InverseJoints();
pivotJoints = pivotBody.Joints.ToDictionary(p => p.Key,
p => CvEx.ConvertPoint3D(p.Value.Position.ToCvPoint3D(), frameSeq.ToWorldConversions[trustData.recordIndex]));
pivotBodyRightVector = this.CalcRightChestCrossVector(pivotJoints);
pivotBodyLeftVector = this.CalcLeftChestCrossVector(pivotJoints);
pivotBodyCrossVector = CvEx.Normalize(pivotBodyRightVector + pivotBodyLeftVector);
}
// 繰り返し範囲の連続indexを生成して回す
IEnumerable<int> continuousRange = this.GenerateContinuousRange(trustData.frameIndex, iterationRange.Item2);
foreach (int frameIndex in continuousRange)
{
// 前のpivotとのベクトルの差が小さいやつを選んでいく投票空間
double[] bodyCos = new double[frameSeq.recordNum];
CvPoint3D64f[] bodyCrosses = new CvPoint3D64f[frameSeq.recordNum];
bool[] validFlags = frameSeq.Frames[frameIndex].GetValidFlags();
for (int recordNo = 0; recordNo < frameSeq.recordNum; recordNo++)
{
// pivotと一致した場合
if (trustData.recordIndex == recordNo && trustData.frameIndex == frameIndex)
{
bodyCos[recordNo] = 1;
bodyCrosses[recordNo] = pivotBodyCrossVector;
continue;
}
SerializableBody body = frameSeq.Frames[frameIndex].GetSelectedBody(recordNo, integratedId: trustData.integratedBodyId);
if (body == null || body == default(SerializableBody) || body.Joints.Count == 0 || validFlags[recordNo] == false)
{
bodyCos[recordNo] = -1;
continue;
}
Dictionary<JointType, CvPoint3D64f> joints = body.Joints.ToDictionary(p => p.Key,
p => CvEx.ConvertPoint3D(p.Value.Position.ToCvPoint3D(), frameSeq.ToWorldConversions[recordNo]));
// 右胸、左胸の外積ベクトル（正規化済み）
CvPoint3D64f rightVector = this.CalcRightChestCrossVector(joints);
CvPoint3D64f leftVector = this.CalcLeftChestCrossVector(joints);
// 前フレームの基準ベクトルとの角度(cos)
double bothCrossAngle = CvEx.Cos(rightVector, leftVector);
double rightPivotAngle = CvEx.Cos(rightVector, pivotBodyCrossVector);
double leftPivotAngle = CvEx.Cos(leftVector, pivotBodyCrossVector);
bool removedFlag = false;
// そもそも骨がなかった場合
if (rightVector == default(CvPoint3D64f))
{
body.RemoveJoints(Utility.RightBody);
removedFlag = true;
}
if (leftVector == default(CvPoint3D64f))
{
body.RemoveJoints(Utility.LeftBody);
removedFlag = true;
}
// 右と左のベクトルが離れすぎてる場合
if (bothCrossAngle <= 0)
{
body.RemoveJoints(Utility.UpperBody);
removedFlag = true;
}
if (removedFlag)
{
bodyCos[recordNo] = -1;
continue;
}
CvPoint3D64f bodyCrossVector = CvEx.Normalize(rightVector + leftVector);
double bodyCrossdiff = CvEx.Cos(bodyCrossVector, pivotBodyCrossVector);
// reverse check
if (bodyCrossdiff <= -0.8)
{
// reverse and update
body.InverseJoints();
joints = body.Joints.ToDictionary(p => p.Key,
p => CvEx.ConvertPoint3D(p.Value.Position.ToCvPoint3D(), frameSeq.ToWorldConversions[recordNo]));
rightVector = this.CalcRightChestCrossVector(joints);
leftVector = this.CalcLeftChestCrossVector(joints);
bodyCrossVector = CvEx.Normalize(rightVector + leftVector);
bodyCrossdiff = CvEx.Cos(bodyCrossVector, pivotBodyCrossVector);
}
// update body angle
bodyCos[recordNo] = bodyCrossdiff;
bodyCrosses[recordNo] = bodyCrossVector;
}
// 前のpivotVectorと似ているほどよい. つまり1に近いほど
int pivotRecordNo = bodyCos.ToList().IndexOf(bodyCos.Max());
// update pivot body vector
pivotBodyCrossVector = bodyCrosses[pivotRecordNo];
}
}
}
/// <summary>
/// 指定した点と直線の距離を返す
/// </summary>
/// <param name="x"></param>
/// <param name="y"></param>
/// <param name="z"></param>
#else
/// <summary>
/// Returns the distance between this line and the specified point
/// </summary>
/// <param name="x"></param>
/// <param name="y"></param>
/// <param name="z"></param>
#endif
public double Distance(double x, double y, double z)
{
CvPoint3D64f p = new CvPoint3D64f(x,y,z);
CvPoint3D64f a = new CvPoint3D64f(X1, Y1, Z1);
CvPoint3D64f b = new CvPoint3D64f(X1+Vx, Y1+Vy, Z1+Vz);
CvPoint3D64f ab = new CvPoint3D64f {X = b.X - a.X, Y = b.Y - a.Y, Z = b.Z - a.Z};
CvPoint3D64f ap = new CvPoint3D64f {X = p.X - a.X, Y = p.Y - a.Y, Z = p.Z - a.Z};
// AB, APを外積 -> 平行四辺形Dの面積
double d = VectorLength(CrossProduct(ab, ap));
// AB間の距離
double l = VertexDistance(a, b);
// 平行四辺形の高さ(垂線)
double h = d / l;
return h;
}
/// <summary>
/// 二つのベクトルの二乗距離を返します
/// </summary>
/// <param name="first"></param>
/// <param name="second"></param>
/// <returns></returns>
public static double GetDistanceSq(CvPoint3D64f first, CvPoint3D64f second)
{
return(GetLengthSq(first - second));
}
/// <summary>
/// ベクトルの外積
/// </summary>
/// <param name="vl"></param>
/// <param name="vr"></param>
/// <returns></returns>
private CvPoint3D64f CrossProduct(CvPoint3D64f vl, CvPoint3D64f vr)
{
CvPoint3D64f ret = new CvPoint3D64f
{
X = (vl.Y*vr.Z) - (vl.Z*vr.Y),
Y = (vl.Z*vr.X) - (vl.X*vr.Z),
Z = (vl.X*vr.Y) - (vl.Y*vr.X)
};
return ret;
}
/// <summary>
/// 射影行列をキャリブレーション行列と回転行列とカメラの同次座標系での位置ベクトルに分解する
/// </summary>
/// <param name="projMatr">入力の3x4の射影行列 P</param>
/// <param name="calibMatr">出力の3x3の内部キャリブレーション行列 K</param>
/// <param name="rotMatr">出力の3x3の外部回転行列 R</param>
/// <param name="posVect">出力の4x1の同次座標系での外部位置ベクトル C</param>
/// <param name="rotMatrX">オプション出力の3x3のx軸周りでの回転行列</param>
/// <param name="rotMatrY">オプション出力の3x3のy軸周りでの回転行列</param>
/// <param name="rotMatrZ">オプション出力の3x3のz軸周りでの回転行列</param>
/// <param name="eulerAngles">オプション出力の回転のオイラー角</param>
#else
/// <summary>
/// Computes projection matrix decomposition
/// </summary>
/// <param name="projMatr">The 3x4 input projection matrix P</param>
/// <param name="calibMatr">The output 3x3 internal calibration matrix K</param>
/// <param name="rotMatr">The output 3x3 external rotation matrix R</param>
/// <param name="posVect">The output 4x1 external homogenious position vector C</param>
/// <param name="rotMatrX">Optional 3x3 rotation matrix around x-axis</param>
/// <param name="rotMatrY">Optional 3x3 rotation matrix around y-axis</param>
/// <param name="rotMatrZ">Optional 3x3 rotation matrix around z-axis</param>
/// <param name="eulerAngles">Optional 3 points containing the three Euler angles of rotation</param>
#endif
public static void DecomposeProjectionMatrix(CvMat projMatr, CvMat calibMatr, CvMat rotMatr, CvMat posVect,
CvMat rotMatrX, CvMat rotMatrY, CvMat rotMatrZ, out CvPoint3D64f eulerAngles)
{
if (projMatr == null)
throw new ArgumentNullException("projMatr");
if (calibMatr == null)
throw new ArgumentNullException("calibMatr");
if (rotMatr == null)
throw new ArgumentNullException("rotMatr");
if (posVect == null)
throw new ArgumentNullException("posVect");
IntPtr rotMatrXPtr = (rotMatrX == null) ? IntPtr.Zero : rotMatrX.CvPtr;
IntPtr rotMatrYPtr = (rotMatrY == null) ? IntPtr.Zero : rotMatrY.CvPtr;
IntPtr rotMatrZPtr = (rotMatrZ == null) ? IntPtr.Zero : rotMatrZ.CvPtr;
eulerAngles = new CvPoint3D64f();
NativeMethods.cvDecomposeProjectionMatrix(projMatr.CvPtr, calibMatr.CvPtr, rotMatr.CvPtr, posVect.CvPtr,
rotMatrXPtr, rotMatrYPtr, rotMatrZPtr, ref eulerAngles);
}
/// <summary>
/// ベクトルの長さ(原点からの距離)
/// </summary>
/// <param name="v"></param>
/// <returns></returns>
private double VectorLength(CvPoint3D64f v)
{
return Math.Sqrt(v.X * v.X + v.Y * v.Y + v.Z * v.Z);
}
public CvMat Solve()
{
// 重心の計算
CvPoint3D64f fromCenter = new CvPoint3D64f();
CvPoint3D64f toCenter = new CvPoint3D64f();
double weightSum = 0;
foreach (var tuple in _correspondings)
{
fromCenter += tuple.Item1 * tuple.Item3;
toCenter += tuple.Item2 * tuple.Item3;
weightSum += tuple.Item3;
}
if (weightSum != 0)
{
fromCenter *= 1.0 / weightSum;
toCenter *= 1.0 / weightSum;
}
// q: quaternion; 4x1
// fn, tn: from[n], to[n]; 3x1
// Xn: (tn - fn, (tn+fn)×[1,0,0], (tn+fn)×[0,1,0], (tn+fn)×[0,0,1]); 3x4
// M: Σi(Xi^t Wi Xi); 4x4
// Wi: I; 3x3
// J = q^t Mq -> min
// 最小二乗法
using (CvMat M = new CvMat(4, 4, MatrixType.F64C1))
{
M.Zero();
foreach (var tuple in _correspondings)
{
// 重心からの距離
CvPoint3D64f fromVector = tuple.Item1 - fromCenter;
CvPoint3D64f toVector = tuple.Item2 - toCenter;
using (CvMat Xi = new CvMat(3, 4, MatrixType.F64C1))
{
CvPoint3D64f diff = toVector - fromVector;
CvPoint3D64f sum = toVector + fromVector;
CvPoint3D64f second = CvEx.Cross(sum, new CvPoint3D64f(1, 0, 0));
CvPoint3D64f third = CvEx.Cross(sum, new CvPoint3D64f(0, 1, 0));
CvPoint3D64f fourth = CvEx.Cross(sum, new CvPoint3D64f(0, 0, 1));
CvEx.FillCvMat(Xi, new double[] { diff.X, second.X, third.X, fourth.X, diff.Y, second.Y, third.Y, fourth.Y, diff.Z, second.Z, third.Z, fourth.Z });
using (CvMat XiTranspose = Xi.Transpose())
using (CvMat addend = XiTranspose * Xi * tuple.Item3)
{
M.Add(addend, M);
}
}
}
using (CvMat MTemp = CvEx.CloneCvMat(M))
using (CvMat eVals = new CvMat(4, 1, MatrixType.F64C1))
using (CvMat eVects = new CvMat(4, 4, MatrixType.F64C1))
{
//Cv.EigenVV(MTemp, eVects, eVals, 0.000001);
Cv.SVD(MTemp, eVals, eVects, null, SVDFlag.U_T | SVDFlag.ModifyA);
int minEIndex = 3;
/*
if (false)
{
double minE = double.MaxValue;
for (int i = 0; i < 4; i++)
{
double eVal = Math.Abs(eVals[i, 0]);
if (eVal < minE)
{
minE = eVal;
minEIndex = i;
}
}
}
*/
CvMat ret = new CvMat(4, 4, MatrixType.F64C1);
ret.Zero();
ret[3, 3] = 1.0;
CvMat rotateConversion;
/*
if (false)
{
// こっちの変換はほとんど恒等のときに誤差が大きい
CvMat q = eVects.GetRow(minEIndex);
// クォータニオンから回転ベクトルを計算
double theta = Math.Acos(q[0, 0]) * 2;
double sin = Math.Sin(theta / 2);
CvPoint3D64f rot = new CvPoint3D64f(q[0, 1] / sin * theta, q[0, 2] / sin * theta, q[0, 3] / sin * theta);
// 回転ベクトルから回転行列を計算
ret.GetSubRect(out rotateConversion, new CvRect(0, 0, 3, 3));
using (CvMat rotVector = new CvMat(1, 3, MatrixType.F64C1))
{
rotVector[0, 0] = rot.X;
rotVector[0, 1] = rot.Y;
rotVector[0, 2] = rot.Z;
Cv.Rodrigues2(rotVector, rotateConversion);
}
}
else
{*/
CvMat rotationMat = CvEx.QuaternionToMat3D(eVects[minEIndex, 0], eVects[minEIndex, 1], eVects[minEIndex, 2], eVects[minEIndex, 3]);
ret.GetSubRect(out rotateConversion, new CvRect(0, 0, 3, 3));
rotationMat.Copy(rotateConversion);
//}
// 回転後の重心の並進成分の計算
using (CvMat fromCenterMat = new CvMat(3, 1, MatrixType.F64C1))
{
CvEx.FillCvMat(fromCenterMat, new double[] { fromCenter.X, fromCenter.Y, fromCenter.Z });
using (CvMat rotFromCenterMat = rotateConversion * fromCenterMat)
{
CvPoint3D64f rotFromCenter = new CvPoint3D64f(rotFromCenterMat[0, 0], rotFromCenterMat[1, 0], rotFromCenterMat[2, 0]);
CvPoint3D64f offset = toCenter - rotFromCenter;
ret[0, 3] = offset.X;
ret[1, 3] = offset.Y;
ret[2, 3] = offset.Z;
return ret;
}
}
}
}
}
/// <summary>
/// 2点間(2ベクトル)の距離
/// </summary>
/// <param name="p1"></param>
/// <param name="p2"></param>
/// <returns></returns>
private double VertexDistance(CvPoint3D64f p1, CvPoint3D64f p2)
{
return Math.Sqrt((p2.X - p1.X) * (p2.X - p1.X) +
(p2.Y - p1.Y) * (p2.Y - p1.Y) +
(p2.Z - p1.Z) * (p2.Z - p1.Z));
}
public void PutPoint(CvPoint3D64f from, CvPoint3D64f to, double weight)
{
_correspondings.Add(new Tuple<CvPoint3D64f, CvPoint3D64f, double>(from, to, weight));
}
/// <summary>
/// 二つのベクトルの二乗距離を返します
/// </summary>
/// <param name="first"></param>
/// <param name="second"></param>
/// <returns></returns>
public static double GetDistanceSq(CvPoint3D64f first, CvPoint3D64f second)
{
return GetLengthSq(first - second);
}
/// <summary>
/// 2ベクトルの外積を求めます
/// </summary>
/// <param name="v1"></param>
/// <param name="v2"></param>
/// <returns></returns>
public static CvPoint3D64f Cross(CvPoint3D64f v1, CvPoint3D64f v2)
{
return(new CvPoint3D64f(v1.Y * v2.Z - v1.Z * v2.Y, v1.Z * v2.X - v1.X * v2.Z, v1.X * v2.Y - v1.Y * v2.X));
}
/// <summary>
/// ベクトルのユークリッド距離における長さを返します
/// </summary>
/// <param name="vector"></param>
/// <returns></returns>
public static double GetLengthSq(CvPoint3D64f vector)
{
return vector.X * vector.X + vector.Y * vector.Y + vector.Z * vector.Z;
}
/// <summary>
/// クォータニオンの値から3x3回転行列を求めます．
/// </summary>
/// <param name="w"></param>
/// <param name="V"></param>
/// <returns></returns>
public static CvMat QuaternionToMat3D(double w, CvPoint3D64f V)
{
return(QuaternionToMat3D(w, V.X, V.Y, V.Z));
}
/// <summary>
/// ベクトルのユークリッド距離における長さを返します
/// </summary>
/// <param name="vector"></param>
/// <returns></returns>
public static double GetLength(CvPoint3D64f vector)
{
return Math.Sqrt(vector.X * vector.X + vector.Y * vector.Y + vector.Z * vector.Z);
}
public static UserSegmentation[] Identification(FrameSequence frameseq, double maxDistance)
{
if (frameseq.Segmentations.Any(seg => seg == null))
{
throw new InvalidOperationException("ユーザトラッキングデータがセグメンテーションされていません");
}
HashSet <Tuple <RecordAndUser, RecordAndUser> > contemporaryList = new HashSet <Tuple <RecordAndUser, RecordAndUser> >();
for (int recordNo = 0; recordNo < frameseq.recordNum; recordNo++)
{
IEnumerable <MotionData> record = frameseq.GetMotionDataSequence(recordNo);
int frameIndex = 0;
foreach (MotionData motionData in record)
{
IList <ulong> users = motionData.bodies.ToList().Select(b => b.TrackingId).ToList();
foreach (var tuple in users.SelectMany(u => users.Select(v => new Tuple <RecordAndUser, RecordAndUser>(new RecordAndUser(recordNo, u), new RecordAndUser(recordNo, v)))))
{
contemporaryList.Add(tuple);
}
frameIndex++;
}
}
DateTime beginTime = frameseq.startTime;
DateTime endTime = frameseq.endTime;
double frequency = frameseq.frameRate;
TimeSpan increment = new TimeSpan((long)(10000000 / frequency));
long totalCount = (endTime.Ticks - beginTime.Ticks) / increment.Ticks;
long totalIndex = 0;
Dictionary <Tuple <RecordAndUser, RecordAndUser>, List <double> > distanceListMatrix = new Dictionary <Tuple <RecordAndUser, RecordAndUser>, List <double> >();
foreach (Frame frame in frameseq.Frames)
{
// 現在の時刻での各レコードの各ユーザの各骨格の絶対座標を求める
Dictionary <ulong, Dictionary <JointType, CvPoint3D64f> >[] absPositions = new Dictionary <ulong, Dictionary <JointType, CvPoint3D64f> > [frameseq.recordNum];
for (int recordNo = 0; recordNo < frameseq.recordNum; recordNo++)
{
Dictionary <ulong, Dictionary <JointType, CvPoint3D64f> > recordUserPositions = new Dictionary <ulong, Dictionary <JointType, CvPoint3D64f> >();
foreach (SerializableBody body in frame.GetBodyList(recordNo))
{
Dictionary <JointType, CvPoint3D64f> userPositions = new Dictionary <JointType, CvPoint3D64f>();
if (body.Joints == null)
{
continue;
}
foreach (var jointPair in body.Joints)
{
CvPoint3D64f posInCamera = jointPair.Value.Position.ToCvPoint3D();
CvPoint3D64f posInWorld = CvEx.ConvertPoint3D(posInCamera, frameseq.ToWorldConversions[recordNo]);
userPositions[jointPair.Key] = posInWorld;
}
recordUserPositions[body.TrackingId] = userPositions;
}
absPositions[recordNo] = recordUserPositions;
}
// 現在の時刻で各レコード間のユーザ間の距離を求める
for (int i = 0; i < frameseq.recordNum; i++)
{
if (absPositions[i] == null)
{
continue;
}
for (int j = i + 1; j < frameseq.recordNum; j++)
{
if (absPositions[j] == null)
{
continue;
}
foreach (var userJoint1 in absPositions[i])
{
RecordAndUser recordUser1 = new RecordAndUser(i, userJoint1.Key);
foreach (var userJoint2 in absPositions[j])
{
RecordAndUser recordUser2 = new RecordAndUser(j, userJoint2.Key);
double distanceNormal = getDistance(userJoint1.Value, userJoint2.Value, false);
double distanceMirrored = getDistance(userJoint1.Value, userJoint2.Value, true);
double distance = Math.Min(distanceNormal, distanceMirrored);
Tuple <RecordAndUser, RecordAndUser> key = new Tuple <RecordAndUser, RecordAndUser>(recordUser1, recordUser2);
List <double> distanceList;
if (!distanceListMatrix.TryGetValue(key, out distanceList))
{
distanceListMatrix[key] = distanceList = new List <double>();
}
distanceList.Add(distance);
}
}
}
}
totalIndex++;
}
// 中央値で集計して小さい順に並べる
Dictionary <Tuple <RecordAndUser, RecordAndUser>, double> distanceMatrix = distanceListMatrix.ToDictionary(p => p.Key, p => CalcEx.GetMedian(p.Value));
List <Tuple <RecordAndUser, RecordAndUser, double> > neighborList = (
from x in distanceMatrix
orderby x.Value
select new Tuple <RecordAndUser, RecordAndUser, double>(x.Key.Item1, x.Key.Item2, x.Value)
).ToList();
IdentificationSet <RecordAndUser> identificationSet = new IdentificationSet <RecordAndUser>();
// 同一判定をする
foreach (var neighbor in neighborList)
{
if (neighbor.Item3 > maxDistance)
{
identificationSet.Add(neighbor.Item1);
identificationSet.Add(neighbor.Item2);
continue;
}
IList <RecordAndUser> recordUsers1 = identificationSet.GetEquivalentElements(neighbor.Item1);
IList <RecordAndUser> recordUsers2 = identificationSet.GetEquivalentElements(neighbor.Item2);
// 同フレーム内にいるか判定
bool contemporary = (
from ru1 in recordUsers1
from ru2 in recordUsers2
select new Tuple <RecordAndUser, RecordAndUser>(ru1, ru2)).Any(pair => contemporaryList.Contains(pair));
if (!contemporary)
{
// 同フレーム内にいなければ同一視
identificationSet.MakeEquivalent(neighbor.Item1, neighbor.Item2);
}
}
// 番号を圧縮
identificationSet.CompactIdentificationNumber();
// 新しいセグメンテーション番号を与える
UserSegmentation[] ret = Enumerable.Range(0, frameseq.recordNum).Select(i => new UserSegmentation()).ToArray();
for (int recordNo = 0; recordNo < frameseq.recordNum; recordNo++)
{
foreach (var pair in frameseq.Segmentations[recordNo].Conversions)
{
int frameIndex = pair.Key;
Dictionary <ulong, int> newConversions = new Dictionary <ulong, int>();
foreach (var conv in pair.Value)
{
int ident = identificationSet.ConvertToIdentificationNumber(new RecordAndUser(recordNo, conv.Key));
newConversions[conv.Key] = ident;
}
ret[recordNo].Conversions[frameIndex] = newConversions;
}
ret[recordNo].fixNumUsers();
}
return(ret);
}
/// <summary>
/// 3次元点(縦ベクトル)を4x4行列で(左掛けで)変換します。
/// </summary>
/// <param name="from">変換前の三次元点の座標</param>
/// <param name="conversion">64bit浮動小数点1チャンネル4x4行列</param>
/// <returns></returns>
public static CvPoint3D64f ConvertPoint3D(CvPoint3D64f from, CvMat conversion)
{
double x = from.X * conversion[0, 0] + from.Y * conversion[0, 1] + from.Z * conversion[0, 2] + conversion[0, 3];
double y = from.X * conversion[1, 0] + from.Y * conversion[1, 1] + from.Z * conversion[1, 2] + conversion[1, 3];
double z = from.X * conversion[2, 0] + from.Y * conversion[2, 1] + from.Z * conversion[2, 2] + conversion[2, 3];
return new CvPoint3D64f(x, y, z);
using (CvMat fromMat = new CvMat(4, 1, MatrixType.F64C1))
{
fromMat[0, 0] = from.X;
fromMat[1, 0] = from.Y;
fromMat[2, 0] = from.Z;
fromMat[3, 0] = 1;
using (CvMat worldPoint = conversion * fromMat)
{
return new CvPoint3D64f(worldPoint[0, 0], worldPoint[1, 0], worldPoint[2, 0]);
}
}
}
public static extern void cvRQDecomp3x3(IntPtr matrixM, IntPtr matrixR, IntPtr matrixQ,
IntPtr matrixQx, IntPtr matrixQy, IntPtr matrixQz, ref CvPoint3D64f eulerAngles);
/// <summary>
///
/// </summary>
/// <param name="point"></param>
#else
/// <summary>
///
/// </summary>
/// <param name="point"></param>
#endif
public CvPoint3D64f PerpendicularFoot(CvPoint3D64f point)
{
return PerpendicularFoot(point.X, point.Y, point.Z);
}
/// <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="point"></param>
#else
/// <summary>
/// Returns the distance between this line and the specified point
/// </summary>
/// <param name="point"></param>
#endif
public double Distance(CvPoint3D64f point)
{
return Distance(point.X, point.Y, point.Z);
}
