/// <summary>
/// Bodyの関節を反転する
/// </summary>
public void InverseJoints()
{
Dictionary <JointType, Joint> newJoints = new Dictionary <JointType, Joint>();
Dictionary <JointType, Point> newColorPoints = new Dictionary <JointType, Point>();
Dictionary <JointType, Point> newDepthPoints = new Dictionary <JointType, Point>();
foreach (JointType key in Enum.GetValues(typeof(JointType)))
{
JointType newKey = CalcEx.GetMirroredJoint(key);
if (this.Joints.ContainsKey(key))
{
newJoints[newKey] = this.Joints[key];
}
if (this.colorSpacePoints.ContainsKey(key))
{
newColorPoints[newKey] = this.colorSpacePoints[key];
}
if (this.depthSpacePoints.ContainsKey(key))
{
newDepthPoints[newKey] = this.depthSpacePoints[key];
}
}
this.Joints = newJoints;
this.colorSpacePoints = newColorPoints;
this.depthSpacePoints = newDepthPoints;
}
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);
}
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>
/// 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)));
}
/// <summary>
/// 統計情報を計算し格納する
/// z = 0.904をデフォルトとする、このとき中央値から65%を網羅できる
/// </summary>
/// <param name="z">標準正規分布表のZ</param>
public void CalcMedianBoneRange(double z = 0.904)
{
foreach (Bone bone in this.bones)
{
List <double> data = this.boneLengthSqLog[bone];
int skip = (int)(data.Count() * 0.3);
int take = data.Count() - skip * 2;
data.Sort();
// 上下30%を削除
data = data.Skip(skip).Take(take).ToList();
double median = CalcEx.GetMedian(data);
double average = data.Average();
double std = Math.Abs(Math.Sqrt(data.Select(d => Math.Pow(d - average, 2)).Sum() / (data.Count() - 1)));
double minLength = median - std * z;
double maxLength = median + std * z;
BoneStatistics bs = new BoneStatistics(minLength, maxLength, median, average, std);
this.boneLengthSqStatistics.Add(bone, bs);
}
}
/// <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);
}
private void buttonFlatDepthCalib_Click(object sender, RoutedEventArgs e)
{
// 改造
string path1;
MotionDataHandler handler1;
{
if (openMotionData(out handler1, out path1))
{
IEnumerable <CvMat> depthImages1 = null;
Utility.LoadImages(handler1.GetDepthImagePaths(), out depthImages1);
List <double> errorVarLog = new List <double>();
CvMat resultMat = null;
if (ProgressData.DoAction(progress =>
{
int length = handler1.FrameCount;
progress.InitProgress("Calculating...", length);
DepthUndistortionLinearCalibrator undistort = new DepthUndistortionLinearCalibrator(this.UndistortionData.CameraStruct, 1);
CvMat mat = null;
foreach (CvMat depthMat in depthImages1)
{
progress.CurrentValue++;
CalcEx.SmoothDepthStep(ref mat, depthMat, 19);
undistort.PutDepthImage(ref resultMat, mat, _undistortion);
viewDepthUndistionMat(resultMat, depthMat);
}
this.UndistortionData.SetUndistortionDepthMat(undistort.GetUndistortCoefMat(), path1);
}, "Flat Depth Calib", true))
{
displayLabels();
viewDepthUndistionMat(this.UndistortionData.UndistortionDepthMat);
}
}
}
}
/// <summary>
/// 1チャンネル画像のサブピクセル値を返します．座標の小数点以下が0の場合にその座標の値と等しくなります
/// </summary>
/// <param name="mat">1チャンネル画像</param>
/// <param name="point">座標</param>
/// <param name="invalidValue">無効とみなされる値</param>
/// <returns></returns>
public static double?Get2DSubPixel(CvMat mat, CvPoint2D64f point, double invalidValue)
{
int preX = (int)Math.Floor(point.X);
int preY = (int)Math.Floor(point.Y);
int postX = preX == point.X ? preX : preX + 1;
int postY = preY == point.Y ? preY : preY + 1;
double fracX = point.X - preX;
double fracY = point.Y - preY;
if (postX >= mat.Cols || postY >= mat.Rows)
{
return(null);
}
double depth00 = mat[preY, preX];
double depth01 = mat[preY, postX];
double depth10 = mat[postY, preX];
double depth11 = mat[postY, postX];
if (depth00 == invalidValue || depth01 == invalidValue || depth10 == invalidValue || depth11 == invalidValue)
{
return(null);
}
return(CalcEx.Lerp(CalcEx.Lerp(depth00, depth01, fracX), CalcEx.Lerp(depth10, depth11, fracX), fracY));
}
/// <summary>
/// フレーム範囲における座標変換行列を骨格情報から計算する
/// </summary>
/// <param name="frames"></param>
public static List <CvMat> GetConvMatrixFromBoneFrameSequence(FrameSequence frameSeq, int startIndex, int endIndex)
{
Dictionary <Tuple <int, int>, int> cooccurenceCount = new Dictionary <Tuple <int, int>, int>();
IEnumerable <Frame> frames = frameSeq.Frames.Skip(startIndex).Take(endIndex);
List <SerializableBody> bodies;
foreach (Frame frame in frames)
{
if (frameSeq.Segmentations == null)
{
bodies = frame.GetSelectedBodyList(originalIds: frameSeq.selectedOriginalIdList);
}
else
{
bodies = frame.GetSelectedBodyList(integratedIds: frameSeq.selecteedIntegretedIdList);
}
if (bodies.Count() != frame.recordNum)
{
continue;
}
bool[] validFlags = frame.GetValidFlags();
for (int i = 0; i < frame.recordNum; i++)
{
for (int j = i + 1; j < frame.recordNum; j++)
{
if (validFlags[i] && validFlags[j] == false)
{
continue;
}
Dictionary <JointType, Joint> joint1 = Utility.GetValidJoints(bodies[i].Joints);
Dictionary <JointType, Joint> joint2 = Utility.GetValidJoints(bodies[j].Joints);
foreach (JointType jointType in joint1.Keys.Intersect(joint2.Keys))
{
Tuple <int, int> key = new Tuple <int, int>(i, j);
int count;
if (!cooccurenceCount.TryGetValue(key, out count))
{
count = 0;
}
cooccurenceCount[key] = count + 1;
}
}
}
}
// 依存関係のツリーを作る
int baseRecordIndex;
Dictionary <int, int> dependencies;
// とりあえず先頭フレームのレコード数にしてるけど、プロジェクトとかが持つべき値
if (!CalcEx.GetDependencyTree(frameSeq.recordNum, cooccurenceCount, list => list.Sum(), out baseRecordIndex, out dependencies))
{
System.Windows.MessageBox.Show("骨格が他のレコードと同時に映っているフレームがないレコードがあるため計算できませんでした");
return(frameSeq.ToWorldConversions);
}
else
{
Dictionary <int, ICoordConversion3D> conversionsPerDependencyKey = new Dictionary <int, ICoordConversion3D>();
foreach (Frame frame in frameSeq.Frames)
{
if (frameSeq.Segmentations == null)
{
bodies = frame.GetSelectedBodyList(originalIds: frameSeq.selectedOriginalIdList);
}
else
{
bodies = frame.GetSelectedBodyList(integratedIds: frameSeq.selecteedIntegretedIdList);
}
if (bodies.Count() != frame.recordNum)
{
continue;
}
List <CvSize> depthUsersizeList = frame.DepthUserSize;
bool[] validFlags = frame.GetValidFlags();
foreach (KeyValuePair <int, int> dependencyPair in CalcEx.EnumerateDependencyPairs(baseRecordIndex, dependencies))
{
if (validFlags[dependencyPair.Key] && validFlags[dependencyPair.Value] == false)
{
continue;
}
// 変換計算用オブジェクトを拾ってくる
ICoordConversion3D conv;
if (!conversionsPerDependencyKey.TryGetValue(dependencyPair.Key, out conv))
{
conversionsPerDependencyKey[dependencyPair.Key] = conv = new CoordRotTransConversion();
}
Dictionary <JointType, Joint> joint1 = Utility.GetValidJoints(bodies[dependencyPair.Key].Joints);
Dictionary <JointType, Joint> joint2 = Utility.GetValidJoints(bodies[dependencyPair.Value].Joints);
foreach (JointType jointType in joint1.Keys.Intersect(joint2.Keys))
{
CvPoint3D64f camPoint1 = joint1[jointType].Position.ToCvPoint3D();
CvPoint3D64f camPoint2 = joint2[jointType].Position.ToCvPoint3D();
// それぞれのカメラ座標系におけるそれぞれの対応点をセットに入れる
conv.PutPoint(camPoint1, camPoint2, 1);
}
}
}
List <CvMat> convList = frameSeq.ToWorldConversions;
foreach (KeyValuePair <int, int> dependencyPair in CalcEx.EnumerateDependencyPairs(baseRecordIndex, dependencies))
{
CvMat relConv = conversionsPerDependencyKey[dependencyPair.Key].Solve();
CvMat baseConversion = convList[dependencyPair.Value];
convList[dependencyPair.Key] = baseConversion * relConv;
}
return(convList);
}
}
public 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>
/// カメラ座標系における骨格位置の水平垂直の標準偏差の積を求めます。
/// </summary>
/// <param name="joints"></param>
/// <param name="undist"></param>
/// <param name="imageSize"></param>
/// <returns></returns>
public static double GetSkeletonCamPosVariance(ICollection <CvPoint3D64f> cameraPos)
{
// return CalcEx.GetVariance(absPositions, p => p.X) + CalcEx.GetVariance(absPositions, p => p.Y) + CalcEx.GetVariance(absPositions, p => p.Z);
return(CalcEx.GetStdDev(cameraPos, p => p.X) * CalcEx.GetStdDev(cameraPos, p => p.Y));
}
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();
}
}
}
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();
}
}
/// <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);
}
}
}
}
}
}
}
/// <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 Dictionary <JointType, CvPoint3D64f> IntegrateSkeleton(DateTime time, int userInt, FrameSequence frameSeq)
{
List <CvMat> ToWorldConversions = frameSeq.ToWorldConversions;
List <CameraIntrinsics> cameraInfo = frameSeq.CameraInfo;
List <UserSegmentation> segm = frameSeq.Segmentations;
Dictionary <JointType, CvPoint3D64f>[] jointsArr = new Dictionary <JointType, CvPoint3D64f> [frameSeq.recordNum];
Dictionary <JointType, CvPoint3D64f>[] pivotCandidate = new Dictionary <JointType, CvPoint3D64f> [frameSeq.recordNum];
double[] reliabilityArr = new double[frameSeq.recordNum];
double[] weightArr = new double[frameSeq.recordNum];
for (int recordNo = 0; recordNo < frameSeq.recordNum; recordNo++)
{
MotionData prevData = frameSeq.GetPrevData(recordNo, time);
MotionData nextData = frameSeq.GetNextData(recordNo, time);
if (prevData == null || nextData == null || prevData.bodies.Length * nextData.bodies.Length == 0 || !prevData.isValid || !nextData.isValid)
{
jointsArr[recordNo] = null;
reliabilityArr[recordNo] = 0;
weightArr[recordNo] = 0;
continue;
}
SerializableBody prevBody = prevData.bodies.Where(b => b.integratedId == userInt).FirstOrDefault();
SerializableBody nextBody = nextData.bodies.Where(b => b.integratedId == userInt).FirstOrDefault();
if (prevBody == null || nextBody == null || prevBody.Equals(default(SerializableBody)) || nextBody.Equals(default(SerializableBody)))
{
jointsArr[recordNo] = null;
reliabilityArr[recordNo] = 0;
weightArr[recordNo] = 0;
continue;
}
// 統計情報によるフィルタリング
Dictionary <JointType, Joint> prevJoints;
Dictionary <JointType, Joint> nextJoints;
if (prevBody.Joints == null || nextBody.Joints == null)
{
continue;
}
// pivotが設定されてるとき、つまり、本番統合のとき
if (pivot != null)
{
// mirror矯正
prevJoints = this.CorrectMirrorJoint(prevBody.Joints, ToWorldConversions[recordNo]);
nextJoints = this.CorrectMirrorJoint(nextBody.Joints, ToWorldConversions[recordNo]);
// next pivot候補つめつめ
pivotCandidate[recordNo] = Utility.GetValidJoints(nextJoints).ToDictionary(p => p.Key, p => (CvPoint3D64f)p.Value.Position.ToCvPoint3D());
}
else
{
prevJoints = prevBody.Joints;
nextJoints = nextBody.Joints;
}
// 統計情報があるとき
if (frameSeq.BodyStat != null)
{
prevJoints = frameSeq.BodyStat.FilterBonesByStatistics(prevJoints);
nextJoints = frameSeq.BodyStat.FilterBonesByStatistics(nextJoints);
}
else
{
prevJoints = Utility.GetValidJoints(prevBody.Joints);
nextJoints = Utility.GetValidJoints(nextBody.Joints);
}
jointsArr[recordNo] = this.InterpolateSkeleton(prevData, nextData, prevJoints, nextJoints, time, ToWorldConversions[recordNo]);
reliabilityArr[recordNo] = this.GetSkeletonReliability(prevData, nextData, prevJoints, nextJoints, time, cameraInfo[recordNo]);
weightArr[recordNo] = this.GetVarianceWeight(prevData, nextData, prevJoints, nextJoints, time);
}
// pivot更新
pivot = new Dictionary <JointType, CvPoint3D64f>();
foreach (var candidate in pivotCandidate)
{
if (candidate != null && pivot.Count <= candidate.Count)
{
pivot = candidate;
}
}
double maxWeight = weightArr.Max();
double[] modifiedReliabilityList = weightArr.Select(w => Math.Max(0, (w / maxWeight) - _weightBase)).Zip(reliabilityArr, (a, b) => a * b).ToArray();
var keys = jointsArr.Where(j => j != null).SelectMany(j => j.Keys).Distinct().ToList();
if (maxWeight == 0)
{
return(null);
}
return(CalcEx.LinearMedianSkeletons(jointsArr, modifiedReliabilityList));
}
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();
}
}
