function d = disparity(I1,I2,varargin)
% DISPARITY Compute disparity map.
%   D = DISPARITY(I1,I2) returns the disparity map for a pair of stereo
%   images, I1 and I2. I1 and I2 must have the same size and must be
%   rectified such that the corresponding points are located on the same
%   rows. The returned disparity map, D, has the same size as I1 and I2.
%   Each element of D specifies the disparity for the corresponding pixel
%   in I1.
% 
%   The disparity map D is computed in three steps: (1) Compute a measure
%   of contrast of the image by using the Sobel filter; (2) Compute the
%   disparity for each of the pixels by using block matching and sum of
%   absolute differences (SAD); (3) Optionally, mark the pixels for which
%   disparity was not computed reliably with -REALMAX('single').
% 
%   D = DISPARITY(I1,I2,ROI1,ROI2) specifies the regions-of-interest, ROI1
%   and ROI2, for computing the disparity map. Both ROI1 and ROI2 must have
%   the format of [X Y WIDTH HEIGHT] and must be fully contained in I1 and
%   I2. ROI1 and ROI2 must have the same size.
% 
%   D = DISPARITY(...,PARAM1,VAL1,PARAM2,VAL2,...) sets parameters for
%   computing the disparity map:
% 
%   'ContrastThreshold'    A scalar value, 0 < ContrastThreshold <= 1,
%                          defining the range of contrast values. Set it to
%                          1 to saturate the least number of pixels and
%                          reduce it to saturate more pixels.
% 
%                          Default: 0.5
% 
%   'BlockSize'            An odd integer, 5 <= BlockSize <= 255. Size of 
%                          the block is BlockSize-by-BlockSize pixels.
% 
%                          Default: 15
% 
%   'DisparityRange'       A two-element vector, [MinDisparity MaxDisparity],
%                          defining the range of disparity. MinDisparity
%                          must be an integer and the difference between
%                          MaxDisparity and MinDisparity must be divisible
%                          by 16.
% 
%                          Default: [0 64]
% 
%   'TextureThreshold'     A scalar value, 0 <= TextureThreshold <= 1, 
%                          defining the minimum texture. If the texture of
%                          a pixel is smaller, the computed disparity is
%                          less reliable. When you increase this parameter,
%                          more pixels with low texture will be marked as
%                          unreliable. You can also set this parameter to 0
%                          to disable it. See notes for more details.
% 
%                          Default: 0.0002
% 
%   'UniquenessThreshold'  A non-negative integer defining the minimum
%                          value of uniqueness. If a pixel is less unique,
%                          the disparity computed for it is less reliable.
%                          Increasing this parameter will result in marking
%                          more pixels unreliable. You can set this
%                          parameter to 0 to disable it. See notes for more
%                          details.
% 
%                          Default: 15
% 
%   'DistanceThreshold'    A non-negative integer defining the maximum
%                          distance for left-right checking. If the
%                          left-right checking distance for a pixel is
%                          larger, the disparity computed for this pixel is
%                          less reliable. You can decrease this parameter
%                          to mark more pixels with large left-right
%                          checking distance, and to make the disparity map
%                          more reliable. You can also set this parameter
%                          to an empty matrix [] to disable it. See notes
%                          for more details.
% 
%                          Default: [] (disabled)
% 
% Notes
% -----
% - The returned disparity values are rounded to 1/16th pixel.
% 
% - The texture, T, of a pixel is defined as the sum of saturated contrast
%   computed over the BlockSize-by-BlockSize window around the pixel. If
%   T < X * TextureThreshold * BlockSize^2, where X is the maximum value
%   supported by the class of I1 and I2, the disparity computed for the
%   pixel is considered unreliable and will be marked.
% 
% - Uniqueness is defined as a ratio of the optimal disparity estimation
%   and the less optimal disparity estimation. Let K and V be the best
%   estimated disparity and their corresponding SAD value, respectively.
%   V is the smallest SAD value over the whole disparity range. Let v be
%   the smallest SAD value over the whole disparity range excluding K,
%   K-1, and K+1. If v < V * (1+0.01*UniquenessThreshold), the disparity D
%   is considered unreliable and will be marked.
% 
% - DistanceThreshold specifies the maximum distance between two points,
%   if they match to the same point in the other image. Let p1 be a point
%   in I1. The function searches for p1's best match in I2 (left-to-right
%   check) and finds, say, point p2. The function then searches for p2's
%   best match in I1 (right-to-left check) and finds, say, point p3. If
%   the distance between p1 and p3 is greater than DistanceThreshold, the
%   disparity for point p1 is considered unreliable and will be
%   marked.
% 
% - You can improve the speed of the function by setting the class of I1
%   and I2 to uint8, and the number of columns to be divisible by 4.
% 
% Class Support
% -------------
%   All inputs must be real, finite, and nonsparse. I1 and I2 must have the
%   same class and must be uint8, uint16, int16, single, or double. ROI1
%   and ROI2 must have the same class and must be integer, single, or
%   double. The output is single.
% 
% Example
% -------
%   % Load the images and convert them to gray scale.
%   I1 = rgb2gray(imread('scene_left.png'));
%   I2 = rgb2gray(imread('scene_right.png'));
%   % Show the stereo images.
%   cvexShowMatches(I1, I2);
%   % Compute the disparity map
%   d = disparity(I1, I2, 'BlockSize', 35, ...
%     'DisparityRange', [-6 10], 'UniquenessThreshold', 0);
%   % Show the disparity map
%   cvexShowDisparity(d);
% 
% See also estimateFundamentalMatrix, estimateUncalibratedRectification
% 
% References:
% -----------
% [1] K. Konolige, "Small Vision Systems: Hardware and Implementation,"
%     Proceedings of the 8th International Symposium in Robotic Research,
%     pages 203-212, 1997.
%
% [2] G. Bradski and A. Kaehler, "Learning OpenCV : Computer Vision with
%     the OpenCV Library," O'Reilly, Sebastopol, CA, 2008.

% Copyright  The MathWorks, Inc.
% $Revision: 1.1.6.2 $  $Date: 2011/05/09 03:42:49 $

%--------------------------------------------------------------------------
% Parse the inputs
%--------------------------------------------------------------------------
r = parseInputs(I1, I2, varargin);

%--------------------------------------------------------------------------
% Parameters accessible from the main interface
%--------------------------------------------------------------------------
opt.preFilterCap        = int32(ceil(63 * r.ContrastThreshold));
opt.SADWindowSize       = int32(r.BlockSize);
opt.minDisparity        = int32(r.DisparityRange(1));
opt.numberOfDisparities = int32(r.DisparityRange(2) - r.DisparityRange(1));
opt.textureThreshold    = int32(255 * r.TextureThreshold * r.BlockSize^2);
opt.uniquenessRatio     = int32(r.UniquenessThreshold);

if isempty(r.DistanceThreshold)
  opt.disp12MaxDiff       = int32(-1);
else
  opt.disp12MaxDiff       = int32(r.DistanceThreshold);
end

% Convert ROI1 and ROI2 from 1-based to 0-based
opt.roi1                = int32(r.ROI1) - int32([1 1 0 0]);
opt.roi2                = int32(r.ROI2) - int32([1 1 0 0]);

%--------------------------------------------------------------------------
% Other OpenCV parameters which are not exposed in the main interface
%--------------------------------------------------------------------------
opt.preFilterType       = int32(1);    % Fixed to CV_STEREO_BM_XSOBEL
opt.preFilterSize       = int32(15);
opt.speckleWindowSize   = int32(0);
opt.speckleRange        = int32(0);
opt.trySmallerWindows   = int32(0);

%--------------------------------------------------------------------------
% Compute disparity
%--------------------------------------------------------------------------
if ~isa(I1, 'uint8')
  I1 = im2uint8(I1);
  I2 = im2uint8(I2);
end
d = ocvDisparityBM(I1, I2, opt);


%==========================================================================
% Parse and check inputs
%========================================================================== 
function r = parseInputs(I1, I2, optionalParameters)
% Instantiate an input parser
parser = inputParser;
parser.FunctionName = mfilename;
parser.CaseSensitive = true;

%--------------------------------------------------------------------------
% Check I1 and I2
%--------------------------------------------------------------------------
checkImage('I1', I1, 1);
checkImage('I2', I2, 2);
imageSize = size(I1);
if size(I2,1)~=imageSize(1) || size(I2,2)~=imageSize(2)
  error('vision:disparity:imageSizeNotMatch',...
    'Expected I1 and I2 to have the same size.');
end
if ~strcmp(class(I1), class(I2))
  error('vision:disparity:imageClassNotMatch',...
    'Expected I1 and I2 to have the same class.');
end

%--------------------------------------------------------------------------
% Specify the optional parameters, ROI1 and ROI2
%--------------------------------------------------------------------------
parser.addOptional('ROI1', int32([1 1 0 0]), ...
  @(x)(checkROI('ROI1', x, imageSize, 3)));
parser.addOptional('ROI2', int32([1 1 0 0]), ...
  @(x)(checkROI('ROI2', x, imageSize, 4)));

%--------------------------------------------------------------------------
% Specify the optional parameter value pairs
%--------------------------------------------------------------------------
parser.addParamValue('ContrastThreshold', 0.5, ...
  @(x)(checkContrastThreshold(x)));
parser.addParamValue('BlockSize', 15, ...
  @(x)(checkBlockSize(x, imageSize)));
parser.addParamValue('DisparityRange', [0 64], ...
  @(x)(checkDisparityRange(x, imageSize)));
parser.addParamValue('TextureThreshold', 0.0002, ...
  @(x)(checkTextureThreshold(x)));
parser.addParamValue('UniquenessThreshold', 15, ...
  @(x)(checkUniquenessThreshold(x)));
parser.addParamValue('DistanceThreshold', [], ...
  @(x)(checkDistanceThreshold(x, imageSize)));

%--------------------------------------------------------------------------
% Parse and check the optional parameters
%--------------------------------------------------------------------------
parser.parse(optionalParameters{:});
r = parser.Results;

%--------------------------------------------------------------------------
% Check if ROI1 and ROI2 have the same size
%--------------------------------------------------------------------------
if any(r.ROI1(3:4) ~= r.ROI2(3:4))
  error('vision:disparity:ROISizeMismatch',...
    'Expected ROI1 and ROI2 to have the same size.');
end
if ~isa(r.ROI1, class(r.ROI2))
  error('vision:disparity:ROIClassMismatch',...
    'Expected ROI1 and ROI2 to have the same class.');
end

%--------------------------------------------------------------------------
% Check if BlockSize is smaller than both image size and the ROI size.
%--------------------------------------------------------------------------
if all(r.ROI1(3:4) > 0)
  checkBlockSize(r.BlockSize, r.ROI1([4,3]));
end
checkBlockSize(r.BlockSize, imageSize);

%========================================================================== 
function r = checkImage(name, value, position)
validateattributes(value, ...
  {'uint8', 'uint16', 'int16', 'single', 'double'}, ...
  {'real', 'nonempty', 'nonsparse', '2d'},...
  mfilename, name, position);
r = 1;

%========================================================================== 
function r = checkROI(name, value, imageSize, position)
validateattributes(value, {'numeric'}, ...
  {'real', 'nonsparse', 'integer', 'positive', 'finite', 'size', [1, 4]},...
  mfilename, name, position);
if value(1)+value(3) > imageSize(2)+1 || value(2)+value(4) > imageSize(1)+1
  error('vision:disparity:invalidROIValue',...
    'Expected ROI1 and ROI2 to be fully contained in I1 and I2.');
end
r = 1;

%========================================================================== 
function r = checkContrastThreshold(value)
validateattributes(value, {'numeric'}, ...
  {'real', 'scalar', '>', 0, '<=', 1},...
  mfilename, 'ContrastThreshold');
r = 1;

%========================================================================== 
function r = checkBlockSize(value, imageSize)
maxBlockSize = min([imageSize, 255]);
validateattributes(value, {'numeric'}, ...
  {'real', 'scalar', 'integer', 'odd', '>=', 5, '<=', maxBlockSize},...
  mfilename, 'BlockSize');
r = 1;

%========================================================================== 
function r = checkDisparityRange(value,imageSize)
maxValue = min(imageSize);
validateattributes(value, {'numeric'}, ...
  {'real', 'nonsparse', 'integer', 'finite', 'size', [1,2], ...
  '>', -maxValue, '<', maxValue},...
  mfilename, 'DisparityRange');

if value(2) <= value(1) || mod(value(2) - value(1), 16) ~= 0
  error('vision:disparity:invalidDisparityRange', ...
    ['Expected the second element of DisparityRange to be greater ',...
    'than the first element, and their difference be divisible by 16.']);
end
r = 1;

%========================================================================== 
function r = checkTextureThreshold(value)
validateattributes(value, {'numeric'}, ...
  {'real', 'scalar', 'finite', 'nonnegative', '<', 1},...
  mfilename, 'TextureThreshold');
r = 1;

%========================================================================== 
function r = checkUniquenessThreshold(value)
validateattributes(value, {'numeric'}, ...
  {'real', 'scalar', 'integer', 'finite', 'nonnegative'},...
  mfilename, 'UniquenessThreshold');
r = 1;

%========================================================================== 
function r = checkDistanceThreshold(value, imageSize)
maxValue = min(imageSize);
if ~isempty(value)
  validateattributes(value, {'numeric'}, ...
    {'real', 'scalar', 'integer', 'finite', 'nonnegative','<',maxValue},...
    mfilename, 'DistanceThreshold');
end
r = 1;
