function rgbdvideotest(SEQ)
% detect objects in real scenes

rootdir = [pwd '/'];

% addpath
addpath(genpath([rootdir '../']));

SEQ = sscanf(SEQ,'%d');

ENDFRAME = 0;
DATA_SKIP = 10;

load([rootdir sprintf('results_det/all_category_joint_%d.mat',SEQ)]);
resPrefix = 'cat_';

START_FRAME = 1;
ENDFRAMES = 10:10:300;
if ENDFRAME > 0
   END_FRAME = ENDFRAMES(ENDFRAME);
else
   END_FRAME = length(bbox);
end

% Data Location
POSEDIR = [rootdir 'poses/'];
DATADIR = '/projects/robomap3/nobackup/kevinlai/rgbddata/';
TESTDIR = [DATADIR 'test/'];
TRUTHDIR = [DATADIR 'groundtruth/'];

% Algorithm Parameters
VOXEL_SIZE = 0.01;
BG_WEIGHT = 0.1
CONVEX_EPS = 1/49
SMOOTHWEIGHT = 100;

CATEGORIES = {'bowl', 'cap', 'cereal_box', 'coffee_mug', 'soda_can'};

for cat=1:length(CATEGORIES)
   testcategory = CATEGORIES{cat};
   load([rootdir 'model/' testcategory '_' 'joint']);
   modelcell{cat} = model;
end

% set image directory
[imdir, labeldir] = gettestdir(TESTDIR,TRUTHDIR);
index = SEQ;

for i = 1:length(index)
    subimdir{i} = imdir{index(i)};
    sublabeldir{i} = labeldir{index(i)};
end

% Build 3D scene from individual video frames
bbox = bbox(START_FRAME:END_FRAME);
it = 0;
i = 1;
    % get the sequence name
    depthname = dir([subimdir{i} '*_cleandepth.png']);
    nums = zeros(1,length(depthname));
    for j=1:length(depthname)
       parts = regexp(depthname(j).name,'_','split');
       nums(j) = str2num(parts{end-1});
    end
    [Y I] = sort(nums);
    depthname = depthname(I);
    depthname = depthname(START_FRAME:END_FRAME);

    parts = regexp(subimdir{i}, '/', 'split');
    poses = load([POSEDIR parts{end-1} '.txt']);
    poses = poses(1:DATA_SKIP:end,3:end);
    poses = poses(1:length(bbox),:);
    ptFrameNums = zeros(0,1);

    % Initialize detections using windows from single-frame classifier responses
    scenePts = struct('pt',zeros(0,3),'score',zeros(0,length(modelcell)));
    curPt = 1;
    for k = 1:length(bbox)
       [k length(bbox)]
       depth = imread([subimdir{i} depthname(k).name]);
       mask = false(size(depth));
       mask(1:4:end,1:4:end) = true;
       mask(depth > 5000) = false;
       for o = 1:length(bbox{k})
          mask(isnan(bbox{k}(o).pixScores)) = false;
       end
       depth(~mask) = 0;
       [pcloud dist] = depthToCloud(depth);

       pts = reshape(pcloud,size(pcloud,1)*size(pcloud,2),3);

    if ~isempty(bbox{k})
       scores = zeros(size(pts,1),length(modelcell));
       for o = 1:length(bbox{k})
          det = bbox{k}(o);
          for m=1:length(modelcell)
             if strcmp(modelcell{m}.category, det.category) && modelcell{m}.instance == det.instance
                dInd = m;
                break;
             end
          end
          scores(:,dInd) = reshape(det.pixScores,size(pcloud,1)*size(pcloud,2),1);

       end
    else
       scores = nan(size(pts,1),length(modelcell));
    end
       ptInds = isnan(pts(:,1));
       pts(ptInds,:) = [];
       scores(ptInds,:) = [];
       pts = bsxfun(@plus,poses(k,5:7),qrot3d(pts,poses(k,1:4)));

       inds = curPt:(curPt+size(pts,1)-1);
       scenePts.pt(inds,:) = pts;
       scenePts.score(inds,:) = scores;
       ptFrameNums(inds,:) = k;
       curPt=curPt+size(pts,1);

    end
    clear bbox pts scores pcloud depth;

% Voxelize and run MRF
pts = scenePts.pt;
probs = scenePts.score;
clear scenePts;
probs = [probs BG_WEIGHT*(1-max(probs,[],2))];
logprobs = log(probs);
clear probs;

colors = zeros(length(modelcell),3,'uint8');
colors(1,:) = [255 0 0];
colors(2,:) = [0 255 0];
colors(3,:) = [0 0 255];
colors(4,:) = [255 255 0];
colors(5,:) = [0 255 255];
colors(6,:) = [128 128 128];

voxels = cell(1,length(modelcell));
[counts bins] = voxelize(pts,ones(size(pts,1),1),VOXEL_SIZE);

nzinds = find(counts > 0);
norms = nonzeros(counts);
[I J K] = ind2sub(size(counts),nzinds);
vox = [I J K];
[Y I] = ismember(bins,vox,'rows');
logprobs(Y,:) = bsxfun(@rdivide,logprobs(Y,:),norms(I(Y)));
logprobs(isnan(logprobs(:,1)),:) = 0;

for i=1:size(logprobs,2)
   voxels{i} = voxelize(pts,logprobs(:,i),VOXEL_SIZE);
end

nzinds = find(counts > 0);
inds = counts(nzinds) < 13;
bginds = nzinds(inds);
if nnz(inds) > 0
   counts(bginds) = 0;
   for i=1:(size(logprobs,2)-1)
      voxels{i}(bginds) = 0;
   end
   voxels{end}(bginds) = 1;
end
clear logprobs;

nzinds = find(counts > 0);
l = zeros(length(nzinds),length(voxels));
for i=1:length(voxels)
   l(:,i) = voxels{i}(nzinds);
end
[Y voxLabels] = max(l,[],2);
[I J K] = ind2sub(size(counts),nzinds);
V = [I J K]; clear I J K;
for c=1:(length(voxels)-1)
   inds = voxLabels == c;
   if nnz(inds) == 0, continue; end;
   [I J K] = ind2sub(size(counts),nzinds(inds));
   O = [I J K]; clear I J K;
   neighborhood = repmat(sum(O.^2,2),1,size(V,1)) - 2*O*V' + repmat(sum(V.^2,2),1,size(O,1))' <= sqrt(3*(1^2));
   for i=1:size(O,1)
      neighbors = find(neighborhood(i,:));
      if length(neighbors) < 2, continue; end;
      nScores = voxels{c}(nzinds(neighbors));
      neighbors(nScores >= voxels{c}(O(i,1),O(i,2),O(i,3))) = [];
      if ~isempty(neighbors)
         voxels{c}(nzinds(neighbors)) = voxels{c}(O(i,1),O(i,2),O(i,3));
      end
   end
end

% Max-based labeling
nzinds = find(counts > 0);
l = zeros(length(nzinds),length(voxels));
for i=1:length(voxels)
   l(:,i) = voxels{i}(nzinds);
end
[Y voxLabels] = max(l,[],2);
[I J K] = ind2sub(size(counts),nzinds);
vox = [I J K];

maxLabel.voxLabels = voxLabels;

% Write out voxel grid for MRF
datacosts = zeros(length(nzinds),length(voxels),'int32');
for c=1:length(voxels)
   costs = zeros(length(nzinds),1);
   costs(:) = full(-voxels{c}(nzinds)*10000);
   datacosts(:,c) = round(costs);
end
[I J K] = ind2sub(size(counts),nzinds);
V = [I J K];
clear I J K;
normNeighbors = computeNeighbors(V,sqrt(size(V,2)*(3^2)),5000,false);
normals = zeros(size(V,1),3);
perm = randperm(size(pts,1));
nPts = zeros(size(normNeighbors,1),1);
for i=1:size(normNeighbors,1)
   neighbors = normNeighbors(i,normNeighbors(i,:) > 0);
   nPts(i) = length(neighbors);
   if length(neighbors) > 2
      coeff = princomp(V(neighbors,:)+rand(length(neighbors),size(V,2))*1e-3);
      normals(i,:) = coeff(:,3);
      inds = find(bins(:,1) == V(i,1) & bins(:,2) == V(i,2) & bins(:,3) == V(i,3));
      if (poses(mode(ptFrameNums(inds,:)),5:7)-pts(inds(1),:))*normals(i,:)' < 0
          normals(i,:) = -normals(i,:);
      end
   end
end
clear normNeighbors;
neighbors = computeNeighbors(V,sqrt(size(V,2)*(1^2)),5000,true);
normalWeights = zeros(size(neighbors));
normalDirs = zeros(size(neighbors));
for i=1:size(neighbors,1)
   inds = neighbors(i,:) > 0;
   normalDirs(i,inds) =  sum(bsxfun(@minus,normals(i,:),normals(neighbors(i,inds),:)).*bsxfun(@minus,V(i,:),V(neighbors(i,inds),:)),2);
   normalWeights(i,inds) = 1./(sqrt(sum(bsxfun(@minus,normals(i,:),normals(neighbors(i,inds),:)).^2,2))+0.01);
end
normalWeights(normalDirs > 0) = normalWeights(normalDirs > 0)*(1 + CONVEX_EPS); % convex
normalWeights(normalDirs < 0) = normalWeights(normalDirs < 0)*CONVEX_EPS; % concave

smoothcost = SMOOTHWEIGHT*ones(length(voxels),length(voxels),'int32');
for c=1:length(voxels) smoothcost(c,c) = 0; end;

neighbormat = sparse(size(neighbors,1),size(neighbors,1));
for i=1:size(neighbors,1)
   neighbormat(i,neighbors(i,neighbors(i,:) > 0)) = round(normalWeights(i,neighbors(i,:) > 0)*10000);
end
clear neighbors normalWeights;

% Run MRF
gcomodel = GCO_Create(size(datacosts,1),length(voxels));
GCO_SetDataCost(gcomodel,datacosts');
GCO_SetSmoothCost(gcomodel,smoothcost);
GCO_SetNeighbors(gcomodel,neighbormat);
GCO_Expansion(gcomodel);
mrfLabels = GCO_GetLabeling(gcomodel);
GCO_Delete(gcomodel);

resPrefix = 'cat_';

load([rootdir sprintf('scene/%d.mat',SEQ)]);

predLabels = zeros(size(scene,1),1);
nzinds = find(counts > 0);
[I J K] = ind2sub(size(counts),nzinds);
mrfvox = [I J K];
[I J K] = ind2sub(size(counts),bginds);
bgvox = [I J K];

minPoint = min(pts);
bins = round(bsxfun(@minus,scene(:,1:3),minPoint)./VOXEL_SIZE)+1;
inInds = find(bins(:,1) > 0 | bins(:,2) > 0 | bins(:,3) > 0 | bins(:,1) <= size(counts,1) | bins(:,2) <= size(counts,2) | bins(:,3) <= size(counts,3));
[bgPtInds I] = ismember(bins(inInds,:),bgvox,'rows');
predLabels(inInds(bgPtInds)) = 6;
[objPtInds I] = ismember(bins(inInds,:),mrfvox,'rows');
predLabels(inInds(objPtInds)) = mrfLabels(I(objPtInds));

sceneLabels(predLabels <= 0) = [];
scene(predLabels <= 0,:) = [];
predLabels(predLabels <= 0) = [];
labels = 1:6;
confMat = zeros(length(labels));
for r=1:size(confMat)
for p=1:size(confMat)
   confMat(r,p) = nnz(sceneLabels == r & predLabels == p);
end
end
rConfMat = zeros(size(confMat));
for r=1:size(confMat)
   if sum(confMat(r,:)) > 0
      rConfMat(r,:) = confMat(r,:)/sum(confMat(r,:));
   end
end
pConfMat = zeros(size(confMat));
for p=1:size(confMat)
   if sum(confMat(:,p)) > 0
      pConfMat(:,p) = confMat(:,p)/sum(confMat(:,p));
   end
end
confMat
rConfMat
pConfMat
[mean(nonzeros(diag(rConfMat))) mean(nonzeros(diag(pConfMat)))]
[diag(rConfMat) diag(pConfMat)]


mkdir([rootdir 'results_mrf']);
save([rootdir sprintf('results_mrf/out_mrf_%d',SEQ)],'*onfMat');

data = scene(:,1:3);
color = colors(predLabels,:);
perm = randperm(size(data,1));
data = data(perm(1:min(200000,size(data,1))),:); color = color(perm(1:min(200000,size(data,1))),:);
writePly(data,[rootdir sprintf('results_mrf/mrf_%d.ply',SEQ)],color);


