public override void FinalizeForPixel(IntVector2 pixelBase)
{
if(_minIdx == -1)
{
// There was no disparity for pixel : set as invalid
DisparityMap.Set(pixelBase.Y, pixelBase.X,
new Disparity(pixelBase, pixelBase, double.PositiveInfinity, 0.0, (int)DisparityFlags.Invalid));
return;
}
Disparity bestDisp = _dispForPixel[_minIdx];
bestDisp.Confidence = ConfidenceComp.ComputeConfidence(_dispForPixel, _minIdx, _min2Idx);
if(_minIdx > 0 && _minIdx < _dispForPixel.Count - 1)
{
// D'[d-1] = D[d] + c[d-1]/c[d] * (D[d-1] - D[d])
// D'[d+1] = D[d] + c[d+1]/c[d] * (D[d+1] - D[d])
// Fit quadratic func. to d,d-1,d+1
double c_n = _dispForPixel[_minIdx - 1].Cost / _minCost;
double c_p = _dispForPixel[_minIdx + 1].Cost / _minCost;
double dx = _dispForPixel[_minIdx].DX;
double dy = _dispForPixel[_minIdx].DY;
double dx_n = dx + c_n * (_dispForPixel[_minIdx - 1].DX - dx);
double dy_n = dy + c_n * (_dispForPixel[_minIdx - 1].DY - dy);
double dx_p = dx + c_p * (_dispForPixel[_minIdx + 1].DX - dx);
double dy_p = dy + c_p * (_dispForPixel[_minIdx + 1].DY - dy);
_D.At(0, 0, (_minIdx - 1) * (_minIdx - 1));
_D.At(0, 1, _minIdx - 1);
_D.At(1, 0, _minIdx * _minIdx);
_D.At(1, 1, _minIdx);
_D.At(2, 0, (_minIdx + 1) * (_minIdx + 1));
_D.At(2, 1, (_minIdx + 1));
_d.At(0, dx_n);
_d.At(1, dx);
_d.At(2, dx_p);
_a = SvdSolver.Solve(_D, _d);
bestDisp.SubDX = _a.At(0) * _minIdx * _minIdx + _a.At(1) * _minIdx + _a.At(2);
_d.At(0, dy_n);
_d.At(1, dy);
_d.At(2, dy_p);
_a = SvdSolver.Solve(_D, _d);
bestDisp.SubDY = _a.At(0) * _minIdx * _minIdx + _a.At(1) * _minIdx + _a.At(2);
}
IntVector2 pm = bestDisp.GetMatchedPixel(pixelBase);
DisparityMap.Set(pm.Y, pm.X, bestDisp);
_dispForPixel = new List<Disparity>(2 * _dispForPixel.Count);
_minIdx = 0;
_min2Idx = 0;
_minCost = double.PositiveInfinity;
_min2Cost = double.PositiveInfinity;
}
public Disparity(IntVector2 pixelBase, IntVector2 pixelMacthed, double cost = 0.0, double confidence = 0.0, int flags = (int)DisparityFlags.Invalid)
{
DX = pixelMacthed.X - pixelBase.X;
DY = pixelMacthed.Y - pixelBase.Y;
SubDX = DX;
SubDY = DY;
Cost = cost;
Confidence = confidence;
Flags = flags;
}
public override double GetCost(IntVector2 pixelBase, IntVector2 pixelMatched)
{
int cost = 0;
for(int dx = -CorrMaskWidth; dx <= CorrMaskWidth; ++dx)
{
for(int dy = -CorrMaskHeight; dy <= CorrMaskHeight; ++dy)
{
cost += Math.Abs(RankBase[pixelBase.Y + dy, pixelBase.X + dx] -
RankMatched[pixelMatched.Y + dy, pixelMatched.X + dx]);
}
}
return cost;
}
public override double GetCost_Border(IntVector2 pixelBase, IntVector2 pixelMatched)
{
int cost = 0;
int px_b, px_m, py_b, py_m;
for(int dx = -CorrMaskWidth; dx <= CorrMaskWidth; ++dx)
{
for(int dy = -CorrMaskHeight; dy <= CorrMaskHeight; ++dy)
{
px_b = Math.Max(0, Math.Min(ImageBase.ColumnCount - 1, pixelBase.X + dx));
py_b = Math.Max(0, Math.Min(ImageBase.RowCount - 1, pixelBase.Y + dy));
px_m = Math.Max(0, Math.Min(ImageMatched.ColumnCount - 1, pixelMatched.X + dx));
py_m = Math.Max(0, Math.Min(ImageMatched.RowCount - 1, pixelMatched.Y + dy));
cost += Math.Abs(RankBase[py_b, px_b] - RankMatched[py_m, px_m]);
}
}
return cost;
}
public override void ComputeMatchingCosts()
{
// For each pixel pb in ImageBase:
// 1) Find corresponding epiline on matched image
// == 2) Find y0 = e(x=0) (point on epiline with x = 0) == NOPE
// 3) Find xmax = min(Im.cols, xd=e(y=Im.rows)) (point where epilines corsses border)
// 4) For each xd = [0, 1, ... , xmax-1]
// 4.1) find yd0 = e(x=xd) and yd1 = e(x=xd+1)
// === 4b) if (int)yd0 == (int)yd1 : skip ( it will be later or was checked already) === NOPE
// 4.2) For each yd = [(int)yd0, (int)yd1] (may also use range [(int)yd0-1, (int)yd1+1] - or [(int)yd0+1, (int)yd1-1] if yd0 > yd1)
// - Set disparity D = (px-xd,px-yd) (or (xd,yd))
// - Compute cost for disparity
for(int c = 0; c < ImageBase.ColumnCount; ++c)
{
for(int r = 0; r < ImageBase.RowCount; ++r)
{
if(ImageBase.HaveValueAt(r, c))
{
Vector2 pb_d = new Vector2(x: c, y: r);
CurrentPixel = new IntVector2(x: c, y: r);
Vector<double> epiLine = IsLeftImageBase ?
FindCorrespondingEpiline_OnRightImage(pb_d) : FindCorrespondingEpiline_OnLeftImage(pb_d);
if(Math.Abs(epiLine[0]) < Math.Abs(epiLine[1]) * 1e-12)
{
// Horizotal
FindDisparitiesCosts_HorizontalEpiline(r);
}
else if(Math.Abs(epiLine[1]) < Math.Abs(epiLine[0]) * 1e-12)
{
// Vertical
FindDisparitiesCosts_VerticalEpiline(c);
}
else
{
FindDisparitiesCosts(CurrentPixel, epiLine);
}
DispComp.FinalizeForPixel(CurrentPixel);
}
}
}
}
public override void ComputeMatchingCosts()
{
IntVector2 pm = new IntVector2();
int mindx = IsLeftImageBase ? -MaxDisp_NegX : -MaxDisp_PosX;
int maxdx = IsLeftImageBase ? MaxDisp_PosX : MaxDisp_NegX;
int mindy = IsLeftImageBase ? -MaxDisp_NegY : -MaxDisp_PosY;
int maxdy = IsLeftImageBase ? MaxDisp_PosY : MaxDisp_NegY;
for(int c = 0; c < ImageBase.ColumnCount; ++c)
{
int xmin = Math.Max(0, c + mindx);
int xmax = Math.Min(ImageBase.ColumnCount, c + maxdx);
for(int r = 0; r < ImageBase.RowCount; ++r)
{
CurrentPixel = new IntVector2(x: c, y: r);
if(ImageBase.HaveValueAt(r, c))
{
int ymin = Math.Max(0, r + mindx);
int ymax = Math.Min(ImageBase.RowCount, r + maxdy);
for(int xm = xmin; xm < xmax; ++xm)
{
for(int ym = ymin; ym < ymax; ++ym)
{
if(ImageMatched.HaveValueAt(ym, xm))
{
pm.X = xm;
pm.Y = ym;
double cost = CostComp.GetCost_Border(CurrentPixel, pm);
DispComp.StoreDisparity(CurrentPixel, pm, cost);
}
}
}
}
DispComp.FinalizeForPixel(CurrentPixel);
}
}
}
public override void FinalizeForPixel(IntVector2 pixelBase)
{
if(_minIdx == -1)
{
// There was no disparity for pixel : set as invalid
DisparityMap.Set(pixelBase.Y, pixelBase.X,
new Disparity(pixelBase, pixelBase, double.PositiveInfinity, 0.0, (int)DisparityFlags.Invalid));
return;
}
Disparity bestDisp = _dispForPixel[_minIdx];
bestDisp.Confidence = ConfidenceComp.ComputeConfidence(_dispForPixel, _minIdx, _min2Idx);
// IntVector2 pm = bestDisp.GetMatchedPixel(pixelBase);
DisparityMap.Set(pixelBase.Y, pixelBase.X, bestDisp);
_dispForPixel = new List<Disparity>(2 * _dispForPixel.Count);
_minIdx = -1;
_min2Idx = -1;
_minCost = double.PositiveInfinity;
_min2Cost = double.PositiveInfinity;
}
public Vector2(IntVector2 other)
{
_x = (double)other.X;
_y = (double)other.Y;
}
public int DotProduct(IntVector2 v)
{
return X * v.X + Y * v.Y;
}
public int DistanceToSquared(IntVector2 v)
{
return (X - v.X) * X - v.X + (Y - v.Y) * Y - v.Y;
}
public static IntVector2 GetBorderPixel(IntVector2 pixel, int rows, int columns)
{
if(columns - 1 - pixel.X <= pixel.Y * 2) // Bounded by x
{
return new IntVector2(columns - 1, pixel.Y - (columns - 1 - pixel.X) / 2);
}
else // Bounded by y
{
IntVector2 pb = new IntVector2(pixel.X + pixel.Y * 2, 0);
pb.X = ((columns - 1 - pixel.X) & 1) != 0 ? pb.X + 1 : pb.X;
return pb;
}
}
public override void StoreDisparity(IntVector2 pixelBase, IntVector2 pixelMatched, double cost)
{
StoreDisparity(new Disparity(pixelBase, pixelMatched, cost, 0.0, (int)DisparityFlags.Valid));
}
// Returns cost of matching pixels from base / matched image for int pixel coords // Check if matching mask is inside bounds of image and if outside bounds uses mirrored values public abstract double GetCost_Border(IntVector2 pixelBase, IntVector2 pixelMatched);
private void FindDisparitiesCosts_HorizontalEpiline(int y)
{
IntVector2 pm = new IntVector2();
for(int xm = 0; xm < ImageBase.ColumnCount; ++xm)
{
if(ImageMatched.HaveValueAt(y, xm))
{
pm.X = xm;
pm.Y = y;
double cost = CostComp.GetCost_Border(CurrentPixel, pm);
DispComp.StoreDisparity(CurrentPixel, pm, cost);
}
}
}
public override void FinalizeForPixel(IntVector2 pixelBase)
{
if(_idx == 0)
{
// There was no disparity for pixel : set as invalid
DisparityMap.Set(pixelBase.Y, pixelBase.X,
new Disparity(pixelBase, pixelBase, double.PositiveInfinity, 0.0, (int)DisparityFlags.Invalid));
return;
}
// 1) Sort by disparity
Array.Sort(_dispForPixel, 0, _idx, new DisparityComparer());
int start = 0;
int count = _idx;
bool costLower = true;
// 2) Find weighted mean m of disparities and distances s to m, s = ||m - d|| (or ||m - d||^2)
// Cost function: C = sum(||m - d||) / n^2 || C = sqrt(sum(||m - d||^2)) / n^2
double mean = _meanComputer(start, count);
double cost = _costComputer(mean, start, count);
do
{
// 3) Remove one disp from ends and check if cost is lower
double mean1 = _meanComputer(start + 1, count - 1);
double cost1 = _costComputer(mean1, start + 1, count - 1);
double mean2 = _meanComputer(start, count - 1);
double cost2 = _costComputer(mean2, start, count - 1);
if(cost > cost1 || cost > cost2)
{
if(cost1 < cost2) // Remove first one -> move start by one pos
{
start += 1;
cost = cost1;
mean = mean1;
}
else // Remove last one -> just decrement count
{
cost = cost2;
mean = mean2;
}
count -= 1;
costLower = true;
}
else
costLower = false;
}
while(costLower && count > 3); // 4) Repeat untill cost is minimised
// 5) Confidence ?
DisparityMap.Set(pixelBase.Y, pixelBase.X, new Disparity()
{
DX = mean.Round(),
DY = 0,
SubDX = mean,
SubDY = 0.0,
Cost = CostComp.GetCost_Border(pixelBase, new IntVector2(pixelBase.X + mean.Round(), pixelBase.Y)),
Confidence = ((double)count / (double)_idx) * (1.0 / (cost + 1.0)),
Flags = (int)DisparityFlags.Valid
});
_idx = 0;
}
public static IntVector2 GetBorderPixel(IntVector2 pixel, int rows, int columns)
{
return new IntVector2(pixel.X, 0);
}
public static IntVector2 GetBorderPixel(IntVector2 pixel, int rows, int columns)
{
int d = Math.Min(pixel.X, pixel.Y);
return new IntVector2(pixel.X - d, pixel.Y - d);
}
public static IntVector2 GetBorderPixel(IntVector2 pixel, int rows, int columns)
{
int d = Math.Min(columns - pixel.X - 1, rows - pixel.Y - 1);
return new IntVector2(pixel.X + d, pixel.Y + d);
}
public static IntVector2 GetBorderPixel(IntVector2 pixel, int rows, int columns)
{
if(pixel.Y <= pixel.X * 2) // Bounded by y
{
return new IntVector2(pixel.X - pixel.Y / 2, 0);
}
else // Bounded by x
{
IntVector2 pb = new IntVector2(0, pixel.Y - pixel.X * 2);
pb.Y = ((pixel.Y & 1) & 1) != 0 ? pb.Y - 1 : pb.Y;
return pb;
}
}
public static IntVector2 GetBorderPixel(IntVector2 pixel, int rows, int columns)
{
if(pixel.Y <= (columns - 1 - pixel.X) * 2) // Bounded by y
{
return new IntVector2(pixel.X + pixel.Y / 2, 0);
}
else // Bounded by x
{
IntVector2 pb = new IntVector2(columns - 1, pixel.Y - (columns - 1 - pixel.X) * 2);
pb.Y = ((rows - 1 - pixel.Y) & 1) != 0 ? pb.Y + 1 : pb.Y;
return pb;
}
}
public static IntVector2 GetBorderPixel(IntVector2 pixel, int rows, int columns)
{
if(pixel.X <= pixel.Y * 2) // Bounded by x
{
return new IntVector2(0, pixel.Y - pixel.X / 2);
}
else // Bounded by y
{
IntVector2 pb = new IntVector2(pixel.X - pixel.Y * 2, 0);
pb.X = (pixel.X & 1) != 0 ? pb.X - 1 : pb.X;
return pb;
}
}
public IntVector2(IntVector2 other)
{
_x = other.X;
_y = other.Y;
}
private void FindDisparitiesCosts(IntVector2 pb, Vector<double> epiLine)
{
epiLine.DivideThis(epiLine.At(1));
IntVector2 pm = new IntVector2();
int xmax = FindXmax(epiLine);
int x0 = FindX0(epiLine);
xmax = xmax - 1;
for(int xm = x0 + 1; xm < xmax; ++xm)
{
double ym0 = FindYd(xm, epiLine);
double ym1 = FindYd(xm + 1, epiLine);
for(int ym = (int)ym0; ym < (int)ym1; ++ym)
{
if(ImageMatched.HaveValueAt(ym, xm))
{
pm.X = xm;
pm.Y = ym;
double cost = CostComp.GetCost_Border(CurrentPixel, pm);
DispComp.StoreDisparity(CurrentPixel, pm, cost);
}
}
}
}
public abstract void FinalizeForPixel(IntVector2 pixelBase);
private void FindDisparitiesCosts_VerticalEpiline(int x)
{
IntVector2 pm = new IntVector2();
for(int ym = 0; ym < ImageBase.RowCount; ++ym)
{
if(ImageMatched.HaveValueAt(ym, x))
{
pm.X = x;
pm.Y = ym;
double cost = CostComp.GetCost_Border(CurrentPixel, pm);
DispComp.StoreDisparity(CurrentPixel, pm, cost);
}
}
}
// Moments vector : [m00, m01, m10, m11, m02, m20, m12, m21, m03, m30]
Vector<double> ComputeMomentVectorForPatch(IntVector2 centerPixel, IImage image)
{
Vector<double> moments = new DenseVector(10);
// M00 = total_intensity
// Mpq = sum{x,y}( x^p*y^q*I(x,y) )
for(int dx = -WindowRadius; dx <= WindowRadius; ++dx)
{
for(int dy = -_ybounds[dx + WindowRadius]; dy <= _ybounds[dx + WindowRadius]; ++dy)
{
if(image.HaveValueAt(centerPixel.Y + dy, centerPixel.X + dx))
{
double val = image[centerPixel.Y + dy, centerPixel.X + dx];
moments[m00] += val;
moments[m01] += dy * val;
moments[m10] += dx * val;
moments[m11] += dx * dy * val;
moments[m02] += dy * dy * val;
moments[m20] += dx * dx * val;
moments[m12] += dx * dy * dy * val;
moments[m21] += dx * dx * dy * val;
moments[m03] += dy * dy * dy * val;
moments[m30] += dx * dx * dx * val;
}
}
}
return moments;
}
public override void ComputeMatchingCosts_Rectified()
{
// For each pixel pb in ImageBase:
// 1) Find corresponding epiline on matched image (as its recified its just x' = px)
// 2) For each xd = [0, 1, ... , xmax] (xmax = Im.cols)
// - Set disparity D = (px-xd, 0) (or (xd,yd))
// - Compute cost for disparity
// - Save only best one, but delegate each disparity to DisparityComputer
// 3) 2 may be repeated for yd = 1/-1
for(int c = 0; c < ImageBase.ColumnCount; ++c)
{
for(int r = 0; r < ImageBase.RowCount; ++r)
{
Vector2 pb_d = new Vector2(x: c, y: r);
CurrentPixel = new IntVector2(x: c, y: r);
FindDisparitiesCosts_HorizontalEpiline(r);
DispComp.FinalizeForPixel(CurrentPixel);
}
}
}
public double DistanceTo(IntVector2 v)
{
return (double)Math.Sqrt(DistanceToSquared(v));
}
public abstract void StoreDisparity(IntVector2 pixelBase, IntVector2 pixelMatched, double cost);
public override double GetCost_Border(IntVector2 pixelBase, IntVector2 pixelMatched)
{
return _aggFunction(
CostComputer_Red.GetCost_Border(pixelBase, pixelMatched),
CostComputer_Green.GetCost_Border(pixelBase, pixelMatched),
CostComputer_Blue.GetCost_Border(pixelBase, pixelMatched));
}
