public object Clone()
{
var cloned = new DisparityMap(RowCount, ColumnCount);
for(int r = 0; r < RowCount; ++r)
{
for(int c = 0; c < ColumnCount; ++c)
{
cloned.Set(r, c, (Disparity)Disparities[r, c].Clone());
}
}
return cloned;
}
public static DisparityMap CreateFromNode(XmlNode mapNode)
{
int rows = int.Parse(mapNode.Attributes["rows"].Value);
int cols = int.Parse(mapNode.Attributes["cols"].Value);
DisparityMap map = new DisparityMap(rows, cols);
XmlNode rowNode = mapNode.FirstChildWithName("Row");
for(int r = 0; r < rows; ++r)
{
XmlNode dispNode = rowNode.FirstChildWithName("Disparity");
for(int c = 0; c < cols; ++c)
{
Disparity disp = Disparity.CreateFromNode(dispNode);
map.Disparities[r, c] = disp;
dispNode = dispNode.NextSibling;
}
rowNode = rowNode.NextSibling;
}
return map;
}
public void Test_EpiScan_Ideal()
{
EpilineScanAggregator agg = new EpilineScanAggregator();
RankCostComputer cost = new RankCostComputer();
MatchConfidenceComputer conf = new MatchConfidenceComputer();
cost.ImageBase = new GrayScaleImage() { ImageMatrix = _imageLeft };
cost.ImageMatched = new GrayScaleImage() { ImageMatrix = _imageRight };
cost.RankMaskHeight = 3;
cost.RankMaskWidth = 3;
cost.CorrMaskWidth = 3;
cost.CorrMaskHeight = 3;
//cost.MaskHeight = 3;
//cost.MaskWidth = 3;
agg.CostComp = cost;
conf.CostComp = cost;
conf.UsedConfidenceMethod = ConfidenceMethod.TwoAgainstTwo;
DisparityMap disp = new DisparityMap(_imageLeft.RowCount, _imageLeft.ColumnCount);
agg.DisparityMap = disp;
agg.ImageBase = cost.ImageBase;
agg.ImageMatched = cost.ImageMatched;
agg.IsLeftImageBase = true;
agg.Fundamental = _F;
cost.Init();
agg.ComputeMatchingCosts();
}
public void Test_BSGM_NoNoise()
{
SGMAggregator agg = new SGMAggregator();
CensusCostComputer cost = new CensusCostComputer();
WTADisparityComputer dispComp = new WTADisparityComputer();
cost.ImageBase = new GrayScaleImage() { ImageMatrix = _imageLeft };
cost.ImageMatched = new GrayScaleImage() { ImageMatrix = _imageRight };
cost.MaskWidth = 3;
cost.MaskHeight = 3;
agg.CostComp = cost;
dispComp.ConfidenceComp.UsedConfidenceMethod = ConfidenceMethod.TwoAgainstTwo;
dispComp.CostComp = cost;
dispComp.ImageBase = cost.ImageBase;
dispComp.ImageMatched = cost.ImageMatched;
agg.DispComp = dispComp;
DisparityMap disp = new DisparityMap(_imageLeft.RowCount, _imageLeft.ColumnCount);
agg.DisparityMap = disp;
dispComp.DisparityMap = disp;
agg.ImageBase = cost.ImageBase;
agg.ImageMatched = cost.ImageMatched;
agg.IsLeftImageBase = true;
agg.Fundamental = _F;
// agg.PathsLength = 10;
agg.LowPenaltyCoeff = 0.02;
agg.HighPenaltyCoeff = 0.04;
agg.MaxDisparity = 10;
agg.MinDisparity = 1;
dispComp.Init();
agg.Init();
agg.ComputeMatchingCosts();
}
private void _alg_StatusChanged(object sender, CamCore.AlgorithmEventArgs e)
{
if(e.CurrentStatus == AlgorithmStatus.Finished)
{
Dispatcher.Invoke(() =>
{
MapLeft = _alg.MapLeft;
MapRight = _alg.MapRight;
});
}
}
public DisparityMap FilterMap(DisparityMap map)
{
DisparityMap filtered = new DisparityMap(map.RowCount, map.ColumnCount);
Disparity[] window = new Disparity[9];
int middle = 4;
int invalidCount = 0;
Disparity invalidDisparity = new Disparity()
{
SubDX = 1e12,
SubDY = 1e12,
Flags = (int)DisparityFlags.Invalid
};
for(int r = 1; r < map.RowCount - 1; ++r)
{
for(int c = 1; c < map.ColumnCount - 1; ++c)
{
int n = 0;
invalidCount = 0;
for(int y = -1; y <= 1; ++y)
{
for(int x = -1; x <= 1; ++x)
{
if((map[r + y, c - 1].Flags & (int)DisparityFlags.Invalid) != 0)
{
window[n] = invalidDisparity;
++invalidCount;
}
else
window[n] = map[r + y, c - 1];
++n;
}
}
Array.Sort(window, (d1, d2) =>
{
double r1 = (d1.SubDX * d1.SubDX + d1.SubDY * d1.SubDY);
double r2 = (d2.SubDX * d2.SubDX + d2.SubDY * d2.SubDY);
return r1 < r2 ? 1 : r1 > r2 ? -1 : 0;
});
// Set value of image to be median of window
filtered.Set(r, c - 1, (Disparity)window[middle + (invalidCount >> 2)].Clone()); // For each 2 invalid cells move middle by 1 pos
// c - 1 to negate some strange horizontal shift
}
}
for(int r = 0; r < map.RowCount; ++r)
{
filtered.Set(r, 0, (Disparity)map[r, 0].Clone());
filtered.Set(r, map.ColumnCount - 2, (Disparity)map[r, map.ColumnCount - 2].Clone());
filtered.Set(r, map.ColumnCount - 1, (Disparity)map[r, map.ColumnCount - 1].Clone());
}
for(int c = 0; c < map.ColumnCount; ++c)
{
filtered.Set(0, c, (Disparity)map[0, c].Clone());
filtered.Set(map.RowCount - 1, c, (Disparity)map[map.RowCount - 1, c].Clone());
}
return filtered;
}
private void RestoreMap(object sender, RoutedEventArgs e)
{
MapLeftCurrent = (DisparityMap)_storedLeft.Clone();
MapRightCurrent = (DisparityMap)_storedRight.Clone();
}
private void StoreMap(object sender, RoutedEventArgs e)
{
_storedLeft = (DisparityMap)MapLeftCurrent.Clone();
_storedRight = (DisparityMap)MapRightCurrent.Clone();
}
private void ResetMaps(object sender, RoutedEventArgs e)
{
if(MapLeftBase != null)
MapLeftCurrent = (DisparityMap)MapLeftBase.Clone();
else
MapLeftCurrent = null;
if(MapRightBase != null)
MapRightCurrent = (DisparityMap)MapRightBase.Clone();
else
MapRightCurrent = null;
}
private void ApplyOnCurrent(object sender, RoutedEventArgs e)
{
if(MapLeftCurrent == null || MapRightCurrent == null)
return;
int index = (int)((Button)sender).Tag;
RefinementBlock block = _refinerBlocks[index];
var refiner = block.Refiner;
refiner.ImageLeft = ImageLeft;
refiner.ImageRight = ImageRight;
refiner.MapLeft = MapLeftCurrent;
refiner.MapRight = MapRightCurrent;
refiner.Init();
refiner.RefineMaps();
MapLeftCurrent = refiner.MapLeft;
MapRightCurrent = refiner.MapRight;
}
private void ApplyOnBase(object sender, RoutedEventArgs e)
{
if(_baseLeft == null || _baseRight == null)
return;
int index = (int)((Button)sender).Tag;
RefinementBlock block = _refinerBlocks[index];
var refiner = block.Refiner;
refiner.ImageLeft = ImageLeft;
refiner.ImageRight = ImageRight;
refiner.MapLeft = (DisparityMap)_baseLeft.Clone();
refiner.MapRight = (DisparityMap)_baseRight.Clone();
refiner.Init();
refiner.RefineMaps();
MapLeftCurrent = refiner.MapLeft;
MapRightCurrent = refiner.MapRight;
}
public DisparityMap FilterMap(DisparityMap map)
{
// Initialize pixel cells
_segmentedMap = new Cell[map.RowCount, map.ColumnCount];
_segments = new List<List<TPoint2D<int>>>();
for(int r = 0; r < map.RowCount; ++r)
{
for(int c = 0; c < map.ColumnCount; ++c)
{
_segmentedMap[r, c] = new Cell()
{
Disparity =
Math.Sqrt(map[r, c].SubDX * map[r, c].SubDX +
map[r, c].SubDY * map[r, c].SubDY),
Visited = false,
SegmentIndex = 0
};
}
}
// For each pixel, find its segment
for(int r = 0; r < map.RowCount; ++r)
{
for(int c = 0; c < map.ColumnCount; ++c)
{
FloodFillSegments(map, r, c);
}
}
// We have found segments
for(int i = 0; i < _segments.Count; ++i)
{
var segment = _segments[i];
// For each:
// 1) Check if it is smaller than MinSegment
if(segment.Count < MinSegmentSize)
{
// 2) Invalidate all disparities in segment
for(int p = 0; p < segment.Count; ++p)
{
map[segment[p].Y, segment[p].X].Flags = (int)DisparityFlags.Invalid;
}
// 3) If disparities should be interpolated : do so (but use only valid ones)
if(InterpolateInvalidated)
{
for(int p = 0; p < segment.Count; ++p)
{
Point2D point = segment[p];
if(point.X < 1 || point.Y < 1 || point.X >= map.ColumnCount - 1 || point.Y >= map.RowCount - 1)
continue;
double intDx = 0.0, intDy = 0.0;
double n = 0;
for(int dy = -1; dy <= 1; ++dy)
{
for(int dx = -1; dx <= 1; ++dx)
{
int py = point.Y + dy;
int px = point.X + dx;
if(map[py, px].IsValid())
{
intDx += map[py, px].SubDX;
intDy += map[py, px].SubDY;
n += 1;
}
}
}
if(n > 3)
{
map[point.Y, point.X].Flags = (int)DisparityFlags.Valid;
map[point.Y, point.X].SubDX = intDx / n;
map[point.Y, point.X].SubDY = intDy / n;
map[point.Y, point.X].DX = map[point.Y, point.X].SubDX.Round();
map[point.Y, point.X].DY = map[point.Y, point.X].SubDY.Round();
}
}
}
}
}
return map;
}
public void FloodFillSegments(DisparityMap map, int y, int x)
{
if(_segmentedMap[y, x].Visited)
return;
_segmentedMap[y, x].Visited = true;
_currentSegment = new List<Point2D>();
_currentSegment.Add(new Point2D(x, y));
_segmentedMap[y, x].SegmentIndex = _segments.Count;
_pointStack.Push(new Point2D(x, y));
while(_pointStack.Count > 0)
{
Point2D point = _pointStack.Pop();
if(point.X == 197 && point.Y == 97)
{
CheckAndAddToSegment(point.X, point.Y, point.X, point.Y);
}
if(point.Y > 0)
{
CheckAndAddToSegment(point.X, point.Y, point.X, point.Y - 1);
}
if(point.Y + 1 < _segmentedMap.GetLength(0))
{
CheckAndAddToSegment(point.X, point.Y, point.X, point.Y + 1);
}
if(point.X > 0)
{
CheckAndAddToSegment(point.X, point.Y, point.X - 1, point.Y);
}
if(point.X + 1 < _segmentedMap.GetLength(1))
{
CheckAndAddToSegment(point.X, point.Y, point.X + 1, point.Y);
}
}
_segments.Add(_currentSegment);
}
