/// <summary>
/// 骨のデータを蓄積する
/// </summary>
/// <param name="joints"></param>
public void StoreBoneLength(Dictionary <JointType, Joint> joints)
{
Dictionary <JointType, Joint> validJoints = Utility.GetValidJoints(joints);
Joint firstJoint, secondJoint;
Bone boneKey;
List <double> lengthVal;
foreach (Bone bone in bones)
{
if (validJoints.TryGetValue(bone.Item1, out firstJoint) && validJoints.TryGetValue(bone.Item2, out secondJoint))
{
double lengthSq = CvEx.GetDistanceSq(firstJoint.Position.ToCvPoint3D(), secondJoint.Position.ToCvPoint3D());
boneKey = Tuple.Create(firstJoint.JointType, secondJoint.JointType);
if (boneLengthSqLog.TryGetValue(boneKey, out lengthVal))
{
lengthVal.Add(lengthSq);
}
else
{
boneLengthSqLog[boneKey] = new List <double>()
{
lengthSq
};
}
}
}
}
/// <summary>
/// あるフレーム範囲の全ユーザの点群を出力する
/// </summary>
/// <param name="sender"></param>
/// <param name="e"></param>
private void ExportFrameRangeUserPointClouds_Click(object sender, RoutedEventArgs e)
{
List <Frame> frames = frameSequence.Slice(startIndex, endIndex);
for (int i = 0; i < frameSequence.recordNum; i++)
{
List <List <float[]> > pointsSequence = new List <List <float[]> >();
string path = System.IO.Path.Combine(Environment.GetFolderPath(Environment.SpecialFolder.DesktopDirectory), i.ToString() + "_UserPointsCloud.dump");
for (int frameNo = 0; frameNo < frames.Count(); frameNo++)
{
// フレームレート半分
if (frameNo % 2 == 0)
{
continue;
}
Frame frame = frames[frameNo];
List <Tuple <CvPoint3D64f, CvColor> > colors = frameSequence.LocalCoordinateMappers[i].GetUserColorPoints(frame.DepthMatList[i], frame.ColorMatList[i], frame.UserMatList[i]);
colors = colors.Select(t => Tuple.Create(CvEx.ConvertPoint3D(t.Item1, frameSequence.ToWorldConversions[i]), t.Item2)).ToList();
List <float[]> dumpColors = colors.Select(t => new float[] { (float)t.Item1.X, (float)t.Item1.Y, (float)t.Item1.Z, t.Item2.R, t.Item2.G, t.Item2.B }).ToList();
// 点の数1/10
dumpColors = dumpColors.Where((fs, index) => index % 10 == 0).ToList();
pointsSequence.Add(dumpColors);
}
Utility.SaveToBinary(pointsSequence.ToArray(), path);
}
}
/// <summary>
/// 次元を指定するコンストラクタ
/// </summary>
/// <param name="dimension"></param>
public MemorySavingLeastSquare(int dimension)
{
_dimension = dimension;
_left = CvEx.InitCvMat(dimension, dimension, MatrixType.F64C1);
_right = CvEx.InitCvMat(dimension, 1, MatrixType.F64C1);
_left.Zero();
_right.Zero();
}
CvMat getDepthUndistortMatImage(out float maxValue, out float minValue, CvMat src, float?center)
{
CvMat ret = CvEx.InitCvMat(src, MatrixType.U8C1);
List <double> values = new List <double>();
for (int y = 0; y < src.Rows; y++)
{
for (int x = 0; x < src.Cols; x++)
{
if (!_undistortion.CameraStruct.IsInFocalLength(x, y))
{
continue;
}
int i = y * src.Cols + x;
float value = src.DataArraySingle[i];
// if (value < 2)
// continue;
values.Add(value);
}
}
float max = 0;
float min = float.MaxValue;
if (values.Count >= 1)
{
max = (float)CalcEx.GetNth(values, (int)(values.Count * 0.99));
min = (float)CalcEx.GetNth(values, (int)(values.Count * 0.01));
}
max = (max - 1) * 1.5f + 1;
min = (min - 1) * 1.5f + 1;
if (center.HasValue)
{
float maxRange = Math.Max(max - center.Value, center.Value - min);
max = center.Value + maxRange;
min = center.Value - maxRange;
}
//max = 1.05f;
//min = 0.95f;
maxValue = max;
minValue = min;
if (max == min)
{
max += 0.5f;
min -= 0.5f;
}
for (int i = 0; i < src.Rows * src.Cols; i++)
{
float value = src.DataArraySingle[i];
float output = 255 * (value - min) / (max - min);
ret.DataArrayByte[i] = (byte)output;
}
return(ret);
}
/// <summary>
/// 最小二乗法で解を得ます
/// </summary>
/// <returns></returns>
public double[] Solve()
{
CvMat invLeft = CvEx.InitCvMat(_left);
_left.Invert(invLeft, InvertMethod.Cholesky);
CvMat ret = invLeft * _right;
return(ret.Select(r => r.Val0).ToArray());
}
void drawUndistortedCornerFrame(CvMat displayMat, CvPoint2D32f[] corners, CvSize boardSize)
{
CvMat cornerMat = new CvMat(1, corners.Length, MatrixType.F32C2);
CvEx.FillCvMat(cornerMat, corners.Select(x => new CvScalar(x.X, x.Y)).ToList());
CvMat undistMat = CvEx.InitCvMat(cornerMat);
Cv.UndistortPoints(cornerMat, undistMat, this.UndistortionData.CameraStruct.CreateCvMat(), this.UndistortionData.DistortStruct.CreateCvMat(true), null, this.UndistortionData.CameraStruct.CreateCvMat());
CvEx.DrawChessboardCornerFrame(displayMat, boardSize, undistMat.Select(x => new CvPoint2D32f(x.Val0, x.Val1)).ToArray(), new CvScalar(216, 216, 216));
}
void putImage(CvMat mat, PixelFormat format)
{
if (!this.Dispatcher.CheckAccess())
{
this.Dispatcher.Invoke(new Action <CvMat, PixelFormat>(putImage), mat, format);
return;
}
CvEx.GetBmpFromMat(ref _bmp, mat, format);
imageTrack.Source = _bmp;
}
/// <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 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));
}
private void button1_Click_2(object sender, RoutedEventArgs e)
{
//以下修正
MotionDataHandler handler;
string path;
if (openMotionData(out handler, out path))
{
CvMat resultMat = null;
int length = handler.FrameCount;
IEnumerable <CvMat> depthImages;
Utility.LoadImages(handler.GetDepthImagePaths(), out depthImages);
foreach (CvMat depthMat in depthImages)
{
CvSize depthUserSize = new CvSize(depthMat.Cols, depthMat.Rows);
CvEx.InitCvMat(ref resultMat, depthMat, MatrixType.U8C3);
resultMat.Zero();
double avgDepth = depthMat.Select(v => v.Val0).Where(v => v != 0).Average();
double pDepth = CvEx.Get2DSubPixel(depthMat, new CvPoint2D32f(_undistortion.CameraStruct.PrincipalX, _undistortion.CameraStruct.PrincipalY), 0) ?? 0;
List <double>[] diffs = Enumerable.Range(0, depthUserSize.Width).Select(x => new List <double>()).ToArray();
unsafe
{
short *depthArr = depthMat.DataInt16;
for (int y = 0; y < depthUserSize.Height; y++)
{
int offset = y * depthUserSize.Width;
for (int x = 0; x < depthUserSize.Width - 1; x++)
{
short l = depthArr[offset + x];
short r = depthArr[offset + x + 1];
if (l != 0 && r != 0)
{
double ll = Math.Log(l);
double rl = Math.Log(r);
diffs[x].Add(ll - rl);
}
}
}
}
double[] median = diffs.Select(x => x.Count > 0 ? CalcEx.GetMedian(x) : 0).ToArray();
double max = median.Select(x => Math.Abs(x)).Max();
for (int x = 0; x < depthUserSize.Width; x++)
{
resultMat.DrawLine(new CvPoint(x, 0), new CvPoint(x, resultMat.Rows), new CvScalar(Math.Max(median[x] / max * 255, 0), Math.Max(-median[x] / max * 255, 0), 0));
}
resultMat.PutText(avgDepth.ToString("0.00000"), new CvPoint(0, 20), new CvFont(FontFace.HersheyPlain, 1, 1), new CvScalar(255, 255, 255));
resultMat.PutText(pDepth.ToString("0.00000"), new CvPoint(0, 40), new CvFont(FontFace.HersheyPlain, 1, 1), new CvScalar(255, 255, 255));
putImage(resultMat, PixelFormats.Rgb24);
}
}
}
/// <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>
/// 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>
/// 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>
/// Jointから絶対座標を引くDictionary二つの間の距離を求めます
/// </summary>
/// <param name="jointWithAbsPosition1">骨格ごとの絶対座標1</param>
/// <param name="jointWithAbsPosition2">骨格ごとの絶対座標2</param>
/// <param name="mirrored">一方の骨格を左右反転した結果で距離を求めるかどうか</param>
/// <returns></returns>
static double getDistance(IDictionary <JointType, CvPoint3D64f> jointWithAbsPosition1, IDictionary <JointType, CvPoint3D64f> jointWithAbsPosition2, bool mirrored)
{
IEnumerable <JointType> joints2Alt = jointWithAbsPosition2.Keys.Select(j => mirrored ? CalcEx.GetMirroredJoint(j) : j);
List <JointType> intersect = jointWithAbsPosition1.Keys.Intersect(joints2Alt).ToList();
List <double> distanceSqList = new List <double>();
foreach (JointType joint in intersect)
{
JointType jointAlt = mirrored ? CalcEx.GetMirroredJoint(joint) : joint;
distanceSqList.Add(CvEx.GetDistanceSq(jointWithAbsPosition1[joint], jointWithAbsPosition2[jointAlt]));
}
// 中央値の平方根
return(Math.Sqrt(CalcEx.GetMedian(distanceSqList)));
}
public Dictionary <JointType, CvPoint3D64f> InterpolateSkeleton(MotionData prevFrame, MotionData nextFrame, Dictionary <JointType, Joint> prevJoints, Dictionary <JointType, Joint> nextJoints,
DateTime time, CvMat ToWorldConversion)
{
double prevWeight;
if (prevFrame.TimeStamp >= nextFrame.TimeStamp)
{
prevWeight = 1;
}
else
{
prevWeight = (time - prevFrame.TimeStamp).TotalSeconds / (nextFrame.TimeStamp - prevFrame.TimeStamp).TotalSeconds;
}
double nextWeight = 1.0 - prevWeight;
if (prevJoints == null || nextJoints == null)
{
return(null);
}
Dictionary <JointType, CvPoint3D64f> prevData = prevJoints.ToDictionary(p => p.Key, p => (CvPoint3D64f)p.Value.Position.ToCvPoint3D());
Dictionary <JointType, CvPoint3D64f> nextData = nextJoints.ToDictionary(p => p.Key, p => (CvPoint3D64f)p.Value.Position.ToCvPoint3D());
List <JointType> joints = prevData.Keys.Union(nextData.Keys).ToList();
Dictionary <JointType, CvPoint3D64f> ret = new Dictionary <JointType, CvPoint3D64f>();
foreach (JointType joint in joints)
{
CvPoint3D64f prevPos, nextPos;
bool prevFound = prevData.TryGetValue(joint, out prevPos);
bool nextFound = nextData.TryGetValue(joint, out nextPos);
if ((prevFound && nextFound))
{
ret[joint] = CvEx.ConvertPoint3D(prevPos * prevWeight + nextPos * nextWeight, ToWorldConversion);
}
else if (_omitWhenDataLack)
{
if (prevFound)
{
ret[joint] = CvEx.ConvertPoint3D(prevPos, ToWorldConversion);
}
else if (nextFound)
{
ret[joint] = CvEx.ConvertPoint3D(nextPos, ToWorldConversion);
}
}
}
return(ret);
}
/// <summary>
/// あるフレームの点群を出力する
/// </summary>
/// <param name="sender"></param>
/// <param name="e"></param>
private void ExportFrameRangePointClouds_Click(object sender, RoutedEventArgs e)
{
List <Frame> frames = frameSequence.Slice(startIndex, endIndex);
for (int i = 0; i < frameSequence.recordNum; i++)
{
List <float[]>[] pointsSequence = new List <float[]> [frames.Count()];
string path = System.IO.Path.Combine(Environment.GetFolderPath(Environment.SpecialFolder.DesktopDirectory), i.ToString() + "_PointsCloud.dump");
for (int frameNo = 0; frameNo < frames.Count(); frameNo++)
{
Frame frame = frames[frameNo];
List <Tuple <CvPoint3D64f, CvColor> > colors = frameSequence.LocalCoordinateMappers[i].DepthColorMatToRealPoints(frame.DepthMatList[i], frame.ColorMatList[i]);
colors = colors.Select(t => Tuple.Create(CvEx.ConvertPoint3D(t.Item1, frameSequence.ToWorldConversions[i]), t.Item2)).ToList();
List <float[]> dumpColors = colors.Select(t => new float[] { (float)t.Item1.X, (float)t.Item1.Y, (float)t.Item1.Z, t.Item2.R, t.Item2.G, t.Item2.B }).ToList();
pointsSequence[frameNo] = dumpColors;
}
Utility.SaveToBinary(pointsSequence, path);
}
}
/// <summary>
/// 足の長さを統計情報をもとに正規化する
/// </summary>
/// <param name="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>
/// 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>
/// 統計値から有用な範囲の骨のみを残す
/// </summary>
/// <param name="joints"></param>
/// <returns></returns>
public Dictionary <JointType, Joint> FilterBonesByStatistics(Dictionary <JointType, Joint> joints)
{
// TODO crossから腕とか削除するやつ
Dictionary <JointType, Joint> validJoints = Utility.GetValidJoints(joints);
Dictionary <JointType, Joint> result = validJoints.CloneDeep();
Dictionary <JointType, bool> adaptJoints = new Dictionary <JointType, bool>();
Joint firstJoint, secondJoint;
Bone boneKey;
foreach (Bone bone in bones)
{
if (validJoints.TryGetValue(bone.Item1, out firstJoint) && validJoints.TryGetValue(bone.Item2, out secondJoint))
{
boneKey = Tuple.Create(firstJoint.JointType, secondJoint.JointType);
double lengthSq = CvEx.GetDistanceSq(firstJoint.Position.ToCvPoint3D(), secondJoint.Position.ToCvPoint3D());
if (boneLengthSqStatistics[bone].IsValidBone(lengthSq))
{
adaptJoints[firstJoint.JointType] = true;
adaptJoints[secondJoint.JointType] = true;
}
}
}
bool armDup = this.CheckArmDuplication(validJoints);
if (armDup)
{
foreach (JointType jointType in this.arms)
{
adaptJoints[jointType] = false;
}
}
foreach (JointType jointType in adaptJoints.Keys)
{
if (adaptJoints[jointType] == false)
{
result.Remove(jointType);
}
}
return(result);
}
/// <summary>
/// 反転しているJointsをPivot基準に修正する
/// </summary>
/// <param name="joints"></param>
/// <param name="conversion"></param>
/// <returns></returns>
public Dictionary <JointType, Joint> CorrectMirrorJoint(Dictionary <JointType, Joint> joints, CvMat conversion)
{
HashSet <JointType> mirroredPivotKeys = new HashSet <JointType>(pivot.Keys.Select(j => CalcEx.GetMirroredJoint(j)));
List <JointType> availableKeys = pivot.Keys.Where(j => mirroredPivotKeys.Contains(j)).ToList(); // pivotの左右反転して共通なキー
Dictionary <JointType, CvPoint3D64f> absJoints = joints.ToDictionary(p => p.Key, p => CvEx.ConvertPoint3D(p.Value.Position.ToCvPoint3D(), conversion));
Dictionary <JointType, CvPoint3D64f> absMirroredJoints = absJoints.ToDictionary(p => CalcEx.GetMirroredJoint(p.Key), p => p.Value);
List <JointType> keysNormal = availableKeys.Intersect(absJoints.Keys).ToList();
List <JointType> keysMirrored = availableKeys.Intersect(absMirroredJoints.Keys).ToList();
if (keysNormal.Count > 0 && keysMirrored.Count > 0)
{
double avg1 = keysNormal.Select(j => CvEx.GetDistanceSq(pivot[j], absJoints[j])).Average();
double avg2 = keysMirrored.Select(j => CvEx.GetDistanceSq(pivot[j], absMirroredJoints[j])).Average();
// mirroredのほうが似てる場合
if (avg2 < avg1)
{
return(joints.ToDictionary(p => CalcEx.GetMirroredJoint(p.Key), p => p.Value));
}
}
return(joints);
}
public static void Test()
{
Random rand = new Random();
int dim = rand.Next(2, 7);
MemorySavingLeastSquare msls = new MemorySavingLeastSquare(dim);
double[] ans = Enumerable.Range(0, dim).Select(x => rand.NextDouble() * 200 - 100).ToArray();
List <double[]> leftArr = new List <double[]>();
List <double> rightArr = new List <double>();
for (int k = 0; k < 200; k++)
{
double[] left = Enumerable.Range(0, dim).Select(x => rand.NextDouble() * 200 - 100).ToArray();
double right = Enumerable.Range(0, dim).Select(i => left[i] * ans[i]).Sum() + rand.NextDouble() * 10 - 5;
msls.PutPoint(left, right);
leftArr.Add(left);
rightArr.Add(right);
}
double[] ans1 = CvEx.Solve(leftArr, rightArr, InvertMethod.Svd);
double[] ans2 = msls.Solve();
System.Windows.MessageBox.Show(string.Join(", ", ans.Select(x => x.ToString("0.00000"))) + "\n" + string.Join(", ", ans1.Select(x => x.ToString("0.00000"))) + "\n" + string.Join(", ", ans2.Select(x => x.ToString("0.00000"))));
}
/// <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="frameSeq"></param>
public static void Correct(FrameSequence frameSeq)
{
List <OrderTuple> orders = new List <OrderTuple>();
foreach (var pair in frameSeq.Frames.Select((frame, index) => Tuple.Create(frame, index)))
{
for (int recordNo = 0; recordNo < pair.Item1.recordNum; recordNo++)
{
orders.Add(new OrderTuple(pair.Item1.Time, recordNo, pair.Item2));
}
}
// 前フレームのユーザと対応するBodyを保持する
Dictionary <int, SerializableBody>[] prevUserBodyMapArray = new Dictionary <int, SerializableBody> [frameSeq.recordNum];
for (int i = 0; i < frameSeq.recordNum; i++)
{
prevUserBodyMapArray[i] = new Dictionary <int, SerializableBody>();
}
SerializableBody prevUserBody;
orders = orders.OrderBy(x => x.Timestamp.Ticks).ToList();
SkeletonInterpolator interp = new SkeletonInterpolator(0.5, true);
foreach (OrderTuple tuple in orders)
{
Frame currFrame = frameSeq.Frames[tuple.FrameIndex];
const long maxInterval = (long)(10000000 * 0.1);
DateTime prev = new DateTime(tuple.Timestamp.Ticks - maxInterval);
IEnumerable <int> users = currFrame.GetBodyList(tuple.RecordIndex).Select(b => b.integratedId);
foreach (int user in users)
{
SerializableBody currBody = currFrame.GetSelectedBody(tuple.RecordIndex, integratedId: user);
// 前のBodyと比較して同じ場合は処理をスキップする
if (prevUserBodyMapArray[tuple.RecordIndex].TryGetValue(user, out prevUserBody) && prevUserBody.Joints != null && currBody.Joints != null)
{
double avgDistanceSq = CalcBodyDistanceSq(prevUserBody.Joints, currBody.Joints).Values.Average();
if (avgDistanceSq == 0.0)
{
//currFrame.DeleteBody(tuple.RecordIndex, integratedId: user);
continue;
}
}
prevUserBodyMapArray[tuple.RecordIndex][user] = currBody;
Dictionary <JointType, CvPoint3D64f> prevJoints = interp.IntegrateSkeleton(prev, user, frameSeq);
if (prevJoints != null && currBody.Joints != null)
{
Dictionary <JointType, CvPoint3D64f> currJoints = currBody.Joints.ToDictionary(p => p.Key, p => (CvPoint3D64f)p.Value.Position.ToCvPoint3D());
HashSet <JointType> mirroredPrevKeys = new HashSet <JointType>(prevJoints.Keys.Select(j => CalcEx.GetMirroredJoint(j)));
if (currJoints != null && prevJoints != null)
{
var absJoints = currJoints.ToDictionary(p => p.Key, p => CvEx.ConvertPoint3D(p.Value, frameSeq.ToWorldConversions[tuple.RecordIndex]));
var absMirroredJoints = absJoints.ToDictionary(p => CalcEx.GetMirroredJoint(p.Key), p => p.Value);
var availableKeys = prevJoints.Keys.Where(j => mirroredPrevKeys.Contains(j)).ToList();
var keysNormal = availableKeys.Intersect(absJoints.Keys).ToList();
var keysMirrored = availableKeys.Intersect(absMirroredJoints.Keys).ToList();
if (keysNormal.Count > 0 && keysMirrored.Count > 0)
{
double avg1 = keysNormal.Select(j => CvEx.GetDistanceSq(prevJoints[j], absJoints[j])).Average();
double avg2 = keysMirrored.Select(j => CvEx.GetDistanceSq(prevJoints[j], absMirroredJoints[j])).Average();
if (avg2 < avg1)
{
currFrame.InverseBody(tuple.RecordIndex, integratedId: user);
}
}
}
}
}
}
}
private void buttonScalingScore_Click(object sender, RoutedEventArgs e)
{
int cols, rows;
double horizLength, vertLength;
if (!parseChessboardParameters(out cols, out rows, out horizLength, out vertLength))
{
return;
}
// 以下改造
MotionDataHandler handler;
string path;
if (openMotionData(out handler, out path))
{
CvMat displayMat1 = null;
CvMat displayMat3 = null;
CvMat displayMat4 = null;
CvMat gray = null;
int length = handler.FrameCount;
if (length == 0)
{
return;
}
CvSize boardSize = new CvSize(cols, rows);
CvSize imageSize = new CvSize();
List <Tuple <double, double> > pairs = new List <Tuple <double, double> >();
CvPoint2D32f[] lastCorners = null;
IEnumerable <CvMat> colorImages, depthImages;
Utility.LoadImages(handler.GetColorImagePaths(), out colorImages);
Utility.LoadImages(handler.GetDepthImagePaths(), out depthImages);
var images = colorImages.Zip(depthImages, (first, second) => Tuple.Create(first, second));
foreach (Tuple <CvMat, CvMat> imagePair in images)
{
CvMat imageMat = imagePair.Item1;
CvMat depthMat = imagePair.Item2;
if (displayMat4 == null)
{
displayMat4 = CvEx.InitCvMat(imageMat);
}
imageSize = new CvSize(imageMat.Cols, imageMat.Rows);
CvPoint2D32f[] corners;
int count;
CvEx.InitCvMat(ref gray, imageMat, MatrixType.U8C1);
imageMat.CvtColor(gray, ColorConversion.RgbToGray);
if (gray.FindChessboardCorners(boardSize, out corners, out count, ChessboardFlag.AdaptiveThresh))
{
CvEx.CloneCvMat(ref displayMat1, imageMat);
CvTermCriteria criteria = new CvTermCriteria(50, 0.01);
gray.FindCornerSubPix(corners, count, new CvSize(3, 3), new CvSize(-1, -1), criteria);
CvPoint3D32f?[] cornerPoints = new CvPoint3D32f?[corners.Length];
for (int j = 0; j < corners.Length; j++)
{
CvPoint2D32f corner = corners[j];
double? value = CalcEx.BilateralFilterDepthMatSinglePixel(corner, depthMat, 100, 4, 9);
if (value.HasValue)
{
cornerPoints[j] = new CvPoint3D32f(corner.X, corner.Y, value.Value);
}
}
for (int x = 0; x < cols; x++)
{
for (int y = 0; y < rows; y++)
{
if (!cornerPoints[x + y * cols].HasValue)
{
continue;
}
CvPoint3D32f point1 = cornerPoints[x + y * cols].Value;
CvPoint3D64f undistortPoint1 = this.UndistortionData.GetRealFromScreenPos(point1, imageSize);
foreach (var offset in new[] { new { X = 1, Y = 0, D = horizLength }, new { X = 0, Y = 1, D = vertLength } })
{
int dx = x + offset.X;
int dy = y + offset.Y;
if (dx >= cols || dy >= rows)
{
continue;
}
if (!cornerPoints[dx + dy * cols].HasValue)
{
continue;
}
CvPoint3D32f point2 = cornerPoints[dx + dy * cols].Value;
CvPoint3D64f undistortPoint2 = this.UndistortionData.GetRealFromScreenPos(point2, imageSize);
double distance = Math.Sqrt(CvEx.GetDistanceSq(undistortPoint1, undistortPoint2));
double scale = distance / offset.D;
CvColor color = CalcEx.HSVtoRGB(Math.Max(0, Math.Min(300, scale * 600 - 450)), scale, 2 - scale);
displayMat4.DrawLine((int)point1.X, (int)point1.Y, (int)point2.X, (int)point2.Y, new CvScalar(color.R, color.G, color.B), 1, LineType.AntiAlias);
pairs.Add(new Tuple <double, double>(distance, offset.D));
}
}
}
CvEx.DrawChessboardCornerFrame(displayMat1, boardSize, corners, new CvScalar(64, 128, 64));
displayMat1.DrawChessboardCorners(boardSize, corners, true);
lastCorners = corners;
putImage(displayMat1, PixelFormats.Rgb24);
}
else
{
CvEx.CloneCvMat(ref displayMat3, imageMat);
putImage(displayMat3, PixelFormats.Rgb24);
}
}
CvMat displayMat2 = CvEx.InitCvMat(displayMat1);
displayMat1.Undistort2(displayMat2, this.UndistortionData.CameraStruct.CreateCvMat(), this.UndistortionData.DistortStruct.CreateCvMat(true));
if (lastCorners != null)
{
drawUndistortedCornerFrame(displayMat2, lastCorners, boardSize);
}
displayMat2.PutText(string.Format("Min: {0}", pairs.Min(x => x.Item1 / x.Item2)), new CvPoint(20, 20), new CvFont(FontFace.HersheyPlain, 1, 1), new CvScalar(255, 255, 255));
displayMat2.PutText(string.Format("Max: {0}", pairs.Max(x => x.Item1 / x.Item2)), new CvPoint(20, 40), new CvFont(FontFace.HersheyPlain, 1, 1), new CvScalar(255, 255, 255));
displayMat2.PutText(string.Format("Avg: {0}", pairs.Average(x => x.Item1 / x.Item2)), new CvPoint(20, 60), new CvFont(FontFace.HersheyPlain, 1, 1), new CvScalar(255, 255, 255));
displayMat2.PutText(string.Format("Med: {0}", CalcEx.GetMedian(pairs.Select(x => x.Item1 / x.Item2).ToList())), new CvPoint(20, 80), new CvFont(FontFace.HersheyPlain, 1, 1), new CvScalar(255, 255, 255));
putImage(displayMat4, PixelFormats.Rgb24);
displayLabels();
}
}
private void buttonTest0_Click(object sender, RoutedEventArgs e)
{
int cols, rows;
double horizLength, vertLength;
if (!parseChessboardParameters(out cols, out rows, out horizLength, out vertLength))
{
return;
}
// 以下修正
MotionDataHandler handler;
string path;
if (openMotionData(out handler, out path))
{
CvMat displayMat1 = null;
CvMat displayMat3 = null;
CvMat displayMat4 = null;
CvMat gray = null;
int length = handler.FrameCount;
if (length == 0)
{
return;
}
CvSize boardSize = new CvSize(cols, rows);
CvSize imageSize = new CvSize();
double minVarDistance2d = double.MaxValue;
IEnumerable <CvMat> colorImages, depthImages;
Utility.LoadImages(handler.GetColorImagePaths(), out colorImages);
Utility.LoadImages(handler.GetDepthImagePaths(), out depthImages);
var images = colorImages.Zip(depthImages, (first, second) => Tuple.Create(first, second));
foreach (Tuple <CvMat, CvMat> imagePair in images)
{
CvMat imageMat = imagePair.Item1;
CvMat depthMat = imagePair.Item2;
if (displayMat4 == null)
{
displayMat4 = CvEx.InitCvMat(imageMat);
}
imageSize = new CvSize(imageMat.Cols, imageMat.Rows);
CvSize depthUserSize = new CvSize(depthMat.Cols, depthMat.Rows);
CvPoint2D32f[] corners;
int count;
CvEx.InitCvMat(ref gray, imageMat, MatrixType.U8C1);
imageMat.CvtColor(gray, ColorConversion.RgbToGray);
if (gray.FindChessboardCorners(boardSize, out corners, out count, ChessboardFlag.AdaptiveThresh))
{
CvEx.CloneCvMat(ref displayMat1, imageMat);
CvTermCriteria criteria = new CvTermCriteria(50, 0.01);
gray.FindCornerSubPix(corners, count, new CvSize(3, 3), new CvSize(-1, -1), criteria);
CvPoint3D32f?[] cornerPoints = new CvPoint3D32f?[corners.Length];
for (int j = 0; j < corners.Length; j++)
{
CvPoint2D32f corner = new CvPoint2D32f(corners[j].X - 10, corners[j].Y - 10);
double? value = CvEx.Get2DSubPixel(depthMat, corner, 0);
if (value.HasValue)
{
double depth = UndistortionData.UndistortDepth(corner.X, corner.Y, value.Value, depthUserSize);
cornerPoints[j] = new CvPoint3D32f(corner.X, corner.Y, depth);
}
}
List <double> distance2dList = new List <double>();
for (int x = 0; x < cols; x++)
{
for (int y = 0; y < rows; y++)
{
if (!cornerPoints[x + y * cols].HasValue)
{
continue;
}
int nextX = x + 1;
if (nextX < cols)
{
if (!cornerPoints[nextX + y * cols].HasValue)
{
continue;
}
CvPoint3D32f point = cornerPoints[x + y * cols].Value;
CvPoint3D32f nextPoint = cornerPoints[nextX + y * cols].Value;
distance2dList.Add(Math.Sqrt(Math.Pow(point.X - nextPoint.X, 2) + Math.Pow(point.Y - nextPoint.Y, 2)));
}
int nextY = y + 1;
if (nextY < rows)
{
if (!cornerPoints[x + nextY * cols].HasValue)
{
continue;
}
CvPoint3D32f point = cornerPoints[x + y * cols].Value;
CvPoint3D32f nextPoint = cornerPoints[x + nextY * cols].Value;
distance2dList.Add(Math.Sqrt(Math.Pow(point.X - nextPoint.X, 2) + Math.Pow(point.Y - nextPoint.Y, 2)));
}
}
}
if (distance2dList.Count >= 2)
{
double stdevDistance2d = CalcEx.GetStdDev(distance2dList);
displayMat1.PutText(string.Format("{0:0.00}/{1:0.00}", stdevDistance2d, minVarDistance2d), new CvPoint(0, 20), new CvFont(FontFace.HersheyPlain, 1, 1), new CvScalar(255, 255, 0));
double avgDepth = cornerPoints.Where(p => p.HasValue).Select(p => p.Value.Z).Average();
for (int x = 0; x < cols; x++)
{
for (int y = 0; y < rows; y++)
{
if (!cornerPoints[x + y * cols].HasValue)
{
continue;
}
CvPoint3D32f point = cornerPoints[x + y * cols].Value;
displayMat1.PutText((point.Z - avgDepth).ToString("0.00"), new CvPoint((int)point.X, (int)point.Y), new CvFont(FontFace.HersheyPlain, 0.6, 0.6), new CvScalar(255, 0, 0));
displayMat1.PutText(((point.Z - avgDepth) / avgDepth * 100).ToString("0.000"), new CvPoint((int)point.X, (int)point.Y + 12), new CvFont(FontFace.HersheyPlain, 0.6, 0.6), new CvScalar(0, 255, 255));
}
}
//displayMat1.DrawChessboardCorners(boardSize, corners, true);
if (stdevDistance2d < minVarDistance2d)
{
minVarDistance2d = stdevDistance2d;
CvEx.CloneCvMat(ref displayMat4, displayMat1);
}
//System.Threading.Thread.Sleep(500);
}
putImage(displayMat1, PixelFormats.Rgb24);
}
else
{
CvEx.CloneCvMat(ref displayMat3, imageMat);
putImage(displayMat3, PixelFormats.Rgb24);
}
}
putImage(displayMat4, PixelFormats.Rgb24);
displayLabels();
}
}
/// <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();
}
}
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);
}
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);
}
}
}
}
}
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);
}
private void buttonCalibrateScaleOffset_Click(object sender, RoutedEventArgs e)
{
int cols, rows;
double horizLength, vertLength;
if (!parseChessboardParameters(out cols, out rows, out horizLength, out vertLength))
{
return;
}
// 以下改造
MotionDataHandler handler;
string path;
if (openMotionData(out handler, out path))
{
CvMat displayMat1 = null;
CvMat displayMat3 = null;
CvMat gray = null;
if (ProgressData.DoAction(progress =>
{
int length = handler.FrameCount;
if (length == 0)
{
return;
}
progress.InitProgress("Find Chessboard...", length * 2);
CvSize boardSize = new CvSize(cols, rows);
List <CvPoint3D32f?[]> list = new List <CvPoint3D32f?[]>();
CvSize imageSize = new CvSize();
CvPoint2D32f[] lastCorners = null;
IEnumerable <CvMat> colorImages, depthImages;
Utility.LoadImages(handler.GetColorImagePaths(), out colorImages);
Utility.LoadImages(handler.GetDepthImagePaths(), out depthImages);
var images = colorImages.Zip(depthImages, (first, second) => Tuple.Create(first, second));
foreach (Tuple <CvMat, CvMat> imagePair in images)
{
progress.CurrentValue++;
CvMat imageMat = imagePair.Item1;
CvMat depthMat = imagePair.Item2;
imageSize = new CvSize(imageMat.Cols, imageMat.Rows);
CvPoint2D32f[] corners;
int count;
CvEx.InitCvMat(ref gray, imageMat, MatrixType.U8C1);
imageMat.CvtColor(gray, ColorConversion.RgbToGray);
if (gray.FindChessboardCorners(boardSize, out corners, out count, ChessboardFlag.AdaptiveThresh))
{
CvEx.CloneCvMat(ref displayMat1, imageMat);
CvTermCriteria criteria = new CvTermCriteria(50, 0.01);
gray.FindCornerSubPix(corners, count, new CvSize(3, 3), new CvSize(-1, -1), criteria);
CvPoint3D32f?[] cornerPoints = new CvPoint3D32f?[corners.Length];
for (int j = 0; j < corners.Length; j++)
{
CvPoint2D32f corner = corners[j];
double?value = CalcEx.BilateralFilterDepthMatSinglePixel(corner, depthMat, 100, 4, 9);
if (value.HasValue)
{
cornerPoints[j] = new CvPoint3D32f(corner.X, corner.Y, value.Value);
}
}
list.Add(cornerPoints);
CvEx.DrawChessboardCornerFrame(displayMat1, boardSize, corners, new CvScalar(64, 128, 64));
displayMat1.DrawChessboardCorners(boardSize, corners, true);
lastCorners = corners;
//putImage(displayMat1, PixelFormats.Bgr24);
}
else
{
CvEx.CloneCvMat(ref displayMat3, imageMat);
//putImage(displayMat3, PixelFormats.Bgr24);
}
}
progress.SetProgress("Scale Offset Calibrating...", length);
this.UndistortionData.CalibrateRealScaleAndOffset(list, cols, rows, horizLength, vertLength, imageSize);
CvMat displayMat2 = CvEx.InitCvMat(displayMat1);
displayMat1.Undistort2(displayMat2, this.UndistortionData.CameraStruct.CreateCvMat(), this.UndistortionData.DistortStruct.CreateCvMat(true));
if (lastCorners != null)
{
drawUndistortedCornerFrame(displayMat2, lastCorners, boardSize);
}
displayMat2.PutText(string.Format("XScale: {0}", this.UndistortionData.XScale), new CvPoint(20, 20), new CvFont(FontFace.HersheyPlain, 1, 1), new CvScalar(255, 255, 255));
displayMat2.PutText(string.Format("YScale: {0}", this.UndistortionData.YScale), new CvPoint(20, 40), new CvFont(FontFace.HersheyPlain, 1, 1), new CvScalar(255, 255, 255));
displayMat2.PutText(string.Format("Zoffset: {0}", this.UndistortionData.ZOffset), new CvPoint(20, 60), new CvFont(FontFace.HersheyPlain, 1, 1), new CvScalar(255, 255, 255));
putImage(displayMat2, PixelFormats.Bgr24);
}, "Calibrate Scale Offset", true))
{
displayLabels();
}
}
}