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