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);
}
}
}
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();
}
}
void viewDepthUndistionMat(CvMat src, CvMat depthMat)
{
if (src == null)
{
return;
}
double pX = this.UndistortionData.CameraStruct.PrincipalX;
double pY = this.UndistortionData.CameraStruct.PrincipalY;
int focalX = (int)(this.UndistortionData.CameraStruct.FocalX / 2);
int focalY = (int)(this.UndistortionData.CameraStruct.FocalY / 2);
if (src.ElemChannels == 3)
{
CvMat quadratic = CvEx.InitCvMat(src, MatrixType.F32C1);
CvMat scale = CvEx.InitCvMat(src, MatrixType.F32C1);
CvMat offset = CvEx.InitCvMat(src, MatrixType.F32C1);
src.Split(quadratic, scale, offset, null);
float maxQuadratic, minQuadratic;
float maxScale, minScale;
float maxOffset, minOffset;
using (CvMat displayMatQuadratic = getDepthUndistortMatImage(out maxQuadratic, out minQuadratic, quadratic, 0f))
using (CvMat displayMatScale = getDepthUndistortMatImage(out maxScale, out minScale, scale, 1f))
using (CvMat displayMatOffset = getDepthUndistortMatImage(out maxOffset, out minOffset, offset, null))
{
using (CvMat black = CvEx.InitCvMat(src, MatrixType.U8C1))
{
CvMat total = CvEx.InitCvMat(src, MatrixType.U8C3);
black.Zero();
total.Merge(displayMatQuadratic, displayMatScale, displayMatOffset, null);
total.PutText(string.Format("RangeQuadatic: {0:0.000} to {1:0.000}", minQuadratic, maxQuadratic), new CvPoint(0, 20), new CvFont(FontFace.HersheyPlain, 1, 1), new CvScalar(255, 255, 255));
total.PutText(string.Format("RangeScale: {0:0.000} to {1:0.000}", minScale, maxScale), new CvPoint(0, 40), new CvFont(FontFace.HersheyPlain, 1, 1), new CvScalar(255, 255, 255));
total.PutText(string.Format("RangeOffset: {0:0.000} to {1:0.000}", minOffset, maxOffset), new CvPoint(0, 60), new CvFont(FontFace.HersheyPlain, 1, 1), new CvScalar(255, 255, 255));
total.DrawLine(new CvPoint((int)pX, 0), new CvPoint((int)pX, total.Rows), new CvScalar(255, 255, 0));
total.DrawLine(new CvPoint(0, (int)pY), new CvPoint(total.Cols, (int)pY), new CvScalar(255, 255, 0));
total.DrawEllipse(new CvPoint((int)pX, (int)pY), new CvSize(focalX, focalY), 0, 0, 360, new CvScalar(255, 0, 0));
putImage(total, PixelFormats.Bgr24);
}
}
}
else if (src.ElemChannels == 2)
{
CvMat scale = CvEx.InitCvMat(src, MatrixType.F32C1);
CvMat offset = CvEx.InitCvMat(src, MatrixType.F32C1);
src.Split(scale, offset, null, null);
float maxScale, minScale;
float maxOffset, minOffset;
using (CvMat displayMatScale = getDepthUndistortMatImage(out maxScale, out minScale, scale, 1f))
using (CvMat displayMatOffset = getDepthUndistortMatImage(out maxOffset, out minOffset, offset, null))
{
using (CvMat black = CvEx.InitCvMat(src, MatrixType.U8C1))
{
CvMat total = CvEx.InitCvMat(src, MatrixType.U8C3);
black.Zero();
total.Merge(black, displayMatScale, displayMatOffset, null);
total.PutText(string.Format("RangeScale: {0:0.000} to {1:0.000}", minScale, maxScale), new CvPoint(0, 20), new CvFont(FontFace.HersheyPlain, 1, 1), new CvScalar(255, 255, 255));
total.PutText(string.Format("RangeOffset: {0:0.000} to {1:0.000}", minOffset, maxOffset), new CvPoint(0, 40), new CvFont(FontFace.HersheyPlain, 1, 1), new CvScalar(255, 255, 255));
total.DrawLine(new CvPoint((int)pX, 0), new CvPoint((int)pX, total.Rows), new CvScalar(255, 255, 0));
total.DrawLine(new CvPoint(0, (int)pY), new CvPoint(total.Cols, (int)pY), new CvScalar(255, 255, 0));
total.DrawEllipse(new CvPoint((int)pX, (int)pY), new CvSize(focalX, focalY), 0, 0, 360, new CvScalar(255, 0, 0));
putImage(total, PixelFormats.Bgr24);
}
}
}
else if (src.ElemChannels == 1)
{
float max, min;
using (CvMat displayMat = getDepthUndistortMatImage(out max, out min, src, null))
{
CvMat total = CvEx.InitCvMat(src, MatrixType.U8C3);
total.Merge(displayMat, displayMat, displayMat, null);
total.PutText(string.Format("Range: {0:0.000} to {1:0.000}", min, max), new CvPoint(0, 20), new CvFont(FontFace.HersheyPlain, 1, 1), new CvScalar(255, 255, 0));
if (depthMat != null)
{
double?pDepth = CvEx.Get2DSubPixel(depthMat, new CvPoint2D32f(pX, pY), 0);
if (pDepth.HasValue)
{
total.PutText(pDepth.Value.ToString("0"), new CvPoint((int)pX, (int)pY), new CvFont(FontFace.HersheyPlain, 1, 1), new CvScalar(255, 255, 0));
}
total.DrawLine(new CvPoint((int)pX, 0), new CvPoint((int)pX, total.Rows), new CvScalar(255, 255, 0));
total.DrawLine(new CvPoint(0, (int)pY), new CvPoint(total.Cols, (int)pY), new CvScalar(255, 255, 0));
total.DrawEllipse(new CvPoint((int)pX, (int)pY), new CvSize(focalX, focalY), 0, 0, 360, new CvScalar(255, 0, 0));
}
putImage(total, PixelFormats.Bgr24);
}
}
}