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();
}
}
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();
}
}
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);
}
}
}
