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));
}
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 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 buttonCameraCalibration_Click(object sender, RoutedEventArgs e)
{
int cols, rows;
double horizLength, vertLength;
if (!parseChessboardParameters(out cols, out rows, out horizLength, out vertLength))
{
return;
}
int imageNum;
if (!int.TryParse(textCalibrationCameraIteration.Text, out imageNum) || imageNum <= 0)
{
System.Windows.MessageBox.Show(string.Format("キャリブレーションに使用するイメージ数が不正です: {0}", textCalibrationCameraIteration.Text));
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 <CvPoint2D32f[]> list = new List <CvPoint2D32f[]>();
CvSize imageSize = new CvSize();
CvPoint2D32f[] lastCorners = null;
IEnumerable <CvMat> colorImages;
Utility.LoadImages(handler.GetColorImagePaths(), out colorImages);
foreach (CvMat imageMat in colorImages)
{
progress.CurrentValue++;
using (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(20, 0.1);
gray.FindCornerSubPix(corners, count, new CvSize(11, 11), new CvSize(-1, -1), criteria);
list.Add(corners);
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("Calibrating...", length);
this.UndistortionData.CalibrateCamera(list, cols, rows, (horizLength + vertLength) / 2, imageSize, imageNum, path);
CvMat displayMat2 = CvEx.InitCvMat(displayMat1);
displayMat1.Undistort2(displayMat2, this.UndistortionData.CameraStruct.CreateCvMat(), this.UndistortionData.DistortStruct.CreateCvMat(true));
if (lastCorners != null)
{
drawUndistortedCornerFrame(displayMat2, lastCorners, boardSize);
}
putImage(displayMat2, PixelFormats.Bgr24);
}, "Camera Calib", true))
{
displayLabels();
}
}
}
