diff --git a/README.md b/README.md index 0765aac..b44230b 100644 --- a/README.md +++ b/README.md @@ -1,13 +1,10 @@ -# MIT-Princeton Vision Toolbox for the APC 2016 +# Princeton Vision Toolbox for the APC 2016 -## A Quick-Start: Matlab Demo -Estimates 6D object poses on the sample scene data (in `data/sample`) with pre-computed object segmentation results from [Deep Learning FCN ROS Package](#deep-learning-fcn-ros-package). -1. - ## Documentation +* [A Quick Start: Matlab Demo](#a-quick-start:-matlab-demo) * [6D Pose Estimation ROS Package](#6d-pose-estimation-ros-package) * [Realsense Standalone](#realsense-standalone) * [Realsense ROS Package](#realsense-ros-package) @@ -15,6 +12,10 @@ Estimates 6D object poses on the sample scene data (in `data/sample`) with pre-c * [FCN Training with Marvin](#fcn-training-with-marvin) * [Evaluation Code](#evaluation-code) +## A Quick-Start: Matlab Demo +Estimates 6D object poses on the sample scene data (in `data/sample`) with pre-computed object segmentation results from [Deep Learning FCN ROS Package](#deep-learning-fcn-ros-package). +1. + ## 6D Pose Estimation ROS Package A Matlab ROS Package for estimating 6D object poses by model-fitting with ICP on RGB-D object segmentation results. diff --git a/data/sample/scene-0000/masks/creativity_chenille_stems.thresh.txt b/data/sample/scene-0000/masks/creativity_chenille_stems.thresh.txt new file mode 100644 index 0000000..5b93ec4 --- /dev/null +++ b/data/sample/scene-0000/masks/creativity_chenille_stems.thresh.txt @@ -0,0 +1 @@ +0.30941 diff --git a/data/sample/scene-0000/masks/frame-000000.creativity_chenille_stems.mask.png b/data/sample/scene-0000/masks/frame-000000.creativity_chenille_stems.mask.png new file mode 100644 index 0000000..e79ae47 Binary files /dev/null and b/data/sample/scene-0000/masks/frame-000000.creativity_chenille_stems.mask.png differ diff --git a/data/sample/scene-0000/masks/frame-000000.i_am_a_bunny_book.mask.png b/data/sample/scene-0000/masks/frame-000000.i_am_a_bunny_book.mask.png new file mode 100644 index 0000000..565969a Binary files /dev/null and b/data/sample/scene-0000/masks/frame-000000.i_am_a_bunny_book.mask.png differ diff --git a/data/sample/scene-0000/masks/frame-000000.kleenex_tissue_box.mask.png b/data/sample/scene-0000/masks/frame-000000.kleenex_tissue_box.mask.png new file mode 100644 index 0000000..0c7ec59 Binary files /dev/null and b/data/sample/scene-0000/masks/frame-000000.kleenex_tissue_box.mask.png differ diff --git a/data/sample/scene-0000/masks/frame-000000.laugh_out_loud_joke_book.mask.png b/data/sample/scene-0000/masks/frame-000000.laugh_out_loud_joke_book.mask.png new file mode 100644 index 0000000..c82fcb2 Binary files /dev/null and b/data/sample/scene-0000/masks/frame-000000.laugh_out_loud_joke_book.mask.png differ diff --git a/data/sample/scene-0000/masks/frame-000001.creativity_chenille_stems.mask.png b/data/sample/scene-0000/masks/frame-000001.creativity_chenille_stems.mask.png new file mode 100644 index 0000000..fe6494e Binary files /dev/null and b/data/sample/scene-0000/masks/frame-000001.creativity_chenille_stems.mask.png differ diff --git a/data/sample/scene-0000/masks/frame-000001.i_am_a_bunny_book.mask.png b/data/sample/scene-0000/masks/frame-000001.i_am_a_bunny_book.mask.png new file mode 100644 index 0000000..61ec06b Binary files /dev/null and b/data/sample/scene-0000/masks/frame-000001.i_am_a_bunny_book.mask.png differ diff --git a/data/sample/scene-0000/masks/frame-000001.kleenex_tissue_box.mask.png b/data/sample/scene-0000/masks/frame-000001.kleenex_tissue_box.mask.png new file mode 100644 index 0000000..39f6cd1 Binary files /dev/null and b/data/sample/scene-0000/masks/frame-000001.kleenex_tissue_box.mask.png differ diff --git a/data/sample/scene-0000/masks/frame-000001.laugh_out_loud_joke_book.mask.png b/data/sample/scene-0000/masks/frame-000001.laugh_out_loud_joke_book.mask.png new file mode 100644 index 0000000..1b9a97b Binary files /dev/null and b/data/sample/scene-0000/masks/frame-000001.laugh_out_loud_joke_book.mask.png differ diff --git a/data/sample/scene-0000/masks/frame-000002.creativity_chenille_stems.mask.png b/data/sample/scene-0000/masks/frame-000002.creativity_chenille_stems.mask.png new file mode 100644 index 0000000..7b4cc58 Binary files /dev/null and b/data/sample/scene-0000/masks/frame-000002.creativity_chenille_stems.mask.png differ diff --git a/data/sample/scene-0000/masks/frame-000002.i_am_a_bunny_book.mask.png b/data/sample/scene-0000/masks/frame-000002.i_am_a_bunny_book.mask.png new file mode 100644 index 0000000..36d2fb6 Binary files /dev/null and b/data/sample/scene-0000/masks/frame-000002.i_am_a_bunny_book.mask.png differ diff --git a/data/sample/scene-0000/masks/frame-000002.kleenex_tissue_box.mask.png b/data/sample/scene-0000/masks/frame-000002.kleenex_tissue_box.mask.png new file mode 100644 index 0000000..e449d7a Binary files /dev/null and b/data/sample/scene-0000/masks/frame-000002.kleenex_tissue_box.mask.png differ diff --git a/data/sample/scene-0000/masks/frame-000002.laugh_out_loud_joke_book.mask.png b/data/sample/scene-0000/masks/frame-000002.laugh_out_loud_joke_book.mask.png new file mode 100644 index 0000000..69e5cc1 Binary files /dev/null and b/data/sample/scene-0000/masks/frame-000002.laugh_out_loud_joke_book.mask.png differ diff --git a/data/sample/scene-0000/masks/frame-000003.creativity_chenille_stems.mask.png b/data/sample/scene-0000/masks/frame-000003.creativity_chenille_stems.mask.png new file mode 100644 index 0000000..123a416 Binary files /dev/null and b/data/sample/scene-0000/masks/frame-000003.creativity_chenille_stems.mask.png differ diff --git a/data/sample/scene-0000/masks/frame-000003.i_am_a_bunny_book.mask.png b/data/sample/scene-0000/masks/frame-000003.i_am_a_bunny_book.mask.png new file mode 100644 index 0000000..bfcbe6b Binary files /dev/null and b/data/sample/scene-0000/masks/frame-000003.i_am_a_bunny_book.mask.png differ diff --git a/data/sample/scene-0000/masks/frame-000003.kleenex_tissue_box.mask.png b/data/sample/scene-0000/masks/frame-000003.kleenex_tissue_box.mask.png new file mode 100644 index 0000000..7ce89cd Binary files /dev/null and b/data/sample/scene-0000/masks/frame-000003.kleenex_tissue_box.mask.png differ diff --git a/data/sample/scene-0000/masks/frame-000003.laugh_out_loud_joke_book.mask.png b/data/sample/scene-0000/masks/frame-000003.laugh_out_loud_joke_book.mask.png new file mode 100644 index 0000000..5bdcebb Binary files /dev/null and b/data/sample/scene-0000/masks/frame-000003.laugh_out_loud_joke_book.mask.png differ diff --git a/data/sample/scene-0000/masks/frame-000004.creativity_chenille_stems.mask.png b/data/sample/scene-0000/masks/frame-000004.creativity_chenille_stems.mask.png new file mode 100644 index 0000000..e89c9ff Binary files /dev/null and b/data/sample/scene-0000/masks/frame-000004.creativity_chenille_stems.mask.png differ diff --git a/data/sample/scene-0000/masks/frame-000004.i_am_a_bunny_book.mask.png b/data/sample/scene-0000/masks/frame-000004.i_am_a_bunny_book.mask.png new file mode 100644 index 0000000..590f98c Binary files /dev/null and b/data/sample/scene-0000/masks/frame-000004.i_am_a_bunny_book.mask.png differ diff --git a/data/sample/scene-0000/masks/frame-000004.kleenex_tissue_box.mask.png b/data/sample/scene-0000/masks/frame-000004.kleenex_tissue_box.mask.png new file mode 100644 index 0000000..76be917 Binary files /dev/null and b/data/sample/scene-0000/masks/frame-000004.kleenex_tissue_box.mask.png differ diff --git a/data/sample/scene-0000/masks/frame-000004.laugh_out_loud_joke_book.mask.png b/data/sample/scene-0000/masks/frame-000004.laugh_out_loud_joke_book.mask.png new file mode 100644 index 0000000..2de3061 Binary files /dev/null and b/data/sample/scene-0000/masks/frame-000004.laugh_out_loud_joke_book.mask.png differ diff --git a/data/sample/scene-0000/masks/frame-000005.creativity_chenille_stems.mask.png b/data/sample/scene-0000/masks/frame-000005.creativity_chenille_stems.mask.png new file mode 100644 index 0000000..d2d26d7 Binary files /dev/null and b/data/sample/scene-0000/masks/frame-000005.creativity_chenille_stems.mask.png differ diff --git a/data/sample/scene-0000/masks/frame-000005.i_am_a_bunny_book.mask.png b/data/sample/scene-0000/masks/frame-000005.i_am_a_bunny_book.mask.png new file mode 100644 index 0000000..66bd3c0 Binary files /dev/null and b/data/sample/scene-0000/masks/frame-000005.i_am_a_bunny_book.mask.png differ diff --git a/data/sample/scene-0000/masks/frame-000005.kleenex_tissue_box.mask.png b/data/sample/scene-0000/masks/frame-000005.kleenex_tissue_box.mask.png new file mode 100644 index 0000000..34c2f09 Binary files /dev/null and b/data/sample/scene-0000/masks/frame-000005.kleenex_tissue_box.mask.png differ diff --git a/data/sample/scene-0000/masks/frame-000005.laugh_out_loud_joke_book.mask.png b/data/sample/scene-0000/masks/frame-000005.laugh_out_loud_joke_book.mask.png new file mode 100644 index 0000000..9c3d83a Binary files /dev/null and b/data/sample/scene-0000/masks/frame-000005.laugh_out_loud_joke_book.mask.png differ diff --git a/data/sample/scene-0000/masks/frame-000006.creativity_chenille_stems.mask.png b/data/sample/scene-0000/masks/frame-000006.creativity_chenille_stems.mask.png new file mode 100644 index 0000000..195c6ba Binary files /dev/null and b/data/sample/scene-0000/masks/frame-000006.creativity_chenille_stems.mask.png differ diff --git a/data/sample/scene-0000/masks/frame-000006.i_am_a_bunny_book.mask.png b/data/sample/scene-0000/masks/frame-000006.i_am_a_bunny_book.mask.png new file mode 100644 index 0000000..c7d9bfb Binary files /dev/null and b/data/sample/scene-0000/masks/frame-000006.i_am_a_bunny_book.mask.png differ diff --git a/data/sample/scene-0000/masks/frame-000006.kleenex_tissue_box.mask.png b/data/sample/scene-0000/masks/frame-000006.kleenex_tissue_box.mask.png new file mode 100644 index 0000000..8e83fef Binary files /dev/null and b/data/sample/scene-0000/masks/frame-000006.kleenex_tissue_box.mask.png differ diff --git a/data/sample/scene-0000/masks/frame-000006.laugh_out_loud_joke_book.mask.png b/data/sample/scene-0000/masks/frame-000006.laugh_out_loud_joke_book.mask.png new file mode 100644 index 0000000..fdd4bf7 Binary files /dev/null and b/data/sample/scene-0000/masks/frame-000006.laugh_out_loud_joke_book.mask.png differ diff --git a/data/sample/scene-0000/masks/frame-000007.creativity_chenille_stems.mask.png b/data/sample/scene-0000/masks/frame-000007.creativity_chenille_stems.mask.png new file mode 100644 index 0000000..aefe7f7 Binary files /dev/null and b/data/sample/scene-0000/masks/frame-000007.creativity_chenille_stems.mask.png differ diff --git a/data/sample/scene-0000/masks/frame-000007.i_am_a_bunny_book.mask.png b/data/sample/scene-0000/masks/frame-000007.i_am_a_bunny_book.mask.png new file mode 100644 index 0000000..4518b26 Binary files /dev/null and b/data/sample/scene-0000/masks/frame-000007.i_am_a_bunny_book.mask.png differ diff --git a/data/sample/scene-0000/masks/frame-000007.kleenex_tissue_box.mask.png b/data/sample/scene-0000/masks/frame-000007.kleenex_tissue_box.mask.png new file mode 100644 index 0000000..ba6d9e0 Binary files /dev/null and b/data/sample/scene-0000/masks/frame-000007.kleenex_tissue_box.mask.png differ diff --git a/data/sample/scene-0000/masks/frame-000007.laugh_out_loud_joke_book.mask.png b/data/sample/scene-0000/masks/frame-000007.laugh_out_loud_joke_book.mask.png new file mode 100644 index 0000000..aefb116 Binary files /dev/null and b/data/sample/scene-0000/masks/frame-000007.laugh_out_loud_joke_book.mask.png differ diff --git a/data/sample/scene-0000/masks/frame-000008.creativity_chenille_stems.mask.png b/data/sample/scene-0000/masks/frame-000008.creativity_chenille_stems.mask.png new file mode 100644 index 0000000..47efdf4 Binary files /dev/null and b/data/sample/scene-0000/masks/frame-000008.creativity_chenille_stems.mask.png differ diff --git a/data/sample/scene-0000/masks/frame-000008.i_am_a_bunny_book.mask.png b/data/sample/scene-0000/masks/frame-000008.i_am_a_bunny_book.mask.png new file mode 100644 index 0000000..cd4eee5 Binary files /dev/null and b/data/sample/scene-0000/masks/frame-000008.i_am_a_bunny_book.mask.png differ diff --git a/data/sample/scene-0000/masks/frame-000008.kleenex_tissue_box.mask.png b/data/sample/scene-0000/masks/frame-000008.kleenex_tissue_box.mask.png new file mode 100644 index 0000000..009fd02 Binary files /dev/null and b/data/sample/scene-0000/masks/frame-000008.kleenex_tissue_box.mask.png differ diff --git a/data/sample/scene-0000/masks/frame-000008.laugh_out_loud_joke_book.mask.png b/data/sample/scene-0000/masks/frame-000008.laugh_out_loud_joke_book.mask.png new file mode 100644 index 0000000..4ed5643 Binary files /dev/null and b/data/sample/scene-0000/masks/frame-000008.laugh_out_loud_joke_book.mask.png differ diff --git a/data/sample/scene-0000/masks/frame-000009.creativity_chenille_stems.mask.png b/data/sample/scene-0000/masks/frame-000009.creativity_chenille_stems.mask.png new file mode 100644 index 0000000..a0801d2 Binary files /dev/null and b/data/sample/scene-0000/masks/frame-000009.creativity_chenille_stems.mask.png differ diff --git a/data/sample/scene-0000/masks/frame-000009.i_am_a_bunny_book.mask.png b/data/sample/scene-0000/masks/frame-000009.i_am_a_bunny_book.mask.png new file mode 100644 index 0000000..8ae57fc Binary files /dev/null and b/data/sample/scene-0000/masks/frame-000009.i_am_a_bunny_book.mask.png differ diff --git a/data/sample/scene-0000/masks/frame-000009.kleenex_tissue_box.mask.png b/data/sample/scene-0000/masks/frame-000009.kleenex_tissue_box.mask.png new file mode 100644 index 0000000..c67aa53 Binary files /dev/null and b/data/sample/scene-0000/masks/frame-000009.kleenex_tissue_box.mask.png differ diff --git a/data/sample/scene-0000/masks/frame-000009.laugh_out_loud_joke_book.mask.png b/data/sample/scene-0000/masks/frame-000009.laugh_out_loud_joke_book.mask.png new file mode 100644 index 0000000..24822af Binary files /dev/null and b/data/sample/scene-0000/masks/frame-000009.laugh_out_loud_joke_book.mask.png differ diff --git a/data/sample/scene-0000/masks/frame-000010.creativity_chenille_stems.mask.png b/data/sample/scene-0000/masks/frame-000010.creativity_chenille_stems.mask.png new file mode 100644 index 0000000..17eca29 Binary files /dev/null and b/data/sample/scene-0000/masks/frame-000010.creativity_chenille_stems.mask.png differ diff --git a/data/sample/scene-0000/masks/frame-000010.i_am_a_bunny_book.mask.png b/data/sample/scene-0000/masks/frame-000010.i_am_a_bunny_book.mask.png new file mode 100644 index 0000000..f909700 Binary files /dev/null and b/data/sample/scene-0000/masks/frame-000010.i_am_a_bunny_book.mask.png differ diff --git a/data/sample/scene-0000/masks/frame-000010.kleenex_tissue_box.mask.png b/data/sample/scene-0000/masks/frame-000010.kleenex_tissue_box.mask.png new file mode 100644 index 0000000..8da7df5 Binary files /dev/null and b/data/sample/scene-0000/masks/frame-000010.kleenex_tissue_box.mask.png differ diff --git a/data/sample/scene-0000/masks/frame-000010.laugh_out_loud_joke_book.mask.png b/data/sample/scene-0000/masks/frame-000010.laugh_out_loud_joke_book.mask.png new file mode 100644 index 0000000..7f9c962 Binary files /dev/null and b/data/sample/scene-0000/masks/frame-000010.laugh_out_loud_joke_book.mask.png differ diff --git a/data/sample/scene-0000/masks/frame-000011.creativity_chenille_stems.mask.png b/data/sample/scene-0000/masks/frame-000011.creativity_chenille_stems.mask.png new file mode 100644 index 0000000..131968f Binary files /dev/null and b/data/sample/scene-0000/masks/frame-000011.creativity_chenille_stems.mask.png differ diff --git a/data/sample/scene-0000/masks/frame-000011.i_am_a_bunny_book.mask.png b/data/sample/scene-0000/masks/frame-000011.i_am_a_bunny_book.mask.png new file mode 100644 index 0000000..b432147 Binary files /dev/null and b/data/sample/scene-0000/masks/frame-000011.i_am_a_bunny_book.mask.png differ diff --git a/data/sample/scene-0000/masks/frame-000011.kleenex_tissue_box.mask.png b/data/sample/scene-0000/masks/frame-000011.kleenex_tissue_box.mask.png new file mode 100644 index 0000000..c5a0007 Binary files /dev/null and b/data/sample/scene-0000/masks/frame-000011.kleenex_tissue_box.mask.png differ diff --git a/data/sample/scene-0000/masks/frame-000011.laugh_out_loud_joke_book.mask.png b/data/sample/scene-0000/masks/frame-000011.laugh_out_loud_joke_book.mask.png new file mode 100644 index 0000000..ede93e0 Binary files /dev/null and b/data/sample/scene-0000/masks/frame-000011.laugh_out_loud_joke_book.mask.png differ diff --git a/data/sample/scene-0000/masks/frame-000012.creativity_chenille_stems.mask.png b/data/sample/scene-0000/masks/frame-000012.creativity_chenille_stems.mask.png new file mode 100644 index 0000000..0c8413b Binary files /dev/null and b/data/sample/scene-0000/masks/frame-000012.creativity_chenille_stems.mask.png differ diff --git a/data/sample/scene-0000/masks/frame-000012.i_am_a_bunny_book.mask.png b/data/sample/scene-0000/masks/frame-000012.i_am_a_bunny_book.mask.png new file mode 100644 index 0000000..f997f3f Binary files /dev/null and b/data/sample/scene-0000/masks/frame-000012.i_am_a_bunny_book.mask.png differ diff --git a/data/sample/scene-0000/masks/frame-000012.kleenex_tissue_box.mask.png b/data/sample/scene-0000/masks/frame-000012.kleenex_tissue_box.mask.png new file mode 100644 index 0000000..811ef7c Binary files /dev/null and b/data/sample/scene-0000/masks/frame-000012.kleenex_tissue_box.mask.png differ diff --git a/data/sample/scene-0000/masks/frame-000012.laugh_out_loud_joke_book.mask.png b/data/sample/scene-0000/masks/frame-000012.laugh_out_loud_joke_book.mask.png new file mode 100644 index 0000000..8eee4a4 Binary files /dev/null and b/data/sample/scene-0000/masks/frame-000012.laugh_out_loud_joke_book.mask.png differ diff --git a/data/sample/scene-0000/masks/frame-000013.creativity_chenille_stems.mask.png b/data/sample/scene-0000/masks/frame-000013.creativity_chenille_stems.mask.png new file mode 100644 index 0000000..a81f3e2 Binary files /dev/null and b/data/sample/scene-0000/masks/frame-000013.creativity_chenille_stems.mask.png differ diff --git a/data/sample/scene-0000/masks/frame-000013.i_am_a_bunny_book.mask.png b/data/sample/scene-0000/masks/frame-000013.i_am_a_bunny_book.mask.png new file mode 100644 index 0000000..0e20b70 Binary files /dev/null and b/data/sample/scene-0000/masks/frame-000013.i_am_a_bunny_book.mask.png differ diff --git a/data/sample/scene-0000/masks/frame-000013.kleenex_tissue_box.mask.png b/data/sample/scene-0000/masks/frame-000013.kleenex_tissue_box.mask.png new file mode 100644 index 0000000..caa2a42 Binary files /dev/null and b/data/sample/scene-0000/masks/frame-000013.kleenex_tissue_box.mask.png differ diff --git a/data/sample/scene-0000/masks/frame-000013.laugh_out_loud_joke_book.mask.png b/data/sample/scene-0000/masks/frame-000013.laugh_out_loud_joke_book.mask.png new file mode 100644 index 0000000..692b60f Binary files /dev/null and b/data/sample/scene-0000/masks/frame-000013.laugh_out_loud_joke_book.mask.png differ diff --git a/data/sample/scene-0000/masks/frame-000014.creativity_chenille_stems.mask.png b/data/sample/scene-0000/masks/frame-000014.creativity_chenille_stems.mask.png new file mode 100644 index 0000000..df71fb4 Binary files /dev/null and b/data/sample/scene-0000/masks/frame-000014.creativity_chenille_stems.mask.png differ diff --git a/data/sample/scene-0000/masks/frame-000014.i_am_a_bunny_book.mask.png b/data/sample/scene-0000/masks/frame-000014.i_am_a_bunny_book.mask.png new file mode 100644 index 0000000..5730bfd Binary files /dev/null and b/data/sample/scene-0000/masks/frame-000014.i_am_a_bunny_book.mask.png differ diff --git a/data/sample/scene-0000/masks/frame-000014.kleenex_tissue_box.mask.png b/data/sample/scene-0000/masks/frame-000014.kleenex_tissue_box.mask.png new file mode 100644 index 0000000..b843a35 Binary files /dev/null and b/data/sample/scene-0000/masks/frame-000014.kleenex_tissue_box.mask.png differ diff --git a/data/sample/scene-0000/masks/frame-000014.laugh_out_loud_joke_book.mask.png b/data/sample/scene-0000/masks/frame-000014.laugh_out_loud_joke_book.mask.png new file mode 100644 index 0000000..57e5d0c Binary files /dev/null and b/data/sample/scene-0000/masks/frame-000014.laugh_out_loud_joke_book.mask.png differ diff --git a/data/sample/scene-0000/masks/i_am_a_bunny_book.thresh.txt b/data/sample/scene-0000/masks/i_am_a_bunny_book.thresh.txt new file mode 100644 index 0000000..70ca391 --- /dev/null +++ b/data/sample/scene-0000/masks/i_am_a_bunny_book.thresh.txt @@ -0,0 +1 @@ +0.37183 diff --git a/data/sample/scene-0000/masks/kleenex_tissue_box.thresh.txt b/data/sample/scene-0000/masks/kleenex_tissue_box.thresh.txt new file mode 100644 index 0000000..972bd90 --- /dev/null +++ b/data/sample/scene-0000/masks/kleenex_tissue_box.thresh.txt @@ -0,0 +1 @@ +0.34326 diff --git a/data/sample/scene-0000/masks/laugh_out_loud_joke_book.thresh.txt b/data/sample/scene-0000/masks/laugh_out_loud_joke_book.thresh.txt new file mode 100644 index 0000000..830c1c6 --- /dev/null +++ b/data/sample/scene-0000/masks/laugh_out_loud_joke_book.thresh.txt @@ -0,0 +1 @@ +0.35028 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 f56c112..189028e 100644 --- a/ros-packages/catkin_ws/src/pose_estimation/src/getObjectHypothesis.m +++ b/ros-packages/catkin_ws/src/pose_estimation/src/getObjectHypothesis.m @@ -53,7 +53,10 @@ surfaceMean.Z = surfCentroid(3); % Prepare ROS message -objectHypothesis = rosmessage('pose_estimation/ObjectHypothesis'); +try + objectHypothesis = rosmessage('pose_estimation/ObjectHypothesis'); +catch +end objectHypothesis.Label = objName; objectHypothesis.Pose = poseMsg; objectHypothesis.Pca = pcaMsg; 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 4934cf6..bd11d1d 100644 --- a/ros-packages/catkin_ws/src/pose_estimation/src/getObjectPose.m +++ b/ros-packages/catkin_ws/src/pose_estimation/src/getObjectPose.m @@ -37,6 +37,7 @@ global saveResultImageVis; global emptyShelfModels; global emptyToteModel; +global useGPU; % Parameters for both shelf and tote scenario gridStep = 0.002; % grid size for downsampling point clouds @@ -106,7 +107,11 @@ % Do 3D background subtraction % pcwrite(pointCloud(objSegmPts'),fullfile(visPath,sprintf('vis.seg.%s',objName)),'PLYFormat','binary'); % pcwrite(backgroundPointCloud,fullfile(visPath,sprintf('vis.bg.%s',objName)),'PLYFormat','binary'); -[indicesNN,distsNN] = multiQueryKNNSearchImplGPU(backgroundPointCloud,objSegmPts'); +if useGPU + [indicesNN,distsNN] = multiQueryKNNSearchImplGPU(backgroundPointCloud,objSegmPts'); +else + [indicesNN,distsNN] = multiQueryKNNSearchImpl(backgroundPointCloud,objSegmPts',1); +end objSegmPts(:,find(sqrt(distsNN) < 0.005)) = []; if strcmp(sceneData.env,'shelf') objSegmPtsBg = extBin2Bg(1:3,1:3) * objSegmPts + repmat(extBin2Bg(1:3,4) + [0;0;0.01],1,size(objSegmPts,2)); @@ -310,12 +315,20 @@ if size(currObjSegmPts,2) > 3 tmpObjModelCloud = pointCloud(tmpObjModelPts'); objSegCloud = pointCloud(currObjSegmPts'); - [tform,movingReg,icpRmse] = pcregrigidGPU(objSegCloud,tmpObjModelCloud,'InlierRatio',icpWorstRejRatio,'MaxIterations',200,'Tolerance',[0.0001 0.0009],'Verbose',false,'Extrapolate',true); + if useGPU + [tform,movingReg,icpRmse] = pcregrigidGPU(objSegCloud,tmpObjModelCloud,'InlierRatio',icpWorstRejRatio,'MaxIterations',200,'Tolerance',[0.0001 0.0009],'Verbose',false,'Extrapolate',true); + else + [tform,movingReg,icpRmse] = pcregrigid(objSegCloud,tmpObjModelCloud,'InlierRatio',icpWorstRejRatio,'MaxIterations',200,'Tolerance',[0.0001 0.0009],'Verbose',false,'Extrapolate',true); + end icpRt1 = inv(tform.T'); tmpObjModelPts = tmpObjModelCloud.Location'; tmpObjModelPts = icpRt1(1:3,1:3) * tmpObjModelPts + repmat(icpRt1(1:3,4),1,size(tmpObjModelPts,2)); tmpObjModelCloud = pointCloud(tmpObjModelPts'); - [tform,movingReg,icpRmse] = pcregrigidGPU(objSegCloud,tmpObjModelCloud,'InlierRatio',icpWorstRejRatio/2,'MaxIterations',200,'Tolerance',[0.0001 0.0009],'Verbose',false,'Extrapolate',true); + if useGPU + [tform,movingReg,icpRmse] = pcregrigidGPU(objSegCloud,tmpObjModelCloud,'InlierRatio',icpWorstRejRatio/2,'MaxIterations',200,'Tolerance',[0.0001 0.0009],'Verbose',false,'Extrapolate',true); + else + [tform,movingReg,icpRmse] = pcregrigid(objSegCloud,tmpObjModelCloud,'InlierRatio',icpWorstRejRatio/2,'MaxIterations',200,'Tolerance',[0.0001 0.0009],'Verbose',false,'Extrapolate',true); + end icpRt2 = inv(tform.T'); icpRt = icpRt2*icpRt1; else diff --git a/ros-packages/catkin_ws/src/pose_estimation/src/mdemo.m b/ros-packages/catkin_ws/src/pose_estimation/src/mdemo.m new file mode 100644 index 0000000..8227a9a --- /dev/null +++ b/ros-packages/catkin_ws/src/pose_estimation/src/mdemo.m @@ -0,0 +1,188 @@ +% User configurations (change me) +toolboxPath = '/home/andyz/apc/toolbox'; % Directory of toolbox utilities +tmpDataPath = '/home/andyz/apc/toolbox/data/tmp'; % Temporary directory used by marvin_convnet, where all RGB-D images and detection masks are saved +modelsPath = './models/objects'; % Directory holding pre-scanned object models +scenePath = fullfile(toolboxPath,'data/sample/scene-0000'); % Directory holding the RGB-D data of the sample scene +calibPath = fullfile(toolboxPath,'data/sample/calibration'); % Directory holding camera pose calibration data for the sample scene + +% Add paths and create directories +addpath(genpath(fullfile(toolboxPath,'rgbd-utils'))); +addpath(genpath(fullfile(toolboxPath,'vis-utils'))); + +% Get set of colors for visualizations (from vis-utils) +load('colorPalette.mat'); + +% Load sample scene data +fprintf(' [Processing] Loading scene RGB-D data\n'); +sceneData = loadScene(tmpDataPath); +numFrames = length(sceneData.colorFrames); + +% Calibrate scene +sceneData = loadCalib(calibPath,sceneData); + +% Load pre-scanned object models +global objNames; +global objModels; +objNames = {'barkely_hide_bones', ... + 'cherokee_easy_tee_shirt', ... + 'clorox_utility_brush', ... + 'cloud_b_plush_bear', ... + 'cool_shot_glue_sticks', ... + 'command_hooks', ... + 'crayola_24_ct', ... + 'creativity_chenille_stems', ... + 'dasani_water_bottle', ... + 'dove_beauty_bar', ... + 'dr_browns_bottle_brush', ... + 'easter_turtle_sippy_cup', ... + 'elmers_washable_no_run_school_glue', ... + 'expo_dry_erase_board_eraser', ... + 'fiskars_scissors_red', ... + 'fitness_gear_3lb_dumbbell', ... + 'folgers_classic_roast_coffee', ... + 'hanes_tube_socks', ... + 'i_am_a_bunny_book', ... + 'jane_eyre_dvd', ... + 'kleenex_paper_towels', ... + 'kleenex_tissue_box', ... + 'kyjen_squeakin_eggs_plush_puppies', ... + 'laugh_out_loud_joke_book', ... + 'oral_b_toothbrush_green', ... + 'oral_b_toothbrush_red', ... + 'peva_shower_curtain_liner', ... + 'platinum_pets_dog_bowl', ... + 'rawlings_baseball', ... + 'rolodex_jumbo_pencil_cup', ... + 'safety_first_outlet_plugs', ... + 'scotch_bubble_mailer', ... + 'scotch_duct_tape', ... + 'soft_white_lightbulb', ... + 'staples_index_cards', ... + 'ticonderoga_12_pencils', ... + 'up_glucose_bottle', ... + 'woods_extension_cord', ... + 'womens_knit_gloves'}; +objModels = cell(1,length(objNames)); +fprintf(' [Processing] Loading pre-scanned object models\n'); +for objIdx = 1:length(objNames) + try + objModels{objIdx} = pcread(sprintf('models/objects/%s.ply',objNames{objIdx})); + end +end + +% Load pre-scanned empty tote and shelf bins +global emptyShelfModels; +global emptyToteModel; +emptyShelfModels = cell(1,12); +binIds = 'ABCDEFGHIJKL'; +fprintf(' [Processing] Loading pre-scanned empty tote and shelf bins\n'); +for binIdx = 1:length(binIds) + emptyShelfModels{binIdx} = pcread(sprintf('models/bins/bin%s.ply',binIds(binIdx))); +end +emptyToteModel = pcread('models/bins/tote.ply'); + +global visPath; visPath = fullfile(pwd,'visualizations'); +global savePointCloudVis; savePointCloudVis = false; +global saveResultImageVis; saveResultImageVis = false; +global useGPU; useGPU = false; + +% 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 +fprintf(' [Processing] Loading scene point clouds\n'); +scenePointCloud = getScenePointCloud(sceneData); + +% Load and align empty bin/tote point cloud to observation +binIds = 'ABCDEFGHIJKL'; +if strcmp(sceneData.env,'shelf') + backgroundPointCloud = emptyShelfModels{strfind(binIds,sceneData.binId)}; +else + backgroundPointCloud = emptyToteModel; +end +[tform,backgroundPointCloud] = pcregrigid(backgroundPointCloud,scenePointCloud,'InlierRatio',0.8,'MaxIterations',200,'Tolerance',[0.0001,0.0009],'Verbose',false,'Extrapolate',true); +extBin2Bg = inv(tform.T'); + +if savePointCloudVis + pcwrite(scenePointCloud,fullfile(visPath,'vis.pc.scene'),'PLYFormat','binary'); + pcwrite(backgroundPointCloud,fullfile(visPath,'vis.pc.bg'),'PLYFormat','binary'); +end + +% Parse object names +objList = sceneData.objects; +uniqueObjectList = unique(objList); +for objIdx = 1:length(uniqueObjectList) + objName = uniqueObjectList{objIdx}; + objNum = sum(ismember(objList,objName)); + objModel = objModels{find(ismember(objNames,objName))}; + if isempty(objModel) + fprintf('Error: object model not loaded for %s!\n',objName); + end + + % Do 6D pose estimation for each object and save the results + objPoses = getObjectPose(tmpDataPath,sceneData,scenePointCloud,backgroundPointCloud,extBin2Bg,objName,objModel,objNum); +end + +fprintf(' [Visualization] Drawing predicted object poses\n'); +% 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 canvas for visualization +canvas = sceneData.colorFrames{8}; +canvasSeg = uint8(ones(size(canvas))*255); +canvasPose = canvas; +canvasPose = insertText(canvasPose,[10 10],'Confidence : Object','Font','LucidaSansDemiBold','FontSize',12,'TextColor','white','BoxColor','black'); + +% Loop through each object to save and visualize predicted object pose +uniqueObjList = unique(sceneData.objects); +textPosY = 32; +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 + [canvasSeg,canvasPose] = showObjectPose(currResult.objName, canvasSeg, canvasPose, sceneData.colorFrames{8}, sceneData.depthFrames{8}, sceneData.extCam2World{8}, sceneData.colorK, objPointCloud, currResult.objPoseWorld, objColor); + canvasPose = insertText(canvasPose,[10 textPosY],sprintf(' %f : %s',currResult.confScore,currResult.objName),'Font','LucidaSansDemiBold','FontSize',12,'TextColor',objColor,'BoxColor','black'); + textPosY = textPosY + 22; +end + +imshow(canvasPose); title('Predicted 6D Object Poses'); + + + + + + + + + + + + + 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 57f13c6..0bfcf32 100644 --- a/ros-packages/catkin_ws/src/pose_estimation/src/serviceCallback.m +++ b/ros-packages/catkin_ws/src/pose_estimation/src/serviceCallback.m @@ -19,6 +19,7 @@ global objModels global emptyShelfModels; global emptyToteModel; +global useGPU; % Load RGB-D frames for scene scenePath = reqMsg.SceneFiles; @@ -43,7 +44,11 @@ else backgroundPointCloud = emptyToteModel; end -[tform,backgroundPointCloud] = pcregrigidGPU(backgroundPointCloud,scenePointCloud,'InlierRatio',0.8,'MaxIterations',200,'Tolerance',[0.0001,0.0009],'Verbose',false,'Extrapolate',true); +if useGPU + [tform,backgroundPointCloud] = pcregrigidGPU(backgroundPointCloud,scenePointCloud,'InlierRatio',0.8,'MaxIterations',200,'Tolerance',[0.0001,0.0009],'Verbose',false,'Extrapolate',true); +else + [tform,backgroundPointCloud] = pcregrigid(backgroundPointCloud,scenePointCloud,'InlierRatio',0.8,'MaxIterations',200,'Tolerance',[0.0001,0.0009],'Verbose',false,'Extrapolate',true); +end extBin2Bg = inv(tform.T'); if savePointCloudVis diff --git a/ros-packages/catkin_ws/src/pose_estimation/src/startService.m b/ros-packages/catkin_ws/src/pose_estimation/src/startService.m index c8c76a0..aa8de1e 100644 --- a/ros-packages/catkin_ws/src/pose_estimation/src/startService.m +++ b/ros-packages/catkin_ws/src/pose_estimation/src/startService.m @@ -19,6 +19,7 @@ global visPath; visPath = fullfile(pwd,'visualizations'); % Directory to save visualization files global savePointCloudVis; savePointCloudVis = true; % Option to save point cloud visualizations (can influence pose estimation speed) global saveResultImageVis; saveResultImageVis = true;% Option to save pose estimation result visualizations (can influence pose estimation speed) +global useGPU; useGPU = true; % Option to use GPU (CUDA support only) addpath(genpath(rgbdUtilsPath)); addpath(genpath(pwd)); @@ -103,11 +104,13 @@ emptyToteModel = pcread('models/bins/tote.ply'); % Setup CUDA KNNSearch Kernel Function -global KNNSearchGPU; -tic; -fprintf('Setting up CUDA kernel functions...\n'); -KNNSearchGPU = parallel.gpu.CUDAKernel('KNNSearch.ptx','KNNSearch.cu'); -toc; +if useGPU + global KNNSearchGPU; + tic; + fprintf('Setting up CUDA kernel functions...\n'); + KNNSearchGPU = parallel.gpu.CUDAKernel('KNNSearch.ptx','KNNSearch.cu'); + toc; +end % Start ROS service server = rossvcserver('/pose_estimation', 'pose_estimation/EstimateObjectPose', @serviceCallback);