public DisparityCalibration()
{
model = new Polynomial2D()
{
Coef = new double[, ] {
{ 33.8504792299378, 0.0165929854877602, -8.279223601177E-06 },
{ 0.82796340799605, 1.57301601046165E-05, 0 },
{ -1.30466960028068E-05, 0, 0 }
}
};
}
static public Polynomial2D Fit(double[][] xdata, double[] ydata)
{
var model = new Polynomial2D();
var n = xdata.Count();
var A = new double[n, (Deg1 + 1) * (Deg2 + 1)];
int dmin = Math.Min(Deg1, Deg2), dmax = Math.Max(Deg1, Deg2);
var minPoints = (dmin + 1) * dmin / 2 + (dmax - dmin) * (dmin + 1);
if (n < minPoints)
{
throw new Exception();
}
for (int t = 0; t < n; t++)
{
double x1n = 1, x2n = 1;
int k = 0;
for (int i = 0; i <= Deg2; i++, x2n *= xdata[t][1])
{
x1n = 1;
for (int j = 0; j <= Deg1; j++, x1n *= xdata[t][0])
{
if (i + j <= Math.Max(Deg1, Deg2))
{
A[t, k] = x1n * x2n;
}
k++;
}
}
}
var x = Matrix.Solve(A, ydata, true);
for (int i = 0; i < (Deg1 + 1) * (Deg2 + 1); i++)
{
model.Coef[i / (Deg2 + 1), i % (Deg2 + 1)] = x[i];
}
//Buffer.BlockCopy(x, 0, model.Coef, 0, x.Length);
for (int i = 0; i < Deg1; i++)
{
for (int j = 0; j < Deg2; j++)
{
if (i + j > dmax)
{
model.Coef[i, j] = 0;
}
}
}
model.Coef = model.Coef.Transpose(true);
return(model);
}
private Polynomial2D Ransac(double[][] xdata, double[] ydata,
double threshold = 1.0, int maxEvaluations = 500, int maxSamplings = 30)
{
// We are going to find the best model (which fits
// the maximum number of inlier points as possible).
Polynomial2D bestModel = null;
int[] bestInliers = null;
int maxInliers = 0;
// For this we are going to search for random samples
// of the original points which contains no outliers.
int count = 0; // Total number of trials performed
double N = maxEvaluations; // Estimative of number of trials needed.
// While the number of trials is less than our estimative,
// and we have not surpassed the maximum number of trials
while (count < N && count < maxEvaluations)
{
int[] idx;
Polynomial2D model = null;
// Select at random s datapoints to form a trial model.
idx = Vector.Sample(ydata.Length).Take(maxSamplings).ToArray();
double[][] inlierX = xdata.Get(idx);
double[] inlierY = ydata.Get(idx);
model = Polynomial2D.Fit(inlierX, inlierY);
// Now, evaluate the distances between total points and the model returning the
// indices of the points that are inliers (according to a distance threshold t).
idx = xdata
.Where((x, i) => ydata[i] - model.Evaluate(x[0], x[1]) < threshold)
.Select((x, i) => i).ToArray();
// Check if the model was the model which highest number of inliers:
if (idx.Length > maxInliers)
{
maxInliers = idx.Length; // Set the new maximum,
bestModel = model; // This is the best model found so far,
bestInliers = idx; // Store the indices of the current inliers.
}
count++; // Increase the trial counter.
}
return(bestModel);
}
public void CalibrateCoefficient(Mat decoded, double d0)
{
var imgSize = decoded.Size();
var P = new List <double[]>();
var T = new List <double>();
for (int i = 0; i < imgSize.Height; i += 1)
{
for (int j = 0; j < imgSize.Width; j += 1)
{
var p = (double)decoded.At <float>(i, j);
if (p < 0)
{
continue;
}
P.Add(new[] { p, i });
T.Add(j + d0);
}
}
model = Ransac(P.ToArray(), T.ToArray());
}
