Skip to content

Commit

Permalink
fix pre-scanned object models for objects with no depth; add LICENSE
Browse files Browse the repository at this point in the history
  • Loading branch information
@andyzeng
andyzeng committed Aug 30, 2016
1 parent 0137ec6 commit 5dfba74
Show file tree
Hide file tree
Showing 16 changed files with 318 additions and 381 deletions.
22 changes: 22 additions & 0 deletions LICENSE
View file
Original file line number Diff line number Diff line change
@@ -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.
225 changes: 147 additions & 78 deletions evaluation/benchmark.m
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
%
% ---------------------------------------------------------
Expand All @@ -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


Expand Down
63 changes: 33 additions & 30 deletions ros-packages/catkin_ws/src/pose_estimation/src/getObjectHypothesis.m
View file
Original file line number Diff line number Diff line change
@@ -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);
Expand All @@ -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);
Expand All @@ -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');
Expand All @@ -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
Expand Down
Loading

0 comments on commit 5dfba74

Please sign in to comment.