function [labeledVector, confidenceVector] = graspRegionDetectorInImage(actualImages, images, depthImage, discreteDepthImage, depthMask, points, smoothFeatures, medianFilter, calc3dFeats, multiplyValues, weightOfThisPrediction, baseConfidenceMask, baseNumOfPredictions, runLabeling, displayFlag, trainingFilenameToUse, testFeatureVector)
%Uses theta from the training part stored in "trainingWeightsFilename"

% This code is copyrighted by Stanford University.
% Author: Ashutosh Saxena, Justin Drieymeyer
% Code cannot be used for commercial purposes.
% Use of the code, if used with permission from authors, must acknowledge:
% Learning to Grasp Novel Objects using Vision,
% Ashutosh Saxena, Justin Driemeyer, Justin Kearns, Chioma Osondu, Andrew Y. Ng. In 10th International Symposium on Experimental Robotics, ISER, 2006.
% Robotic Grasping of Novel Obects,
% Ashutosh Saxena, Justing Driemeyer, Justin Kearns, Andrew Y. Ng, In Neural Information Processing Systems (NIPS), 2006.
% More details at: http://ai.stanford.edu/~asaxena/learninggrasp/
% Version 0.1:   Aug 2006.



global trainingWeightsFilename trainingWeightsFilenameZoomed fullFeatsToUse depthBasedFeats
nPerImage = 3;

zeroDepthFeatures = 0;

positiveThreshold = 0.25;

imrows = 48;
imcols = 64;
gridSize = 10;

if nargin < 17
  testFeatureVector = makeFeatureVector(images, depthImage, discreteDepthImage, points, calc3dFeats, displayFlag);
end

if nargin < 6
  depthMask = ones(imrows*gridSize, imcols*gridSize);
end

if nargin < 7
  smoothFeatures = false;
end

if nargin < 8
  medianFilter = true;
end

if nargin < 9
  calc3dFeats = 1;
end

if nargin < 10
  multiplyValues = 0;
end

if nargin < 11
  weightOfThisPrediction = 1;
end

if nargin < 12
  baseConfidenceMask = zeros(imrows, imcols);
end

if nargin < 13
  baseNumOfPredictions = zeros(imrows, imcols);
end

if nargin < 14
  runLabeling = false;
end

if nargin < 15
  displayFlag = false;
end

if nargin < 16
  trainingFilenameToUse = trainingWeightsFilenameZoomed;
end

rowFilter = zeros(imrows, imrows*gridSize);
for r=1:imrows
  rowFilter(r, ((r-1)*gridSize + 1):r*gridSize) = 1;
end
colFilter = zeros(imcols*gridSize, imcols);
for c=1:imcols
  colFilter(((c-1)*gridSize + 1):c*gridSize, c) = 1;
end

validDepth = zeros(imrows, imcols);
numOfObjects = 0+max(max(depthMask));
objectMasks = zeros(imrows, imcols, numOfObjects);
if (numOfObjects > 0)
  for objectID = 1:numOfObjects
    thisObjectMask = zeros(imrows*gridSize, imcols*gridSize);
    thisObjectMask(find(depthMask == objectID)) = 1;
    thisObjectMask = rowFilter * thisObjectMask * colFilter;
    objectMasks(:, :, objectID) = thisObjectMask ~= 0;
    validDepth = validDepth | objectMasks(:, :, objectID);
  end;
end

pic = 1;
if smoothFeatures
  filterRadius = 1;
  baseFilter = fspecial('gaussian');
  numOfFeatures = size(testFeatureVector, 2);

  thisFeatureMatrix = reshape(testFeatureVector(((pic-1)*imrows*imcols + 1):(pic*imrows*imcols), :), [imrows, imcols, numOfFeatures]);
  smoothedFeatureMatrix = zeros(size(thisFeatureMatrix));
  rowAddition = zeros(filterRadius, imcols + 2*filterRadius, numOfFeatures);
  colAddition = zeros(imrows, filterRadius, numOfFeatures);
  thisFeatureMatrix = [rowAddition; colAddition thisFeatureMatrix colAddition; rowAddition];
  if (numOfObjects > 0)
    rowAddition = zeros(filterRadius, imcols + 2*filterRadius, numOfObjects);
    colAddition = zeros(imrows, filterRadius, numOfObjects);
    objectMasks = [rowAddition; colAddition objectMasks colAddition; rowAddition];
    for y=1:imrows;   %48
      for x=1:imcols; %64
        thisFilt = zeros(3,3);
        for objectID = 1:numOfObjects
          if (objectMasks(y+1, x+1, objectID) == 1)
            thisFilt = thisFilt | objectMasks(y:y+2, x:x+2, objectID);
          end
        end
        thisFilt = thisFilt .* baseFilter;
        totalSum = sum(sum(thisFilt));
        if (totalSum == 0)
          thisFilt = [0 0 0; 0 1 0; 0 0 0];
        else
          thisFilt = thisFilt / totalSum;
        end
        for feat = 1:numOfFeatures
          smoothedFeatureMatrix(y, x, feat) = sum(sum(thisFilt .* thisFeatureMatrix(y:y+2, x:x+2, feat)));
        end
      end;
    end;
  end
  testFeatureVector(((pic-1)*imrows*imcols + 1):(pic*imrows*imcols), :) = reshape(smoothedFeatureMatrix, [imrows*imcols, numOfFeatures]);
end

load(trainingFilenameToUse);
if zeroDepthFeatures
  testFeatureVector(:, depthBasedFeats) = 0;
end
testFeatureVector = testFeatureVector(:, fullFeatsToUse);
predictions = glmval(b, testFeatureVector, 'logit');
predictions = (weightOfThisPrediction * predictions);
if ((multiplyValues == 1) && (max(max(baseNumOfPredictions)) > 0))
  predictions = predictions .* (reshape(baseConfidenceMask, size(predictions)));
else
  predictions = predictions + (reshape(baseConfidenceMask, size(predictions)));
end
predictionsDivisor = weightOfThisPrediction * ones(size(predictions)) + reshape(baseNumOfPredictions, size(predictions));
predictions = predictions ./ predictionsDivisor;

labeledImages = reshape(predictions, [imrows, imcols, length(predictions)/imrows/imcols]);

dishwasherMask = validDepth;

for n=1:size(labeledImages,3)
  medianFilteredImages(:,:,n) = labeledImages(:,:,n);
  if medianFilter
    medianFilteredImages(:,:,n) = ordfilt2(medianFilteredImages(:,:,n), 5, true(3) );
  end
  medianFilteredImages(:,:,n) = dishwasherMask .* medianFilteredImages(:,:,n);
end

confidenceVector = reshape(medianFilteredImages, size(predictions));

currConfidenceVector = confidenceVector;

neighborsPatch = ones(3,3); %[0 1 0; 1 1 1; 0 1 0];

labeledVector = zeros( size(predictions));
if runLabeling
  for pic=1: (length(predictions)/imrows/imcols)
    [v ind] = sort( -currConfidenceVector( ((pic-1)*imrows*imcols+1) : (pic*imrows*imcols) )  );
    labeledVector( ((pic-1)*imrows*imcols) + ind(1:nPerImage)) = 1;

    
    labeledVector = zeros( size(predictions));
    thisInd = ind(1);
    nearMask = zeros(imrows, imcols);
    for thisN = 1:nPerImage;
      [currR, currC] = ind2sub([imrows, imcols], thisInd);
      labeledVector( ((pic-1)*imrows*imcols) + thisInd) = 1;
      currConfidenceVector( (pic-1)*imrows*imcols + thisInd ) = 0;
      [v ind] = sort( -currConfidenceVector( ((pic-1)*imrows*imcols+1) : (pic*imrows*imcols) )  );
      
      %% nearMask = zeros(imrows, imcols);
      nearMMinR = currR-1;
      nearMMinC = currC-1;
      nearMMaxR = currR+1;
      nearMMaxC = currC+1;
      nearMNeighborsPatch = neighborsPatch;
      if (nearMMinR < 1)
        numToCut = 1 - nearMMinR;
        nearMNeighborsPatch = nearMNeighborsPatch((1+numToCut):end, :);
        nearMMinR = 1;
      end
      if (nearMMinC < 1)
        numToCut = 1 - nearMMinC;
        nearMNeighborsPatch = nearMNeighborsPatch(:, (1+numToCut):end);
        nearMMinC = 1;
      end
      if (nearMMaxR > imrows)
        numToCut = imrows - nearMMaxR;
        nearMNeighborsPatch = nearMNeighborsPatch(1:(end + numToCut), :);
        nearMMaxR = imrows;
      end
      if (nearMMaxC > imcols)
        numToCut = imcols - nearMMaxC;
        nearMNeighborsPatch = nearMNeighborsPatch(:, 1:(end + numToCut));
        nearMMaxC = imcols;
      end
      nearMask(nearMMinR:nearMMaxR, nearMMinC:nearMMaxC) = nearMask(nearMMinR:nearMMaxR, nearMMinC:nearMMaxC) | nearMNeighborsPatch;
      currNearVector = reshape(currConfidenceVector(((pic-1)*imrows*imcols+1) : (pic*imrows*imcols)), [imrows, imcols]);
      currNearVector = currNearVector .* nearMask;
      currNearVector = reshape(currNearVector, size(predictions));
      [vNear, indNear] = sort( -currNearVector );

      if (displayFlag)      
        [vNear(1) v(1)]
      end

      if vNear(1) <= 0.25 * v(1);
        thisInd = indNear(1);
      else
        thisInd = ind(1);
      end
      
    end
  end
end

temp = reshape(labeledVector, [imrows, imcols, length(predictions)/48/64]);

positiveThreshold = mean( medianFilteredImages(:) ) / 10;%20;

A_LR = actualImages(:,:,:,pic);
        
for y=1:imrows;   %48
  for x=1:imcols; %64
    if temp(y, x)% && objectMaskScaled(y, x)
      A_LR(gridSize*y-(gridSize-1):y*gridSize, gridSize*x-(gridSize-1):x*gridSize, 1) = 256;
    end;
  end;
end;

if displayFlag
  imageBlackAndWhite = (double(actualImages(:, :, 1)) + double(actualImages(:, :, 2)) + double(actualImages(:, :, 3))) / (3*255);
  scaledBaseConfidenceMaskLarge = zeros(size(actualImages(:,:,1)));
  for r = 1:size(medianFilteredImages, 1)
    for c = 1:size(medianFilteredImages, 2)
      scaledBaseConfidenceMaskLarge((((r-1)*gridSize) + 1):(r*gridSize), (((c-1)*gridSize) + 1):(c*gridSize), :) = medianFilteredImages(r, c);
    end
  end
  scaledBaseConfidenceMaskLarge = scaledBaseConfidenceMaskLarge / max(max(scaledBaseConfidenceMaskLarge));
  imageOverlay = cat(3, scaledBaseConfidenceMaskLarge, zeros(size(imageBlackAndWhite)), imageBlackAndWhite);
  for n=1:size(labeledImages,3)
    figure; 
    subplot(2,2,1), image( imageOverlay ); axis equal;
    subplot(2,2,2), imagesc( medianFilteredImages(:,:,n) );    axis equal;
    subplot(2,2,3), imagesc( A_LR );    axis equal;
    subplot(2,2,4), imagesc( images(:,:,:,pic)); axis equal;
    titleString = 'Only Masked';
    if smoothFeatures
      titleString = 'Features Smoothed';
    end
    if medianFilter
      if strcmp(titleString, 'Only Masked')
        titleString = 'Median Filtered';
      else
        titleString = [titleString ' and Median Filtered'];
      end
    end
    title(titleString);
  end
end