diff --git a/LICENSE b/LICENSE
new file mode 100644
index 0000000..0b01721
--- /dev/null
+++ b/LICENSE
@@ -0,0 +1,22 @@
+Copyright (c) 2016, The Trustees of Princeton University
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met: 
+
+1. Redistributions of source code must retain the above copyright notice, this
+   list of conditions and the following disclaimer. 
+2. Redistributions in binary form must reproduce the above copyright notice,
+   this list of conditions and the following disclaimer in the documentation
+   and/or other materials provided with the distribution. 
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
\ No newline at end of file
diff --git a/evaluation/benchmark.m b/evaluation/benchmark.m
index 9a71c39..adb5dc6 100644
--- a/evaluation/benchmark.m
+++ b/evaluation/benchmark.m
@@ -2,7 +2,7 @@
 %
 % Start ROS services marvin_convnet and pose_estimation before running this
 % script (and realsense_camera if necessary)
-% Set marvin_convnet to mode 2 to read RGB-D data from disk
+% Set marvin_convnet to _service_mode:=2 to read RGB-D data from disk
 % Run in same Matlab instance as where ROS service pose_estimation was started
 %
 % ---------------------------------------------------------
@@ -17,107 +17,176 @@
 % User configurations (change me)
 tmpDataPath = '/home/andyz/apc/toolbox/data/tmp'; % Temporary directory used by marvin_convnet, where all RGB-D images and detection masks are saved
 benchmarkPath = '/home/andyz/apc/toolbox/data/benchmark'; % Benchmark directory
+modelsPath = '/home/andyz/apc/toolbox/ros-packages/catkin_ws/src/pose_estimation/src/models/objects'; % Directory holding pre-scanned object models
 rgbdUtilsPath = '/home/andyz/apc/toolbox/rgbd-utils'; % Directory of RGB-D toolbox utilities
+visUtilsPath = '/home/andyz/apc/toolbox/vis-utils'; % Directory of visualization utilities
 resultVisPath = 'results'; % Directory to save visualizations of pose estimation results
 
 % Load benchmark scenes and labels
 sceneList = {};
+sceneNames = {};
 fid = fopen(fullfile(benchmarkPath,'info.txt'));
 while ~feof(fid)
-    scenePath = fullfile(benchmarkPath,fscanf(fid,'%s,'));
+    sceneName = fscanf(fid,'%s,');
+    scenePath = fullfile(benchmarkPath,sceneName);
     if ~feof(fid)
         sceneList{length(sceneList)+1} = scenePath(1:(end-1));
+        sceneNames{length(sceneNames)+1} = sceneName;
     end
 end
 fclose(fid);
 
 % Add paths and create directories
 addpath(genpath(rgbdUtilsPath));
+addpath(genpath(visUtilsPath));
 if ~exist(tmpDataPath,'file')
     mkdir(tmpDataPath);
 end
 if ~exist(resultVisPath,'file')
     mkdir(resultVisPath);
 end
+  
+% Get set of colors for visualizations
+colorPalette = getColorPalette();
 
 % Loop through benchmark scenes
 for sceneIdx = 1:length(sceneList)
-  scenePath = sceneList{sceneIdx};
-  fprintf(sprintf('[Test] %s\n',scenePath));
-  
-  % Remove all files in temporary folder used by marvin_convnet
-  delete(fullfile(tmpDataPath,'*'));
-  
-  % Copy current scene to temporary folder used by marvin_convnet
-  copyfile(fullfile(scenePath,'*'),tmpDataPath);
-  
-  % Load scene
-  sceneData = loadScene(scenePath);
-  numFrames = length(sceneData.colorFrames);
-  
-  % Call marvin_convnet to do 2D object segmentation for each RGB-D frame
-  fprintf('    [Segmentation] Frame ');
-  binIds = 'ABCDEFGHIJKL';
-  binNum = strfind(binIds,sceneData.binId)-1;
-  for frameIdx = 0:(numFrames-1)
-    fprintf('%d ',frameIdx);
-    [client,reqMsg] = rossvcclient('/marvin_convnet');
-    reqMsg.BinId = binNum;
-    reqMsg.ObjectNames = sceneData.objects;
-    reqMsg.FrameId = frameIdx;
-    response = call(client,reqMsg);
-  end
-  fprintf('\n');
-  
-  % Call apc_posest to do object pose estimation for the sequence
-  [client,reqMsg] = rossvcclient('/pose_estimation');
-  reqMsg.SceneFiles = tmpDataPath;
-  if ~isempty(strfind(scenePath,'office'))
-    reqMsg.CalibrationFiles = '/home/andyz/apc/toolbox/data/benchmark/office/calibration';
-  elseif ~isempty(strfind(scenePath,'warehouse'))
-    reqMsg.CalibrationFiles = '/home/andyz/apc/toolbox/data/benchmark/warehouse/calibration';
-  end
-  reqMsg.DoCalibration = true;
-  response = call(client,reqMsg);
-  
-%   % Copy result and visualization files to results folder
-%   resultFiles = dir(fullfile(tmpDataPath,'*.result.txt'));
-%   resultImages = dir(fullfile(tmpDataPath,'*.result.png'));
-%   visFiles = dir('*.ply');
-%   visObjNames = {};
-%   visObjScores = {};
-%   for resultIdx = 1:length(resultFiles)
-%     currResultFile = resultFiles(resultIdx).name;
-%     currResultFilenameDotIdx = strfind(currResultFile,'.');
-%     currObjName = currResultFile(1:(currResultFilenameDotIdx(1)-1));
-%     currResultFileMatrix = dlmread(fullfile(tmpDataPath,currResultFile));
-%     currObjScore = currResultFileMatrix(length(currResultFileMatrix));
-%     visObjNames{length(visObjNames)+1} = currObjName;
-%     visObjScores{length(visObjScores)+1} = currObjScore;
-% %     copyfile(fullfile(tmpPath,resultFiles(resultIdx).name),fullfile(resultPath,[testDir(seqIdx).name,'.',resultFiles(resultIdx).name]));
-%   end
-%   for resultIdx = 1:length(resultImages)
-%     currResultFile = resultImages(resultIdx).name;
-%     visResultImage = imread(fullfile(tmpDataPath,currResultFile));
-%     [sortVals,sortIdx] = sortrows(cell2mat(visObjScores)',-1);
-%     visResultImage = insertText(visResultImage,[10 10],'Confidence :  Object ID','TextColor','white','BoxColor','black');
-%     textY = 32;
-%     for objIdx = 1:length(visObjNames)
-%       if ~isempty(strfind(currResultFile,visObjNames{sortIdx(objIdx)}))%resultIdx == sortIdx(objIdx)
-%         visResultImage = insertText(visResultImage,[10 textY],sprintf('%f  :  %s',visObjScores{sortIdx(objIdx)},visObjNames{sortIdx(objIdx)}),'TextColor','black','BoxColor','green');
-%       else
-%         visResultImage = insertText(visResultImage,[10 textY],sprintf('%f  :  %s',visObjScores{sortIdx(objIdx)},visObjNames{sortIdx(objIdx)}),'TextColor','white','BoxColor','black');
-%       end
-%       textY = textY + 22;
-%     end
-%     imwrite(visResultImage,fullfile(resultPath,strcat(testDir(sceneIdx).name,'.',resultImages(resultIdx).name)));
-% %     copyfile(fullfile(tmpPath,resultImages(resultIdx).name),fullfile(resultPath,[testDir(seqIdx).name,'.',resultImages(resultIdx).name]));
-%   end
-% %   copyfile(fullfile(tmpPath,'frame-000007.color.png'),fullfile(resultPath,[testDir(seqIdx).name,'.','frame-000007.color.png']));
-%   for visFileIdx = 1:length(visFiles)
-%     movefile(visFiles(visFileIdx).name,fullfile(resultPath,[testDir(sceneIdx).name,'.',visFiles(visFileIdx).name]));
-%   end
-  
+    scenePath = sceneList{sceneIdx};
+    fprintf(sprintf('[Test] %s\n',scenePath));
+
+    % Remove all files in temporary folder used by marvin_convnet
+    if exist(fullfile(tmpDataPath,'raw'),'file')
+        rmdir(fullfile(tmpDataPath,'raw'),'s');
+    end
+    if exist(fullfile(tmpDataPath,'results'),'file')
+        rmdir(fullfile(tmpDataPath,'results'),'s');
+    end
+    delete(fullfile(tmpDataPath,'*'));
+
+    % Copy current scene to temporary folder used by marvin_convnet
+    copyfile(fullfile(scenePath,'*'),tmpDataPath);
+
+    % Load scene
+    sceneData = loadScene(scenePath);
+    numFrames = length(sceneData.colorFrames);
+
+    % Call marvin_convnet to do 2D object segmentation for each RGB-D frame
+    fprintf('    [Segmentation] Frame ');
+    binIds = 'ABCDEFGHIJKL';
+    binNum = strfind(binIds,sceneData.binId)-1;
+    for frameIdx = 0:(numFrames-1)
+        fprintf('%d ',frameIdx);
+        [client,reqMsg] = rossvcclient('/marvin_convnet');
+        reqMsg.BinId = binNum;
+        reqMsg.ObjectNames = sceneData.objects;
+        reqMsg.FrameId = frameIdx;
+        response = call(client,reqMsg);
+    end
+    fprintf('\n');
+
+    % Call pose_estimation to do 6D object pose estimation for the sequence
+    [client,reqMsg] = rossvcclient('/pose_estimation');
+    reqMsg.SceneFiles = tmpDataPath;
+    if ~isempty(strfind(scenePath,'office'))
+        reqMsg.CalibrationFiles = '/home/andyz/apc/toolbox/data/benchmark/office/calibration';
+    elseif ~isempty(strfind(scenePath,'warehouse'))
+        reqMsg.CalibrationFiles = '/home/andyz/apc/toolbox/data/benchmark/warehouse/calibration';
+    end
+    reqMsg.DoCalibration = true;
+    respMsg= call(client,reqMsg);
+
+    % Load results (predicted 6D object poses)
+    resultFiles = dir(fullfile(tmpDataPath,'results/*.result.txt'));
+    results = cell(1,length(resultFiles));
+    confScores = [];
+    for resultIdx = 1:length(resultFiles)
+        tmpResultFile = resultFiles(resultIdx).name;
+        tmpResultFilenameDotIdx = strfind(tmpResultFile,'.');
+        tmpResult.objName = tmpResultFile(1:(tmpResultFilenameDotIdx(1)-1));
+        tmpResult.objNum = str2double(tmpResultFile((tmpResultFilenameDotIdx(1)+1):(tmpResultFilenameDotIdx(2)-1)));
+        tmpResult.objPoseWorld = eye(4);
+        tmpResult.objPoseWorld(1:3,4) = dlmread(fullfile(tmpDataPath,'results',tmpResultFile),'\t',[1,0,1,2])';
+        objPoseRotQuat = dlmread(fullfile(tmpDataPath,'results',tmpResultFile),'\t',[4,0,4,3]);
+        tmpResult.objPoseWorld(1:3,1:3) = quat2rotm([objPoseRotQuat(4),objPoseRotQuat(1:3)]);
+        tmpResult.confScore = dlmread(fullfile(tmpDataPath,'results',tmpResultFile),'\t',[28,0,28,0]);
+        confScores = [confScores;tmpResult.confScore];
+        results{resultIdx} = tmpResult;
+    end
+
+    % Sort results by confidence scores
+    [~,sortIdx] = sortrows(confScores,-1);
+
+    % Get random colors per object in the scene
+    randColorIdx = randperm(length(colorPalette),length(results));
+    
+    % Create canvases for visualization
+    if strcmp(sceneData.env,'shelf')
+        canvas = sceneData.colorFrames{8};
+        canvasSeg = uint8(ones(size(canvas))*255);
+        canvasPose = canvas;
+    else
+        canvas1 = sceneData.colorFrames{5};
+        canvas2 = sceneData.colorFrames{14};
+        canvasSeg1 = uint8(ones(size(canvas1))*255);
+        canvasPose1 = canvas1;
+        canvasSeg2 = uint8(ones(size(canvas2))*255);
+        canvasPose2 = canvas2;
+    end
+    
+    % Loop through each object and visualize predicted object pose
+    canvasPose = insertText(canvasPose,[10 10],'Confidence  :  Object','Font','LucidaSansDemiBold','FontSize',16,'TextColor','white','BoxColor','black');
+    textPosY = 38;
+    for resultIdx = 1:length(resultFiles)
+        currResult = results{sortIdx(resultIdx)};
+        objColor = colorPalette{randColorIdx(resultIdx)};
+        
+        % Load pre-scanned object point cloud
+        objPointCloud = pcread(fullfile(modelsPath,sprintf('%s.ply',currResult.objName)));
+
+
+        % Draw projected object model and bounding box
+        if strcmp(sceneData.env,'shelf')
+            [canvasSeg,canvasPose] = dispObjPose(canvasSeg, ...
+                                                 canvasPose, ...
+                                                 sceneData.colorFrames{8}, ...
+                                                 sceneData.depthFrames{8}, ...
+                                                 sceneData.extCam2World{8}, ...
+                                                 sceneData.colorK, ...
+                                                 objPointCloud, ...
+                                                 currResult.objPoseWorld, ...
+                                                 objColor);
+        else
+            [canvasSeg1,canvasPose1] = dispObjPose(canvasSeg1, ...
+                                                   canvasPose1, ...
+                                                   sceneData.colorFrames{5}, ...
+                                                   sceneData.depthFrames{5}, ...
+                                                   sceneData.extCam2World{5}, ...
+                                                   sceneData.colorK, ...
+                                                   objPointCloud, ...
+                                                   currResult.objPoseWorld, ...
+                                                   objColor);
+            [canvasSeg2,canvasPose2] = dispObjPose(canvasSeg2, ...
+                                                   canvasPose2, ...
+                                                   sceneData.colorFrames{14}, ...
+                                                   sceneData.depthFrames{14}, ...
+                                                   sceneData.extCam2World{14}, ...
+                                                   sceneData.colorK, ...
+                                                   objPointCloud, ...
+                                                   currResult.objPoseWorld, ...
+                                                   objColor);
+            canvasSeg = [canvasSeg1;canvasSeg2];
+            canvasPose = [canvasPose1;canvasPose2];
+        end
+        
+        canvasPose = insertText(canvasPose,[10 textPosY],sprintf('  %f  :  %s',currResult.confScore,currResult.objName),'Font','LucidaSansDemiBold','FontSize',16,'TextColor',objColor,'BoxColor','black');
+        textPosY = textPosY + 28;
+    end
+    
+    % Save visualization
+    visFileName = sceneNames{sceneIdx};
+    visFileName = visFileName(1:(end-1));
+    visFileName = strrep(visFileName,'/','_');
+    imwrite(canvasPose,fullfile(resultVisPath,sprintf('%s.png',visFileName)));
 end
 
 
diff --git a/ros-packages/catkin_ws/src/pose_estimation/src/getObjectHypothesis.m b/ros-packages/catkin_ws/src/pose_estimation/src/getObjectHypothesis.m
index c895150..8a1339e 100644
--- a/ros-packages/catkin_ws/src/pose_estimation/src/getObjectHypothesis.m
+++ b/ros-packages/catkin_ws/src/pose_estimation/src/getObjectHypothesis.m
@@ -1,12 +1,12 @@
-function objectHypothesis = getObjectHypothesis(RtPCA,latentPCA,surfaceCentroid,surfaceRange,finalRt,finalScore,dataPath,objName,instanceIdx)
+function objectHypothesis = getObjectHypothesis(surfPCAPoseWorld,latentPCA,surfCentroid,surfRangeWorld,predObjPoseWorld,predObjConfScore,scenePath,objName,instanceIdx)
 
 % Save object pose to ROS message
 poseTrans = rosmessage('geometry_msgs/Point');
-poseTrans.X = finalRt(1,4);
-poseTrans.Y = finalRt(2,4);
-poseTrans.Z = finalRt(3,4);
+poseTrans.X = predObjPoseWorld(1,4);
+poseTrans.Y = predObjPoseWorld(2,4);
+poseTrans.Z = predObjPoseWorld(3,4);
 poseRot = rosmessage('geometry_msgs/Quaternion');
-poseQuat = rotm2quat(finalRt(1:3,1:3));
+poseQuat = rotm2quat(predObjPoseWorld(1:3,1:3));
 poseRot.X = poseQuat(2);
 poseRot.Y = poseQuat(3);
 poseRot.Z = poseQuat(4);
@@ -17,11 +17,11 @@
 
 % Save PCA to ROS message
 pcaTrans = rosmessage('geometry_msgs/Point');
-pcaTrans.X = RtPCA(1,4);
-pcaTrans.Y = RtPCA(2,4);
-pcaTrans.Z = RtPCA(3,4);
+pcaTrans.X = surfPCAPoseWorld(1,4);
+pcaTrans.Y = surfPCAPoseWorld(2,4);
+pcaTrans.Z = surfPCAPoseWorld(3,4);
 pcaRot = rosmessage('geometry_msgs/Quaternion');
-pcaQuat = rotm2quat(RtPCA(1:3,1:3));
+pcaQuat = rotm2quat(surfPCAPoseWorld(1:3,1:3));
 pcaRot.X = pcaQuat(2);
 pcaRot.Y = pcaQuat(3);
 pcaRot.Z = pcaQuat(4);
@@ -32,15 +32,15 @@
 
 % Save PCA variances to ROS message
 pcaLatent = rosmessage('geometry_msgs/Point');
-pcaLatent.X = latentPCA(1)*50;
-pcaLatent.Y = latentPCA(2)*50;
-pcaLatent.Z = latentPCA(3)*50;
+pcaLatent.X = latentPCA(1);
+pcaLatent.Y = latentPCA(2);
+pcaLatent.Z = latentPCA(3);
 
 % Save surface mean to ROS message
 surfaceMean = rosmessage('geometry_msgs/Point');
-surfaceMean.X = surfaceCentroid(1);
-surfaceMean.Y = surfaceCentroid(2);
-surfaceMean.Z = surfaceCentroid(3);
+surfaceMean.X = surfCentroid(1);
+surfaceMean.Y = surfCentroid(2);
+surfaceMean.Z = surfCentroid(3);
 
 % Prepare ROS message 
 objectHypothesis = rosmessage('pose_estimation/ObjectHypothesis');
@@ -49,23 +49,26 @@
 objectHypothesis.Pca = pcaMsg;
 objectHypothesis.Latent = pcaLatent;
 objectHypothesis.Mean = surfaceMean;
-objectHypothesis.RangeX = surfaceRange(1,:);
-objectHypothesis.RangeY = surfaceRange(2,:);
-objectHypothesis.RangeZ = surfaceRange(3,:);
-objectHypothesis.Score = finalScore;
+objectHypothesis.RangeX = surfRangeWorld(1,:);
+objectHypothesis.RangeY = surfRangeWorld(2,:);
+objectHypothesis.RangeZ = surfRangeWorld(3,:);
+objectHypothesis.Score = predObjConfScore;
 
 % Save ROS message to file
-fid = fopen(fullfile(dataPath,strcat(objName,sprintf('.%d.result.txt',instanceIdx))),'w');
-fprintf(fid,'%f\n%f\n%f\n',finalRt(1,4),finalRt(2,4),finalRt(3,4));
-fprintf(fid,'%f\n%f\n%f\n%f\n',poseQuat(2),poseQuat(3),poseQuat(4),poseQuat(1));
-fprintf(fid,'%f\n%f\n%f\n',RtPCA(1,4),RtPCA(2,4),RtPCA(3,4));
-fprintf(fid,'%f\n%f\n%f\n%f\n',pcaQuat(2),pcaQuat(3),pcaQuat(4),pcaQuat(1));
-fprintf(fid,'%f\n%f\n%f\n',latentPCA(1)*50,latentPCA(2)*50,latentPCA(3)*50);
-fprintf(fid,'%f\n%f\n%f\n',surfaceCentroid(1),surfaceCentroid(2),surfaceCentroid(3));
-fprintf(fid,'%f\n%f\n',surfaceRange(1,1),surfaceRange(1,2));
-fprintf(fid,'%f\n%f\n',surfaceRange(2,1),surfaceRange(2,2));
-fprintf(fid,'%f\n%f\n',surfaceRange(3,1),surfaceRange(3,2));
-fprintf(fid,'%f\n',finalScore);
+if ~exist(fullfile(scenePath,'results'),'file')
+    mkdir(fullfile(scenePath,'results'));
+end
+fid = fopen(fullfile(scenePath,'results',strcat(objName,sprintf('.%d.result.txt',instanceIdx))),'w');
+fprintf(fid,'# Predicted object pose translation (x,y,z in world coordinates)\n%15.8e\t %15.8e\t %15.8e\t\n\n',predObjPoseWorld(1,4),predObjPoseWorld(2,4),predObjPoseWorld(3,4));
+fprintf(fid,'# Predicted object pose rotation (x,y,z,w object-to-world quaternion)\n%15.8e\t %15.8e\t %15.8e\t %15.8e\t\n\n',poseQuat(2),poseQuat(3),poseQuat(4),poseQuat(1));
+fprintf(fid,'# Segmented object point cloud median (x,y,z in world coordinates)\n%15.8e\t %15.8e\t %15.8e\t\n\n',surfPCAPoseWorld(1,4),surfPCAPoseWorld(2,4),surfPCAPoseWorld(3,4));
+fprintf(fid,'# Segmented object point cloud PCA rotation (x,y,z,w object-to-world quaternion)\n%15.8e\t %15.8e\t %15.8e\t %15.8e\t\n\n',pcaQuat(2),pcaQuat(3),pcaQuat(4),pcaQuat(1));
+fprintf(fid,'# Segmented object point cloud PCA variance (in PC directions)\n%15.8e\t %15.8e\t %15.8e\t\n\n',latentPCA(1),latentPCA(2),latentPCA(3));
+fprintf(fid,'# Segmented object point cloud centroid/mean (x,y,z in world coordinates)\n%15.8e\t %15.8e\t %15.8e\t\n\n',surfCentroid(1),surfCentroid(2),surfCentroid(3));
+fprintf(fid,'# Segmented object point cloud x-range in world coordinates (bounding box in x-direction)\n%15.8e\t %15.8e\t\n\n',surfRangeWorld(1,1),surfRangeWorld(1,2));
+fprintf(fid,'# Segmented object point cloud y-range in world coordinates (bounding box in y-direction)\n%15.8e\t %15.8e\t\n\n',surfRangeWorld(2,1),surfRangeWorld(2,2));
+fprintf(fid,'# Segmented object point cloud z-range in world coordinates (bounding box in z-direction)\n%15.8e\t %15.8e\t\n\n',surfRangeWorld(3,1),surfRangeWorld(3,2));
+fprintf(fid,'# Prediction confidence score\n%.17g\n',predObjConfScore);
 fclose(fid);
   
 end
diff --git a/ros-packages/catkin_ws/src/pose_estimation/src/getObjectPose.m b/ros-packages/catkin_ws/src/pose_estimation/src/getObjectPose.m
index 1b2775c..c33f5c3 100644
--- a/ros-packages/catkin_ws/src/pose_estimation/src/getObjectPose.m
+++ b/ros-packages/catkin_ws/src/pose_estimation/src/getObjectPose.m
@@ -1,4 +1,4 @@
-function objectHypotheses = getObjectPose(scenePath,sceneData,scenePointCloud,backgroundPointCloud,extBin2Bg,objName,objModel,objNum)
+function objHypotheses = getObjectPose(scenePath,sceneData,scenePointCloud,backgroundPointCloud,extBin2Bg,objName,objModel,objNum)
 
 global visPath;
 global savePointCloudVis; 
@@ -9,7 +9,7 @@
 % Parameters for both shelf and tote scenario
 gridStep = 0.002; % grid size for downsampling point clouds
 icpWorstRejRatio = 0.9; % ratio of outlier points to ignore during ICP
-objectHypotheses = [];
+objHypotheses = [];
 
 % Parameters specific to shelf or tote scenario
 if strcmp(sceneData.env,'tote')
@@ -29,28 +29,29 @@
 [objSegmPts,objSegmConf] = getSegmentedPointCloud(sceneData,frames,objMasks,segmConfMaps);
 
 % Handle objects without depth
-sceneData.extBin2World = inv(sceneData.extBin2World);
-if (strcmp(objName,'dasani_water_bottle') && binID ~= -1) || strcmp(objName,'rolodex_jumbo_pencil_cup') || strcmp(objName,'platinum_pets_dog_bowl')
-  [finalRt,surfaceCentroid,surfaceRange] = getObjectPoseNoDepth(visPath,objSegmPts,objName,binID,frames,sceneData,objMasks,savePointClouds);
-  finalRt = sceneData.extBin2World * finalRt;
-  finalScore = mean(objSegmConf);
+if strcmp(objName,'dasani_water_bottle') && strcmp(sceneData.env,'shelf') || ...
+   strcmp(objName,'rolodex_jumbo_pencil_cup') || ...
+   strcmp(objName,'platinum_pets_dog_bowl')
+  [predObjPoseBin,surfCentroid,surfRangeWorld] = getObjectPoseNoDepth(visPath,objSegmPts,objName,frames,sceneData,objMasks);
+  predObjPoseWorld = sceneData.extBin2World * predObjPoseBin;
+  predObjConfScore = mean(objSegmConf);
   for instanceIdx = 1:objNum
-    if savePointClouds
-      surfaceAxisPtsX = [surfaceRange(1,1):0.001:surfaceRange(1,2)];
-      surfaceAxisPtsX = [surfaceAxisPtsX;repmat(surfaceCentroid(2),1,size(surfaceAxisPtsX,2));repmat(surfaceCentroid(3),1,size(surfaceAxisPtsX,2))];
-      surfaceAxisPtsY = [surfaceRange(2,1):0.001:surfaceRange(2,2)];
-      surfaceAxisPtsY = [repmat(surfaceCentroid(1),1,size(surfaceAxisPtsY,2));surfaceAxisPtsY;repmat(surfaceCentroid(3),1,size(surfaceAxisPtsY,2))];
-      surfaceAxisPtsZ = [surfaceRange(3,1):0.001:surfaceRange(3,2)];
-      surfaceAxisPtsZ = [repmat(surfaceCentroid(1),1,size(surfaceAxisPtsZ,2));repmat(surfaceCentroid(2),1,size(surfaceAxisPtsZ,2));surfaceAxisPtsZ];
+    if savePointCloudVis
+      surfaceAxisPtsX = [surfRangeWorld(1,1):0.001:surfRangeWorld(1,2)];
+      surfaceAxisPtsX = [surfaceAxisPtsX;repmat(surfCentroid(2),1,size(surfaceAxisPtsX,2));repmat(surfCentroid(3),1,size(surfaceAxisPtsX,2))];
+      surfaceAxisPtsY = [surfRangeWorld(2,1):0.001:surfRangeWorld(2,2)];
+      surfaceAxisPtsY = [repmat(surfCentroid(1),1,size(surfaceAxisPtsY,2));surfaceAxisPtsY;repmat(surfCentroid(3),1,size(surfaceAxisPtsY,2))];
+      surfaceAxisPtsZ = [surfRangeWorld(3,1):0.001:surfRangeWorld(3,2)];
+      surfaceAxisPtsZ = [repmat(surfCentroid(1),1,size(surfaceAxisPtsZ,2));repmat(surfCentroid(2),1,size(surfaceAxisPtsZ,2));surfaceAxisPtsZ];
       pcwrite(pointCloud([surfaceAxisPtsX,surfaceAxisPtsY,surfaceAxisPtsZ]','Color',[repmat([255,0,0],size(surfaceAxisPtsX,2),1);repmat([0,255,0],size(surfaceAxisPtsY,2),1);repmat([0,0,255],size(surfaceAxisPtsZ,2),1)]),fullfile(visPath,sprintf('vis.objSurf.%s.%d',objName,instanceIdx)),'PLYFormat','binary');
     end
-    surfaceCentroidWorld = sceneData.extBin2World(1:3,1:3) * surfaceCentroid + repmat(sceneData.extBin2World(1:3,4),1,size(surfaceCentroid,2));
-    surfaceRangeWorld = sceneData.extBin2World(1:3,1:3) * surfaceRange + repmat(sceneData.extBin2World(1:3,4),1,size(surfaceRange,2));
-    if saveResultImages
-      visualizeResults(finalRt,[0 0 0],surfaceCentroidWorld,surfaceRangeWorld,finalRt,finalScore,dataPath,objName,instanceIdx,binID,sceneData,[],objMasks,objModel.Location');
+    surfCentroidWorld = sceneData.extBin2World(1:3,1:3) * surfCentroid + repmat(sceneData.extBin2World(1:3,4),1,size(surfCentroid,2));
+    surfRangeWorld = sceneData.extBin2World(1:3,1:3) * surfRangeWorld + repmat(sceneData.extBin2World(1:3,4),1,size(surfRangeWorld,2));
+    if saveResultImageVis
+      visualizeResults(predObjPoseWorld,[0 0 0],surfCentroidWorld,surfRangeWorld,predObjPoseWorld,predObjConfScore,scenePath,objName,instanceIdx,sceneData,[],objMasks,objModel.Location');
     end
-    currObjectHypothesis = getObjectHypothesis(finalRt,[0 0 0],surfaceCentroidWorld,surfaceRangeWorld,finalRt,finalScore,dataPath,objName,instanceIdx);
-    objectHypotheses = [objectHypotheses currObjectHypothesis];
+    currObjHypothesis = getObjectHypothesis(predObjPoseWorld,[0 0 0],surfCentroidWorld,surfRangeWorld,predObjPoseWorld,predObjConfScore,scenePath,objName,instanceIdx);
+    objHypotheses = [objHypotheses,currObjHypothesis];
   end
   return;
 end
@@ -85,10 +86,10 @@
 
 % If object not found, return dummy pose
 if size(objSegmPts,2) < 200
-  fprintf('%s: 0.000000\n',objName);
+  fprintf('    [Pose Estimation] %s: 0.000000\n',objName);
   for instanceIdx = 1:objNum
-    currObjectHypothesis = getEmptyObjectHypothesis(scenePath,objName,instanceIdx);
-    objectHypotheses = [objectHypotheses,currObjectHypothesis];
+    currObjHypothesis = getEmptyObjectHypothesis(scenePath,objName,instanceIdx);
+    objHypotheses = [objHypotheses,currObjHypothesis];
   end
   return;
 end
@@ -115,113 +116,112 @@
   objModelPts = objModelCloud.Location';
 
   % Remove outliers from the segmented point cloud using PCA
-  currObjSegPts = instancePts{instanceIdx};
+  currObjSegmPts = instancePts{instanceIdx};
   if ~strcmp(objName,'barkely_hide_bones')
     if saveResultImageVis
-      pcwrite(pointCloud(currObjSegPts'),fullfile(visPath,sprintf('vis.objObs.%s.%d',objName,instanceIdx)),'PLYFormat','binary');
+      pcwrite(pointCloud(currObjSegmPts'),fullfile(visPath,sprintf('vis.objObs.%s.%d',objName,instanceIdx)),'PLYFormat','binary');
     end
     try
-      currObjSegPts = denoisePointCloud(currObjSegPts);
+      currObjSegmPts = denoisePointCloud(currObjSegmPts);
     end
     if saveResultImageVis
-      pcwrite(pointCloud(currObjSegPts'),fullfile(visPath,sprintf('vis.objCut.%s.%d',objName,instanceIdx)),'PLYFormat','binary');     
+      pcwrite(pointCloud(currObjSegmPts'),fullfile(visPath,sprintf('vis.objCut.%s.%d',objName,instanceIdx)),'PLYFormat','binary');     
     end
   end
-  fullCurrObjSegPts = currObjSegPts;
+  fullCurrObjSegmPts = currObjSegmPts;
   
   % Print segmentation confidence
-  finalScore = mean(instanceConf{instanceIdx});
+  predObjConfScore = mean(instanceConf{instanceIdx});
   if strcmp(objName,'dasani_water_bottle') && binID == -1
-    finalScore = finalScore/4;
+    predObjConfScore = predObjConfScore/4;
   end
-  if size(currObjSegPts,2) < 100 % If object not found
-    fprintf('%s: 0.000000\n',objName);
-    currObjectHypothesis = getEmptyObjectHypothesis(dataPath,objName,instanceIdx);
-    objectHypotheses = [objectHypotheses currObjectHypothesis];
+  if size(currObjSegmPts,2) < 100 % If object not found
+    fprintf('    [Pose Estimation] %s: 0.000000\n',objName);
+    currObjHypothesis = getEmptyObjectHypothesis(dataPath,objName,instanceIdx);
+    objHypotheses = [objHypotheses currObjHypothesis];
     continue;
   else
-    fprintf('%s: %f\n',objName,finalScore);
+    fprintf('    [Pose Estimation] %s: %f\n',objName,predObjConfScore);
   end
     
   % Downsample segmented point cloud to same resolution as object model
-  currObjSegCloud = pointCloud(currObjSegPts');
+  currObjSegCloud = pointCloud(currObjSegmPts');
   currObjSegCloud = pcdownsample(currObjSegCloud,'gridAverage',gridStep);
   currObjSegCloud = pcdenoise(currObjSegCloud,'NumNeighbors',4);
   
   if size(currObjSegCloud.Location',2) < 200 % If object not found
-    currObjectHypothesis = getEmptyObjectHypothesis(dataPath,objName,instanceIdx);
-    objectHypotheses = [objectHypotheses currObjectHypothesis];
+    currObjHypothesis = getEmptyObjectHypothesis(dataPath,objName,instanceIdx);
+    objHypotheses = [objHypotheses currObjHypothesis];
     continue;
   end
   
   % Compute surface mean 
-  surfaceCentroid = mean(currObjSegPts,2);
-  currObjSegPtsRangeX = currObjSegPts(:,find(((currObjSegPts(2,:)>(surfaceCentroid(2)-0.005)) & (currObjSegPts(2,:)<(surfaceCentroid(2)+0.005)) & ...
-                                              (currObjSegPts(3,:)>(surfaceCentroid(3)-0.005)) & (currObjSegPts(3,:)<(surfaceCentroid(3)+0.005)))));
-  currObjSegPtsRangeY = currObjSegPts(:,find(((currObjSegPts(1,:)>(surfaceCentroid(1)-0.005)) & (currObjSegPts(1,:)<(surfaceCentroid(1)+0.005)) & ...
-                                              (currObjSegPts(3,:)>(surfaceCentroid(3)-0.005)) & (currObjSegPts(3,:)<(surfaceCentroid(3)+0.005)))));
-  currObjSegPtsRangeZ = currObjSegPts(:,find(((currObjSegPts(2,:)>(surfaceCentroid(2)-0.005)) & (currObjSegPts(2,:)<(surfaceCentroid(2)+0.005)) & ...
-                                              (currObjSegPts(1,:)>(surfaceCentroid(1)-0.005)) & (currObjSegPts(1,:)<(surfaceCentroid(1)+0.005)))));
+  surfCentroid = mean(currObjSegmPts,2);
+  currObjSegPtsRangeX = currObjSegmPts(:,find(((currObjSegmPts(2,:)>(surfCentroid(2)-0.005)) & (currObjSegmPts(2,:)<(surfCentroid(2)+0.005)) & ...
+                                              (currObjSegmPts(3,:)>(surfCentroid(3)-0.005)) & (currObjSegmPts(3,:)<(surfCentroid(3)+0.005)))));
+  currObjSegPtsRangeY = currObjSegmPts(:,find(((currObjSegmPts(1,:)>(surfCentroid(1)-0.005)) & (currObjSegmPts(1,:)<(surfCentroid(1)+0.005)) & ...
+                                              (currObjSegmPts(3,:)>(surfCentroid(3)-0.005)) & (currObjSegmPts(3,:)<(surfCentroid(3)+0.005)))));
+  currObjSegPtsRangeZ = currObjSegmPts(:,find(((currObjSegmPts(2,:)>(surfCentroid(2)-0.005)) & (currObjSegmPts(2,:)<(surfCentroid(2)+0.005)) & ...
+                                              (currObjSegmPts(1,:)>(surfCentroid(1)-0.005)) & (currObjSegmPts(1,:)<(surfCentroid(1)+0.005)))));
   if isempty(currObjSegPtsRangeX)
-    currObjSegPtsRangeX = currObjSegPts;
+    currObjSegPtsRangeX = currObjSegmPts;
   end
   if isempty(currObjSegPtsRangeY)
-    currObjSegPtsRangeY = currObjSegPts;
+    currObjSegPtsRangeY = currObjSegmPts;
   end
   if isempty(currObjSegPtsRangeZ)
-    currObjSegPtsRangeZ = currObjSegPts;
+    currObjSegPtsRangeZ = currObjSegmPts;
   end
-  surfaceRangeX = [min(currObjSegPtsRangeX(1,:)),max(currObjSegPtsRangeX(1,:))];
-  surfaceRangeY = [min(currObjSegPtsRangeY(2,:)),max(currObjSegPtsRangeY(2,:))];
-  surfaceRangeZ = [min(currObjSegPtsRangeZ(3,:)),max(currObjSegPtsRangeZ(3,:))];
-  surfaceRange = [surfaceRangeX;surfaceRangeY;surfaceRangeZ];
+  surfRangeX = [min(currObjSegPtsRangeX(1,:)),max(currObjSegPtsRangeX(1,:))];
+  surfRangeY = [min(currObjSegPtsRangeY(2,:)),max(currObjSegPtsRangeY(2,:))];
+  surfRangeZ = [min(currObjSegPtsRangeZ(3,:)),max(currObjSegPtsRangeZ(3,:))];
+  surfRangeWorld = [surfRangeX;surfRangeY;surfRangeZ];
                                             
   % Visualize surface centroid and surface ranges
   if savePointCloudVis
-    surfaceAxisPtsX = [surfaceRangeX(1):0.001:surfaceRangeX(2)];
-    surfaceAxisPtsX = [surfaceAxisPtsX;repmat(surfaceCentroid(2),1,size(surfaceAxisPtsX,2));repmat(surfaceCentroid(3),1,size(surfaceAxisPtsX,2))];
-    surfaceAxisPtsY = [surfaceRangeY(1):0.001:surfaceRangeY(2)];
-    surfaceAxisPtsY = [repmat(surfaceCentroid(1),1,size(surfaceAxisPtsY,2));surfaceAxisPtsY;repmat(surfaceCentroid(3),1,size(surfaceAxisPtsY,2))];
-    surfaceAxisPtsZ = [surfaceRangeZ(1):0.001:surfaceRangeZ(2)];
-    surfaceAxisPtsZ = [repmat(surfaceCentroid(1),1,size(surfaceAxisPtsZ,2));repmat(surfaceCentroid(2),1,size(surfaceAxisPtsZ,2));surfaceAxisPtsZ];
+    surfaceAxisPtsX = [surfRangeX(1):0.001:surfRangeX(2)];
+    surfaceAxisPtsX = [surfaceAxisPtsX;repmat(surfCentroid(2),1,size(surfaceAxisPtsX,2));repmat(surfCentroid(3),1,size(surfaceAxisPtsX,2))];
+    surfaceAxisPtsY = [surfRangeY(1):0.001:surfRangeY(2)];
+    surfaceAxisPtsY = [repmat(surfCentroid(1),1,size(surfaceAxisPtsY,2));surfaceAxisPtsY;repmat(surfCentroid(3),1,size(surfaceAxisPtsY,2))];
+    surfaceAxisPtsZ = [surfRangeZ(1):0.001:surfRangeZ(2)];
+    surfaceAxisPtsZ = [repmat(surfCentroid(1),1,size(surfaceAxisPtsZ,2));repmat(surfCentroid(2),1,size(surfaceAxisPtsZ,2));surfaceAxisPtsZ];
     pcwrite(pointCloud([surfaceAxisPtsX,surfaceAxisPtsY,surfaceAxisPtsZ]','Color',[repmat([255,0,0],size(surfaceAxisPtsX,2),1);repmat([0,255,0],size(surfaceAxisPtsY,2),1);repmat([0,0,255],size(surfaceAxisPtsZ,2),1)]),fullfile(visPath,sprintf('vis.objSurf.%s.%d',objName,instanceIdx)),'PLYFormat','binary');
   end
   
   % Convert surface centroid to world coordinates
-  surfaceCentroid = sceneData.extBin2World(1:3,1:3) * surfaceCentroid + repmat(sceneData.extBin2World(1:3,4),1,size(surfaceCentroid,2));
-  surfaceRange = sceneData.extBin2World(1:3,1:3) * surfaceRange + repmat(sceneData.extBin2World(1:3,4),1,size(surfaceRange,2));
+  surfCentroid = sceneData.extBin2World(1:3,1:3) * surfCentroid + repmat(sceneData.extBin2World(1:3,4),1,size(surfCentroid,2));
+  surfRangeWorld = sceneData.extBin2World(1:3,1:3) * surfRangeWorld + repmat(sceneData.extBin2World(1:3,4),1,size(surfRangeWorld,2));
   
-  % Recompute PCA Rt over denoised and downsampled segmented point cloud
-  currObjSegPts = currObjSegCloud.Location';
-  [coeffPCA,scorePCA,latentPCA] = pca(currObjSegPts');
+  % Recompute PCA pose over denoised and downsampled segmented point cloud
+  currObjSegmPts = currObjSegCloud.Location';
+  [coeffPCA,scorePCA,latentPCA] = pca(currObjSegmPts');
   if size(latentPCA,1) < 3
     latentPCA = [latentPCA;0];
   end
   coeffPCA = [coeffPCA(:,1),coeffPCA(:,2),cross(coeffPCA(:,1),coeffPCA(:,2))]; % Follow righthand rule
-  initRtPCA = [[coeffPCA median(currObjSegPts,2)]; 0 0 0 1];
-%   testPCA = [[coeffPCAoutlier median(currObjSegPts,2)]; 0 0 0 1];
+  surfPCAPoseBin = [[coeffPCA median(currObjSegmPts,2)]; 0 0 0 1];
   if savePointCloudVis
     axisPts = [[[0:0.001:latentPCA(1)*50]; zeros(2,size([0:0.001:latentPCA(1)*50],2))],[zeros(1,size([0:0.001:latentPCA(2)*50],2)); [0:0.001:latentPCA(2)*50]; zeros(1,size([0:0.001:latentPCA(2)*50],2))],[zeros(2,size([0:0.001:latentPCA(3)*50],2)); [0:0.001:latentPCA(3)*50]]];
     axisColors = uint8([repmat([255; 0; 0],1,size([0:0.001:latentPCA(1)*50],2)),repmat([0; 255; 0],1,size([0:0.001:latentPCA(2)*50],2)),repmat([0; 0; 255],1,size([0:0.001:latentPCA(3)*50],2))]);
-    tmpAxisPts = initRtPCA(1:3,1:3) * axisPts + repmat(initRtPCA(1:3,4),1,size(axisPts,2));
+    tmpAxisPts = surfPCAPoseBin(1:3,1:3) * axisPts + repmat(surfPCAPoseBin(1:3,4),1,size(axisPts,2));
     pcwrite(pointCloud(tmpAxisPts','Color',axisColors'),fullfile(visPath,sprintf('vis.objPost.%s.%d',objName,instanceIdx)),'PLYFormat','binary');
   end
-  RtPCA = sceneData.extBin2World * initRtPCA;
+  surfPCAPoseWorld = sceneData.extBin2World * surfPCAPoseBin;
   
   % If object is deformable, return PCA as pose
   if strcmp(objName,'cherokee_easy_tee_shirt') || strcmp(objName,'kyjen_squeakin_eggs_plush_puppies') || strcmp(objName,'womens_knit_gloves') || strcmp(objName,'cloud_b_plush_bear')
-    finalRt = RtPCA;
+    predObjPoseWorld = surfPCAPoseWorld;
     if saveResultImageVis
-      visualizeResults(RtPCA,latentPCA,surfaceCentroid,surfaceRange,finalRt,finalScore,dataPath,objName,instanceIdx,binID,sceneData,fullCurrObjSegPts,objMasks,objModel.Location');
+      visualizeResults(surfPCAPoseWorld,latentPCA,surfCentroid,surfRangeWorld,predObjPoseWorld,predObjConfScore,dataPath,objName,instanceIdx,sceneData,fullCurrObjSegmPts,objMasks,objModel.Location');
     end
-    currObjectHypothesis = getObjectHypothesis(RtPCA,latentPCA,surfaceCentroid,surfaceRange,finalRt,finalScore,dataPath,objName,instanceIdx);
-    objectHypotheses = [objectHypotheses currObjectHypothesis];
+    currObjHypothesis = getObjectHypothesis(surfPCAPoseWorld,latentPCA,surfCentroid,surfRangeWorld,predObjPoseWorld,predObjConfScore,dataPath,objName,instanceIdx);
+    objHypotheses = [objHypotheses,currObjHypothesis];
     continue;
   end
 
   % Push objects back prior to ICP
   pushBackVal = max([objModel.XLimits(2),objModel.YLimits(2),objModel.ZLimits(2)]);
-  initRtPCA(1:3,4) = initRtPCA(1:3,4) + pushBackAxis.*pushBackVal;
+  surfPCAPoseBin(1:3,4) = surfPCAPoseBin(1:3,4) + pushBackAxis.*pushBackVal;
   
   % Attempt 6 different rotations using PCA directions
 %   possibleRotations = cell(1,6);
@@ -232,7 +232,7 @@
 %   possibleRotations{5} = vrrotvec2mat([0 1 0 pi/2]);
 %   possibleRotations{6} = vrrotvec2mat([0,1,0,3*pi/2]);
   bestRotScore = inf;
-  bestRt = initRtPCA;
+  bestRt = surfPCAPoseBin;
 %   for rotIdx = 1:length(possibleRotations)
 %     tmpRt = initRtPCA * [possibleRotations{rotIdx} [0;0;0]; [0,0,0,1]];
 %     tmpObjModelPts = tmpRt(1:3,1:3) * objModelPts + repmat(tmpRt(1:3,4),1,size(objModelPts,2));
@@ -250,19 +250,19 @@
 %   end
 %   initRtPCA = bestRt;
   if strcmp(sceneData.env,'shelf') && (strcmp(objName,'creativity_chenille_stems') || strcmp(objName,'dr_browns_bottle_brush') || strcmp(objName,'peva_shower_curtain_liner') || strcmp(objName,'kleenex_paper_towels'))
-    initRtPCA(1:3,1:3) = eye(3);
+    surfPCAPoseBin(1:3,1:3) = eye(3);
   end
   
   % Apply rigid transform computed prior to ICP
-  tmpObjModelPts = initRtPCA(1:3,1:3) * objModelPts + repmat(initRtPCA(1:3,4),1,size(objModelPts,2));
+  tmpObjModelPts = surfPCAPoseBin(1:3,1:3) * objModelPts + repmat(surfPCAPoseBin(1:3,4),1,size(objModelPts,2));
   if savePointCloudVis
     pcwrite(pointCloud(tmpObjModelPts','Color',repmat(uint8([0 0 255]),size(tmpObjModelPts,2),1)),fullfile(visPath,sprintf('vis.objPre.%s.%d',objName,instanceIdx)),'PLYFormat','binary');
   end
   
   % Perform ICP to align segmented point cloud to object model
-  if size(currObjSegPts,2) > 3
+  if size(currObjSegmPts,2) > 3
     tmpObjModelCloud = pointCloud(tmpObjModelPts');
-    objSegCloud = pointCloud(currObjSegPts');
+    objSegCloud = pointCloud(currObjSegmPts');
     [tform,movingReg,icpRmse] = pcregrigidGPU(objSegCloud,tmpObjModelCloud,'InlierRatio',icpWorstRejRatio,'MaxIterations',200,'Tolerance',[0.0001 0.0009],'Verbose',false,'Extrapolate',true);
     icpRt1 = inv(tform.T');
     tmpObjModelPts = tmpObjModelCloud.Location';
@@ -274,16 +274,11 @@
   else
     icpRt = eye(4);
   end
-  finalRt = icpRt * initRtPCA;
-  
-  tmpObjModelPts = finalRt(1:3,1:3) * objModelPts + repmat(finalRt(1:3,4),1,size(objModelPts,2));
+  predObjPoseBin = icpRt * surfPCAPoseBin;
   if savePointCloudVis
+    tmpObjModelPts = predObjPoseBin(1:3,1:3) * objModelPts + repmat(predObjPoseBin(1:3,4),1,size(objModelPts,2));
     pcwrite(pointCloud(tmpObjModelPts','Color',repmat(uint8([0 255 0]),size(tmpObjModelPts,2),1)),fullfile(visPath,sprintf('vis.objPost.%s.%d',objName,instanceIdx)),'PLYFormat','binary');
   end
-  
-%   plot3(bgCloud.Location(:,1),bgCloud.Location(:,2),bgCloud.Location(:,3),'.r');
-%   hold on; axis equal; grid on; xlabel('x'); ylabel('y'); zlabel('z'); hold off;
-%   pcshow(bgCloud)
 
   % Save pose as a rigid transform from model to world space
   
@@ -298,12 +293,12 @@
 %     continue;
 %   end
   
-  finalRt = sceneData.extBin2World * finalRt;
+  predObjPoseWorld = sceneData.extBin2World * predObjPoseBin;
   if saveResultImageVis
-    visualizeResults(RtPCA,latentPCA,surfaceCentroid,surfaceRange,finalRt,finalScore,scenePath,objName,instanceIdx,sceneData,fullCurrObjSegPts,objMasks,objModel.Location');
+    visualizeResults(surfPCAPoseWorld,latentPCA,surfCentroid,surfRangeWorld,predObjPoseWorld,predObjConfScore,scenePath,objName,instanceIdx,sceneData,fullCurrObjSegmPts,objMasks,objModel.Location');
   end
-  currObjectHypothesis = getObjectHypothesis(RtPCA,latentPCA,surfaceCentroid,surfaceRange,finalRt,finalScore,scenePath,objName,instanceIdx);
-  objectHypotheses = [objectHypotheses,currObjectHypothesis];
+  currObjHypothesis = getObjectHypothesis(surfPCAPoseWorld,latentPCA,surfCentroid,surfRangeWorld,predObjPoseWorld,predObjConfScore,scenePath,objName,instanceIdx);
+  objHypotheses = [objHypotheses,currObjHypothesis];
 end
 
 end
diff --git a/ros-packages/catkin_ws/src/pose_estimation/src/getObjectPoseNoDepth.m b/ros-packages/catkin_ws/src/pose_estimation/src/getObjectPoseNoDepth.m
index 071d74d..b4a00ea 100644
--- a/ros-packages/catkin_ws/src/pose_estimation/src/getObjectPoseNoDepth.m
+++ b/ros-packages/catkin_ws/src/pose_estimation/src/getObjectPoseNoDepth.m
@@ -1,7 +1,9 @@
-function [objRt,surfaceCentroid,surfaceRange] = getObjectPoseNoDepth(visPath,objSegPts,objName,binID,frames,data,objMasks,savePointClouds)
+function [predObjPoseBin,surfCentroid,surfRange] = getObjectPoseNoDepth(visPath,objSegmPts,objName,frames,sceneData,objMasks)
+
+global savePointCloudVis;
 
 % Discretize bin into 3D grid
-if binID ~= -1
+if strcmp(sceneData.env,'shelf')
   [binGridX,binGridY,binGridZ] = ndgrid(-0.05:0.01:0.40,-0.14:0.01:0.17,-0.04:0.01:0.20);
   gridOccupancyThreshold = floor(0.5*length(frames));
 else
@@ -14,12 +16,12 @@
 binGridOccupancy = zeros(size(binGridX(:)));
 for frameIdx = frames
   currObjMask = objMasks{frameIdx};
-  binGridPtsWorld = data.extBin2World(1:3,1:3) * binGridPts + repmat(data.extBin2World(1:3,4),1,size(binGridPts,2));
-  currExtWorld2Cam = inv(data.extCam2World{frameIdx});
+  binGridPtsWorld = sceneData.extBin2World(1:3,1:3) * binGridPts + repmat(sceneData.extBin2World(1:3,4),1,size(binGridPts,2));
+  currExtWorld2Cam = inv(sceneData.extCam2World{frameIdx});
   binGridPtsCam = currExtWorld2Cam(1:3,1:3) * binGridPtsWorld + repmat(currExtWorld2Cam(1:3,4),1,size(binGridPtsWorld,2));
   binGridPtsPix = binGridPtsCam;
-  binGridPtsPix(1,:) = round((binGridPtsPix(1,:).*data.K(1,1))./binGridPtsPix(3,:)+data.K(1,3));
-  binGridPtsPix(2,:) = round((binGridPtsPix(2,:).*data.K(2,2))./binGridPtsPix(3,:)+data.K(2,3));
+  binGridPtsPix(1,:) = round((binGridPtsPix(1,:).*sceneData.colorK(1,1))./binGridPtsPix(3,:)+sceneData.colorK(1,3));
+  binGridPtsPix(2,:) = round((binGridPtsPix(2,:).*sceneData.colorK(2,2))./binGridPtsPix(3,:)+sceneData.colorK(2,3));
   pixWithinImage = find((binGridPtsPix(1,:) <= 640) & (binGridPtsPix(1,:) > 0) & (binGridPtsPix(2,:) <= 480) & (binGridPtsPix(2,:) > 0));
 %     binGridPtsPix = binGridPtsPix(:,pixWithinImage);
 %     binGridPts = binGridPts(:,pixWithinImage);
@@ -37,7 +39,7 @@
 
 
 
-objRt = eye(4);
+predObjPoseBin = eye(4);
 
 bestGuessPtsInd = find(binGridOccupancy > gridOccupancyThreshold);
 if length(bestGuessPtsInd) < 10
@@ -47,29 +49,29 @@
 instanceIdx = 1;
 objPts = sortrows(objPts',1);
 objCentroid = median(objPts);
-objRt(1:3,4) = objCentroid';
+predObjPoseBin(1:3,4) = objCentroid';
 
-if binID ~= -1
+if strcmp(sceneData.env,'shelf')
   if strcmp(objName,'dasani_water_bottle')
     if objCentroid(1) < 0.20 % If on incline
       if objCentroid(3) + 0.03 > 0.06
-        objRt(1:3,1:3) = [0 0 1; -1 0 0; 0 -1 0]';
+        predObjPoseBin(1:3,1:3) = [0 0 1; -1 0 0; 0 -1 0]';
         fprintf('Standing up on front\n');
       else
-          objRt(1:3,1:3) = [-1 0 0; 0 0 1; 0 1 0]';
+          predObjPoseBin(1:3,1:3) = [-1 0 0; 0 0 1; 0 1 0]';
         fprintf('Lying pointing front on front\n');
       end
     else
       if objCentroid(3) + 0.04 > 0.06
-        objRt(1:3,1:3) = [0 0 1; -1 0 0; 0 -1 0]';
+        predObjPoseBin(1:3,1:3) = [0 0 1; -1 0 0; 0 -1 0]';
         fprintf('Standing up on back\n');
       else
         ptsAlongCentroidX = find(objPts(:,1) == max(objPts(:,1)));
         if max(objPts(ptsAlongCentroidX,2))-min(objPts(ptsAlongCentroidX,2)) > 0.10
-          objRt(1:3,1:3) = [0 -1 0; 0 0 1; -1 0 0]';
+          predObjPoseBin(1:3,1:3) = [0 -1 0; 0 0 1; -1 0 0]';
           fprintf('Lying on back sideways\n');
         else
-          objRt(1:3,1:3) = [-1 0 0; 0 0 1; 0 1 0]';
+          predObjPoseBin(1:3,1:3) = [-1 0 0; 0 0 1; 0 1 0]';
           fprintf('Lying on back pointing forward\n');
         end
       end
@@ -80,36 +82,36 @@
   
 else
   if strcmp(objName,'rolodex_jumbo_pencil_cup')
-    objCamPts = objSegPts;
+    objCamPts = objSegmPts;
     objCamPts = objCamPts(:,find((abs(objCamPts(1,:) - objCentroid(1)) < 0.04) & (abs(objCamPts(2,:) - objCentroid(2)) < 0.04)));
     numPtsAroundCentroid = size(objCamPts,2);
     objCamPts = objCamPts(:,find(objCamPts(3,:) > 0.08 & objCamPts(3,:) < 0.15));
     ptsElevatedRatio = size(objCamPts,2)/numPtsAroundCentroid;
     if ptsElevatedRatio > 0.5
       topPts = mean(objCamPts,2);
-      objRt(1:3,1:3) = [0 0 -1; -1 0 0; 0 -1 0]';
+      predObjPoseBin(1:3,1:3) = [0 0 -1; -1 0 0; 0 -1 0]';
     end
   end
 end
 
-surfaceCentroid = objRt(1:3,4);
-if strcmp(objName,'rolodex_jumbo_pencil_cup') && binID == -1 && exist('topPts','var')
-  surfaceRange = [surfaceCentroid(1)-0.05,surfaceCentroid(1)+0.05;surfaceCentroid(2)-0.05,surfaceCentroid(2)+0.05;surfaceCentroid(3)-0.05,topPts(3)];
+surfCentroid = predObjPoseBin(1:3,4);
+if strcmp(objName,'rolodex_jumbo_pencil_cup') && strcmp(sceneData.env,'tote') && exist('topPts','var')
+  surfRange = [surfCentroid(1)-0.05,surfCentroid(1)+0.05;surfCentroid(2)-0.05,surfCentroid(2)+0.05;surfCentroid(3)-0.05,topPts(3)];
 else
-  surfaceRange = [surfaceCentroid(1)-0.05,surfaceCentroid(1)+0.05;surfaceCentroid(2)-0.05,surfaceCentroid(2)+0.05;surfaceCentroid(3)-0.05,surfaceCentroid(3)+0.05];
+  surfRange = [surfCentroid(1)-0.05,surfCentroid(1)+0.05;surfCentroid(2)-0.05,surfCentroid(2)+0.05;surfCentroid(3)-0.05,surfCentroid(3)+0.05];
 end
 
   
 % Enforce dog bowl (which has no depth) to always point upward
 if strcmp(objName,'platinum_pets_dog_bowl')
-  objRt(1:3,1:3) = [-1 0 0; 0 1 0; 0 0 -1];
+  predObjPoseBin(1:3,1:3) = [-1 0 0; 0 1 0; 0 0 -1];
 end
 
-if savePointClouds
+if savePointCloudVis
   pcwrite(pointCloud(objPts,'Color',repmat(uint8([0 255 0]),size(objPts,1),1)),fullfile(visPath,sprintf('vis.objDots.%s.%d',objName,instanceIdx)),'PLYFormat','binary');
   axisPts = [[[0:0.001:0.099]; zeros(2,100)],[zeros(1,100); [0:0.001:0.099]; zeros(1,100)],[zeros(2,100); [0:0.001:0.099]]];
   axisColors = uint8([repmat([255; 0; 0],1,100),repmat([0; 255; 0],1,100),repmat([0; 0; 255],1,100)]);
-  tmpAxisPts = objRt(1:3,1:3) * axisPts + repmat(objRt(1:3,4),1,size(axisPts,2));
+  tmpAxisPts = predObjPoseBin(1:3,1:3) * axisPts + repmat(predObjPoseBin(1:3,4),1,size(axisPts,2));
 %   pcwrite(pointCloud(tmpAxisPts','Color',axisColors'),fullfile(visPath,sprintf('vis.objPost.%s.%d',objName,instanceIdx)),'PLYFormat','binary');
 end
 
diff --git a/ros-packages/catkin_ws/src/pose_estimation/src/models/objects/centerObject.m b/ros-packages/catkin_ws/src/pose_estimation/src/models/objects/centerObject.m
index cc94933..477c6be 100644
--- a/ros-packages/catkin_ws/src/pose_estimation/src/models/objects/centerObject.m
+++ b/ros-packages/catkin_ws/src/pose_estimation/src/models/objects/centerObject.m
@@ -2,7 +2,7 @@
 
 % Load object
 % objCloud = offLoader('data/models/book_joke.off');
-objName = 'woods_extension_cord';
+objName = 'dasani_water_bottle';
 objCloud = pcread(sprintf('%s.ply',objName));
 
 % % Downsample object
diff --git a/ros-packages/catkin_ws/src/pose_estimation/src/models/objects/dasani_water_bottle.ply b/ros-packages/catkin_ws/src/pose_estimation/src/models/objects/dasani_water_bottle.ply
index 1fe36f5..2241df1 100644
Binary files a/ros-packages/catkin_ws/src/pose_estimation/src/models/objects/dasani_water_bottle.ply and b/ros-packages/catkin_ws/src/pose_estimation/src/models/objects/dasani_water_bottle.ply differ
diff --git a/ros-packages/catkin_ws/src/pose_estimation/src/models/objects/rolodex_jumbo_pencil_cup.ply b/ros-packages/catkin_ws/src/pose_estimation/src/models/objects/rolodex_jumbo_pencil_cup.ply
index 9dc9297..e83a035 100644
Binary files a/ros-packages/catkin_ws/src/pose_estimation/src/models/objects/rolodex_jumbo_pencil_cup.ply and b/ros-packages/catkin_ws/src/pose_estimation/src/models/objects/rolodex_jumbo_pencil_cup.ply differ
diff --git a/ros-packages/catkin_ws/src/pose_estimation/src/serviceCallback.m b/ros-packages/catkin_ws/src/pose_estimation/src/serviceCallback.m
index b3368cd..e0adc06 100644
--- a/ros-packages/catkin_ws/src/pose_estimation/src/serviceCallback.m
+++ b/ros-packages/catkin_ws/src/pose_estimation/src/serviceCallback.m
@@ -15,6 +15,11 @@
     sceneData = loadCalib(reqMsg.CalibrationFiles,sceneData);
 end
 
+% Fill holes in depth frames for scene
+for frameIdx = 1:length(sceneData.depthFrames)
+    sceneData.depthFrames{frameIdx} = fillHoles(sceneData.depthFrames{frameIdx});
+end
+
 % Load scene point cloud
 scenePointCloud = getScenePointCloud(sceneData);
 
@@ -37,7 +42,7 @@
 objList = sceneData.objects;
 uniqueObjectList = unique(objList);
 for objIdx = 1:length(uniqueObjectList)
-    tic;
+%     tic;
     objName = uniqueObjectList{objIdx};
     objNum = sum(ismember(objList,objName));
     objModel = objModels{find(ismember(objNames,objName))};
@@ -51,7 +56,7 @@
     for duplicateIdx = 1:length(objPoses)
         respMsg.Objects = [respMsg.Objects; objPoses(duplicateIdx)];
     end
-    toc;
+%     toc;
 end
 resp = true;
 end
diff --git a/visualization/dispObjPose.m b/vis-utils/dispObjPose.m
similarity index 100%
rename from visualization/dispObjPose.m
rename to vis-utils/dispObjPose.m
diff --git a/visualization/getColorPalette.m b/vis-utils/getColorPalette.m
similarity index 100%
rename from visualization/getColorPalette.m
rename to vis-utils/getColorPalette.m
diff --git a/visualization/parseLabels.m b/vis-utils/parseLabels.m
similarity index 100%
rename from visualization/parseLabels.m
rename to vis-utils/parseLabels.m
diff --git a/visualization/splitToteIm.m b/vis-utils/splitToteIm.m
similarity index 100%
rename from visualization/splitToteIm.m
rename to vis-utils/splitToteIm.m
diff --git a/visualization/visObjLabels.m b/vis-utils/visObjLabels.m
similarity index 100%
rename from visualization/visObjLabels.m
rename to vis-utils/visObjLabels.m
diff --git a/visualization/visRepImage.m b/vis-utils/visRepImage.m
similarity index 100%
rename from visualization/visRepImage.m
rename to vis-utils/visRepImage.m
diff --git a/visualization/visualizeResults.m b/visualization/visualizeResults.m
deleted file mode 100644
index 3a04543..0000000
--- a/visualization/visualizeResults.m
+++ /dev/null
@@ -1,159 +0,0 @@
-function resultImage = visualizeResults(RtPCA,latentPCA,surfaceCentroid,surfaceRange,finalRt,finalScore,dataPath,objName,instanceIdx,binID,data,objSegPts,objMasks,objModelPts)
-
-% if strcmp(objName,'rolodex_jumbo_pencil_cup') ||
-
-% Show segmentation result (denoised)
-if binID == -1
-  frames = [5 14];
-else
-  frames = [13];
-end
-
-allObjImage = [];
-displayMode = 'Predicted Rigid Object Pose';
-for baseFrameIdx = frames
-  segImage = data.color{baseFrameIdx};
-  extBin2Cam = inv(data.extCam2World{baseFrameIdx})*data.extBin2World;
-  if strcmp(objName,'rolodex_jumbo_pencil_cup') || strcmp(objName,'dasani_water_bottle') || strcmp(objName,'platinum_pets_dog_bowl') || strcmp(objName,'easter_turtle_sippy_cup')
-    segMask = double(objMasks{baseFrameIdx})*255;
-  else
-    camObjSegPts = extBin2Cam(1:3,1:3) * objSegPts + repmat(extBin2Cam(1:3,4),1,size(objSegPts,2)); 
-    pixX = round(((camObjSegPts(1,:).*data.K(1,1))./camObjSegPts(3,:))+data.K(1,3));
-    pixY = round(((camObjSegPts(2,:).*data.K(2,2))./camObjSegPts(3,:))+data.K(2,3));
-    validPix = find(pixX > 0 & pixX <= 640 & pixY > 0 & pixY <= 480);
-    segMask = zeros(480,640);
-    segMask(sub2ind(size(segMask),pixY(validPix)',pixX(validPix)')) = 255;
-  end
-
-  segImageR = double(segImage(:,:,1));
-  segImageG = double(segImage(:,:,2));
-  segImageB = double(segImage(:,:,3));
-  segImageR(find(segMask)) = segImageR(find(segMask))./2;
-  segImageG(find(segMask)) = (segImageG(find(segMask)) + 255)./2;
-  segImageB(find(segMask)) = segImageB(find(segMask))./2;
-  segImage(:,:,1) = uint8(round(segImageR));
-  segImage(:,:,2) = uint8(round(segImageG));
-  segImage(:,:,3) = uint8(round(segImageB));
-
-  % finalRt
-  % objModelPts
-
-    drawBbox = true;
-    drawModel = true;
-
-  modelRt = extBin2Cam * data.extWorld2Bin * finalRt;
-  RtPCA = data.extWorld2Bin * RtPCA;
-  if strcmp(objName,'cherokee_easy_tee_shirt') || strcmp(objName,'kyjen_squeakin_eggs_plush_puppies') || strcmp(objName,'womens_knit_gloves') || strcmp(objName,'cloud_b_plush_bear') || ...
-     strcmp(objName,'scotch_bubble_mailer')
-    modelRt(1:3,4) = surfaceCentroid;
-    modelRt(1:3,1:3) = RtPCA(1:3,1:3);
-
-    extBin2Obj = inv(data.extWorld2Bin * modelRt);
-    objModelPts = extBin2Obj(1:3,1:3) * objSegPts + repmat(extBin2Obj(1:3,4),1,size(objSegPts,2));
-
-    modelRt = extBin2Cam * data.extWorld2Bin * modelRt;
-
-    drawBbox = false;
-    displayMode = 'Estimated Observable Surface PCA';
-
-    axisRange = [latentPCA(1)*50,latentPCA(2)*50,latentPCA(3)*50];
-  elseif strcmp(objName,'rolodex_jumbo_pencil_cup')
-    drawBbox = false;
-    drawModel = false;
-    axisRange = [0.06,0.06,0.06];
-  elseif strcmp(objName,'dasani_water_bottle')
-    drawBbox = false;
-    drawModel = false;
-    axisRange = [0.1,0.03,0.03];
-  elseif strcmp(objName,'platinum_pets_dog_bowl')
-    drawBbox = false;
-    drawModel = false;
-    axisRange = [0.06,0.06,0.015];
-  else
-    axisRange = [max(objModelPts(1,:)),max(objModelPts(2,:)),max(objModelPts(3,:))];
-  end
-
-  camObjModelPts = modelRt(1:3,1:3) * objModelPts + repmat(modelRt(1:3,4),1,size(objModelPts,2));
-  pixX = round(((camObjModelPts(1,:).*data.K(1,1))./camObjModelPts(3,:))+data.K(1,3));
-  pixY = round(((camObjModelPts(2,:).*data.K(2,2))./camObjModelPts(3,:))+data.K(2,3));
-  validPix = find(pixX > 0 & pixX <= 640 & pixY > 0 & pixY <= 480);
-  objMask = zeros(480,640);
-  objMask(sub2ind(size(objMask),pixY(validPix)',pixX(validPix)')) = 1;
-
-  objImage = data.color{baseFrameIdx};
-  if drawModel
-    objImageR = double(objImage(:,:,1));
-    objImageG = double(objImage(:,:,2));
-    objImageB = double(objImage(:,:,3));
-    objImageR(find(objMask)) = objImageR(find(objMask))./2;
-    objImageG(find(objMask)) = (objImageG(find(objMask)) + 255)./2;
-    objImageB(find(objMask)) = objImageB(find(objMask))./2;
-    objImage(:,:,1) = uint8(round(objImageR));
-    objImage(:,:,2) = uint8(round(objImageG));
-    objImage(:,:,3) = uint8(round(objImageB));
-  end
-
-  axisPts = [[0;0;0],[axisRange(1);0;0],[0;axisRange(2);0],[0;0;axisRange(3)]];
-  camAxisPts = modelRt(1:3,1:3) * axisPts + repmat(modelRt(1:3,4),1,size(axisPts,2));
-  axisPixX = round(((camAxisPts(1,:).*data.K(1,1))./camAxisPts(3,:))+data.K(1,3));
-  axisPixY = round(((camAxisPts(2,:).*data.K(2,2))./camAxisPts(3,:))+data.K(2,3));
-  axisPts2D = [axisPixX; axisPixY];
-  canvasImage = insertShape(objImage, 'line', [axisPts2D(:,1)',axisPts2D(:,2)'], 'LineWidth', 3,'Color', 'red');
-  canvasImage = insertShape(canvasImage, 'line', [axisPts2D(:,1)',axisPts2D(:,3)'], 'LineWidth', 3,'Color', 'green');
-  canvasImage = insertShape(canvasImage, 'line', [axisPts2D(:,1)',axisPts2D(:,4)'], 'LineWidth', 3,'Color', 'blue');
-  % test = insertShape(test, 'FilledCircle', [axisPts2D(:,1)' 2], 'LineWidth', 1,'Opacity',1,'Color', 'black');
-
-  if drawBbox
-    bboxRange = [min(objModelPts(1,:)),max(objModelPts(1,:));min(objModelPts(2,:)),max(objModelPts(2,:));min(objModelPts(3,:)),max(objModelPts(3,:))];
-    bboxCorners = [[bboxRange(1,1);bboxRange(2,1);bboxRange(3,1)], ...
-                   [bboxRange(1,1);bboxRange(2,1);bboxRange(3,2)], ...
-                   [bboxRange(1,1);bboxRange(2,2);bboxRange(3,2)], ...
-                   [bboxRange(1,1);bboxRange(2,2);bboxRange(3,1)], ...
-                   [bboxRange(1,2);bboxRange(2,1);bboxRange(3,1)], ...
-                   [bboxRange(1,2);bboxRange(2,1);bboxRange(3,2)], ...
-                   [bboxRange(1,2);bboxRange(2,2);bboxRange(3,2)], ...
-                   [bboxRange(1,2);bboxRange(2,2);bboxRange(3,1)]];
-    camCornerPts = modelRt(1:3,1:3) * bboxCorners + repmat(modelRt(1:3,4),1,size(bboxCorners,2));
-    cornerPixX = round(((camCornerPts(1,:).*data.K(1,1))./camCornerPts(3,:))+data.K(1,3));
-    cornerPixY = round(((camCornerPts(2,:).*data.K(2,2))./camCornerPts(3,:))+data.K(2,3));
-    cornerPts2D = [cornerPixX; cornerPixY];
-    if max(cornerPts2D(:)) < 1500  % to prevent bad detection to make drawing too slow
-      canvasImage = insertShape(canvasImage, 'line', [cornerPts2D(:,1)',cornerPts2D(:,2)';cornerPts2D(:,2)',cornerPts2D(:,3)';cornerPts2D(:,3)',cornerPts2D(:,4)';cornerPts2D(:,4)',cornerPts2D(:,1)'], 'LineWidth', 1,'Color', 'yellow');
-      canvasImage = insertShape(canvasImage, 'line', [cornerPts2D(:,5)',cornerPts2D(:,6)';cornerPts2D(:,6)',cornerPts2D(:,7)';cornerPts2D(:,7)',cornerPts2D(:,8)';cornerPts2D(:,8)',cornerPts2D(:,5)'], 'LineWidth', 1,'Color', 'yellow');
-      canvasImage = insertShape(canvasImage, 'line', [cornerPts2D(:,1)',cornerPts2D(:,5)';cornerPts2D(:,2)',cornerPts2D(:,6)';cornerPts2D(:,3)',cornerPts2D(:,7)';cornerPts2D(:,4)',cornerPts2D(:,8)'], 'LineWidth', 1,'Color', 'yellow');
-    end
-  end
-  
-  if baseFrameIdx == frames(end);
-    segImage = insertText(segImage,[10 450],'Segmentation Results','TextColor','white','BoxColor','black');
-    canvasImage = insertText(canvasImage,[10 450],sprintf('%s',displayMode),'TextColor','white','BoxColor','black');
-  end
-  
-  paddedSegImage = uint8(zeros(size(segImage,1)+5,size(segImage,2)+5,size(segImage,3)));
-  paddedSegImage(1:480,1:640,:) = segImage;
-  segImage = paddedSegImage;
-  paddedCanvasImage = uint8(zeros(size(canvasImage,1)+5,size(canvasImage,2)+5,size(canvasImage,3)));
-  paddedCanvasImage(1:480,6:645,:) = canvasImage;
-  canvasImage = paddedCanvasImage;
-  
-  allObjImage = cat(1,allObjImage,cat(2,segImage,canvasImage));
-end
-
-paddedAllObjImage = uint8(zeros(size(allObjImage,1)+5,size(allObjImage,2)+10,size(allObjImage,3)));
-paddedAllObjImage(6:end,6:(end-5),:) = allObjImage;
-allObjImage = paddedAllObjImage;
-% objImage = canvasImage;
-
-% resultImage = objImage;
-
-
-
-% imshow(test)
-
-             
-imwrite(allObjImage,fullfile(dataPath,strcat(objName,sprintf('.%d.result.png',instanceIdx))));
-
-
-
-end
-