/// <summary> /// 次元を指定するコンストラクタ /// </summary> /// <param name="dimension"></param> public MemorySavingLeastSquare(int dimension) { _dimension = dimension; _left = CvEx.InitCvMat(dimension, dimension, MatrixType.F64C1); _right = CvEx.InitCvMat(dimension, 1, MatrixType.F64C1); _left.Zero(); _right.Zero(); }
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); }
/// <summary> /// 最小二乗法で解を得ます /// </summary> /// <returns></returns> public double[] Solve() { CvMat invLeft = CvEx.InitCvMat(_left); _left.Invert(invLeft, InvertMethod.Cholesky); CvMat ret = invLeft * _right; return(ret.Select(r => r.Val0).ToArray()); }
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 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); } } }
public CvMat CalculateTransform(int targetModelIndex, bool updateInternalModelTransform, double randomSamplingRatio) { if (targetModelIndex < 0 || targetModelIndex >= _flannModels.Count) { throw new ArgumentOutOfRangeException("targetModelIndex"); } CoordRotTransConversion coordConverter = new CoordRotTransConversion(); //CoordConvertSpring coordConverter = new CoordConvertSpring(_modelTransforms[targetModelIndex]); //foreach (var point in dataPointListInWorldCoordinate) { List <Tuple <CvPoint3D64f, CvColor> > tuples = _flannModels[targetModelIndex].ModelPoints; if (randomSamplingRatio < 1) { Random rand = new Random(); tuples = tuples.Where(x => rand.NextDouble() < randomSamplingRatio).ToList(); } CvMat targetTransform = _modelTransforms[targetModelIndex]; List <CvMat> inverseTransforms = new List <CvMat>(); foreach (CvMat transform in _modelTransforms) { CvMat inv = CvEx.InitCvMat(transform); transform.Invert(inv); inverseTransforms.Add(inv); } float searchDistanceSq = this.SearchDistance * this.SearchDistance; Parallel.ForEach(tuples, tuple => { CvPoint3D64f point = tuple.Item1; CvColor color = tuple.Item2; //foreach (var point in points) { CvPoint3D64f worldPoint = CvEx.ConvertPoint3D(point, targetTransform); int minModelIndex = -1; int minPointIndex = -1; float minDistanceSq = float.MaxValue; for (int modelIndex = 0; modelIndex < _flannModels.Count; modelIndex++) { if (modelIndex == targetModelIndex) { continue; } CvPoint3D64f inversePoint = CvEx.ConvertPoint3D(worldPoint, inverseTransforms[modelIndex]); int[] indices; float[] distances; _flannModels[modelIndex].KnnSearch(inversePoint, color, out indices, out distances, 1); if (indices.Length >= 1) { float distanceSq = distances[0]; if (distanceSq <= searchDistanceSq) { if (distanceSq < minDistanceSq) { minModelIndex = modelIndex; minPointIndex = indices[0]; minDistanceSq = distanceSq; } } } } if (minModelIndex != -1) { Tuple <CvPoint3D64f, CvColor> bestModelPoint = _flannModels[minModelIndex].ModelPoints[minPointIndex]; double weightTo = 1.0 / (Math.Abs(bestModelPoint.Item1.Z - 1500 / 1000f) + 5000 / 1000f); double weightFrom = 1.0 / (Math.Abs(point.Z - 1500 / 1000f) + 5000 / 1000f); //weightFrom = weightTo = 1; double weight = _weightFromDistanceSq(minDistanceSq) * weightFrom * weightTo; CvPoint3D64f from = CvEx.ConvertPoint3D(point, targetTransform); CvPoint3D64f to = CvEx.ConvertPoint3D(bestModelPoint.Item1, _modelTransforms[minModelIndex]); coordConverter.PutPoint(from, to, weight); } }); CvMat ret = coordConverter.Solve() * targetTransform; if (updateInternalModelTransform) { _modelTransforms[targetModelIndex] = ret.Clone(); } return(ret); }
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 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(); } } }
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); } } }