function pfea = pyramidfea(im, depth, model)

% compute pyramid hog, color feature and depth features.
%
% pfea.fea{i} is the i-th level of the feature pyramid.
% pfea.scale{i} is the scaling factor used for the i-th level.

imsize = [size(im, 1) size(im, 2)];
pfea.imsize = imsize;
minscale = 0.25;
maxscale = 1.8;
numscale = ceil(log(maxscale/minscale)/log(model.sfactor))+1;

% our resize function wants floating point values
scale = minscale;
im = im2double(im);
depth = double(depth)/1000; % normalize to meter
cdepth = color(depth);
for i = 1:numscale
  if scale > 2
     break;
  elseif scale < 1
     pfea.fea{i} = getfeatures(im, cdepth, model.grid, scale, model);
  else
     pfea.fea{i} = getfeatures(im, cdepth, model.grid/2.0, scale/2.0, model);
  end
  pfea.scale(i) = scale;
  scale = scale*model.sfactor;
end

function fea = getfeatures(im, depth, grid, scale, model)

if model.imagetag == 1 & model.depthtag == 1
   ims = resize(im, scale);
   depths = resize(depth, scale);
   imfea = imfeatures(ims, grid);
   if grid < model.grid
      depthfea = depthfeatures(depths(:,:,1), grid, 2*scale); % get the true scale
   else
      depthfea = depthfeatures(depths(:,:,1), grid, scale);
   end
   fea = cat(3, imfea, depthfea);

elseif model.imagetag == 1 & model.depthtag == 0
   ims = resize(im, scale);
   fea = imfeatures(ims, grid);

elseif model.imagetag == 0 & model.depthtag == 1
   depths = resize(depth, scale);
   if grid < model.grid
      fea = depthfeatures(depths(:,:,1), grid, 2*scale); % get the true scale
   else
      fea = depthfeatures(depths(:,:,1), grid, scale);
   end
end

function imfea = imfeatures(im, grid)

% hog features over images
imfea = features(im, grid);

%% color features over images
%im = rgb2hsv(im);
%imcolor = cfeatures(im, grid);

%% concatenate all of vision features
%imfea = cat(3, imfea, imcolor);

function depthfea = depthfeatures(depth, grid, scale)

% hog feature over depths
depthfea = hfeatures(depth, grid);

% scale features over depths 
depthscale = sfeatures(depth/scale, grid);

% concatenate all of depth features
depthfea = cat(3, depthfea, depthscale);

