public Point2d IntersectionPoint(FuncApproximation F1, FuncApproximation F2, Point2d PtA, Point2d PtB, bool DispB = false) { this.F1 = F1; this.F2 = F2; Point Pt0 = (Point)((PtA + PtB) * 0.5); RegFuncUtilitySub FS1 = new RegFuncUtilitySub(F1, PtA, PtB); RegFuncUtilitySub FS2 = new RegFuncUtilitySub(F2, PtA, PtB); Point2d PtAns = new Point2d(); Mat resImg = null; try{ if (DispB) { resImg = ImgCheck.CvtColor(ColorConversionCodes.GRAY2BGR); //Gray->Color変換 for (int x = 0; x < ImgCheck.Width; x += 20) { Point P1 = new Point(x, F1.RegXY.Estimate(x)); Point P2 = P1 + (new Point(4, 4)); resImg.Rectangle(P1, P2, Scalar.Orange, 3); } for (int y = 0; y < ImgCheck.Height; y += 20) { Point P1 = new Point(F2.RegXY.Estimate(y), y); Point P2 = P1 + (new Point(4, 4)); resImg.Rectangle(P1, P2, Scalar.Blue, 3); } resImg.Circle(Pt0, 10, Scalar.Red, 5); using (new Window("IntersectionPoint", WindowMode.KeepRatio, resImg)){ Cv2.WaitKey(0); } } Point2d PtAns2 = F2.EstimatePt2Pt(Pt0), PtAns1 = Pt0; int loop = 5; while (--loop > 0) { double vAns1 = FS1.__Solve(PtAns2, ref PtAns1); if (DispB) { resImg.Circle((Point)PtAns2, 5, Scalar.Blue, 3); resImg.Circle((Point)PtAns1, 5, Scalar.Red, loop * 2); using (new Window("IntersectionPoint", WindowMode.KeepRatio, resImg)){ Cv2.WaitKey(0); } } double vAns2 = FS2.__Solve(PtAns1, ref PtAns2); if (DispB) { resImg.Circle((Point)PtAns2, 5, Scalar.Red, 3); resImg.Circle((Point)PtAns1, 5, Scalar.Blue, loop * 2); using (new Window("IntersectionPoint", WindowMode.KeepRatio, resImg)){ Cv2.WaitKey(0); } } if (vAns1 < 0.01 && vAns2 < 0.01) { break; } FS1.SetRange(PtA, PtB); FS2.SetRange(PtA, PtB); } PtAns = FS1.Estimate(PtAns2); } catch (Exception e) { WriteLine(e.Message + "\r" + e.StackTrace); } return(PtAns); }
public void Test() { List <PointEx> LL = new List <PointEx>(); for (int k = 0; k < 10; k++) { double x = k + 1; Point2d P = new Point2d(x, FuncG(x)); PointEx Q = new PointEx(P, new Point2d(1, 1)); LL.Add(Q); WriteLine("{0}, {1}", P.X, P.Y); } var FRegTop = new FuncApproximation(3, LL, CalculusB: true); for (int k = 0; k < 10; k++) { double X = k; double Y = LL[k].Pt.Y; double Ye = FRegTop.RegXY.Estimate(X); double Yd = FRegTop.RegXY.DiffEstimate(X); WriteLine("X:{0} Y;{1} Ye:{2} Yd:{3}", X, Y, Ye, Yd); } }
private Mat CorrectDistortion(Mat imgWhiteB, List <Point2d> PTs, out Point2d[] Q4, bool DispB = false) { Center = new Point2d(PTs.Average(P => P.X), PTs.Average(P => P.Y)); List <PointEx> PTda = PTs.ToList().ConvertAll(P => new PointEx(P, Center)); PTda.Sort((A, B) => (int)((A.Angle - B.Angle) * 100000.0)); PTda.AddRange(PTda); //if(DispB){ for(int k=0; k<PTda.Count(); k++) WriteLine("k:{0} PTda:{1}",k,PTda[k]); } List <List <PointEx> > PTda4 = _SepareteLins(PTda); //BLTR if (PTda4 == null || PTda4.Count < 4) { Q4 = null; return(null); } #if false if (DispB) { Mat resImg = imgWhiteB.CvtColor(ColorConversionCodes.GRAY2BGR); //Gray->Color変換 Scalar[] clrLst = { Scalar.Red, Scalar.Blue, Scalar.Yellow, Scalar.Green }; for (int k = 0; k < 4; k++) { var clr = clrLst[k]; //(k%2==0)? Scalar.Red: Scalar.Blue; PTda4[k].ForEach(Q => resImg.Circle(Q.Pt, 5, clr, 5)); } using (new Window("◆1◆resImg", WindowMode.KeepRatio, resImg)){ Cv2.WaitKey(0); } } #endif int nSize = 1; var FRegTop = new FuncApproximation(nSize, PTda4[2], CalculusB: true); var FRegBtm = new FuncApproximation(nSize, PTda4[0], CalculusB: true); var FRegLft = new FuncApproximation(nSize, PTda4[1], CalculusB: true); var FRegRgt = new FuncApproximation(nSize, PTda4[3], CalculusB: true); Point2d[] PT4 = new Point2d[4]; var FM = new RegFuncUtility(ImgCheck: imgWhiteB); Point2d P00 = new Point2d(0, 0), PGG = new Point2d(imgWhiteB.Width, imgWhiteB.Height); PT4[0] = FM.IntersectionPoint(FRegTop, FRegLft, P00, PGG, DispB: false); PT4[2] = FM.IntersectionPoint(FRegBtm, FRegLft, P00, PGG, DispB: false); PT4[1] = FM.IntersectionPoint(FRegTop, FRegRgt, P00, PGG, DispB: false); PT4[3] = FM.IntersectionPoint(FRegBtm, FRegRgt, P00, PGG, DispB: false); if (DispB) { Mat resImg = imgWhiteB.CvtColor(ColorConversionCodes.GRAY2BGR); //Gray->Color Scalar[] clrLst = { Scalar.Red, Scalar.Blue, Scalar.Yellow, Scalar.Green }; for (int k = 0; k < 4; k++) { var clr = clrLst[k]; //(k%2==0)? Scalar.Red: Scalar.Blue; PTda4[k].ForEach(Q => resImg.Circle((Point)Q.Pt, 5, clr, 5)); } //using( new Window("#1#resImg",WindowMode.KeepRatio,resImg) ){ Cv2.WaitKey(0); } for (int k = 0; k < 4; k++) { var clr = clrLst[k]; PTda4[k].ForEach(Q => resImg.Circle((Point)PT4[k], 10, clr, 5)); } using (new Window("#2#resImg", WindowMode.KeepRatio, resImg)){ Cv2.WaitKey(0); } } FRegTop.CreateFunction(PT4[0], 0, 32 * 9); FRegBtm.CreateFunction(PT4[2], 0, 32 * 9); //FRegLft.CreateFunction(PT4[0],0,32*9); //FRegRgt.CreateFunction(PT4[2],0,32*9); Mat MatCor = new Mat(352, 352, MatType.CV_8UC1, Scalar.White); unsafe { byte * S = imgWhiteB.DataPointer; byte * D = MatCor.DataPointer; int W = imgWhiteB.Width, H = imgWhiteB.Height, W2 = MatCor.Width; double per = 1.0 / (32 * 9); for (int x = 0; x <= 32 * 9 + 3; x++) { double xd = x; Point ptT = (Point)FRegTop.EstimateL2Pt(xd); Point ptB = (Point)FRegBtm.EstimateL2Pt(xd); for (int y = 0; y <= 32 * 9 + 3; y++) { Point pt = (Point)_2_Get_InterPolation(ptT, ptB, per * y); if (pt.X < 0 || pt.X >= W || pt.Y < 0 || pt.Y >= H) { //// using( new Window("#test# imgWhiteB",WindowMode.KeepRatio,imgWhiteB) ){ Cv2.WaitKey(0); } Q4 = null; return(null); } D[(y + 32) * W2 + x + 32] = S[pt.Y * W + pt.X]; } } } Q4 = new Point2d[4]; Q4[0] = new Point2d(32, 32); Q4[1] = new Point2d(320, 32); Q4[2] = new Point2d(32, 320); Q4[3] = new Point2d(320, 320); if (DispB) { new Window("MatCor", WindowMode.KeepRatio, MatCor); } return(MatCor); }
public Point2d IntersectionPointC(FuncApproximation F1, FuncApproximation F2, Point2d PtCenter, bool DispB) { Point2d PtA = new Point2d(0, 0), PtB = new Point2d(PtCenter.X * 2, PtCenter.Y * 2); Point2d[] PTlst = new Point2d[9]; while (true) { Lrepeat: double xMin = Min(PtA.X, PtB.X); double xMax = Max(PtA.X, PtB.X); double yMin = Min(PtA.Y, PtB.Y); double yMax = Max(PtA.Y, PtB.Y); double xSpn = (xMax - xMin) * 0.5; double ySpn = (yMax - yMin) * 0.5; if (xSpn < 0.01 && ySpn < 0.01) { break; } WriteLine(); for (int rc = 0; rc < 9; rc++) { PTlst[rc] = new Point2d(xMin + xSpn * (rc % 3), yMin + ySpn * (rc / 3)); } int k = 0; foreach (var P in PTlst) { WriteLine("{4}: {0:000},{1:000} {2:0.00} {3:0.00}", P.X, P.Y, F1.FuncPt2F(P), F2.FuncPt2F(P), ++k); } Combination cmb = new Combination(9, 2); while (cmb.Successor()) { int s0 = cmb.Index[0], s1 = cmb.Index[1]; if (s0 / 3 == s1 / 3 || s0 % 3 == s1 % 3 || Abs((s0 / 3 - s1 / 3) * (s0 % 3 - s1 % 3)) == 4) { continue; } //int s2=s1/3*3+s0%3, s3=s0/3*3+s1%3; //WriteLine(" {0} {1} {2} {3}", s0, s1, s2, s3 ); PtA = PTlst[s0]; PtB = PTlst[s1]; if (F1.FuncPt2F(PtA) * F1.FuncPt2F(PtB) > 0.0) { continue; } if (F2.FuncPt2F(PtA) * F2.FuncPt2F(PtB) > 0.0) { continue; } int s2 = s1 / 3 * 3 + s0 % 3, s3 = s0 / 3 * 3 + s1 % 3; PtA = PTlst[s2]; PtB = PTlst[s3]; if (F1.FuncPt2F(PtA) * F1.FuncPt2F(PtB) > 0.0) { continue; } if (F2.FuncPt2F(PtA) * F2.FuncPt2F(PtB) > 0.0) { continue; } WriteLine(" {0} {1} {2} {3}", s0, s1, s2, s3); goto Lrepeat; } // WriteLine(); // foreach( var P in PTlst) WriteLine("IntersectionPoint "+P ); } return(PtB); }
public Point2d IntersectionPointB(FuncApproximation F1, FuncApproximation F2, Point2d PtCenter, bool DispB) { Point2d PtA = new Point2d(0, 0), PtB = new Point2d(PtCenter.X * 2, PtCenter.Y * 2); Point2d[,] PTlst = new Point2d[3, 3]; while (true) { double xMin = Min(PtA.X, PtB.X); double xMax = Max(PtA.X, PtB.X); double yMin = Min(PtA.Y, PtB.Y); double yMax = Max(PtA.Y, PtB.Y); double xSpn = (xMax - xMin) * 0.5; double ySpn = (yMax - yMin) * 0.5; if (xSpn < 0.01 && ySpn < 0.01) { break; } for (int r = 0; r < 3; r++) { for (int c = 0; c < 3; c++) { PTlst[r, c] = new Point2d(xMin + xSpn * c, yMin + ySpn * r); } } WriteLine(); int k = 0; foreach (var P in PTlst) { WriteLine("{4}: {0:000.00},{1:000.00} {2:0.00} {3:0.00}", P.X, P.Y, F1.FuncPt2F((Point)P), F2.FuncPt2F((Point)P), ++k); } for (int r = 0; r < 2; r++) { for (int c = 0; c < 2; c++) { PtA = PTlst[r, c]; PtB = PTlst[r + 1, c + 1]; if (F1.FuncPt2F((Point)PtA) * F1.FuncPt2F((Point)PtB) > 0.0) { continue; } if (F2.FuncPt2F((Point)PtA) * F2.FuncPt2F((Point)PtB) > 0.0) { continue; } goto LFond; } } for (int r = 0; r < 2; r++) { for (int c = 0; c < 2; c++) { PtA = PTlst[r + 1, c]; PtB = PTlst[r, c + 1]; if (F1.FuncPt2F((Point)PtA) * F1.FuncPt2F((Point)PtB) > 0.0) { continue; } if (F2.FuncPt2F((Point)PtA) * F2.FuncPt2F((Point)PtB) > 0.0) { continue; } goto LFond; } } // WriteLine(); // foreach( var P in PTlst) WriteLine("IntersectionPoint "+P ); LFond: continue; } return(PtB); }
public Point2d IntersectionPoint__x(FuncApproximation F1, FuncApproximation F2, Point2d PTX, Point2d PTXdummy, bool DispB) { this.F1 = F1; this.F2 = F2; Point2d PT = PTX, PTnxt = new Point2d(-9999, -9999); bool B = F1.XYmode; DenseMatrix M = new DenseMatrix(2, 2), Minv; DenseVector V = new DenseVector(2); double a = 0.0, b = 0.0, c = 0.0, a1 = 0.0, b1 = 0.0, c1 = 0.0, d = 0; int loop; Mat resImg = null; if (DispB) { resImg = ImgCheck.CvtColor(ColorConversionCodes.GRAY2BGR); //Gray->Color変換 WriteLine(); } for (loop = 0; loop < 20; loop++) //Up to 20 times { int r = 2 + loop * 2; if (loop == 0) { F1.GetTangent(ref a, ref b, ref c); F2.GetTangent(ref a1, ref b1, ref c1); } else { double e = Min(d * 0.1, 1.0); F1.GetTangent1(PT, e, ref a, ref b, ref c); F2.GetTangent1(PT, e, ref a1, ref b1, ref c1); } if (DispB) { for (int x = 0; x < ImgCheck.Width; x += 20) { Point P1 = new Point(x, -a * x + c); Point P2 = P1 + (new Point(r, r)); resImg.Rectangle(P1, P2, Scalar.Blue, 3); } for (int y = 0; y < ImgCheck.Height; y += 20) { Point P1 = new Point(-b1 * y + c1, y); Point P2 = P1 + (new Point(r, r)); resImg.Rectangle(P1, P2, Scalar.Red, 3); } using (new Window("IntersectionPoint", WindowMode.KeepRatio, resImg)){ Cv2.WaitKey(0); } } bool matrixB = true; if (Abs(a) < 0.1) { PTnxt.Y = c / b; PTnxt.X = F2.RegXY.Estimate(PTnxt.Y); matrixB = false; } if (Abs(a1) < 0.1) { PTnxt.Y = c1 / b1; PTnxt.X = F1.RegXY.Estimate(PTnxt.Y); matrixB = false; } if (Abs(b) < 0.1) { PTnxt.X = c / a; PTnxt.Y = F2.RegXY.Estimate(PTnxt.X); matrixB = false; } if (Abs(b1) < 0.1) { PTnxt.X = c1 / a1; PTnxt.Y = F1.RegXY.Estimate(PTnxt.X); matrixB = false; } if (DispB) { string st = "IntersectionPoint loop:" + loop; st += string.Format(" a:{0:0.0000} b:{1:0.0000} c:{2:0.0000}", a, b, c); st += string.Format(" a1:{0:0.0000} b1:{1:0.0000} c1:{2:0.0000} ", a1, b1, c1); st += matrixB? "..": "◆"; st += string.Format("PT:(X:{0:0.0},Y:{1:0.0}) PTnxt:(X:{2:0.0},Y:{3:0.0})", PT.X, PT.Y, PTnxt.X, PTnxt.Y); WriteLine(st); } if (matrixB) { M[0, 0] = a; M[0, 1] = b; V[0] = c; M[1, 0] = a1; M[1, 1] = b1; V[1] = c1; Minv = (DenseMatrix)M.Inverse(); V = (DenseVector)Minv.Multiply(V); PTnxt.X = V[0]; PTnxt.Y = V[1]; } if (DispB) { Point P1 = (Point)PTnxt; Point P2 = P1 + (new Point(r * 5, r * 5)); resImg.Rectangle(P1, P2, Scalar.Green, 3); using (new Window("IntersectionPoint", WindowMode.KeepRatio, resImg)){ Cv2.WaitKey(0); } } d = PT.DistanceTo(PTnxt); if (d < 1.0e-1) { break; } PT = PTnxt; } return(PT); }
public RegFuncUtilitySub(FuncApproximation F, Point2d PtA, Point2d PtB) { this.F = F; SetRange(PtA, PtB); }