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