public CvMat Solve() { // 重心の計算 CvPoint3D64f fromCenter = new CvPoint3D64f(); CvPoint3D64f toCenter = new CvPoint3D64f(); double weightSum = 0; foreach (var tuple in _correspondings) { fromCenter += tuple.Item1 * tuple.Item3; toCenter += tuple.Item2 * tuple.Item3; weightSum += tuple.Item3; } if (weightSum != 0) { fromCenter *= 1.0 / weightSum; toCenter *= 1.0 / weightSum; } // q: quaternion; 4x1 // fn, tn: from[n], to[n]; 3x1 // Xn: (tn - fn, (tn+fn)×[1,0,0], (tn+fn)×[0,1,0], (tn+fn)×[0,0,1]); 3x4 // M: Σi(Xi^t Wi Xi); 4x4 // Wi: I; 3x3 // J = q^t Mq -> min // 最小二乗法 using (CvMat M = new CvMat(4, 4, MatrixType.F64C1)) { M.Zero(); foreach (var tuple in _correspondings) { // 重心からの距離 CvPoint3D64f fromVector = tuple.Item1 - fromCenter; CvPoint3D64f toVector = tuple.Item2 - toCenter; using (CvMat Xi = new CvMat(3, 4, MatrixType.F64C1)) { CvPoint3D64f diff = toVector - fromVector; CvPoint3D64f sum = toVector + fromVector; CvPoint3D64f second = CvEx.Cross(sum, new CvPoint3D64f(1, 0, 0)); CvPoint3D64f third = CvEx.Cross(sum, new CvPoint3D64f(0, 1, 0)); CvPoint3D64f fourth = CvEx.Cross(sum, new CvPoint3D64f(0, 0, 1)); CvEx.FillCvMat(Xi, new double[] { diff.X, second.X, third.X, fourth.X, diff.Y, second.Y, third.Y, fourth.Y, diff.Z, second.Z, third.Z, fourth.Z }); using (CvMat XiTranspose = Xi.Transpose()) using (CvMat addend = XiTranspose * Xi * tuple.Item3) { M.Add(addend, M); } } } using (CvMat MTemp = CvEx.CloneCvMat(M)) using (CvMat eVals = new CvMat(4, 1, MatrixType.F64C1)) using (CvMat eVects = new CvMat(4, 4, MatrixType.F64C1)) { //Cv.EigenVV(MTemp, eVects, eVals, 0.000001); Cv.SVD(MTemp, eVals, eVects, null, SVDFlag.U_T | SVDFlag.ModifyA); int minEIndex = 3; /* * if (false) * { * double minE = double.MaxValue; * for (int i = 0; i < 4; i++) * { * double eVal = Math.Abs(eVals[i, 0]); * if (eVal < minE) * { * minE = eVal; * minEIndex = i; * } * } * } */ CvMat ret = new CvMat(4, 4, MatrixType.F64C1); ret.Zero(); ret[3, 3] = 1.0; CvMat rotateConversion; /* * if (false) * { * // こっちの変換はほとんど恒等のときに誤差が大きい * CvMat q = eVects.GetRow(minEIndex); * * // クォータニオンから回転ベクトルを計算 * double theta = Math.Acos(q[0, 0]) * 2; * double sin = Math.Sin(theta / 2); * CvPoint3D64f rot = new CvPoint3D64f(q[0, 1] / sin * theta, q[0, 2] / sin * theta, q[0, 3] / sin * theta); * // 回転ベクトルから回転行列を計算 * ret.GetSubRect(out rotateConversion, new CvRect(0, 0, 3, 3)); * using (CvMat rotVector = new CvMat(1, 3, MatrixType.F64C1)) * { * rotVector[0, 0] = rot.X; * rotVector[0, 1] = rot.Y; * rotVector[0, 2] = rot.Z; * Cv.Rodrigues2(rotVector, rotateConversion); * } * } * else * {*/ CvMat rotationMat = CvEx.QuaternionToMat3D(eVects[minEIndex, 0], eVects[minEIndex, 1], eVects[minEIndex, 2], eVects[minEIndex, 3]); ret.GetSubRect(out rotateConversion, new CvRect(0, 0, 3, 3)); rotationMat.Copy(rotateConversion); //} // 回転後の重心の並進成分の計算 using (CvMat fromCenterMat = new CvMat(3, 1, MatrixType.F64C1)) { CvEx.FillCvMat(fromCenterMat, new double[] { fromCenter.X, fromCenter.Y, fromCenter.Z }); using (CvMat rotFromCenterMat = rotateConversion * fromCenterMat) { CvPoint3D64f rotFromCenter = new CvPoint3D64f(rotFromCenterMat[0, 0], rotFromCenterMat[1, 0], rotFromCenterMat[2, 0]); CvPoint3D64f offset = toCenter - rotFromCenter; ret[0, 3] = offset.X; ret[1, 3] = offset.Y; ret[2, 3] = offset.Z; 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 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(); } }
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(); } } }