diff --git a/CHANGELOG.md b/CHANGELOG.md new file mode 100644 index 0000000..eaf1bc8 --- /dev/null +++ b/CHANGELOG.md @@ -0,0 +1,15 @@ +# Changelog +All notable changes to this project will be documented in this file. + +## [1.0.0] - 2019-01-24 +### Added +- Support different deep learning frameworks: OpenVINO, CAFFE, custom. +- Support C++11. + +### Changed +- Use smart pointers instead of raw pointers. +- Reorganize code structure. + +### Removed +- Remove CAFFE dependency. +- Remove ROS dependency. diff --git a/CMakeLists.txt b/CMakeLists.txt index 0827783..a0b2f7a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,41 +1,36 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.8 FATAL_ERROR) project(gpd) +set(CMAKE_BUILD_TYPE Debug) +set(CMAKE_CXX_STANDARD 14) -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS cmake_modules eigen_conversions geometry_msgs message_generation roscpp - sensor_msgs std_msgs) - -# Eigen3 -find_package(Eigen3) +# Eigen library +include_directories(${EIGEN3_INCLUDE_DIR}) # PCL -find_package(PCL REQUIRED) +find_package(PCL 1.9 REQUIRED) +#find_package(PCL 1.8 REQUIRED) include_directories(${PCL_INCLUDE_DIRS}) link_directories(${PCL_LIBRARY_DIRS}) add_definitions(${PCL_DEFINITIONS}) # OpenCV -find_package(OpenCV) - -# custom messages -find_package(message_generation) +find_package(OpenCV REQUIRED) +#find_package(OpenCV REQUIRED PATHS /usr/local NO_DEFAULT_PATH) -find_library(GENERATOR_LIB grasp_candidates_generator) -find_path(GENERATOR_LIB_INCLUDE_DIR gpg/grasp.h) -include_directories(${GENERATOR_LIB_INCLUDE_DIR}) +## Set compiler optimization flags +# set(CMAKE_CXX_FLAGS "-O3 -fopenmp -fPIC -Wno-deprecated -Wenum-compare -std=c++14") +set(CMAKE_CXX_FLAGS "-O3 -fopenmp -fPIC -Wno-deprecated -Wenum-compare -Wno-ignored-attributes -std=c++14") +# set(CMAKE_CXX_FLAGS "-O3 -fopenmp -fPIC -Wno-deprecated -Wenum-compare") +# set(CMAKE_CXX_FLAGS "-O3 -fopenmp -march=native -mfpmath=sse -funroll-loops -fPIC -Wno-deprecated -Wenum-compare") # no improvement +# set(CMAKE_CXX_FLAGS "-frename-registers -Ofast -march=native -fopenmp -fPIC -Wno-deprecated -Wenum-compare") # no improvement -# Caffe -option(USE_CAFFE "use Caffe framework" ON) -if(USE_CAFFE STREQUAL "ON") - find_package(Caffe) - include_directories(${Caffe_INCLUDE_DIRS}) - add_definitions(${Caffe_DEFINITIONS}) -endif() +## Specify additional locations of header files +include_directories(include ${PCL_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS}) -# OpenVINO -option(USE_OPENVINO "use OpenVINO toolkit" OFF) +# Classifier depends on what libraries are installed. +option(USE_OPENVINO "use classifier based on OpenVINO toolkit" OFF) +option(USE_CAFFE "use classifier based on Caffe framework" OFF) +option(USE_OPENCV "use classifier based on OpenCV framework" OFF) if(USE_OPENVINO STREQUAL "ON") find_package(InferenceEngine 1.0) if (NOT InferenceEngine_FOUND) @@ -45,245 +40,257 @@ if(USE_OPENVINO STREQUAL "ON") link_directories(${InferenceEngine_LIBRARY_DIRS}) add_definitions(-DUSE_OPENVINO) get_filename_component(MODELS_DIR "models/openvino" ABSOLUTE) - configure_file(include/gpd/openvino_classifier.h.in gpd/openvino_classifier.h) + configure_file(include/gpd/net/openvino_classifier.h.in gpd/net/openvino_classifier.h) include_directories(${CMAKE_CURRENT_BINARY_DIR}) - set(classifier_src src/${PROJECT_NAME}/classifier.cpp src/${PROJECT_NAME}/openvino_classifier.cpp) + set(classifier_src src/${PROJECT_NAME}/net/classifier.cpp src/${PROJECT_NAME}/net/openvino_classifier.cpp) set(classifier_dep ${InferenceEngine_LIBRARIES}) -else() - set(classifier_src src/${PROJECT_NAME}/classifier.cpp src/${PROJECT_NAME}/caffe_classifier.cpp) + message("Using OpenVINO") +elseif(USE_CAFFE STREQUAL "ON") + find_package(Caffe) + if (NOT Caffe_FOUND) + message(FATAL_ERROR "Please install Caffe https://caffe.berkeleyvision.org/installation.html") + endif() + include_directories(${Caffe_INCLUDE_DIRS}) + add_definitions(${Caffe_DEFINITIONS}) + add_definitions(-DUSE_CAFFE) + set(classifier_src src/${PROJECT_NAME}/net/classifier.cpp src/${PROJECT_NAME}/net/caffe_classifier.cpp) set(classifier_dep ${Caffe_LIBRARIES} ${OpenCV_LIBRARIES}) +elseif(USE_OPENCV STREQUAL "ON") + message(FATAL_ERROR "Not supported yet") +else() + add_library(${PROJECT_NAME}_conv_layer src/${PROJECT_NAME}/net/conv_layer.cpp) + add_library(${PROJECT_NAME}_dense_layer src/${PROJECT_NAME}/net/dense_layer.cpp) + set(classifier_src src/${PROJECT_NAME}/net/classifier.cpp src/${PROJECT_NAME}/net/eigen_classifier.cpp) + set(classifier_dep ${PROJECT_NAME}_conv_layer ${PROJECT_NAME}_dense_layer ${OpenCV_LIBRARIES}) endif() -## Set compiler optimization flags -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DNDEBUG -O3 -fopenmp -flto -Wno-deprecated -Wenum-compare -std=c++11") -# set(CMAKE_CXX_FLAGS "-DNDEBUG -O3 -fopenmp -flto -mavx -mfma -Wno-deprecated -Wenum-compare") - -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() - -################################################ -## Declare ROS messages, services and actions ## -################################################ - -## To declare and build messages, services or actions from within this -## package, follow these steps: -## * Let MSG_DEP_SET be the set of packages whose message types you use in -## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). -## * In the file package.xml: -## * add a build_depend tag for "message_generation" -## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET -## * If MSG_DEP_SET isn't empty the following dependency has been pulled in -## but can be declared for certainty nonetheless: -## * add a run_depend tag for "message_runtime" -## * In this file (CMakeLists.txt): -## * add "message_generation" and every package in MSG_DEP_SET to -## find_package(catkin REQUIRED COMPONENTS ...) -## * add "message_runtime" and every package in MSG_DEP_SET to -## catkin_package(CATKIN_DEPENDS ...) -## * uncomment the add_*_files sections below as needed -## and list every .msg/.srv/.action file to be processed -## * uncomment the generate_messages entry below -## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) - -## Generate messages in the 'msg' folder -add_message_files(FILES CloudIndexed.msg CloudSamples.msg CloudSources.msg GraspConfig.msg GraspConfigList.msg - SamplesMsg.msg) - -## Generate services in the 'srv' folder -add_service_files(DIRECTORY srv - FILES - SetParameters.srv -) - -## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) - -## Generate added messages and services with any dependencies listed here -generate_messages(DEPENDENCIES geometry_msgs sensor_msgs std_msgs) - -################################################ -## Declare ROS dynamic reconfigure parameters ## -################################################ - -## To declare and build dynamic reconfigure parameters within this -## package, follow these steps: -## * In the file package.xml: -## * add a build_depend and a run_depend tag for "dynamic_reconfigure" -## * In this file (CMakeLists.txt): -## * add "dynamic_reconfigure" to -## find_package(catkin REQUIRED COMPONENTS ...) -## * uncomment the "generate_dynamic_reconfigure_options" section below -## and list every .cfg file to be processed - -## Generate dynamic reconfigure parameters in the 'cfg' folder -# generate_dynamic_reconfigure_options( -# cfg/DynReconf1.cfg -# cfg/DynReconf2.cfg -# ) - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if you package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( -INCLUDE_DIRS include -CATKIN_DEPENDS cmake_modules eigen_conversions geometry_msgs message_runtime roscpp sensor_msgs std_msgs -DEPENDS EIGEN3 OpenCV PCL -) - -########### -## Build ## -########### +# Optional PCL GPU operations +option(USE_PCL_GPU "use PCL GPU for point cloud operations" OFF) +if(USE_PCL_GPU STREQUAL "ON") + FIND_PACKAGE(CUDA) + if (NOT CUDA_FOUND) + message(FATAL_ERROR "Please install CUDA and compile PCL with Nvidia GPU flags http://pointclouds.org/documentation/tutorials/gpu_install.php") + endif() + INCLUDE(FindCUDA) + add_definitions(-DUSE_PCL_GPU) + message("Using the GPU with PCL") +endif() -## Specify additional locations of header files -## Your package locations should be listed before other locations -include_directories(include ${catkin_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS}) +# Optional data generation (requires OpenCV Contrib) +option(BUILD_DATA_GENERATION "build data generation (requires OpenCV Contrib for HDF5)" OFF) +if(BUILD_DATA_GENERATION STREQUAL "ON") + add_library(${PROJECT_NAME}_data_generator src/${PROJECT_NAME}/data_generator.cpp) + target_link_libraries(${PROJECT_NAME}_data_generator + ${PROJECT_NAME}_grasp_detector + ${PCL_LIBRARIES} + ${OpenCV_LIBS}) + add_executable(${PROJECT_NAME}_generate_data src/generate_data.cpp) + target_link_libraries(${PROJECT_NAME}_generate_data + ${PROJECT_NAME}_data_generator) + message("Building data generation module") +endif() + +# Generate the shared library from the sources +add_library(${PROJECT_NAME}_grasp_detector SHARED src/${PROJECT_NAME}/grasp_detector.cpp) -## Declare a C++ library add_library(${PROJECT_NAME}_classifier ${classifier_src}) +target_link_libraries(${PROJECT_NAME}_classifier + ${classifier_dep}) + add_library(${PROJECT_NAME}_clustering src/${PROJECT_NAME}/clustering.cpp) -if(USE_CAFFE STREQUAL "ON") - add_library(${PROJECT_NAME}_data_generator src/${PROJECT_NAME}/data_generator.cpp) -endif() -add_library(${PROJECT_NAME}_grasp_detector src/${PROJECT_NAME}/grasp_detector.cpp) -add_library(${PROJECT_NAME}_learning src/${PROJECT_NAME}/learning.cpp) add_library(${PROJECT_NAME}_sequential_importance_sampling src/${PROJECT_NAME}/sequential_importance_sampling.cpp) -add_library(${PROJECT_NAME}_grasp_image src/${PROJECT_NAME}/grasp_image.cpp) -add_library(${PROJECT_NAME}_grasp_image_15_channels src/${PROJECT_NAME}/grasp_image_15_channels.cpp) -## Add cmake target dependencies of the library -## as an example, code may need to be generated before libraries -## either from message generation or dynamic reconfigure -# add_dependencies(grasp_candidates_classifier ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +# namespace candidate +add_library(${PROJECT_NAME}_antipodal src/${PROJECT_NAME}/candidate/antipodal.cpp) +add_library(${PROJECT_NAME}_candidates_generator src/${PROJECT_NAME}/candidate/candidates_generator.cpp) +add_library(${PROJECT_NAME}_finger_hand src/${PROJECT_NAME}/candidate/finger_hand.cpp) +add_library(${PROJECT_NAME}_frame_estimator src/${PROJECT_NAME}/candidate/frame_estimator.cpp) +add_library(${PROJECT_NAME}_hand src/${PROJECT_NAME}/candidate/hand.cpp) +add_library(${PROJECT_NAME}_hand_set src/${PROJECT_NAME}/candidate/hand_set.cpp) +add_library(${PROJECT_NAME}_hand_geometry src/${PROJECT_NAME}/candidate/hand_geometry.cpp) +add_library(${PROJECT_NAME}_hand_search src/${PROJECT_NAME}/candidate/hand_search.cpp) +add_library(${PROJECT_NAME}_local_frame src/${PROJECT_NAME}/candidate/local_frame.cpp) + +# namespace util +add_library(${PROJECT_NAME}_cloud src/${PROJECT_NAME}/util/cloud.cpp) +add_library(${PROJECT_NAME}_config_file src/${PROJECT_NAME}/util/config_file.cpp) +add_library(${PROJECT_NAME}_eigen_utils src/${PROJECT_NAME}/util/eigen_utils.cpp) +add_library(${PROJECT_NAME}_plot src/${PROJECT_NAME}/util/plot.cpp) +add_library(${PROJECT_NAME}_point_list src/${PROJECT_NAME}/util/point_list.cpp) + +# namespace descriptor +add_library(${PROJECT_NAME}_image_strategy src/${PROJECT_NAME}/descriptor/image_strategy.cpp) +add_library(${PROJECT_NAME}_image_1_channels_strategy src/${PROJECT_NAME}/descriptor/image_1_channels_strategy.cpp) +add_library(${PROJECT_NAME}_image_3_channels_strategy src/${PROJECT_NAME}/descriptor/image_3_channels_strategy.cpp) +add_library(${PROJECT_NAME}_image_12_channels_strategy src/${PROJECT_NAME}/descriptor/image_12_channels_strategy.cpp) +add_library(${PROJECT_NAME}_image_15_channels_strategy src/${PROJECT_NAME}/descriptor/image_15_channels_strategy.cpp) +add_library(${PROJECT_NAME}_image_geometry src/${PROJECT_NAME}/descriptor/image_geometry.cpp) + +add_library(${PROJECT_NAME}_image_generator src/${PROJECT_NAME}/descriptor/image_generator.cpp) ## Declare C++ executables -add_executable(${PROJECT_NAME}_detect_grasps src/nodes/grasp_detection_node.cpp) -add_executable(${PROJECT_NAME}_generate_candidates src/nodes/generate_candidates.cpp) -add_executable(${PROJECT_NAME}_create_grasp_images src/nodes/create_grasp_images.cpp) -if(USE_CAFFE STREQUAL "ON") - add_executable(${PROJECT_NAME}_create_training_data src/nodes/create_training_data.cpp) -endif() -add_executable(${PROJECT_NAME}_classify_candidates src/nodes/classify_candidates.cpp) -if(USE_CAFFE STREQUAL "ON") - add_executable(${PROJECT_NAME}_test_occlusion src/tests/test_occlusion.cpp) -endif() +add_executable(${PROJECT_NAME}_cem_detect_grasps src/cem_detect_grasps.cpp) +add_executable(${PROJECT_NAME}_detect_grasps src/detect_grasps.cpp) +add_executable(${PROJECT_NAME}_generate_candidates src/generate_candidates.cpp) +add_executable(${PROJECT_NAME}_label_grasps src/label_grasps.cpp) +add_executable(${PROJECT_NAME}_test_grasp_image src/tests/test_grasp_image.cpp) +# add_executable(${PROJECT_NAME}_test_conv_layer src/tests/test_conv_layer.cpp) +# add_executable(${PROJECT_NAME}_test_hdf5 src/tests/test_hdf5.cpp) -## Add cmake target dependencies of the executable -## same as for the library above -# add_dependencies(grasp_candidates_classifier_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) ## Specify libraries to link a library or executable target against -target_link_libraries(${PROJECT_NAME}_classifier - ${classifier_dep}) - target_link_libraries(${PROJECT_NAME}_clustering - ${GENERATOR_LIB}) - -target_link_libraries(${PROJECT_NAME}_create_grasp_images - ${PROJECT_NAME}_learning - ${GENERATOR_LIB} - ${catkin_LIBRARIES} - ${PCL_LIBRARIES}) +${PROJECT_NAME}_hand) -if(USE_CAFFE STREQUAL "ON") - target_link_libraries(${PROJECT_NAME}_create_training_data - ${PROJECT_NAME}_data_generator) -endif() - -if(USE_CAFFE STREQUAL "ON") - target_link_libraries(${PROJECT_NAME}_data_generator - ${PROJECT_NAME}_learning - ${GENERATOR_LIB} - ${Caffe_LIBRARIES} - ${catkin_LIBRARIES} - ${PCL_LIBRARIES}) -endif() +target_link_libraries(${PROJECT_NAME}_grasp_detector + ${PROJECT_NAME}_clustering + ${PROJECT_NAME}_image_generator + ${PROJECT_NAME}_classifier + ${PROJECT_NAME}_candidates_generator + ${PROJECT_NAME}_hand_geometry + ${PROJECT_NAME}_hand_set + ${PROJECT_NAME}_config_file + ${PROJECT_NAME}_plot) target_link_libraries(${PROJECT_NAME}_generate_candidates - ${GENERATOR_LIB} - ${catkin_LIBRARIES} - ${PCL_LIBRARIES}) + ${PROJECT_NAME}_config_file +${PROJECT_NAME}_candidates_generator) + +# Linking for libraries +target_link_libraries(${PROJECT_NAME}_antipodal +${PROJECT_NAME}_point_list) + +target_link_libraries(${PROJECT_NAME}_cloud + ${PROJECT_NAME}_eigen_utils + ${PCL_LIBRARIES}) + +target_link_libraries(${PROJECT_NAME}_eigen_utils +${EIGEN_LIBRARIES}) + +target_link_libraries(${PROJECT_NAME}_frame_estimator + ${PROJECT_NAME}_cloud +${PROJECT_NAME}_local_frame) + +target_link_libraries(${PROJECT_NAME}_hand +${PROJECT_NAME}_finger_hand) + +target_link_libraries(${PROJECT_NAME}_hand_set + ${PROJECT_NAME}_antipodal + ${PROJECT_NAME}_config_file + ${PROJECT_NAME}_hand + ${PROJECT_NAME}_hand_geometry + ${PROJECT_NAME}_local_frame +${PROJECT_NAME}_point_list) + +target_link_libraries(${PROJECT_NAME}_hand_geometry +${PROJECT_NAME}_config_file) + +target_link_libraries(${PROJECT_NAME}_hand_search + ${PROJECT_NAME}_antipodal + ${PROJECT_NAME}_cloud + ${PROJECT_NAME}_frame_estimator + ${PROJECT_NAME}_hand_set + ${PROJECT_NAME}_hand_geometry + ${PROJECT_NAME}_plot) + +target_link_libraries(${PROJECT_NAME}_plot + ${PROJECT_NAME}_cloud + ${PROJECT_NAME}_hand_set + ${PROJECT_NAME}_image_geometry +${PROJECT_NAME}_local_frame) + +target_link_libraries(${PROJECT_NAME}_candidates_generator + ${PROJECT_NAME}_config_file + ${PROJECT_NAME}_hand_geometry +${PROJECT_NAME}_hand_search) + +target_link_libraries(${PROJECT_NAME}_point_list +${PROJECT_NAME}_eigen_utils) + +target_link_libraries(${PROJECT_NAME}_image_strategy + ${PROJECT_NAME}_image_geometry + ${PROJECT_NAME}_image_1_channels_strategy + ${PROJECT_NAME}_image_3_channels_strategy + ${PROJECT_NAME}_image_12_channels_strategy + ${PROJECT_NAME}_image_15_channels_strategy + ${PROJECT_NAME}_hand_set +${OpenCV_LIBS}) + + +target_link_libraries(${PROJECT_NAME}_image_1_channels_strategy +${PROJECT_NAME}_image_strategy) + +target_link_libraries(${PROJECT_NAME}_image_3_channels_strategy +${PROJECT_NAME}_image_strategy) + +target_link_libraries(${PROJECT_NAME}_image_12_channels_strategy +${PROJECT_NAME}_image_strategy) + +target_link_libraries(${PROJECT_NAME}_image_15_channels_strategy +${PROJECT_NAME}_image_strategy) + +target_link_libraries(${PROJECT_NAME}_image_geometry +${PROJECT_NAME}_config_file) + +target_link_libraries(${PROJECT_NAME}_image_generator + ${PROJECT_NAME}_image_strategy + ${PROJECT_NAME}_hand_set + ${PROJECT_NAME}_image_strategy + ${PROJECT_NAME}_cloud +${PROJECT_NAME}_eigen_utils) -target_link_libraries(${PROJECT_NAME}_classify_candidates - ${PROJECT_NAME}_grasp_detector - ${PROJECT_NAME}_sequential_importance_sampling - ${GENERATOR_LIB} - ${catkin_LIBRARIES} - ${PCL_LIBRARIES}) +target_link_libraries(${PROJECT_NAME}_sequential_importance_sampling + ${PROJECT_NAME}_grasp_detector) -target_link_libraries(${PROJECT_NAME}_grasp_detector - ${PROJECT_NAME}_classifier - ${PROJECT_NAME}_clustering - ${PROJECT_NAME}_learning - ${GENERATOR_LIB} - ${catkin_LIBRARIES} - ${classifier_dep}) +target_link_libraries(${PROJECT_NAME}_test_grasp_image + ${PROJECT_NAME}_image_generator + ${PROJECT_NAME}_classifier + ${PROJECT_NAME}_plot + ${PROJECT_NAME}_candidates_generator +${PCL_LIBRARIES}) -target_link_libraries(${PROJECT_NAME}_grasp_image - ${OpenCV_LIBRARIES}) +target_link_libraries(${PROJECT_NAME}_detect_grasps + ${PROJECT_NAME}_grasp_detector + ${PROJECT_NAME}_config_file +${PCL_LIBRARIES}) -target_link_libraries(${PROJECT_NAME}_grasp_image_15_channels - ${PROJECT_NAME}_grasp_image) +target_link_libraries(${PROJECT_NAME}_label_grasps + ${PROJECT_NAME}_grasp_detector + ${PROJECT_NAME}_config_file +${PCL_LIBRARIES}) -target_link_libraries(${PROJECT_NAME}_learning - ${PROJECT_NAME}_grasp_image_15_channels - ${GENERATOR_LIB}) +target_link_libraries(${PROJECT_NAME}_cem_detect_grasps + ${PROJECT_NAME}_sequential_importance_sampling + ${PROJECT_NAME}_config_file + ${PCL_LIBRARIES}) -target_link_libraries(${PROJECT_NAME}_detect_grasps - ${PROJECT_NAME}_grasp_detector - ${PROJECT_NAME}_sequential_importance_sampling - ${GENERATOR_LIB} - ${PCL_LIBRARIES}) +target_link_libraries(${PROJECT_NAME}_image_generator + ${PROJECT_NAME}_hand_set + ${PROJECT_NAME}_image_strategy) -target_link_libraries(${PROJECT_NAME}_sequential_importance_sampling - ${PROJECT_NAME}_grasp_detector) - -if(USE_CAFFE STREQUAL "ON") - target_link_libraries(${PROJECT_NAME}_test_occlusion - ${PROJECT_NAME}_learning - ${GENERATOR_LIB} - ${catkin_LIBRARIES} - ${PCL_LIBRARIES} - ${Caffe_LIBRARIES}) -endif() +# Rename targets to simplify their names. +set_target_properties(${PROJECT_NAME}_test_grasp_image + PROPERTIES OUTPUT_NAME test_grasp_image PREFIX "") -target_link_libraries(${PROJECT_NAME}_learning - ${GENERATOR_LIB} - ${catkin_LIBRARIES} - ${Caffe_LIBRARIES}) +set_target_properties(${PROJECT_NAME}_cem_detect_grasps + PROPERTIES OUTPUT_NAME cem_detect_grasps PREFIX "") -# Rename targets to simplify their names. set_target_properties(${PROJECT_NAME}_detect_grasps - PROPERTIES OUTPUT_NAME detect_grasps - PREFIX "") + PROPERTIES OUTPUT_NAME detect_grasps PREFIX "") -set_target_properties(${PROJECT_NAME}_classify_candidates - PROPERTIES OUTPUT_NAME classify_candidates - PREFIX "") +if(BUILD_DATA_GENERATION STREQUAL "ON") + set_target_properties(${PROJECT_NAME}_generate_data + PROPERTIES OUTPUT_NAME generate_data PREFIX "") +endif() -set_target_properties(${PROJECT_NAME}_create_grasp_images - PROPERTIES OUTPUT_NAME create_grasp_images - PREFIX "") +set_target_properties(${PROJECT_NAME}_label_grasps + PROPERTIES OUTPUT_NAME label_grasps PREFIX "") -if(USE_CAFFE STREQUAL "ON") - set_target_properties(${PROJECT_NAME}_create_training_data - PROPERTIES OUTPUT_NAME create_training_data - PREFIX "") -endif() +set_target_properties(${PROJECT_NAME}_grasp_detector + PROPERTIES OUTPUT_NAME gpd) -if(USE_CAFFE STREQUAL "ON") - set_target_properties(${PROJECT_NAME}_test_occlusion - PROPERTIES OUTPUT_NAME test_occlusion - PREFIX "") -endif() + +# Set the location for library installation +install(TARGETS ${PROJECT_NAME}_grasp_detector DESTINATION lib) + +install(DIRECTORY include/gpd DESTINATION include) diff --git a/LICENSE.md b/LICENSE.md index a498186..962ffc5 100644 --- a/LICENSE.md +++ b/LICENSE.md @@ -1,6 +1,6 @@ BSD 2-Clause License -Copyright (c) 2017, Andreas ten Pas +Copyright (c) 2018, Andreas ten Pas All rights reserved. Redistribution and use in source and binary forms, with or without diff --git a/README.md b/README.md index b84e804..85a433c 100644 --- a/README.md +++ b/README.md @@ -1,172 +1,264 @@ # Grasp Pose Detection (GPD) -* **Author:** Andreas ten Pas (atp@ccs.neu.edu) -* **Version:** 1.0.0 -* **Author's website:** [http://www.ccs.neu.edu/home/atp/](http://www.ccs.neu.edu/home/atp/) -* **License:** BSD -* **Branch without Caffe dependency**: [forward](https://github.com/atenpas/gpd/tree/forward) -* **Repository for C++11:** [gpd2](https://github.com/atenpas/gpd2) +* [Author's website](http://www.ccs.neu.edu/home/atp/) +* [License](https://github.com/atenpas/gpd2/blob/master/LICENSE.md) +* [ROS wrapper](https://github.com/atenpas/gpd2_ros/) +Grasp Pose Detection (GPD) is a package to detect 6-DOF grasp poses (3-DOF +position and 3-DOF orientation) for a 2-finger robot hand (e.g., a parallel +jaw gripper) in 3D point clouds. GPD takes a point cloud as input and produces +pose estimates of viable grasps as output. The main strengths of GPD are: +- works for novel objects (no CAD models required for detection), +- works in dense clutter, and +- outputs 6-DOF grasp poses (enabling more than just top-down grasps). -## 1) Overview - -This package detects 6-DOF grasp poses for a 2-finger grasp (e.g. a parallel jaw gripper) in 3D point clouds. - - - -Grasp pose detection consists of three steps: sampling a large number of grasp candidates, classifying these candidates -as viable grasps or not, and clustering viable grasps which are geometrically similar. - -The reference for this package is: [High precision grasp pose detection in dense clutter](http://arxiv.org/abs/1603.01564). - - -### UR5 Video +" target="_blank"> +GPD consists of two main steps: sampling a large number of grasp candidates, and classifying these candidates as viable grasps or not. -## 2) Requirements +##### Example Input and Output + -1. [PCL 1.7 or later](http://pointclouds.org/) -2. [Eigen 3.0 or later](https://eigen.tuxfamily.org) -3. [Caffe](http://caffe.berkeleyvision.org/) -4. ROS Indigo and Ubuntu -14.04 *or* ROS Kinetic -and Ubuntu 16.04 +The reference for this package is: +[Grasp Pose Detection in Point Clouds](http://arxiv.org/abs/1706.09911). +## Table of Contents +1. [Requirements](#requirements) +1. [Installation](#install) +1. [Generate Grasps for a Point Cloud File](#pcd) +1. [Parameters](#parameters) +1. [Views](#views) +1. [Input Channels for Neural Network](#cnn_channels) +1. [CNN Frameworks](#cnn_frameworks) +1. [GPU Support With PCL](#pcl_gpu) +1. [Network Training](#net_train) +1. [Grasp Image](#descriptor) +1. [References](#References) -## 3) Prerequisites + +## 1) Requirements -The following instructions work for **Ubuntu 14.04** or **Ubuntu 16.04**. Similar instructions should work for other -Linux distributions that support ROS. +1. [PCL 1.9 or newer](http://pointclouds.org/) +2. [Eigen 3.0 or newer](https://eigen.tuxfamily.org) +3. [OpenCV 3.4 or newer](https://opencv.org) - 1. Install Caffe [(Instructions)](http://caffe.berkeleyvision.org/installation.html). Follow the -[CMake Build instructions](http://caffe.berkeleyvision.org/installation.html#cmake-build). **Notice for Ubuntu 14.04:** -Due to a conflict between the Boost version required by Caffe (1.55) and the one installed as a dependency with the -Debian package for ROS Indigo (1.54), you need to checkout an older version of Caffe that worked with Boost 1.54. So, -when you clone Caffe, please use this command. - - ``` - git clone https://github.com/BVLC/caffe.git && cd caffe - git checkout 923e7e8b6337f610115ae28859408bc392d13136 - ``` + +## 2) Installation + +The following instructions have been tested on **Ubuntu 16.04**. Similar +instructions should work for other Linux distributions. -2. Install ROS. In Ubuntu 14.04, install ROS Indigo [(Instructions)](http://wiki.ros.org/indigo/Installation/Ubuntu). -In Ubuntu 16.04, install ROS Kinetic [(Instructions)](http://wiki.ros.org/kinetic/Installation/Ubuntu). +1. Install [PCL](http://pointclouds.org/) and +[Eigen](https://eigen.tuxfamily.org). If you have ROS Indigo or Kinetic +installed, you should be good to go. +2. Install OpenCV 3.4 ([tutorial](https://www.python36.com/how-to-install-opencv340-on-ubuntu1604/)). -3. Clone the [grasp_pose_generator](https://github.com/atenpas/gpg) repository into some folder: +3. Clone the repository into some folder: ``` - cd - git clone https://github.com/atenpas/gpg.git + git clone https://github.com/atenpas/gpd2 ``` -4. Build and install the *grasp_pose_generator*: +4. Build the package: ``` - cd gpg + cd gpd2 mkdir build && cd build cmake .. - make - sudo make install + make -j ``` +You can optionally install GPD with `sudo make install` so that it can be used by other projects as a shared library. + + +## 3) Generate Grasps for a Point Cloud File -## 4) Compiling GPD +Run GPD on an point cloud file (PCD or PLY): -1. Clone this repository. - ``` - cd - git clone https://github.com/atenpas/gpd.git + ./detect_grasps ../cfg/eigen_params.cfg ../tutorials/krylon.pcd ``` -2. Build your catkin workspace. +The output should look similar to the screenshot shown below. The window is the PCL viewer. You can press [q] to close the window and [h] to see a list of other commands. + + + +Below is a visualization of the convention that GPD uses for the grasp pose (position and orientation) of a grasp. The grasp position is indicated by the orange cross and the orientation by the colored arrows. + + + + +## 4) Parameters + +Brief explanations of parameters are given in *cfg/eigen_params.cfg*. + +The two parameters that you typically want to play with to improve on the +number of grasps found are *workspace* and *num_samples*. The first defines the +volume of space in which to search for grasps as a cuboid of dimensions [minX, +maxX, minY, maxY, minZ, maxZ], centered at the origin of the point cloud frame. +The second is the number of samples that are drawn from the point cloud to +detect grasps. You should set the workspace as small as possible and the number +of samples as large as possible. + + +## 5) Views + +![rviz screenshot](readme/views.png "Single View and Two Views") + +You can use this package with a single or with two depth sensors. The package +comes with CAFFE model files for both. You can find these files in +*models/caffe/15channels*. For a single sensor, use +*single_view_15_channels.caffemodel* and for two depth sensors, use +*two_views_15_channels_[angle]*. The *[angle]* is the angle between the two +sensor views, as illustrated in the picture below. In the two-views setting, you +want to register the two point clouds together before sending them to GPD. + +Providing the camera position to the configuration file (*.cfg) is important, +as it enables PCL to estimate the correct normals direction (which is to point +toward the camera). Alternatively, using the +[ROS wrapper](https://github.com/atenpas/gpd2_ros/), multiple camera positions +can be provided. + +![rviz screenshot](readme/view_angle.png "Angle Between Sensor Views") + +To switch between one and two sensor views, change the parameter `weight_file` +in your config file. + + +## 6) Input Channels for Neural Network + +The package comes with weight files for two different input representations for +the neural network that is used to decide if a grasp is viable or not: 3 or 15 +channels. The default is 15 channels. However, you can use the 3 channels to +achieve better runtime for a loss in grasp quality. For more details, please see +the references below. + + +## 7) CNN Frameworks + +GPD comes with a number of different classifier frameworks that +exploit different hardware and have different dependencies. Switching +between the frameworks requires to run CMake with additional arguments. +For example, to use the OpenVino framework: ``` - cd - catkin_make + cmake .. -DUSE_OPENVINO=ON ``` +You can use `ccmake` to check out all possible CMake options. + +GPD supports the following three frameworks: + +1. [OpenVino](https://software.intel.com/en-us/openvino-toolkit): [installation instructions](https://github.com/opencv/dldt/blob/2018/inference-engine/README.md) for open source version +(CPUs, GPUs, FPGAs from Intel) +1. [Caffe](https://caffe.berkeleyvision.org/) (GPUs from Nvidia or CPUs) +1. Custom LeNet implementation using the Eigen library (CPU) -## 5) Generate Grasps for a Point Cloud File +Additional classifiers can be added by sub-classing the `classifier` interface. + +##### OpenVino + +To use OpenVino, you need to run the following command before compiling GPD. -Launch the grasp pose detection on an example point cloud: - ``` - roslaunch gpd tutorial0.launch + export InferenceEngine_DIR=/path/to/dldt/inference-engine/build/ ``` -Within the GUI that appears, press r to center the view, and q to quit the GUI and load the next visualization. -The output should look similar to the screenshot shown below. -![rviz screenshot](readme/file.png "Grasps visualized in PCL") + +## 8) GPU Support With PCL +GPD can use GPU methods provided within PCL to speed up point cloud processing. -## 6) Tutorials +1. [PCL GPU Install](http://pointclouds.org/documentation/tutorials/gpu_install.php) +1. Build GPD with `USE_PCL_GPU` cmake flag: + ``` + cd gpd + mkdir build && cd build + cmake .. -DUSE_PCL_GPU=ON + make -j + ``` -1. [Detect Grasps With an RGBD camera](tutorials/tutorial_1_grasps_camera.md) -2. [Detect Grasps on a Specific Object](tutorials/tutorial_2_grasp_select.md) -3. [Detect Grasps with OpenVINO](tutorials/tutorial_openvino.md) +**Note:** unfortunately, the surface normals produced by the PCL GPU module +can often be incorrect. The correctness of these normals depends on the number +of neighbors (a parameter). + +## 9) Network Training -## 7) Parameters +To create training data with the C++ code, you need to install [OpenCV 3.4 Contribs](https://www.python36.com/how-to-install-opencv340-on-ubuntu1604/). +Next, you need to compile GPD with the flag `DBUILD_DATA_GENERATION` like this: -Brief explanations of parameters are given in *launch/classify_candidates_file_15_channels.launch* for using PCD files. -For use on a robot, see *launch/ur5_15_channels.launch*. The two parameters that you typically want to play with to -improve on then number of grasps found are *workspace* and *num_samples*. The first defines the volume of space in which -to search for grasps as a cuboid of dimensions [minX, maxX, minY, maxY, minZ, maxZ], centered at the origin. The second -is the number of samples that are drawn from the point cloud to detect grasps. You should set the workspace as small as -possible and the number of samples as large as possible. + ``` + cd gpd + mkdir build && cd build + cmake .. -DBUILD_DATA_GENERATION=ON + make -j + ``` +There are four steps to train a network to predict grasp poses. First, we need to create grasp images. -## 8) Views + ``` + ./generate_data ../cfg/generate_data.cfg + ``` -![rviz screenshot](readme/views.png "Single View and Two Views") +You should modify `generate_data.cfg` according to your needs. -You can use this package with a single or with two depth sensors. The package comes with weight files for Caffe -for both options. You can find these files in *gpd/caffe/15channels*. For a single sensor, use -*single_view_15_channels.caffemodel* and for two depth sensors, use *two_views_15_channels_[angle]*. The *[angle]* is -the angle between the two sensor views, as illustrated in the picture below. In the two-views setting, you want to -register the two point clouds together before sending them to GPD. +Next, you need to resize the created databases to `train_offset` and `test_offset` (see the terminal output of `generate_data`). For example, to resize the training set, use the following commands with `size` set to the value of `train_offset`. + ``` + cd pytorch + python reshape_hdf5.py pathToTrainingSet.h5 out.h5 size + ``` -![rviz screenshot](readme/view_angle.png "Angle Between Sensor Views") +The third step is to train a neural network. The easiest way to training the network is with the existing code. This requires the **pytorch** framework. To train a network, use the following commands. + + ``` + cd pytorch + python train_net3.py pathToTrainingSet.h5 pathToTestSet.h5 num_channels + ``` -To switch between one and two sensor views, change the parameter *trained_file* in the launch file -*launch/caffe/ur5_15channels.launch*. +The fourth step is to convert the model to the ONNX format. + ``` + python torch_to_onxx.py pathToPytorchModel.pwf pathToONNXModel.onnx num_channels + ``` -## 9) Input Channels for Neural Network +The last step is to convert the ONNX file to an OpenVINO compatible format: [tutorial](https://software.intel.com/en-us/articles/OpenVINO-Using-ONNX#inpage-nav-4). This gives two files that can be loaded with GPD by modifying the `weight_file` and `model_file` parameters in a CFG file. -The package comes with weight files for two different input representations for the neural network that is used to -decide if a grasp is viable or not: 3 or 15 channels. The default is 15 channels. However, you can use the 3 channels -to achieve better runtime for a loss in grasp quality. For more details, please see the reference below. + +## 10) Grasp Image/Descriptor +Generate some grasp poses and their corresponding images/descriptors: + ``` + ./test_grasp_image ../tutorials/krylon.pcd 3456 1 ../models/lenet/15channels/params/ + ``` -## 10) Citation + -If you like this package and use it in your own work, please cite our paper(s): +For details on how the grasp image is created, check out our [journal paper](http://arxiv.org/abs/1706.09911). -[1] Andreas ten Pas, Marcus Gualtieri, Kate Saenko, and Robert Platt. [**Grasp Pose Detection in Point -Clouds**](http://arxiv.org/abs/1706.09911). The International Journal of Robotics Research, Vol 36, Issue 13-14, -pp. 1455 - 1473. October 2017. + +## 11) References -[2] Marcus Gualtieri, Andreas ten Pas, Kate Saenko, and Robert Platt. [**High precision grasp pose detection in dense -clutter**](http://arxiv.org/abs/1603.01564). IROS 2016. 598-605. +If you like this package and use it in your own work, please cite our journal +paper [1]. If you're interested in the (shorter) conference version, check out +[2]. +[1] Andreas ten Pas, Marcus Gualtieri, Kate Saenko, and Robert Platt. [**Grasp +Pose Detection in Point Clouds**](http://arxiv.org/abs/1706.09911). The +International Journal of Robotics Research, Vol 36, Issue 13-14, pp. 1455-1473. +October 2017. -## 11) Troubleshooting +[2] Marcus Gualtieri, Andreas ten Pas, Kate Saenko, and Robert Platt. [**High +precision grasp pose detection in dense +clutter**](http://arxiv.org/abs/1603.01564). IROS 2016, pp. 598-605. -* GCC 4.8: The package [might not compile](https://github.com/atenpas/gpd/issues/14#issuecomment-324789077) with -GCC 4.8. This is due to [a bug](https://gcc.gnu.org/bugzilla/show_bug.cgi?id=58251) in GCC. **Solution:** Upgrade to -GCC 4.9. +## 12) Troubleshooting Tips -* During `catkin_make`, you get this error: *[...]/caffe/include/caffe/util/cudnn.hpp:8:34: fatal error: caffe/proto/caffe.pb.h: No such file or directory*. **Solution ([source](https://github.com/muupan/dqn-in-the-caffe/issues/3)):** - ``` - # In the directory you installed Caffe to - protoc src/caffe/proto/caffe.proto --cpp_out=. - mkdir include/caffe/proto - mv src/caffe/proto/caffe.pb.h include/caffe/proto - ``` +1. Remove the `cmake` cache: `CMakeCache.txt` +1. `make clean` +1. Remove the `build` folder and rebuild. +1. Update *gcc* and *g++* to a version > 5. diff --git a/cfg/all_axes_vino_12channels.cfg b/cfg/all_axes_vino_12channels.cfg new file mode 100644 index 0000000..27214b6 --- /dev/null +++ b/cfg/all_axes_vino_12channels.cfg @@ -0,0 +1,88 @@ +# Path to config file for robot hand geometry +hand_geometry_filename = ../cfg/hand_geometry.cfg + +# Path to config file for volume and image geometry +image_geometry_filename = ../cfg/image_geometry_12channels.cfg + +# (OpenVINO) Path to directory that contains neural network parameters +model_file = ../models/openvino/two_views_12_channels_all_axes.xml +weights_file = ../models/openvino/two_views_12_channels_all_axes.bin +device = 0 + +# Preprocessing of point cloud +# voxelize: if the cloud gets voxelized/downsampled +# remove_outliers: if statistical outliers are removed from the cloud (used to remove noise) +# workspace: the workspace of the robot (dimensions of a cube centered at origin of point cloud) +# camera_position: the position of the camera from which the cloud was taken +# remove_plane_for_sampling: only draws samples which do not belong to the table plane +voxelize = 1 +remove_outliers = 0 +workspace = -1.0 1.0 -1.0 1.0 -1.0 1.0 +camera_position = 0 0 0 +remove_plane_for_sampling = 0 + +# Grasp candidate generation +# num_samples: the number of samples to be drawn from the point cloud +# num_threads: the number of CPU threads to be used +# nn_radius: the radius for the neighborhood search +# num_orientations: the number of robot hand orientations to evaluate +# rotation_axes: the axes about which the point neighborhood gets rotated +num_samples = 500 +num_threads = 4 +nn_radius = 0.01 +num_orientations = 8 +num_finger_placements = 10 +hand_axes = 0 1 2 +deepen_hand = 1 + +# Filtering of candidates +# min_aperture: the minimum gripper width +# max_aperture: the maximum gripper width +# workspace_grasps: dimensions of a cube centered at origin of point cloud; should be smaller than +min_aperture = 0.0 +max_aperture = 0.85 +workspace_grasps = -1 1 -1 1 -1 1 + +# Filtering of grasps which are too low on (i.e. too close to) support surface (e.g., table or floor) +# filter_table_side_grasps: turn this filter on/off +# vertical_axis: the vertical axis in the point cloud frame +# angle_thresh: threshold to determine which grasps are considered to be side grasps +# table_height: the height of the support surface (along vertical axis) +# table_thresh: thresold below which grasps are considered to close too the support surface +filter_table_side_grasps = 0 +vertical_axis = 0 0 1 +angle_thresh = 0.1 +table_height = 0.0 +table_thresh = 0.05 + +# Grasp image creation +# remove_plane_before_image_calculation: remove support plane from point cloud to speed up image computations +# create_image_batches: creates grasp images in batches (less memory usage) +remove_plane_before_image_calculation = 0 +create_image_batches = 0 + +# Clustering of grasps +# min_inliers: minimum number of inliers per cluster; set to 0 to turn off clustering +min_inliers = 1 + +# Grasp selection +# min_score: threshold to sort out "bad" grasps +# num_selected: number of selected grasps (sorted by score) +min_score = 0.5 +num_selected = 100 + +# Visualization +# plot_normals: plot the surface normals +# plot_samples: plot the samples +# plot_candidates: plot the grasp candidates +# plot_filtered_candidates: plot the grasp candidates which remain after filtering +# plot_valid_grasps: plot the candidates that are identified as valid grasps +# plot_clustered_grasps: plot the grasps that after clustering +# plot_selected_grasps: plot the selected grasps (final output) +plot_normals = 0 +plot_samples = 0 +plot_candidates = 0 +plot_filtered_candidates = 0 +plot_valid_grasps = 0 +plot_clustered_grasps = 0 +plot_selected_grasps = 1 diff --git a/cfg/bigbird_train_params.cfg b/cfg/bigbird_train_params.cfg new file mode 100644 index 0000000..88c76cd --- /dev/null +++ b/cfg/bigbird_train_params.cfg @@ -0,0 +1,92 @@ +data_root = /home/andreas/data/bigbird/ +objects_file_location = /home/andreas/data/gpd/bigbird_pcds/objects.txt +output_root = /home/andreas/data/gpd/descriptors/ +num_views_per_object = 20 +test_views = 2 5 8 13 16 + +# Path to config file for robot hand geometry +hand_geometry_filename = ../cfg/hand_geometry.cfg + +# Path to config file for volume and image geometry +#image_geometry_filename = ../cfg/image_geometry_15channels.cfg +image_geometry_filename = ../cfg/image_geometry_12channels.cfg +# image_geometry_filename = ../cfg/image_geometry_3channels.cfg + +# Path to directory that contains neural network parameters +lenet_params_dir = ../lenet/15channels/params/ +# lenet_params_dir = /home/andreas/data/gpd/caffe/15channels/params/ +# lenet_params_dir = /home/andreas/data/gpd/caffe/3channels/params/ + +# Preprocessing of point cloud +# voxelize: if the cloud gets voxelized/downsampled +# remove_outliers: if statistical outliers are removed from the cloud (used to remove noise) +# workspace: the workspace of the robot (dimensions of a cube centered at origin of point cloud) +# camera_position: the position of the camera from which the cloud was taken +voxelize = 1 +remove_outliers = 0 +workspace = -1 1 -1 1 -1 1 +camera_position = 0 0 1 + +# Grasp candidate generation +# num_samples: the number of samples to be drawn from the point cloud +# num_threads: the number of CPU threads to be used +# nn_radius: the radius for the neighborhood search +# num_orientations: the number of robot hand orientations to evaluate +# rotation_axes: the axes about which the point neighborhood gets rotated +num_samples = 500 +num_threads = 4 +nn_radius = 0.01 +num_orientations = 8 +hand_axes = 2 + +# Filtering of candidates +# min_aperture: the minimum gripper width +# max_aperture: the maximum gripper width +# workspace_grasps: dimensions of a cube centered at origin of point cloud; should be smaller than +min_aperture = 0.0 +max_aperture = 0.85 +workspace_grasps = -0.5 1 -0.35 0.35 -0.12 1 + +# Filtering of grasps which are too low on (i.e. too close to) support surface (e.g., table or floor) +# filter_table_side_grasps: turn this filter on/off +# vertical_axis: the vertical axis in the point cloud frame +# angle_thresh: threshold to determine which grasps are considered to be side grasps +# table_height: the height of the support surface (along vertical axis) +# table_thresh: thresold below which grasps are considered to close too the support surface +filter_table_side_grasps = 0 +vertical_axis = 0 0 1 +angle_thresh = 0.1 +table_height = 0.0 +table_thresh = 0.05 + +# Grasp image creation +# remove_plane_before_image_calculation: remove support plane from point cloud to speed up image computations +# create_image_batches: creates grasp images in batches (less memory usage) +remove_plane_before_image_calculation = 0 +create_image_batches = 0 + +# Clustering of grasps +# min_inliers: minimum number of inliers per cluster; set to 0 to turn off clustering +min_inliers = 1 + +# Grasp selection +# min_score: threshold to sort out "bad" grasps +# num_selected: number of selected grasps (sorted by score) +min_score = -5000 +num_selected = 100 + +# Visualization +# plot_normals: plot the surface normals +# plot_samples: plot the samples +# plot_candidates: plot the grasp candidates +# plot_filtered_candidates: plot the grasp candidates which remain after filtering +# plot_valid_grasps: plot the candidates that are identified as valid grasps +# plot_clustered_grasps: plot the grasps that after clustering +# plot_selected_grasps: plot the selected grasps (final output) +plot_normals = 0 +plot_samples = 0 +plot_candidates = 0 +plot_filtered_candidates = 0 +plot_valid_grasps = 0 +plot_clustered_grasps = 0 +plot_selected_grasps = 0 diff --git a/cfg/caffe_params.cfg b/cfg/caffe_params.cfg new file mode 100644 index 0000000..7143873 --- /dev/null +++ b/cfg/caffe_params.cfg @@ -0,0 +1,79 @@ +# Path to config file for robot hand geometry +hand_geometry_filename = ../cfg/hand_geometry.cfg + +# Path to config file for volume and image geometry +image_geometry_filename = ../cfg/image_geometry_15channels.cfg + +# Caffe specific parameters. +# model_file: path to file that contains network architecture +# weights_file: path to file that contains neural network parameters +# batch_size: number of images per batch +model_file = ../models/caffe/15channels/lenet_15_channels.prototxt +weights_file = ../models/caffe/15channels/two_views_15_channels_53_deg.caffemodel +batch_size = 64 + +# Preprocessing of point cloud +# voxelize: if the cloud gets voxelized/downsampled +# remove_outliers: if statistical outliers are removed from the cloud (used to remove noise) +# workspace: the workspace of the robot (dimensions of a cube centered at origin of point cloud) +# camera_position: the position of the camera from which the cloud was taken +# sample_above_plane: only draws samples which do not belong to the table plane +voxelize = 1 +remove_outliers = 0 +workspace = -1.0 1.0 -1.0 1.0 -1.0 1.0 +camera_position = 0 0 0 +sample_above_plane = 0 + +# Grasp candidate generation +# num_samples: the number of samples to be drawn from the point cloud +# num_threads: the number of CPU threads to be used +# nn_radius: the radius for the neighborhood search +# num_orientations: the number of robot hand orientations to evaluate +# rotation_axes: the axes about which the point neighborhood gets rotated +num_samples = 30 +num_threads = 4 +nn_radius = 0.01 +num_orientations = 8 +num_finger_placements = 10 +hand_axes = 2 +deepen_hand = 1 + +# Filtering of candidates +# min_aperture: the minimum gripper width +# max_aperture: the maximum gripper width +# workspace_grasps: dimensions of a cube centered at origin of point cloud; should be smaller than +min_aperture = 0.0 +max_aperture = 0.85 +workspace_grasps = -1 1 -1 1 -1 1 + +# Filtering of candidates based on their approach direction +# filter_approach_direction: turn filtering on/off +# direction: the direction to compare against +# angle_thresh: angle in radians above which grasps are filtered +filter_approach_direction = 0 +direction = 1 0 0 +thresh_rad = 2.0 + +# Clustering of grasps +# min_inliers: minimum number of inliers per cluster; set to 0 to turn off clustering +min_inliers = 0 + +# Grasp selection +# num_selected: number of selected grasps (sorted by score) +num_selected = 5 + +# Visualization +# plot_normals: plot the surface normals +# plot_samples: plot the samples +# plot_candidates: plot the grasp candidates +# plot_filtered_candidates: plot the grasp candidates which remain after filtering +# plot_valid_grasps: plot the candidates that are identified as valid grasps +# plot_clustered_grasps: plot the grasps that after clustering +# plot_selected_grasps: plot the selected grasps (final output) +plot_normals = 0 +plot_samples = 0 +plot_candidates = 0 +plot_filtered_candidates = 0 +plot_valid_grasps = 0 +plot_clustered_grasps = 0 +plot_selected_grasps = 1 diff --git a/cfg/cem_vino_params.cfg b/cfg/cem_vino_params.cfg new file mode 100644 index 0000000..0544ed6 --- /dev/null +++ b/cfg/cem_vino_params.cfg @@ -0,0 +1,84 @@ +# Path to config file for robot hand geometry +hand_geometry_filename = ../cfg/hand_geometry.cfg + +# Path to config file for volume and image geometry +image_geometry_filename = ../cfg/image_geometry_12channels.cfg + +# (OpenVINO) Path to directory that contains neural network parameters +model_file = ../models/openvino/two_views_12_channels_curv_axis.xml +weights_file = ../models/openvino/two_views_12_channels_curv_axis.bin +device = 0 +batch_size = 64 + +# Preprocessing of point cloud +# voxelize: if the cloud gets voxelized/downsampled +# remove_outliers: if statistical outliers are removed from the cloud (used to remove noise) +# workspace: the workspace of the robot (dimensions of a cube centered at origin of point cloud) +# camera_position: the position of the camera from which the cloud was taken +# sample_above_plane: only draws samples which do not belong to the table plane +voxelize = 1 +remove_outliers = 0 +workspace = -1.0 1.0 -1.0 1.0 -1.0 1.0 +camera_position = 0 0 0 +sample_above_plane = 1 + +# Cross Entropy Method +# sampling_method: 0 -> sum of Gaussians, 1 -> max of Gaussians +num_iterations = 10 +num_init_samples = 50 +num_samples_per_iteration = 50 +prob_rand_samples = 0.3 +standard_deviation = 1.5 +sampling_method = 1 +min_score = 0 +visualize_rounds = 0 +visualize_steps = 0 +visualize_results = 1 + +# Grasp candidate generation +# num_threads: the number of CPU threads to be used +# nn_radius: the radius for the neighborhood search +# num_orientations: the number of robot hand orientations to evaluate +# rotation_axes: the axes about which the point neighborhood gets rotated +num_threads = 4 +nn_radius = 0.01 +num_orientations = 8 +num_finger_placements = 10 +hand_axes = 2 +deepen_hand = 1 + +# Filtering of candidates +# min_aperture: the minimum gripper width +# max_aperture: the maximum gripper width +# workspace_grasps: dimensions of a cube centered at origin of point cloud; should be smaller than +min_aperture = 0.0 +max_aperture = 0.085 +workspace_grasps = -1 1 -1 1 -1 1 + +# Filtering of candidates based on their approach direction +# filter_approach_direction: turn filtering on/off +# direction: the direction to compare against +# angle_thresh: angle in radians above which grasps are filtered +filter_approach_direction = 1 +direction = 1 0 0 +thresh_rad = 2.0 + +# Clustering of grasps +# min_inliers: minimum number of inliers per cluster; set to 0 to turn off clustering +min_inliers = 0 + +# Visualization +# plot_normals: plot the surface normals +# plot_samples: plot the samples +# plot_candidates: plot the grasp candidates +# plot_filtered_candidates: plot the grasp candidates which remain after filtering +# plot_valid_grasps: plot the candidates that are identified as valid grasps +# plot_clustered_grasps: plot the grasps that after clustering +# plot_selected_grasps: plot the selected grasps (final output) +plot_normals = 0 +plot_samples = 0 +plot_candidates = 0 +plot_filtered_candidates = 0 +plot_valid_grasps = 0 +plot_clustered_grasps = 0 +plot_selected_grasps = 0 diff --git a/cfg/eigen_params.cfg b/cfg/eigen_params.cfg new file mode 100644 index 0000000..cfab104 --- /dev/null +++ b/cfg/eigen_params.cfg @@ -0,0 +1,82 @@ +# Path to config file for robot hand geometry +hand_geometry_filename = ../cfg/hand_geometry.cfg + +# Path to config file for volume and image geometry +image_geometry_filename = ../cfg/image_geometry_15channels.cfg + +# Path to directory that contains neural network parameters +weights_file = ../models/lenet/15channels/params/ + +# Preprocessing of point cloud +# voxelize: if the cloud gets voxelized/downsampled +# remove_outliers: if statistical outliers are removed from the cloud (used to remove noise) +# workspace: workspace of the robot (dimensions of a cube centered at origin of point cloud) +# camera_position: position of the camera from which the cloud was taken +# sample_above_plane: only draws samples which do not belong to the table plane +voxelize = 1 +voxel_size = 0.003 +remove_outliers = 0 +workspace = -1.0 1.0 -1.0 1.0 -1.0 1.0 +camera_position = 0 0 0 +sample_above_plane = 0 + +# Grasp candidate generation +# num_samples: number of samples to be drawn from the point cloud +# num_threads: number of CPU threads to be used +# nn_radius: neighborhood search radius for the local reference frame estimation +# num_orientations: number of robot hand orientations to evaluate +# num_finger_placements: number of finger placements to evaluate +# hand_axes: axes about which the point neighborhood gets rotated (0: approach, 1: binormal, 2: axis) +# (see https://raw.githubusercontent.com/atenpas/gpd2/master/readme/hand_frame.png) +# deepen_hand: if the hand is pushed forward onto the object +# friction_coeff: angle of friction cone in degrees +# min_viable: minimum number of points required on each side to be antipodal +num_samples = 30 +num_threads = 4 +nn_radius = 0.01 +num_orientations = 8 +num_finger_placements = 10 +hand_axes = 2 +deepen_hand = 1 +friction_coeff = 20 +min_viable = 6 + +# Filtering of candidates +# min_aperture: the minimum gripper width +# max_aperture: the maximum gripper width +# workspace_grasps: dimensions of a cube centered at origin of point cloud; should be smaller than +min_aperture = 0.0 +max_aperture = 0.085 +workspace_grasps = -1 1 -1 1 -1 1 + +# Filtering of candidates based on their approach direction +# filter_approach_direction: turn filtering on/off +# direction: direction to compare against +# angle_thresh: angle in radians above which grasps are filtered +filter_approach_direction = 0 +direction = 1 0 0 +thresh_rad = 2.0 + +# Clustering of grasps +# min_inliers: minimum number of inliers per cluster; set to 0 to turn off clustering +min_inliers = 0 + +# Grasp selection +# num_selected: number of selected grasps (sorted by score) +num_selected = 5 + +# Visualization +# plot_normals: plot the surface normals +# plot_samples: plot the samples +# plot_candidates: plot the grasp candidates +# plot_filtered_candidates: plot the grasp candidates which remain after filtering +# plot_valid_grasps: plot the candidates that are identified as valid grasps +# plot_clustered_grasps: plot the grasps that after clustering +# plot_selected_grasps: plot the selected grasps (final output) +plot_normals = 1 +plot_samples = 0 +plot_candidates = 0 +plot_filtered_candidates = 0 +plot_valid_grasps = 0 +plot_clustered_grasps = 0 +plot_selected_grasps = 1 diff --git a/cfg/generate_data.cfg b/cfg/generate_data.cfg new file mode 100644 index 0000000..54c2ab1 --- /dev/null +++ b/cfg/generate_data.cfg @@ -0,0 +1,57 @@ +data_root = /home/andreas/data/gpd/bigbird_pcds/ +objects_file_location = /home/andreas/data/gpd/bigbird_pcds/objects.txt +output_root = /home/andreas/data/gpd/models/test/ +chunk_size = 1000 +max_in_memory = 80000 +num_views_per_object = 20 +min_grasps_per_view = 100 +max_grasps_per_view = 500 +test_views = 2 5 8 13 16 + +# Hand geometry +hand_geometry_filename = 0 +finger_width = 0.01 +hand_outer_diameter = 0.12 +hand_depth = 0.06 +hand_height = 0.02 +init_bite = 0.01 + +# Image geometry +image_geometry_filename = 0 +volume_width = 0.10 +volume_depth = 0.06 +volume_height = 0.02 +image_size = 60 +image_num_channels = 12 + +# Point cloud preprocessing +remove_nans = 0 +voxel_size = 0.003 +reverse_mesh_normals = 1 +reverse_view_normals = 1 + +# Grasp candidate generation +# num_samples: number of samples to be drawn from the point cloud +# num_threads: number of CPU threads to be used +# nn_radius: neighborhood search radius for the local reference frame estimation +# num_orientations: number of robot hand orientations to evaluate +# num_finger_placements: number of finger placements to evaluate +# hand_axes: axes about which the point neighborhood gets rotated (0: approach, 1: binormal, 2: axis) +# (see https://raw.githubusercontent.com/atenpas/gpd2/master/readme/hand_frame.png) +# deepen_hand: if the hand is pushed forward onto the object +# friction_coeff: angle of friction cone in degrees +# min_viable: minimum number of points required on each side to be antipodal +num_samples = 400 +num_threads = 4 +nn_radius = 0.01 +num_orientations = 8 +num_finger_placements = 10 +# hand_axes = 0 1 2 +# hand_axes = 0 +# hand_axes = 1 +hand_axes = 2 +deepen_hand = 1 +friction_coeff = 20 +min_viable = 6 + +plot_samples = 0 diff --git a/cfg/hand_geometry.cfg b/cfg/hand_geometry.cfg new file mode 100644 index 0000000..cfebf1e --- /dev/null +++ b/cfg/hand_geometry.cfg @@ -0,0 +1,12 @@ +# ==== Robot Hand Geometry Parameters ==== +# ======================================== +# finger_width: the width of the finger +# outer_diameter: the diameter of the robot hand (= maximum aperture + 2 * finger width) +# hand_depth: the finger length (measured from hand base to finger tip) +# hand_height: the height of the hand +# init_bite: the minimum amount of the object to be covered by the hand +finger_width = 0.01 +hand_outer_diameter = 0.12 +hand_depth = 0.06 +hand_height = 0.02 +init_bite = 0.01 diff --git a/cfg/image_geometry.cfg b/cfg/image_geometry.cfg new file mode 100644 index 0000000..38a771f --- /dev/null +++ b/cfg/image_geometry.cfg @@ -0,0 +1,12 @@ +# ==== Robot Hand Geometry Parameters ==== +# ======================================== +# volume_width: the width of the volume +# volume_depth: the depth of the volume +# volume_height: the height of the volume +# image_size: the size of the image (width and height; image is square) +# image_num_channels: the number of channels in the image +volume_width = 0.10 +volume_depth = 0.06 +volume_height = 0.02 +image_size = 60 +image_num_channels = 15 diff --git a/cfg/image_geometry_12channels.cfg b/cfg/image_geometry_12channels.cfg new file mode 100644 index 0000000..ecdf63c --- /dev/null +++ b/cfg/image_geometry_12channels.cfg @@ -0,0 +1,12 @@ +# ==== Robot Hand Geometry Parameters ==== +# ======================================== +# volume_width: the width of the volume +# volume_depth: the depth of the volume +# volume_height: the height of the volume +# image_size: the size of the image (width and height; image is square) +# image_num_channels: the number of channels in the image +volume_width = 0.10 +volume_depth = 0.06 +volume_height = 0.02 +image_size = 60 +image_num_channels = 12 diff --git a/cfg/image_geometry_15channels.cfg b/cfg/image_geometry_15channels.cfg new file mode 100644 index 0000000..fe87e0a --- /dev/null +++ b/cfg/image_geometry_15channels.cfg @@ -0,0 +1,12 @@ +# ==== Robot Hand Geometry Parameters ==== +# ======================================== +# volume_width: the width of the volume +# volume_depth: the depth of the volume +# volume_height: the height of the volume +# image_size: the size of the image (width and height; image is square) +# image_num_channels: the number of channels in the image +volume_width = 0.10 +volume_depth = 0.06 +volume_height = 0.02 +image_size = 60 +image_num_channels = 15 diff --git a/cfg/image_geometry_1channels.cfg b/cfg/image_geometry_1channels.cfg new file mode 100644 index 0000000..1b50470 --- /dev/null +++ b/cfg/image_geometry_1channels.cfg @@ -0,0 +1,12 @@ +# ==== Robot Hand Geometry Parameters ==== +# ======================================== +# volume_width: the width of the volume +# volume_depth: the depth of the volume +# volume_height: the height of the volume +# image_size: the size of the image (width and height; image is square) +# image_num_channels: the number of channels in the image +volume_width = 0.10 +volume_depth = 0.06 +volume_height = 0.02 +image_size = 60 +image_num_channels = 1 diff --git a/cfg/image_geometry_3channels.cfg b/cfg/image_geometry_3channels.cfg new file mode 100644 index 0000000..3e37fdd --- /dev/null +++ b/cfg/image_geometry_3channels.cfg @@ -0,0 +1,12 @@ +# ==== Robot Hand Geometry Parameters ==== +# ======================================== +# volume_width: the width of the volume +# volume_depth: the depth of the volume +# volume_height: the height of the volume +# image_size: the size of the image (width and height; image is square) +# image_num_channels: the number of channels in the image +volume_width = 0.10 +volume_depth = 0.06 +volume_height = 0.02 +image_size = 60 +image_num_channels = 3 diff --git a/cfg/ros_eigen_params.cfg b/cfg/ros_eigen_params.cfg new file mode 100644 index 0000000..5b07554 --- /dev/null +++ b/cfg/ros_eigen_params.cfg @@ -0,0 +1,98 @@ +# Path to config file for robot hand geometry +hand_geometry_filename = 0 + +# Path to config file for volume and image geometry +image_geometry_filename = 0 + +# ==== Robot Hand Geometry ==== +# finger_width: the width of the finger +# outer_diameter: the diameter of the robot hand (= maximum aperture + 2 * finger width) +# hand_depth: the finger length (measured from hand base to finger tip) +# hand_height: the height of the hand +# init_bite: the minimum amount of the object to be covered by the hand +finger_width = 0.01 +hand_outer_diameter = 0.12 +hand_depth = 0.06 +hand_height = 0.02 +init_bite = 0.01 + +# ==== Grasp Descriptor ==== +# volume_width: the width of the cube inside the robot hand +# volume_depth: the depth of the cube inside the robot hand +# volume_height: the height of the cube inside the robot hand +# image_size: the size of the image (width and height; image is square) +# image_num_channels: the number of image channels +volume_width = 0.10 +volume_depth = 0.06 +volume_height = 0.02 +image_size = 60 +image_num_channels = 15 + +# (OpenVINO) Path to directory that contains neural network parameters +weights_file = /home/andreas/projects/gpd/lenet/15channels/params/ + +# Preprocessing of point cloud +# voxelize: if the cloud gets voxelized/downsampled +# remove_outliers: if statistical outliers are removed from the cloud (used to remove noise) +# workspace: the workspace of the robot (dimensions of a cube centered at origin of point cloud) +# camera_position: the position of the camera from which the cloud was taken +# sample_above_plane: only draws samples which do not belong to the table plane +voxelize = 1 +remove_outliers = 0 +workspace = -1.0 1.0 -1.0 1.0 -1.0 1.0 +camera_position = 0 0 0 +sample_above_plane = 0 + +# Grasp candidate generation +# num_samples: the number of samples to be drawn from the point cloud +# num_threads: the number of CPU threads to be used +# nn_radius: the radius for the neighborhood search +# num_orientations: the number of robot hand orientations to evaluate +# rotation_axes: the axes about which the point neighborhood gets rotated +num_samples = 500 +num_threads = 4 +nn_radius = 0.01 +num_orientations = 8 +num_finger_placements = 10 +hand_axes = 2 +deepen_hand = 1 + +# Filtering of candidates +# min_aperture: the minimum gripper width +# max_aperture: the maximum gripper width +# workspace_grasps: dimensions of a cube centered at origin of point cloud; should be smaller than +min_aperture = 0.0 +max_aperture = 0.85 +workspace_grasps = -1 1 -1 1 -1 1 + +# Filtering of candidates based on their approach direction +# filter_approach_direction: turn filtering on/off +# direction: the direction to compare against +# angle_thresh: angle in radians above which grasps are filtered +filter_approach_direction = 0 +direction = 1 0 0 +thresh_rad = 2.0 + +# Clustering of grasps +# min_inliers: minimum number of inliers per cluster; set to 0 to turn off clustering +min_inliers = 1 + +# Grasp selection +# num_selected: number of selected grasps (sorted by score) +num_selected = 100 + +# Visualization +# plot_normals: plot the surface normals +# plot_samples: plot the samples +# plot_candidates: plot the grasp candidates +# plot_filtered_candidates: plot the grasp candidates which remain after filtering +# plot_valid_grasps: plot the candidates that are identified as valid grasps +# plot_clustered_grasps: plot the grasps that after clustering +# plot_selected_grasps: plot the selected grasps (final output) +plot_normals = 0 +plot_samples = 0 +plot_candidates = 0 +plot_filtered_candidates = 0 +plot_valid_grasps = 0 +plot_clustered_grasps = 0 +plot_selected_grasps = 1 diff --git a/cfg/ros_vino_params.cfg b/cfg/ros_vino_params.cfg new file mode 100644 index 0000000..bd7e8a1 --- /dev/null +++ b/cfg/ros_vino_params.cfg @@ -0,0 +1,100 @@ +# Path to config file for robot hand geometry +hand_geometry_filename = 0 + +# Path to config file for volume and image geometry +image_geometry_filename = 0 + +# ==== Robot Hand Geometry ==== +# finger_width: the width of the finger +# outer_diameter: the diameter of the robot hand (= maximum aperture + 2 * finger width) +# hand_depth: the finger length (measured from hand base to finger tip) +# hand_height: the height of the hand +# init_bite: the minimum amount of the object to be covered by the hand +finger_width = 0.01 +hand_outer_diameter = 0.10 +hand_depth = 0.06 +hand_height = 0.02 +init_bite = 0.01 + +# ==== Grasp Descriptor ==== +# volume_width: the width of the cube inside the robot hand +# volume_depth: the depth of the cube inside the robot hand +# volume_height: the height of the cube inside the robot hand +# image_size: the size of the image (width and height; image is square) +# image_num_channels: the number of image channels +volume_width = 0.10 +volume_depth = 0.06 +volume_height = 0.02 +image_size = 60 +image_num_channels = 12 + +# (OpenVINO) Path to directory that contains neural network parameters +model_file = /home/ur5/projects/gpd/models/openvino/two_views_12_channels_curv_axis.xml +weights_file = /home/ur5/projects/gpd/models/openvino/two_views_12_channels_curv_axis.bin +device = 0 + +# Preprocessing of point cloud +# voxelize: if the cloud gets voxelized/downsampled +# remove_outliers: if statistical outliers are removed from the cloud (used to remove noise) +# workspace: the workspace of the robot (dimensions of a cube centered at origin of point cloud) +# camera_position: the position of the camera from which the cloud was taken +# sample_above_plane: only draws samples which do not belong to the table plane +voxelize = 1 +remove_outliers = 0 +workspace = -1.0 1.0 -1.0 1.0 -1.0 1.0 +camera_position = 0 0 0 +sample_above_plane = 0 + +# Grasp candidate generation +# num_samples: the number of samples to be drawn from the point cloud +# num_threads: the number of CPU threads to be used +# nn_radius: the radius for the neighborhood search +# num_orientations: the number of robot hand orientations to evaluate +# rotation_axes: the axes about which the point neighborhood gets rotated +num_samples = 500 +num_threads = 6 +nn_radius = 0.01 +num_orientations = 8 +num_finger_placements = 10 +hand_axes = 2 +deepen_hand = 1 + +# Filtering of candidates +# min_aperture: the minimum gripper width +# max_aperture: the maximum gripper width +# workspace_grasps: dimensions of a cube centered at origin of point cloud; should be smaller than +min_aperture = 0.0 +max_aperture = 0.85 +workspace_grasps = -1.0 1.0 -1.0 1.0 -1.0 1.0 + +# Filtering of candidates based on their approach direction +# filter_approach_direction: turn filtering on/off +# direction: the direction to compare against +# angle_thresh: angle in radians above which grasps are filtered +filter_approach_direction = 0 +direction = 1 0 0 +thresh_rad = 2.0 + +# Clustering of grasps +# min_inliers: minimum number of inliers per cluster; set to 0 to turn off clustering +min_inliers = 1 + +# Grasp selection +# num_selected: number of selected grasps (sorted by score) +num_selected = 100 + +# Visualization +# plot_normals: plot the surface normals +# plot_samples: plot the samples +# plot_candidates: plot the grasp candidates +# plot_filtered_candidates: plot the grasp candidates which remain after filtering +# plot_valid_grasps: plot the candidates that are identified as valid grasps +# plot_clustered_grasps: plot the grasps that after clustering +# plot_selected_grasps: plot the selected grasps (final output) +plot_normals = 0 +plot_samples = 0 +plot_candidates = 0 +plot_filtered_candidates = 0 +plot_valid_grasps = 0 +plot_clustered_grasps = 0 +plot_selected_grasps = 0 diff --git a/cfg/ur5.cfg b/cfg/ur5.cfg new file mode 100644 index 0000000..2d1ac36 --- /dev/null +++ b/cfg/ur5.cfg @@ -0,0 +1,76 @@ +# Path to config file for robot hand geometry +hand_geometry_filename = /home/ur5/projects/andreas/gpd_no_ros/cfg/ur5_hand_geometry.cfg +# hand_geometry_filename = ../cfg/ur5_hand_geometry.cfg + +# Path to config file for volume and image geometry +image_geometry_filename = /home/ur5/projects/andreas/gpd_no_ros/cfg/image_geometry_15channels.cfg +# image_geometry_filename = ../cfg/image_geometry_3channels.cfg + +# Path to directory that contains neural network parameters +lenet_params_dir = /home/ur5/projects/andreas/gpd_no_ros/lenet/15channels/params/ + +# Preprocessing of point cloud +# voxelize: if the cloud gets voxelized/downsampled +# remove_outliers: if statistical outliers are removed from the cloud (used to remove noise) +# workspace: the workspace of the robot (dimensions of a cube centered at origin of point cloud) +# camera_position: the position of the camera from which the cloud was taken +# sample_above_plane: only draws samples which do not belong to the table plane +voxelize = 1 +remove_outliers = 0 +workspace = -1 1 -1 1 -1 1 +camera_position = 0 0 1 +sample_above_plane = 1 + +# Grasp candidate generation +# num_samples: the number of samples to be drawn from the point cloud +# num_threads: the number of CPU threads to be used +# nn_radius: the radius for the neighborhood search +# num_orientations: the number of robot hand orientations to evaluate +# rotation_axes: the axes about which the point neighborhood gets rotated +num_samples = 500 +num_threads = 4 +nn_radius = 0.01 +num_orientations = 8 +num_finger_placements = 10 +hand_axes = 2 +deepen_hand = 1 + +# Filtering of candidates +# min_aperture: the minimum gripper width +# max_aperture: the maximum gripper width +# workspace_grasps: dimensions of a cube centered at origin of point cloud; should be smaller than +min_aperture = 0.0 +max_aperture = 0.85 +workspace_grasps = -1 1 -1 1 -1 1 + +# Filtering of candidates based on their approach direction +# filter_approach_direction: turn filtering on/off +# direction: the direction to compare against +# angle_thresh: angle in radians above which grasps are filtered +filter_approach_direction = 0 +direction = 1 0 0 +thresh_rad = 2.0 + +# Clustering of grasps +# min_inliers: minimum number of inliers per cluster; set to 0 to turn off clustering +min_inliers = 1 + +# Grasp selection +# num_selected: number of selected grasps (sorted by score) +num_selected = 100 + +# Visualization (lead tp crash when used with Python interface) +# plot_normals: plot the surface normals +# plot_samples: plot the samples +# plot_candidates: plot the grasp candidates +# plot_filtered_candidates: plot the grasp candidates which remain after filtering +# plot_valid_grasps: plot the candidates that are identified as valid grasps +# plot_clustered_grasps: plot the grasps that after clustering +# plot_selected_grasps: plot the selected grasps (final output) +plot_normals = 0 +plot_samples = 0 +plot_candidates = 0 +plot_filtered_candidates = 0 +plot_valid_grasps = 0 +plot_clustered_grasps = 0 +plot_selected_grasps = 0 diff --git a/cfg/ur5_hand_geometry.cfg b/cfg/ur5_hand_geometry.cfg new file mode 100644 index 0000000..ad139b7 --- /dev/null +++ b/cfg/ur5_hand_geometry.cfg @@ -0,0 +1,12 @@ +# ==== Robot Hand Geometry Parameters ==== +# ======================================== +# finger_width: the width of the finger +# outer_diameter: the diameter of the robot hand (= maximum aperture + 2 * finger width) +# hand_depth: the finger length (measured from hand base to finger tip) +# hand_height: the height of the hand +# init_bite: the minimum amount of the object to be covered by the hand +finger_width = 0.01 +hand_outer_diameter = 0.105 +hand_depth = 0.06 +hand_height = 0.02 +init_bite = 0.01 diff --git a/cfg/vino_params_12channels.cfg b/cfg/vino_params_12channels.cfg new file mode 100644 index 0000000..89cd04e --- /dev/null +++ b/cfg/vino_params_12channels.cfg @@ -0,0 +1,94 @@ +# Path to config file for robot hand geometry +hand_geometry_filename = ../cfg/hand_geometry.cfg + +# Path to config file for volume and image geometry +image_geometry_filename = ../cfg/image_geometry_12channels.cfg + +# (OpenVINO) Path to directory that contains neural network parameters +model_file = ../models/openvino/two_views_12_channels_curv_axis.xml +weights_file = ../models/openvino/two_views_12_channels_curv_axis.bin +device = 0 +batch_size = 64 + +# Preprocessing of point cloud +# voxelize: if the cloud gets voxelized/downsampled +# voxel_size: size of voxel +# remove_outliers: if statistical outliers are removed from the cloud (used to remove noise) +# workspace: workspace of the robot (dimensions of a cube centered at origin of point cloud) +# camera_position: position of the camera from which the cloud was taken +# sample_above_plane: only draws samples which do not belong to the table plane +# refine_normals_k: if 0, do not refine. If > 0, this is the number of neighbors used for refinement. +# centered_at_origin: if the object is centered at the origin +voxelize = 1 +voxel_size = 0.003 +remove_outliers = 0 +workspace = -1.0 1.0 -1.0 1.0 -1.0 1.0 +camera_position = 0 0 0 +sample_above_plane = 0 +normals_radius = 0.01 +refine_normals_k = 0 +centered_at_origin = 1 + +# Grasp candidate generation +# num_samples: number of samples to be drawn from the point cloud +# num_threads: number of CPU threads to be used +# nn_radius: neighborhood search radius for the local reference frame estimation +# num_orientations: number of robot hand orientations to evaluate +# num_finger_placements: number of finger placements to evaluate +# hand_axes: axes about which the point neighborhood gets rotated (0: approach, 1: binormal, 2: axis) +# (see https://raw.githubusercontent.com/atenpas/gpd2/master/readme/hand_frame.png) +# deepen_hand: if the hand is pushed forward onto the object +# friction_coeff: angle of friction cone in degrees +# min_viable: minimum number of points required on each side to be antipodal +num_samples = 100 +num_threads = 4 +nn_radius = 0.01 +num_orientations = 8 +num_finger_placements = 10 +# hand_axes = 0 1 2 +# hand_axes = 0 +# hand_axes = 1 +hand_axes = 2 +deepen_hand = 1 +friction_coeff = 20 +min_viable = 6 + +# Filtering of grasp candidates +# min_aperture: minimum gripper width +# max_aperture: maximum gripper width +# workspace_grasps: dimensions of a cube centered at origin of point cloud; should be smaller than +min_aperture = 0.0 +max_aperture = 0.085 +workspace_grasps = -1 1 -1 1 -1 1 + +# Filtering of grasp candidates based on their approach direction +# filter_approach_direction: turn filtering on/off +# direction: direction to compare against +# angle_thresh: angle in radians above which grasps are filtered +filter_approach_direction = 0 +direction = 1 0 0 +thresh_rad = 2.0 + +# Clustering of grasps +# min_inliers: minimum number of inliers per cluster; set to 0 to turn off clustering +min_inliers = 1 + +# Grasp selection +# num_selected: number of selected grasps (sorted by score) +num_selected = 50 + +# Visualization +# plot_normals: plot the surface normals +# plot_samples: plot the samples +# plot_candidates: plot the grasp candidates +# plot_filtered_candidates: plot the grasp candidates which remain after filtering +# plot_valid_grasps: plot the candidates that are identified as valid grasps +# plot_clustered_grasps: plot the grasps that after clustering +# plot_selected_grasps: plot the selected grasps (final output) +plot_normals = 0 +plot_samples = 0 +plot_candidates = 1 +plot_filtered_candidates = 0 +plot_valid_grasps = 1 +plot_clustered_grasps = 0 +plot_selected_grasps = 1 diff --git a/contrib/multiple_transform.py b/contrib/multiple_transform.py new file mode 100644 index 0000000..1841d79 --- /dev/null +++ b/contrib/multiple_transform.py @@ -0,0 +1,78 @@ +# -*- coding: utf-8 -*- +""" +Created on Wed Jul 25 16:22:08 2018 +@author: haoshu fang +""" +import os +import numpy as np +import h5py as h5 +from scipy.misc import imread +import IPython +import math +from open3d import * +from tqdm import tqdm +import sys + +def getTransform(calibration, viewpoint_camera, referenceCamera, transformation): + CamFromRefKey = "H_{0}_from_{1}".format(viewpoint_camera, referenceCamera) + print CamFromRefKey + CamFromRef = calibration[CamFromRefKey][:] + + TableFromRefKey = "H_table_from_reference_camera" + TableFromRef = transformation[TableFromRefKey][:] + print TableFromRef + sys.exit() + return np.dot(TableFromRef, np.linalg.inv(CamFromRef)) + + +if __name__ == "__main__": + + if len(sys.argv) < 3: + print "Align points cloud with mesh and store one fused point cloud per viewpoint" + print "Usage: python multiple_transform.py DATA_DIRECTORY OBJECTS_LIST_FILE" + sys.exit(-1) + + data_directory = sys.argv[1] + + file = open(sys.argv[2]) + objects = [line.strip() for line in file] + viewpoint_cameras = ["NP1", "NP2", "NP3", "NP4", "NP5"] # Camera which the viewpoint will be generated. + viewpoint_angles = [str(x) for x in range(0,360,3)] # Relative angle of the object w.r.t the camera (angle of the turntable). + + for object_ in objects: + referenceCamera = "NP5" + output_directory = os.path.join(data_directory, object_, "clouds_aligned") + if not os.path.exists(output_directory): + os.makedirs(output_directory) + for viewpoint_camera in tqdm(viewpoint_cameras): + points_concat = [] + for viewpoint_angle in viewpoint_angles: + basename = "{0}_{1}".format(viewpoint_camera, viewpoint_angle) + cloudFilename = os.path.join(data_directory, object_, "clouds", basename + ".pcd") + + calibrationFilename = os.path.join(data_directory, object_, "calibration.h5") + calibration = h5.File(calibrationFilename) + + transformationFilename = os.path.join(data_directory, object_, "poses", referenceCamera+"_"+viewpoint_angle+"_pose.h5") + transformation = h5.File(transformationFilename) + + H_table_from_cam = getTransform(calibration, viewpoint_camera, referenceCamera, transformation) + + pcd = read_point_cloud(cloudFilename) + points = np.asarray(pcd.points) + ones = np.ones(len(points))[:,np.newaxis] + + points_ = np.concatenate((points,ones),axis=1) + new_points_ = np.dot(H_table_from_cam, points_.T) + new_points = new_points_.T[:,:3] + + points_concat.append(new_points) + + new_points = np.zeros((0,3)) + for p in points_concat: + new_points = np.concatenate((new_points, p)) + ply = PointCloud() + ply.points = Vector3dVector(new_points) + ply.colors = Vector3dVector(pcd.colors) + resFilename = os.path.join(output_directory, str(viewpoint_camera) + "_merged.pcd") + write_point_cloud(resFilename, ply) diff --git a/contrib/transform.py b/contrib/transform.py new file mode 100644 index 0000000..c96d195 --- /dev/null +++ b/contrib/transform.py @@ -0,0 +1,70 @@ +# -*- coding: utf-8 -*- +""" +Created on Wed Jul 25 16:22:08 2018 +@author: haoshu fang +""" +import os +import numpy as np +import h5py as h5 +from scipy.misc import imread +import IPython +import math +from open3d import * +from tqdm import tqdm +import sys + +def getTransform(calibration, viewpoint_camera, referenceCamera, transformation): + CamFromRefKey = "H_{0}_from_{1}".format(viewpoint_camera, referenceCamera) + CamFromRef = calibration[CamFromRefKey][:] + + TableFromRefKey = "H_table_from_reference_camera" + TableFromRef = transformation[TableFromRefKey][:] + return np.dot(TableFromRef, np.linalg.inv(CamFromRef)) + + +if __name__ == "__main__": + + if len(sys.argv) < 3: + print "Align point cloud with mesh" + print "Usage: python transform.py DATA_DIRECTORY OBJECTS_LIST_FILE" + sys.exit(-1) + + data_directory = sys.argv[1] + + file = open(sys.argv[2]) + objects = [line.strip() for line in file] + viewpoint_cameras = ["NP1", "NP2", "NP3", "NP4", "NP5"] # Camera which the viewpoint will be generated. + viewpoint_angles = [str(x) for x in range(0,360,3)] # Relative angle of the object w.r.t the camera (angle of the turntable). + + for object_ in objects: + referenceCamera = "NP5" + output_directory = os.path.join(data_directory, object_, "clouds_aligned") + if not os.path.exists(output_directory): + os.makedirs(output_directory) + for viewpoint_camera in viewpoint_cameras: + for viewpoint_angle in tqdm(viewpoint_angles): + basename = "{0}_{1}".format(viewpoint_camera, viewpoint_angle) + cloudFilename = os.path.join(data_directory, object_, "clouds", basename + ".pcd") + + calibrationFilename = os.path.join(data_directory, object_, "calibration.h5") + calibration = h5.File(calibrationFilename) + + transformationFilename = os.path.join(data_directory, object_, "poses", referenceCamera+"_"+viewpoint_angle+"_pose.h5") + transformation = h5.File(transformationFilename) + + H_table_from_cam = getTransform(calibration, viewpoint_camera, referenceCamera, transformation) + + pcd = read_point_cloud(cloudFilename) + points = np.asarray(pcd.points) + ones = np.ones(len(points))[:,np.newaxis] + + points_ = np.concatenate((points,ones),axis=1) + new_points_ = np.dot(H_table_from_cam, points_.T) + new_points = new_points_.T[:,:3] + + + ply = PointCloud() + ply.points = Vector3dVector(new_points) + ply.colors = Vector3dVector(pcd.colors) + resFilename = os.path.join(output_directory, basename + ".pcd") + write_point_cloud(resFilename, ply) diff --git a/include/gpd/candidate/antipodal.h b/include/gpd/candidate/antipodal.h new file mode 100644 index 0000000..0d3f946 --- /dev/null +++ b/include/gpd/candidate/antipodal.h @@ -0,0 +1,95 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, Andreas ten Pas + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * 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. + */ + +#ifndef ANTIPODAL_H_ +#define ANTIPODAL_H_ + +#include +#include +#include +#include +#include + +#include + +#include + +namespace gpd { +namespace candidate { + +/** + * + * \brief Check if a grasp is antipodal. + * + * This class checks if a grasp candidate satisfies the antipodal condition. + * + */ +class Antipodal { + public: + Antipodal(double friction_coeff, int min_viable) + : friction_coeff_(friction_coeff), min_viable_(min_viable) {} + + /** + * \brief Check if a grasp is antipodal. + * \param point_list the list of points associated with the grasp + * \param extremal_threshold + * \param lateral_axis the closing direction of the robot hand + * \param forward_axis the forward direction of the robot hand + * \param vertical_axis the vertical direction of the robot hand + * \return 0 if it's not antipodal, 1 if one finger is antipodal, 2 if the + * grasp is antipodal + */ + int evaluateGrasp(const util::PointList &point_list, double extremal_thresh, + int lateral_axis = 0, int forward_axis = 1, + int vertical_axis = 2) const; + + /** + * \brief Check if a grasp is antipodal. + * \note Deprecated method. + * \param normals the set of surface normals associated with the grasp + * \param thresh_half the threshold to consider the grasp half-antipodal + * \param thresh_full the threshold to conisder the grasp full-antipodal + */ + int evaluateGrasp(const Eigen::Matrix3Xd &normals, double thresh_half, + double thresh_full) const; + + double friction_coeff_; ///< angle of friction cone in degrees + int min_viable_; ///< minimum number of points on each side to be antipodal + + static const int NO_GRASP; // normals point not toward any finger + static const int HALF_GRASP; // normals point towards one finger + static const int FULL_GRASP; // normals point towards both fingers +}; + +} // namespace candidate +} // namespace gpd + +#endif /* ANTIPODAL_H_ */ diff --git a/include/gpd/candidate/candidates_generator.h b/include/gpd/candidate/candidates_generator.h new file mode 100644 index 0000000..f84c498 --- /dev/null +++ b/include/gpd/candidate/candidates_generator.h @@ -0,0 +1,145 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, Andreas ten Pas + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * 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. + */ + +#ifndef GRASP_CANDIDATES_GENERATOR_H +#define GRASP_CANDIDATES_GENERATOR_H + +// System +#include +#include + +// PCL +#include +#include +#include +#include +#include + +// Custom +#include +#include +#include +#include +#include + +namespace gpd { +namespace candidate { + +/** + * + * \brief Generate grasp candidates. + * + * This class generates grasp candidates by searching for feasible robot hand + * placements in a point cloud. + * + */ +class CandidatesGenerator { + public: + /** + * \brief Parameters for the candidates generator. + */ + struct Parameters { + bool remove_statistical_outliers_; ///< if statistical outliers are removed + /// from the point cloud + bool sample_above_plane_; ///< if samples are drawn above the support plane + bool voxelize_; ///< if the point cloud gets voxelized + double voxel_size_; ///< voxel size + double normals_radius_; ///< neighborhood search radius used for normal + ///< estimation + int refine_normals_k_; ///< If 0, do not refine. If > 0, this is the number + ///< of neighbors used for refinement. + int num_samples_; ///< the number of samples to be used in the search + int num_threads_; ///< the number of CPU threads to be used in the search + std::vector workspace_; ///< the robot's workspace + }; + + /** + * \brief Constructor. + * \param params the parameters to be used for the candidate generation + * \param hand_search_params the parameters to be used for the hand search + */ + CandidatesGenerator(const Parameters ¶ms, + const HandSearch::Parameters &hand_search_params); + + /** + * \brief Preprocess the point cloud. + * \param cloud_cam the point cloud + */ + void preprocessPointCloud(util::Cloud &cloud); + + /** + * \brief Generate grasp candidates given a point cloud. + * \param cloud_cam the point cloud + * \return list of grasp candidates + */ + std::vector> generateGraspCandidates( + const util::Cloud &cloud_cam); + + /** + * \brief Generate grasp candidate sets given a point cloud. + * \param cloud_cam the point cloud + * \return lust of grasp candidate sets + */ + std::vector> generateGraspCandidateSets( + const util::Cloud &cloud_cam); + + /** + * \brief Reevaluate grasp candidates on a given point cloud. + * \param cloud the point cloud + * \param grasps the grasps to evaluate + */ + std::vector reevaluateHypotheses( + const util::Cloud &cloud, std::vector> &grasps); + + /** + * \brief Set the number of samples. + * \param num_samples the number of samples + */ + void setNumSamples(int num_samples) { params_.num_samples_ = num_samples; } + + /** + * \brief Return the hand search parameters. + * \return the hand search parameters + */ + const HandSearch::Parameters &getHandSearchParams() const { + return hand_search_->getParams(); + } + + private: + std::unique_ptr hand_search_; + + Parameters params_; +}; + +} // namespace candidate +} // namespace gpd + +#endif /* GRASP_CANDIDATES_GENERATOR_H */ diff --git a/include/gpd/candidate/finger_hand.h b/include/gpd/candidate/finger_hand.h new file mode 100644 index 0000000..5432d7c --- /dev/null +++ b/include/gpd/candidate/finger_hand.h @@ -0,0 +1,276 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, Andreas ten Pas + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * 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. + */ + +#ifndef FINGER_HAND_H +#define FINGER_HAND_H + +#include +#include +#include + +namespace gpd { +namespace candidate { + +/** + * + * \brief Calculate collision-free finger placements. + * + * This class calculates collision-free finger placements. The parameters are + * the outer diameter, the width of the fingers, and the length of the fingers + * of the robot hand. Also considers the "bite" that the grasp must have, i.e., + * by how much the robot hand can be moved onto the object. + * + */ +class FingerHand { + public: + /** + * \brief Default constructor. + */ + FingerHand() {} + + /** + * \brief Constructor. + * \param finger_width the width of the fingers + * \param hand_outer_diameter the maximum aperture of the robot hand + * \param hand_depth the length of the fingers + */ + FingerHand(double finger_width, double hand_outer_diameter, double hand_depth, + int num_placements); + + /** + * \brief Find possible finger placements. + * + * Finger placements need to be collision-free and contain at least one point + * in between the fingers. + * + * \param points the points to checked for possible finger placements + * \param bite how far the robot can be moved into the object + * \param idx if this is larger than -1, only check the -th finger + * placement + */ + void evaluateFingers(const Eigen::Matrix3Xd &points, double bite, + int idx = -1); + + /** + * \brief Chhose the middle among all valid finger placements. + * \return the index of the middle finger placement + */ + int chooseMiddleHand(); + + /** + * \brief Try to move the robot hand as far as possible onto the object. + * \param points the points that the finger placement is evaluated on, assumed + * to be rotated into the hand frame and + * cropped by hand height + * \param min_depth the minimum depth that the hand can be moved onto the + * object + * \param max_depth the maximum depth that the hand can be moved onto the + * object + * \return the index of the middle finger placement + */ + int deepenHand(const Eigen::Matrix3Xd &points, double min_depth, + double max_depth); + + /** + * \brief Compute which of the given points are located in the closing region + * of the robot hand. + * \param points the points + * \param idx if this is larger than -1, only check the -th finger + * placement + * \return the points that are located in the closing region + */ + std::vector computePointsInClosingRegion(const Eigen::Matrix3Xd &points, + int idx = -1); + + /** + * \brief Check which 2-finger placements are feasible. + */ + void evaluateHand(); + + /** + * \brief Check the 2-finger placement at a given index. + * \param idx the index of the finger placement + */ + void evaluateHand(int idx); + + /** + * \brief Return the depth of the hand. + * \return the hand depth + */ + double getHandDepth() const { return hand_depth_; } + + /** + * \brief Return the finger placement evaluations. + * \return the hand configuration evaluations + */ + const Eigen::Array &getHand() const { return hand_; } + + /** + * \brief Return the finger placement evaluations. + * \return the hand configuration evaluations + */ + const Eigen::Array &getFingers() const { + return fingers_; + } + + /** + * \brief Return where the bottom of the hand is (where the hand base is). + * \return the bottom of the hand + */ + double getBottom() const { return bottom_; } + + /** + * \brief Return where the left finger is. + * \return the left finger's location + */ + double getLeft() const { return left_; } + + /** + * \brief Return where the right finger is. + * \return the right finger's location + */ + double getRight() const { return right_; } + + /** + * \brief Return where the top of the hand is (where the fingertips are). + * \return the top of the hand + */ + double getTop() const { return top_; } + + /** + * \brief Return where the center of the hand is. + * \return the center of the hand + */ + double getCenter() const { return center_; } + + /** + * \brief Return where the bottom of the hand is in the point cloud. + * \return the bottom of the hand in the point cloud + */ + double getSurface() const { return surface_; } + + /** + * \brief Return the index of the forward axis. + * \return the index of the forward axis + */ + int getForwardAxis() const { return forward_axis_; } + + /** + * \brief Set the index of the forward axis. + * \param forward_axis the index of the forward axis + */ + void setForwardAxis(int forward_axis) { forward_axis_ = forward_axis; } + + /** + * \brief Get the index of the lateral axis (hand closing direction). + * \param the index of the lateral axis + */ + int getLateralAxis() const { return lateral_axis_; } + + /** + * \brief Set the index of the lateral axis (hand closing direction). + * \param lateral_axis the index of the lateral axis + */ + void setLateralAxis(int lateral_axis) { lateral_axis_ = lateral_axis; } + + /** + * \brief Set the bottom of the hand (robot hand base). + * \param bottom the hand bottom + */ + void setBottom(double bottom) { bottom_ = bottom; } + + /** + * \brief Set the center of the hand. + * \param bottom the hand center + */ + void setCenter(double center) { center_ = center; } + + /** + * \brief Set the lateral position of the left finger. + * \param left the lateral position of the left finger + */ + void setLeft(double left) { left_ = left; } + + /** + * \brief Set the lateral position of the right finger. + * \param right the lateral position of the right finger + */ + void setRight(double right) { right_ = right; } + + /** + * \brief Set where the bottom of the hand is in the point cloud. + * \param left where the bottom of the hand is in the point cloud + */ + void setSurface(double surface) { surface_ = surface; } + + /** + * \brief Set where the top of the hand is (where the fingertips are). + * \param top where the top of the hand is + */ + void setTop(double top) { top_ = top; } + + private: + /** + * \brief Check that a given finger does not collide with the point cloud. + * \param points the points to be checked for collision (size: 3 x n) + * \param indices the indices of the points to be checked for collision + * \param idx the index of the finger to be checked + * \return true if it does not collide, false if it collides + */ + bool isGapFree(const Eigen::Matrix3Xd &points, + const std::vector &indices, int idx); + + int forward_axis_; ///< the index of the horizontal axis in the hand frame + ///(grasp approach direction) + int lateral_axis_; ///< the index of the vertical axis in the hand frame + ///(closing direction of the robot hand) + + double finger_width_; ///< the width of the robot hand fingers + double hand_depth_; ///< the hand depth (finger length) + + Eigen::VectorXd finger_spacing_; ///< the possible finger placements + Eigen::Array + fingers_; ///< indicates the feasible fingers + Eigen::Array + hand_; ///< indicates the feasible 2-finger placements + double bottom_; ///< the base of the hand + double top_; ///< the top of the hand, where the fingertips are + double left_; ///< the left side of the gripper bounding box + double right_; ///< the right side of the gripper bounding box + double center_; ///< the horizontal center of the gripper bounding box + double surface_; ///< the corresponding vertical base point of the hand in + /// the point cloud +}; + +} // namespace candidate +} // namespace gpd + +#endif diff --git a/include/gpd/candidate/frame_estimator.h b/include/gpd/candidate/frame_estimator.h new file mode 100644 index 0000000..d382ec4 --- /dev/null +++ b/include/gpd/candidate/frame_estimator.h @@ -0,0 +1,118 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, Andreas ten Pas + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * 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. + */ + +#ifndef FRAME_ESTIMATOR_H +#define FRAME_ESTIMATOR_H + +#include + +#include + +#include + +#include + +#include +#include + +namespace gpd { +namespace candidate { + +typedef pcl::PointCloud PointCloudRGBA; + +/** + * + * \brief Estimate local reference frames. + * + * This class estimates local reference frames (LRFs) for point neighborhoods. + * + */ +class FrameEstimator { + public: + /** + * \brief Constructor. + * \param num_threads the number of CPU threads to be used + */ + FrameEstimator(int num_threads) : num_threads_(num_threads) {} + + /** + * \brief Calculate local reference frames given a list of point cloud + * indices. + * \param cloud_cam the point cloud + * \param indices the list of indices into the point cloud + * \param radius the radius for the point neighborhood search + * \param kdtree the kdtree used for faster neighborhood search + * \return the list of local reference frames + */ + std::vector calculateLocalFrames( + const util::Cloud &cloud_cam, const std::vector &indices, + double radius, const pcl::KdTreeFLANN &kdtree) const; + + /** + * \brief Calculate local reference frames given a list of (x,y,z) samples. + * \param cloud_cam the point cloud + * \param samples the list of (x,y,z) samples + * \param radius the radius for the point neighborhood search + * \param kdtree the kdtree used for faster neighborhood search + * \return the list of local reference frames + */ + std::vector calculateLocalFrames( + const util::Cloud &cloud_cam, const Eigen::Matrix3Xd &samples, + double radius, const pcl::KdTreeFLANN &kdtree) const; + + /** + * \brief Calculate a local reference frame given a list of surface normals. + * \param normals the list of surface normals + * \param sample the center of the point neighborhood + * \param radius the radius of the point neighborhood + * \param kdtree the kdtree used for faster neighborhood search + * \return the local reference frame + */ + std::unique_ptr calculateFrame( + const Eigen::Matrix3Xd &normals, const Eigen::Vector3d &sample, + double radius, const pcl::KdTreeFLANN &kdtree) const; + + private: + /** + * \brief Convert an Eigen::Vector3d object to a pcl::PointXYZRGBA. + * \param v the Eigen vector + * \reutrn the pcl point + */ + pcl::PointXYZRGBA eigenVectorToPcl(const Eigen::Vector3d &v) const; + + int num_threads_; ///< number of CPU threads to be used for calculating local + /// reference frames +}; + +} // namespace candidate +} // namespace gpd + +#endif /* FRAME_ESTIMATOR_H */ diff --git a/include/gpd/candidate/hand.h b/include/gpd/candidate/hand.h new file mode 100644 index 0000000..930694c --- /dev/null +++ b/include/gpd/candidate/hand.h @@ -0,0 +1,282 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, Andreas ten Pas + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * 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. + */ + +#ifndef HAND_H_ +#define HAND_H_ + +// Eigen +#include + +// System +#include +#include +#include + +// custom +#include + +namespace gpd { +namespace candidate { + +/** + *\brief Label information + */ +struct Label { + double score_{0.0}; ///< the score given by the classifier + bool full_antipodal_{false}; ///< whether the grasp is antipodal + bool half_antipodal_{false}; ///< whether the grasp is indeterminate + + Label(double score, bool full_antipodal, bool half_antipodal) + : score_(score), + full_antipodal_(full_antipodal), + half_antipodal_(half_antipodal) {} +}; + +/** + *\brief 2-D bounding box of hand closing region with respect to hand frame + */ +struct BoundingBox { + double center_; + double top_; + double bottom_; +}; + +/** + * + * \brief Grasp represented as a robot hand pose + * + * This class represents a grasp candidate by the position and orientation of + * the robot hand at the grasp before the fingers are closed. + * + */ +class Hand { + public: + /** + * \brief Default constructor. + */ + Hand(); + + /** + * \brief Constructor. + * \param sample the center of the point neighborhood associated with the + * grasp + * \param frame the orientation of the grasp as a rotation matrix + * \param finger_hand the FingerHand object describing a feasible finger + * placement + * \param width the opening width of the robot hand + */ + Hand(const Eigen::Vector3d &sample, const Eigen::Matrix3d &frame, + const FingerHand &finger_hand, double width); + + /** + * \brief Constructor. + * \param sample the center of the point neighborhood associated with the + * grasp + * \param frame the orientation of the grasp as a rotation matrix + * \param finger_hand the FingerHand object describing a feasible finger + * placement + */ + Hand(const Eigen::Vector3d &sample, const Eigen::Matrix3d &frame, + const FingerHand &finger_hand); + + /** + * \brief Set properties of the grasp. + * \param finger_hand the FingerHand object describing a feasible finger + * placement + */ + void construct(const FingerHand &finger_hand); + + /** + * \brief Write a list of grasps to a file. + * \param filename location of the file + * \param hands the list of grasps + */ + void writeHandsToFile(const std::string &filename, + const std::vector &hands) const; + + /** + * \brief Print a description of the grasp candidate to the systen's standard + * output. + */ + void print() const; + + /** + * \brief Return the approach vector of the grasp. + * \return 3x1 grasp approach vector + */ + const Eigen::Vector3d getApproach() const { return orientation_.col(0); } + + /** + * \brief Return the binormal of the grasp. + * \return 3x1 binormal + */ + const Eigen::Vector3d getBinormal() const { return orientation_.col(1); } + + /** + * \brief Return the hand axis of the grasp. + * \return 3x1 hand axis + */ + const Eigen::Vector3d getAxis() const { return orientation_.col(2); } + + /** + * \brief Return whether the grasp is antipodal. + * \return true if the grasp is antipodal, false otherwise + */ + bool isFullAntipodal() const { return label_.full_antipodal_; } + + /** + * \brief Return the position of the grasp. + * \return the grasp position + */ + const Eigen::Vector3d &getPosition() const { return position_; } + + /** + * \brief Return the orientation of the grasp. + * \return the grasp orientation (rotation matrix) + */ + const Eigen::Matrix3d &getOrientation() const { return orientation_; } + + /** + * \brief Return the width of the object contained in the grasp. + * \return the width of the object contained in the grasp + */ + double getGraspWidth() const { return grasp_width_; } + + /** + * \brief Return whether the grasp is indeterminate. + * \return true if the grasp is indeterminate, false otherwise + */ + bool isHalfAntipodal() const { return label_.half_antipodal_; } + + /** + * \brief Set whether the grasp is antipodal. + * \param b whether the grasp is antipodal + */ + void setFullAntipodal(bool b) { label_.full_antipodal_ = b; } + + /** + * \brief Set whether the grasp is indeterminate. + * \param b whether the grasp is indeterminate + */ + void setHalfAntipodal(bool b) { label_.half_antipodal_ = b; } + + /** + * \brief Set the width of the object contained in the grasp. + * \param w the width of the object contained in the grasp + */ + void setGraspWidth(double w) { grasp_width_ = w; } + + /** + * \brief Set the position of the grasp. + * \param position the grasp position + */ + void setPosition(const Eigen::Vector3d &position) { position_ = position; } + + /** + * \brief Get the score of the grasp. + * \return the score + */ + double getScore() const { return label_.score_; } + + /** + * \brief Set the score of the grasp. + * \param score the score + */ + void setScore(double score) { label_.score_ = score; } + + /** + * \brief Return the center of the point neighborhood associated with the + * grasp. + * \return the center + */ + const Eigen::Vector3d &getSample() const { return sample_; } + + /** + * \brief Return the grasp orientation. + * \return the orientation of the grasp as a rotation matrix + */ + const Eigen::Matrix3d &getFrame() const { return orientation_; } + + /** + * \brief Return the center coordinate of the hand closing region along the + * closing direction/axis of the robot hand. + */ + double getCenter() const { return closing_box_.center_; } + + /** + * \brief Return the bottom coordinate of the hand closing region along the + * closing direction/axis of the robot hand.. + */ + double getBottom() const { return closing_box_.bottom_; } + + /** + * \brief Return the top coordinate of the hand closing region along the + * closing direction/axis of the robot hand. + */ + double getTop() const { return closing_box_.top_; } + + /** + * \brief Return the index of the finger placement. + * \return the index of the finger placement + */ + int getFingerPlacementIndex() const { return finger_placement_index_; } + + private: + /** + * \brief Calculate grasp positions (bottom, top, surface). + * \param finger_hand the FingerHand object describing a feasible finger + * placement + */ + void calculateGraspPositions(const FingerHand &finger_hand); + + /** + * \brief Convert an Eigen vector to a string. + * \param v the vector + * \return the string + */ + std::string vectorToString(const Eigen::VectorXd &v) const; + + Eigen::Vector3d position_; ///< grasp position (bottom center of robot hand) + Eigen::Matrix3d orientation_; ///< grasp orientation (rotation of robot hand) + + Eigen::Vector3d sample_; ///< the sample at which the grasp was found + double grasp_width_; ///< the width of object enclosed by the fingers + + Label label_; ///< labeling information + int finger_placement_index_; ///< index of the finger placement that resulted + /// in this grasp + BoundingBox closing_box_; ///< defines region surrounded by fingers +}; + +} // namespace candidate +} // namespace gpd + +#endif /* HAND_H_ */ diff --git a/include/gpd/candidate/hand_geometry.h b/include/gpd/candidate/hand_geometry.h new file mode 100644 index 0000000..b63eb92 --- /dev/null +++ b/include/gpd/candidate/hand_geometry.h @@ -0,0 +1,87 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, Andreas ten Pas + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * 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. + */ + +#ifndef HAND_GEOMETRY_H_ +#define HAND_GEOMETRY_H_ + +#include + +namespace gpd { +namespace candidate { + +/** + * + * \brief Store robot hand geometry + * + * This class stores parameters which define the geometry of the robot hand. + * This geometry is used to calculate the grasp candidates. + * + */ +class HandGeometry { + public: + /** + * \brief Constructor. + */ + HandGeometry(); + + /** + * \brief Constructor. + * \param finger_width the width of a robot finger + * \param outer_diameter the width of the robot hand (including fingers) + * \param hand_depth the hand depth (length of finger) + * \param hand_height the hand height: the hand extends plus/minus this + * along the hand axis + * \param init_bite the minimum object depth to be covered by the fingers + */ + HandGeometry(double finger_width, double outer_diameter, double hand_depth, + double hand_height, double init_bite); + + /** + * \brief Constructor that uses a given configuration file to read in the + * parameters of the robot hand. + * \param filepath the filepath to the configuration file + */ + HandGeometry(const std::string &filepath); + + double finger_width_; ///< the width of the robot fingers + double outer_diameter_; ///< the width of the robot hand including fingers + double depth_; ///< the hand depth (length of fingers) + double height_; ///< the hand extends plus/minus this along the hand axis + double init_bite_; ///< the minimum object depth to be covered by the fingers +}; + +std::ostream &operator<<(std::ostream &stream, + const HandGeometry &hand_geometry); + +} // namespace candidate +} // namespace gpd + +#endif /* HAND_GEOMETRY_H_ */ diff --git a/include/gpd/candidate/hand_search.h b/include/gpd/candidate/hand_search.h new file mode 100644 index 0000000..f80ecab --- /dev/null +++ b/include/gpd/candidate/hand_search.h @@ -0,0 +1,198 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, Andreas ten Pas + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * 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. + */ + +#ifndef HAND_SEARCH_H +#define HAND_SEARCH_H + +#include + +#include +#include +#include +#include + +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace gpd { +namespace candidate { + +typedef pcl::PointCloud PointCloudRGB; + +/** + * + * \brief Search for grasp candidates. + * + * This class searches for grasp candidates in a point cloud by first + * calculating a local reference frame (LRF) for a point neighborhood, and then + * finding geometrically feasible robot hand placements. Each feasible grasp + * candidate is checked for mechanical stability (antipodal grasp or not). + * + */ +class HandSearch { + public: + /** + * \brief Parameters for the hand search. + */ + struct Parameters { + /** LRF estimation parameters */ + double nn_radius_frames_; ///< radius for point neighborhood search for LRF + + /** grasp candidate generation */ + int num_threads_; ///< the number of CPU threads to be used + int num_samples_; ///< the number of samples to be used + int num_orientations_; ///< number of hand orientations to evaluate + int num_finger_placements_; ///< number of finger placements to evaluate + std::vector hand_axes_; ///< the axes about which different hand + /// orientations are generated + bool deepen_hand_; ///< if the hand is pushed forward onto the object + + /** antipodal grasp check */ + double friction_coeff_; ///< angle of friction cone in degrees + int min_viable_; ///< minimum number of points required to be antipodal + + HandGeometry hand_geometry_; ///< robot hand geometry + }; + + /** + * \brief Constructor. + * \param params Parameters for the hand search + */ + HandSearch(Parameters params); + + /** + * \brief Search robot hand configurations. + * \param cloud the point cloud + * \return list of grasp candidate sets + */ + std::vector> searchHands( + const util::Cloud &cloud) const; + + /** + * \brief Reevaluate a list of grasp candidates. + * \note Used to calculate ground truth. + * \param cloud_cam the point cloud + * \param grasps the list of grasp candidates + * \return the list of reevaluated grasp candidates + */ + std::vector reevaluateHypotheses( + const util::Cloud &cloud_cam, + std::vector> &grasps, + bool plot_samples = false) const; + + /** + * \brief Return the parameters for the hand search. + * \return params the hand search parameters + */ + const Parameters &getParams() const { return params_; } + + /** + * \brief Set the parameters for the hand search. + * \param params the parameters + */ + void setParameters(const Parameters ¶ms) { params_ = params; } + + private: + /** + * \brief Search robot hand configurations given a list of local reference + * frames. + * \param cloud_cam the point cloud + * \param frames the list of local reference frames + * \param kdtree the KDTree object used for fast neighborhood search + * \return the list of robot hand configurations + */ + std::vector> evalHands( + const util::Cloud &cloud_cam, + const std::vector &frames, + const pcl::KdTreeFLANN &kdtree) const; + + /** + * \brief Reevaluate a grasp candidate. + * \param point_list the point neighborhood associated with the grasp + * \param hand the grasp + * \param finger_hand the FingerHand object that describes a valid finger + * placement + * \param point_list_cropped the point neigborhood transformed into the hand + * frame + */ + bool reevaluateHypothesis(const util::PointList &point_list, + const candidate::Hand &hand, + FingerHand &finger_hand, + util::PointList &point_list_cropped) const; + + /** + * \brief Calculate the label for a grasp candidate. + * \param point_list the point neighborhood associated with the grasp + * \param finger_hand the FingerHand object that describes a valid finger + * placement + * \return the label + */ + int labelHypothesis(const util::PointList &point_list, + FingerHand &finger_hand) const; + + /** + * \brief Convert an Eigen::Vector3d object to a pcl::PointXYZRGBA. + * \param v the Eigen vector + * \reutrn the pcl point + */ + pcl::PointXYZRGBA eigenVectorToPcl(const Eigen::Vector3d &v) const; + + Parameters params_; ///< parameters for the hand search + + double nn_radius_; ///< radius for nearest neighbors search + + std::unique_ptr antipodal_; + std::unique_ptr plot_; + + /** plotting parameters (optional, not read in from config file) **/ + bool plots_local_axes_; ///< if the LRFs are plotted + + /** constants for rotation axis */ + static const int ROTATION_AXIS_NORMAL; ///< normal axis of LRF + static const int ROTATION_AXIS_BINORMAL; ///< binormal axis of LRF + static const int ROTATION_AXIS_CURVATURE_AXIS; ///< curvature axis of LRF +}; + +} // namespace candidate +} // namespace gpd + +#endif /* HAND_SEARCH_H */ diff --git a/include/gpd/candidate/hand_set.h b/include/gpd/candidate/hand_set.h new file mode 100644 index 0000000..654abe4 --- /dev/null +++ b/include/gpd/candidate/hand_set.h @@ -0,0 +1,300 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, Andreas ten Pas + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * 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. + */ + +#ifndef HAND_SET_H_ +#define HAND_SET_H_ + +// System +#include +#include + +// Boost +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// Eigen +#include +#include + +// Custom +#include +#include +#include +#include +#include +#include +#include + +// The hash and equality functions below are necessary for boost's unordered +// set. +namespace boost { +template <> +struct hash { + inline size_t operator()(Eigen::Vector3i const &v) const { + std::size_t seed = 0; + + for (int i = 0; i < v.size(); i++) { + boost::hash_combine(seed, v(i)); + } + + return seed; + } +}; +} // namespace boost + +namespace gpd { +namespace candidate { + +struct Vector3iEqual { + inline bool operator()(const Eigen::Vector3i &a, + const Eigen::Vector3i &b) const { + return a(0) == b(0) && a(1) == b(1) && a(2) == b(2); + } +}; + +typedef boost::unordered_set, + Vector3iEqual, std::allocator> + Vector3iSet; + +/** + * + * \brief Calculate a set of grasp candidates. + * + * This class calculate sets of grasp candidates. The grasp candidates in the + * same set share the same point neighborhood and the same local reference + * frame (LRF). The grasps are calculated by a local search over discretized + * orientations and positions. + * + * \note For details, check out: Andreas ten Pas, Marcus Gualtieri, Kate + * Saenko, and Robert Platt. Grasp Pose Detection in Point Clouds. The + * International Journal of Robotics Research, Vol 36, Issue 13-14, + * pp. 1455 - 1473. October 2017. https://arxiv.org/abs/1706.09911 + * + */ +class HandSet { + public: + /** + * Constructor. + */ + // HandSet(); + + /** + * \brief Constructor. + * \param hand_geometry the hand geometry parameters + * \param angles the angles to be evaluated + * \param hand_axes the hand axes about which to rotate + * \param num_finger_placements the number of finger placements + * \param deepen_hand if the hand is pushed forward onto the object + */ + HandSet(const HandGeometry &hand_geometry, const Eigen::VectorXd &angles, + const std::vector &hand_axes, int num_finger_placements, + bool deepen_hand, Antipodal &antipodal); + + /** + * \brief Calculate a set of grasp candidates given a local reference frame. + * \param point_list the point neighborhood + * \param local_frame the local reference frame + */ + void evalHandSet(const util::PointList &point_list, + const LocalFrame &local_frame); + + /** + * \brief Calculate grasp candidates for a given rotation axis. + * \param point_list the point neighborhood + * \param local_frame the local reference frame + * \param axis the index of the rotation axis + * \param start the index of the first free element in `hands_` + */ + void evalHands(const util::PointList &point_list, + const LocalFrame &local_frame, int axis, int start); + + /** + * \brief Calculate the "shadow" of the point neighborhood. + * \param point_list the point neighborhood + * \param shadow_length the length of the shadow + */ + Eigen::Matrix3Xd calculateShadow(const util::PointList &point_list, + double shadow_length) const; + + /** + * \brief Return the grasps contained in this grasp set. + * \return the grasps contained in this grasp set + */ + const std::vector> &getHands() const { return hands_; } + + /** + * \brief Return the grasps contained in this grasp set (mutable). + * \return the grasps contained in this grasp set + */ + std::vector> &getHands() { return hands_; } + + /** + * \brief Return the center of the point neighborhood. + * \return the center of the point neighborhood + */ + const Eigen::Vector3d &getSample() const { return sample_; } + + /** + * \brief Return the local reference frame. + * \return the local reference frame (3 x 3 rotation matrix) + */ + const Eigen::Matrix3d &getFrame() const { return frame_; } + + /** + * \brief Set the center of the point neighborhood. + * \param sample the center of the point neighborhood + */ + void setSample(const Eigen::Vector3d &sample) { sample_ = sample; } + + /** + * \brief Return a list of booleans that indicate for each grasp if it is + * valid or not. + * \return the list of booleans + */ + const Eigen::Array &getIsValid() const { + return is_valid_; + } + + /** + * \brief Set, for each grasp, if it is valid or not. + * \param isValid the list of booleans which indicate if each grasp is valid + * or not + */ + void setIsValid(const Eigen::Array &isValid) { + is_valid_ = isValid; + } + + /** + * \brief Set a single grasp to be valid or not. + * \param idx the index of the grasp + * \param val true if the grasp is valid, false if it is not + */ + void setIsValidWithIndex(int idx, bool val) { is_valid_[idx] = val; } + + private: + /** + * \brief Calculate shadow for one camera. + * \param[in] points the list of points for which the shadow is calculated + * \param[in] shadow_vec the vector that describes the direction of the shadow + * relative to the camera origin + * \param[in] num_shadow_points the number of shadow points to be calculated + * \param[in] voxel_grid_size the size of the voxel grid + * \param[out] shadow_set the set of shadow points + */ + void calculateShadowForCamera(const Eigen::Matrix3Xd &points, + const Eigen::Vector3d &shadow_vec, + int num_shadow_points, double voxel_grid_size, + Vector3iSet &shadow_set) const; + + /** + * \brief Modify a grasp candidate. + * \param hand the grasp candidate to be modified + * \param point_list the point neighborhood + * \param indices the indices of the points in the hand closing region + * \param finger_hand the FingerHand object that describes valid finger + * placements + * \return the modified grasp candidate + */ + void modifyCandidate(Hand &hand, const util::PointList &point_list, + const std::vector &indices, + const FingerHand &finger_hand) const; + + /** + * \brief Label a grasp candidate as a viable grasp or not. + * \param point_list the point neighborhood associated with the grasp + * \param finger_hand the FingerHand object that describes valid finger + * placements + * \param hand the grasp + */ + void labelHypothesis(const util::PointList &point_list, + const FingerHand &finger_hand, Hand &hand) const; + + /** + * \brief Convert shadow voxels to shadow points. + * \param voxels the shadow voxels + * \param voxel_grid_size the size of the voxel grid + * \return the shadow points + */ + Eigen::Matrix3Xd shadowVoxelsToPoints( + const std::vector &voxels, double voxel_grid_size) const; + + /** + * \brief Calculate the intersection of two shadows. + * \param set1 the first shadow + * \param set2 the second shadow + * \return the intersection + */ + Vector3iSet intersection(const Vector3iSet &set1, + const Vector3iSet &set2) const; + + /** + * \brief Generate a random integer. + * \note Source: + * http://software.intel.com/en-us/articles/fast-random-number-generator-on- + * the-intel-pentiumr-4-processor/ + */ + inline int fastrand() const; + + Eigen::Vector3d sample_; ///< the center of the point neighborhood + Eigen::Matrix3d frame_; ///< the local reference frame + std::vector> + hands_; ///< the grasp candidates contained in this set + Eigen::Array + is_valid_; ///< indicates for each grasp candidate if it is valid or not + Eigen::VectorXd + angles_; ///< the hand orientations to consider in the local search + bool deepen_hand_; ///< if the hand is pushed forward onto the object + int num_finger_placements_; ///< the number of finger placements to evaluate + + HandGeometry hand_geometry_; ///< the robot hand geometry + std::vector hand_axes_; ///< the axes about which the hand frame is + /// rotated to evaluate different orientations + + Antipodal &antipodal_; + + static int seed_; ///< seed for the random generator in fastrand() + + static const Eigen::Vector3d AXES[3]; ///< standard rotation axes + + static const bool MEASURE_TIME; ///< if runtime is measured +}; + +} // namespace candidate +} // namespace gpd + +#endif /* HAND_SET_H_ */ diff --git a/include/gpd/candidate/local_frame.h b/include/gpd/candidate/local_frame.h new file mode 100644 index 0000000..21a851d --- /dev/null +++ b/include/gpd/candidate/local_frame.h @@ -0,0 +1,111 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, Andreas ten Pas + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * 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. + */ + +#ifndef LOCAL_FRAME_H +#define LOCAL_FRAME_H + +#include +#include +#include + +namespace gpd { +namespace candidate { + +/** + * + * \brief Local reference frame. + * + * This class estimates the local reference frame (LRF) for a point + * neighborhood. The coordinate axes of the local frame are the normal, + * binormal, and curvature axis. + */ +class LocalFrame { + public: + /** + * \brief Constructor. + * \param T_cams the camera poses + * \param sample the sample for which the point neighborhood was found + */ + LocalFrame(const Eigen::Vector3d &sample) : sample_(sample) {} + + /** + * \brief Estimate the average normal axis for the point neighborhood. + * \param normals the 3xn matrix of normals found for points in the point + * neighborhood + */ + void findAverageNormalAxis(const Eigen::MatrixXd &normals); + + /** + * \brief Print a description of the local reference frame. + */ + void print(); + + /** + * \brief Return the sample for which the point neighborhood was found. + * \return the 3x1 sample for which the point neighborhood was found + */ + const Eigen::Vector3d &getSample() const { return sample_; } + + /** + * \brief Return the binormal. + * \return the 3x1 binormal vector + */ + const Eigen::Vector3d &getBinormal() const { return binormal_; } + + /** + * \brief Return the curvature axis. + * \return the 3x1 curvature axis vector + */ + const Eigen::Vector3d &getCurvatureAxis() const { return curvature_axis_; } + + /** + * \brief Return the normal. + * \return the 3x1 normal vector + */ + const Eigen::Vector3d &getNormal() const { return normal_; } + + /** + * \brief Set the sample for the point neighborhood. + * \param sample the sample to be used + */ + void setSample(const Eigen::Vector3d &sample) { sample_ = sample; } + + private: + Eigen::Vector3d sample_; + Eigen::Vector3d curvature_axis_; + Eigen::Vector3d normal_; + Eigen::Vector3d binormal_; +}; + +} // namespace candidate +} // namespace gpd + +#endif // LOCAL_FRAME_H_ diff --git a/include/gpd/clustering.h b/include/gpd/clustering.h index 9754ee0..ac9290e 100644 --- a/include/gpd/clustering.h +++ b/include/gpd/clustering.h @@ -29,55 +29,62 @@ * POSSIBILITY OF SUCH DAMAGE. */ - #ifndef CLUSTERING_H_ #define CLUSTERING_H_ - #include +#include #include #include -#include +#include +namespace gpd { -/** Clustering class +/** * * \brief Group grasp candidates in clusters - * - * This class searches for clusters of grasp candidates. Grasps in the same cluster are geometrically aligned. - * + * + * This class searches for clusters of grasps. Grasps in the same cluster are + * geometrically similar. + * */ -class Clustering -{ -public: - +class Clustering { + public: /** * \brief Constructor. - * \param min_inliers the minimum number of grasps a cluster is required to have + * \param min_inliers the minimum number of grasps a cluster is required to + * contain */ - Clustering(int min_inliers) : min_inliers_(min_inliers) { }; + Clustering(int min_inliers) : min_inliers_(min_inliers){}; /** - * \brief Search for handles given a list of grasp hypotheses. - * \param hand_list the list of grasp hypotheses + * \brief Search for clusters given a list of grasps. + * \param hand_list the list of grasps + * \param remove_inliers if grasps already assigned to a cluster are ignored + * for the next cluster */ - std::vector findClusters(const std::vector& hand_list, bool remove_inliers = false); - - int getMinInliers() const - { - return min_inliers_; - } - - void setMinInliers(int min_inliers) - { - min_inliers_ = min_inliers; - } + std::vector> findClusters( + const std::vector> &hand_list, + bool remove_inliers = false); + /** + * \brief Return the minimum number of cluster inliers. + * \return the minimum number of cluster inliers + */ + int getMinInliers() const { return min_inliers_; } -private: + /** + * \brief Set the minimum number of cluster inliers. + * \param min_inliers the minimum number of cluster inliers + */ + void setMinInliers(int min_inliers) { min_inliers_ = min_inliers; } - int min_inliers_; ///< minimum number of geometrically aligned candidates required to form a cluster + private: + int min_inliers_; ///< minimum number of geometrically aligned candidates + /// required to form a cluster }; +} // namespace gpd + #endif /* CLUSTERING_H_ */ diff --git a/include/gpd/data_generator.h b/include/gpd/data_generator.h index b65acff..c00d7a7 100644 --- a/include/gpd/data_generator.h +++ b/include/gpd/data_generator.h @@ -1,139 +1,193 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Andreas ten Pas + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * 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. + */ + #ifndef DATA_GENERATOR_H_ #define DATA_GENERATOR_H_ +#include +#include -// Caffe -#include -#include +#include // OpenCV +#include +#include +#include #include -// ROS -#include +// PCL +#include + +// Grasp Pose Generator +#include +#include // Custom -#include -#include +#include -#include "../gpd/learning.h" +namespace gpd { +typedef pcl::PointCloud PointCloudRGB; -struct Instance -{ - cv::Mat image_; +struct Instance { + std::unique_ptr image_; bool label_; - Instance(const cv::Mat& image, bool label) : image_(image), label_(label) { } + Instance(std::unique_ptr image, bool label) + : image_(std::move(image)), label_(label) {} }; - -class DataGenerator -{ - public: - - /** - * \brief Constructor. - * \param node ROS node handle - */ - DataGenerator(ros::NodeHandle& node); - - /** - * \brief Destructor. - */ - ~DataGenerator(); - - /** - * \brief Create training data. - * \param node ROS node handle - */ - void createTrainingData(ros::NodeHandle& node); - - - private: - - /** - * \brief Create a grasp candidates generator. - * \param node ROS node handle - * \return the grasp candidates generator - */ - CandidatesGenerator* createCandidatesGenerator(ros::NodeHandle& node); - - /** - * \brief Create a learning object to generate grasp images. - * \param node ROS node handle - * \return the learning object - */ - Learning* createLearning(ros::NodeHandle& node); - - /** - * \brief Create grasp images. - * \param node ROS node handle - * \return the grasp candidates generator - */ - bool createGraspImages(CloudCamera& cloud_cam, std::vector& grasps_out, std::vector& images_out); - - /** - * \brief Load a point cloud given ROS launch parameters. - * \param node ROS node handle - * \return the point cloud - */ - CloudCamera loadCloudCameraFromFile(ros::NodeHandle& node); - - /** - * \brief Load a point cloud and surface normals given ROS launch parameters. - * \param mesh_file_path location of the point cloud file - * \param normals_file_path location of the surface normals file - * \return the point cloud with surface normals - */ - CloudCamera loadMesh(const std::string& mesh_file_path, const std::string& normals_file_path); - - /** - * \brief Load all point cloud files from a folder. - * \param cloud_folder location of the folder - * \return list of point cloud files - */ - std::vector loadPointCloudFiles(const std::string& cloud_folder); - - /** - * \brief Load object names from a file. - * \param objects_file_location location of the file - * \return list of object names - */ - std::vector loadObjectNames(const std::string& objects_file_location); - - /** - * \brief Balance the number of positive and negative examples. - * \param max_grasps_per_view maximum number of examples per camera view - * \param positives_in positive examples - * \param negatives_in negative examples - * \param[out] positives_out positive examples after balancing - * \param[out] negatives_out negative examples after balancing - */ - void balanceInstances(int max_grasps_per_view, const std::vector& positives_in, - const std::vector& negatives_in, std::vector& positives_out, std::vector& negatives_out); - - /** - * \brief Add examples to the dataset. - * \param grasps grasp candidates - * \param images grasp images - * \param positives indices of positive/viable grasps - * \param negatives indices of negative/non-viable grasps - * \param dataset the dataset that the examples are added to - */ - void addInstances(const std::vector& grasps, const std::vector& images, - const std::vector& positives, const std::vector& negatives, std::vector& dataset); - - /** - * \brief Store the dataset as an LMDB. - * \param dataset the dataset - * \param file_location location where the LMDB is stored - */ - void storeLMDB(const std::vector& dataset, const std::string& file_location); - - CandidatesGenerator* candidates_generator_; ///< object to generate grasp candidates - Learning* learning_; ///< object to generate grasp images +class DataGenerator { + public: + /** + * \brief Constructor. + * \param node ROS node handle + */ + DataGenerator(const std::string &config_filename); + + void generateDataBigbird(); + + /** + * \brief Create training data. + * \param node ROS node handle + */ + void generateData(); + + util::Cloud createMultiViewCloud(const std::string &object, int camera, + const std::vector angles, + int reference_camera) const; + + private: + void createDatasetsHDF5(const std::string &filepath, int num_data); + + void reshapeHDF5(const std::string &in, const std::string &out, + const std::string &dataset, int num, int chunk_size, + int max_in_memory); + + /** + * \brief Load a point cloud and surface normals given ROS launch parameters. + * \param mesh_file_path location of the point cloud file + * \param normals_file_path location of the surface normals file + * \return the point cloud with surface normals + */ + util::Cloud loadMesh(const std::string &mesh_file_path, + const std::string &normals_file_path); + + /** + * \brief Load object names from a file. + * \param objects_file_location location of the file + * \return list of object names + */ + std::vector loadObjectNames( + const std::string &objects_file_location); + + void splitInstances(const std::vector &labels, + std::vector &positives, std::vector &negatives); + + /** + * \brief Balance the number of positive and negative examples. + * \param max_grasps_per_view maximum number of examples per camera view + * \param positives_in positive examples + * \param negatives_in negative examples + * \param[out] positives_out positive examples after balancing + * \param[out] negatives_out negative examples after balancing + */ + void balanceInstances(int max_grasps_per_view, + const std::vector &positives_in, + const std::vector &negatives_in, + std::vector &positives_out, + std::vector &negatives_out); + + /** + * \brief Add examples to the dataset. + * \param grasps grasp candidates + * \param images grasp images + * \param positives indices of positive/viable grasps + * \param negatives indices of negative/non-viable grasps + * \param dataset the dataset that the examples are added to + */ + void addInstances(const std::vector> &grasps, + std::vector> &images, + const std::vector &positives, + const std::vector &negatives, + std::vector &dataset); + + int insertIntoHDF5(const std::string &file_path, + const std::vector &dataset, int offset); + + /** + * \brief Store the dataset as an HDF5 file. + * \param dataset the dataset + * \param file_location location where the LMDB is stored + */ + void storeHDF5(const std::vector &dataset, + const std::string &file_location); + + void printMatrix(const cv::Mat &mat); + + void printMatrix15(const cv::Mat &mat); + + void copyMatrix(const cv::Mat &src, cv::Mat &dst, int idx_in, int *dims_img); + + Eigen::Matrix4f calculateTransform(const std::string &object, int camera, + int angle, int reference_camera) const; + + Eigen::Matrix4f readPoseFromHDF5(const std::string &hdf5_filename, + const std::string &dsname) const; + + std::unique_ptr + detector_; ///< object to generate grasp candidates and images + + std::string data_root_; + std::string objects_file_location_; + std::string output_root_; + int num_views_per_object_; + int min_grasps_per_view_; + int max_grasps_per_view_; + int chunk_size_; + int max_in_memory_; + int num_threads_; + int num_samples_; + double voxel_size_views_; + double normals_radius_; + bool remove_nans_; + bool reverse_mesh_normals_; + bool reverse_view_normals_; + std::vector test_views_; + std::vector all_cam_sources_; + + static const std::string IMAGES_DS_NAME; + static const std::string LABELS_DS_NAME; }; +} // namespace gpd #endif /* GENERATOR_H_ */ diff --git a/include/gpd/descriptor/image_12_channels_strategy.h b/include/gpd/descriptor/image_12_channels_strategy.h new file mode 100644 index 0000000..63fccd5 --- /dev/null +++ b/include/gpd/descriptor/image_12_channels_strategy.h @@ -0,0 +1,96 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, Andreas ten Pas + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * 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. + */ + +#ifndef IMAGE_12_CHANNELS_STRATEGY_H_ +#define IMAGE_12_CHANNELS_STRATEGY_H_ + +#include + +#include +#include + +#include +#include + +namespace gpd { +namespace descriptor { + +/** + * + * \brief Calculate 12-channels grasp image. + * + * The 12 channels contain height maps and surface normals of the points + * contained inside the robot hand's closing region from three different views. + * + */ +class Image12ChannelsStrategy : public ImageStrategy { + public: + /** + * \brief Create a strategy for calculating grasp images. + * \param image_params the grasp image parameters + * \param num_threads the number of CPU threads to be used + * \param num_orientations the number of robot hand orientations + * \param is_plotting if the images are visualized + * \return the strategy for calculating grasp images + */ + Image12ChannelsStrategy(const ImageGeometry &image_params, int num_threads, + int num_orientations, bool is_plotting) + : ImageStrategy(image_params, num_threads, num_orientations, + is_plotting) {} + + /** + * \brief Create grasp images given a list of grasp candidates. + * \param hand_set the grasp candidates + * \param nn_points the point neighborhoods used to calculate the images + * \return the grasp images + */ + std::vector> createImages( + const candidate::HandSet &hand_set, + const util::PointList &nn_points) const; + + protected: + void createImage(const util::PointList &point_list, + const candidate::Hand &hand, cv::Mat &image) const; + + private: + cv::Mat calculateImage(const Eigen::Matrix3Xd &points, + const Eigen::Matrix3Xd &normals) const; + + std::vector calculateChannels(const Eigen::Matrix3Xd &points, + const Eigen::Matrix3Xd &normals) const; + + void showImage(const cv::Mat &image) const; +}; + +} // namespace descriptor +} // namespace gpd + +#endif /* IMAGE_12_CHANNELS_STRATEGY_H_ */ diff --git a/include/gpd/descriptor/image_15_channels_strategy.h b/include/gpd/descriptor/image_15_channels_strategy.h new file mode 100644 index 0000000..cb375e9 --- /dev/null +++ b/include/gpd/descriptor/image_15_channels_strategy.h @@ -0,0 +1,110 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, Andreas ten Pas + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * 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. + */ + +#ifndef IMAGE_15_CHANNELS_STRATEGY_H_ +#define IMAGE_15_CHANNELS_STRATEGY_H_ + +#include + +#include +#include + +#include +#include + +namespace gpd { +namespace descriptor { + +/** + * + * \brief Calculate 15-channels grasp image. + * + * The 15 channels contain height maps and surface normals of the points + * contained inside the robot hand's closing region from three different views + * like in the 12-channels image. The three additional channels contain the + * "shadow" of the points. + * + */ +class Image15ChannelsStrategy : public ImageStrategy { + public: + /** + * \brief Create a strategy for calculating grasp images. + * \param image_params the grasp image parameters + * \param num_threads the number of CPU threads to be used + * \param num_orientations the number of robot hand orientations + * \param is_plotting if the images are visualized + * \return the strategy for calculating grasp images + */ + Image15ChannelsStrategy(const ImageGeometry &image_params, int num_threads, + int num_orientations, bool is_plotting) + : ImageStrategy(image_params, num_threads, num_orientations, + is_plotting) { + // Calculate shadow length (= max length of shadow required to fill image + // window). + Eigen::Vector3d image_dims; + image_dims << image_params_.depth_, image_params_.height_ / 2.0, + image_params_.outer_diameter_; + shadow_length_ = image_dims.maxCoeff(); + } + + /** + * \brief Create grasp images given a list of grasp candidates. + * \param hand_set the grasp candidates + * \param nn_points the point neighborhoods used to calculate the images + * \return the grasp images + */ + std::vector> createImages( + const candidate::HandSet &hand_set, + const util::PointList &nn_points) const; + + protected: + void createImage(const util::PointList &point_list, + const candidate::Hand &hand, const Eigen::Matrix3Xd &shadow, + cv::Mat &image) const; + + private: + cv::Mat calculateImage(const Eigen::Matrix3Xd &points, + const Eigen::Matrix3Xd &normals, + const Eigen::Matrix3Xd &shadow) const; + + std::vector calculateChannels(const Eigen::Matrix3Xd &points, + const Eigen::Matrix3Xd &normals, + const Eigen::Matrix3Xd &shadow) const; + + void showImage(const cv::Mat &image) const; + + double shadow_length_; +}; + +} // namespace descriptor +} // namespace gpd + +#endif /* IMAGE_15_CHANNELS_STRATEGY_H_ */ diff --git a/include/gpd/descriptor/image_1_channels_strategy.h b/include/gpd/descriptor/image_1_channels_strategy.h new file mode 100644 index 0000000..34956c6 --- /dev/null +++ b/include/gpd/descriptor/image_1_channels_strategy.h @@ -0,0 +1,85 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, Andreas ten Pas + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * 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. + */ + +#ifndef IMAGE_1_CHANNELS_STRATEGY_H_ +#define IMAGE_1_CHANNELS_STRATEGY_H_ + +#include +#include + +#include +#include + +namespace gpd { +namespace descriptor { + +/** + * + * \brief Calculate binary grasp image. + * + * The binary image represents the shape of what is contained inside the robot + * hand's closing region. + * + */ +class Image1ChannelsStrategy : public ImageStrategy { + public: + /** + * \brief Create a strategy for calculating grasp images. + * \param image_params the grasp image parameters + * \param num_threads the number of CPU threads to be used + * \param num_orientations the number of robot hand orientations + * \param is_plotting if the images are visualized + * \return the strategy for calculating grasp images + */ + Image1ChannelsStrategy(const ImageGeometry &image_params, int num_threads, + int num_orientations, bool is_plotting) + : ImageStrategy(image_params, num_threads, num_orientations, + is_plotting) {} + + /** + * \brief Create grasp images given a list of grasp candidates. + * \param hand_set the grasp candidates + * \param nn_points the point neighborhoods used to calculate the images + * \return the grasp images + */ + std::vector> createImages( + const candidate::HandSet &hand_set, + const util::PointList &nn_points) const; + + protected: + void createImage(const util::PointList &point_list, + const candidate::Hand &hand, cv::Mat &image) const; +}; + +} // namespace descriptor +} // namespace gpd + +#endif /* IMAGE_1_CHANNELS_STRATEGY_H_ */ diff --git a/include/gpd/descriptor/image_3_channels_strategy.h b/include/gpd/descriptor/image_3_channels_strategy.h new file mode 100644 index 0000000..4161bed --- /dev/null +++ b/include/gpd/descriptor/image_3_channels_strategy.h @@ -0,0 +1,85 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, Andreas ten Pas + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * 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. + */ + +#ifndef IMAGE_3_CHANNELS_STRATEGY_H_ +#define IMAGE_3_CHANNELS_STRATEGY_H_ + +#include +#include + +#include +#include + +namespace gpd { +namespace descriptor { + +/** + * + * \brief Calculate 3-channels grasp image. + * + * The 3 channels contain the surface normals of the points contained inside + * the robot hand's closing region from three different views + * + */ +class Image3ChannelsStrategy : public ImageStrategy { + public: + /** + * \brief Create a strategy for calculating grasp images. + * \param image_params the grasp image parameters + * \param num_threads the number of CPU threads to be used + * \param num_orientations the number of robot hand orientations + * \param is_plotting if the images are visualized + * \return the strategy for calculating grasp images + */ + Image3ChannelsStrategy(const ImageGeometry &image_params, int num_threads, + int num_orientations, bool is_plotting) + : ImageStrategy(image_params, num_threads, num_orientations, + is_plotting) {} + + /** + * \brief Create grasp images given a list of grasp candidates. + * \param hand_set the grasp candidates + * \param nn_points the point neighborhoods used to calculate the images + * \return the grasp images + */ + std::vector> createImages( + const candidate::HandSet &hand_set, + const util::PointList &nn_points) const; + + protected: + void createImage(const util::PointList &point_list, + const candidate::Hand &hand, cv::Mat &image) const; +}; + +} // namespace descriptor +} // namespace gpd + +#endif /* IMAGE_3_CHANNELS_STRATEGY_H_ */ diff --git a/include/gpd/descriptor/image_generator.h b/include/gpd/descriptor/image_generator.h new file mode 100644 index 0000000..e8d979b --- /dev/null +++ b/include/gpd/descriptor/image_generator.h @@ -0,0 +1,134 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, Andreas ten Pas + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * 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. + */ + +#ifndef IMAGE_GENERATOR_H_ +#define IMAGE_GENERATOR_H_ + +#include +#include +#include +#include +#include +#include + +#include +#include + +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +typedef std::pair Matrix3XdPair; +typedef pcl::PointCloud PointCloudRGBA; + +namespace gpd { +namespace descriptor { + +/** + * + * \brief Create grasp images for classification. + * + * Creates images for the input layer of a convolutional neural network. Each + * image represents a grasp candidate. We call these "grasp images". + * + */ +class ImageGenerator { + public: + /** + * \brief Constructor. + * \param params parameters for grasp images + * \param num_threads number of CPU threads to be used + * \param is_plotting if images are visualized + * \param remove_plane if the support/table plane is removed before + * calculating images + */ + ImageGenerator(const descriptor::ImageGeometry &image_geometry, + int num_threads, int num_orientations, bool is_plotting, + bool remove_plane); + + /** + * \brief Create a list of grasp images for a given list of grasp candidates. + * \param cloud_cam the point cloud + * \param hand_set_list the list of grasp candidates + * \return the list of grasp images + */ + void createImages( + const util::Cloud &cloud_cam, + const std::vector> &hand_set_list, + std::vector> &images_out, + std::vector> &hands_out) const; + + /** + * \brief Return the parameters of the grasp image. + * \return the grasp image parameters + */ + const descriptor::ImageGeometry &getImageGeometry() const { + return image_params_; + } + + private: + /** + * \brief Remove the plane from the point cloud. Sets to all + * non-planar points if the plane is found, otherwise has the + * same points as . + * \param cloud the cloud + * \param point_list the list of points corresponding to the cloud + */ + void removePlane(const util::Cloud &cloud_cam, + util::PointList &point_list) const; + + void createImageList( + const std::vector> &hand_set_list, + const std::vector &nn_points_list, + std::vector> &images_out, + std::vector> &hands_out) const; + + int num_threads_; + int num_orientations_; + descriptor::ImageGeometry image_params_; + std::unique_ptr image_strategy_; + bool is_plotting_; + bool remove_plane_; +}; + +} // namespace descriptor +} // namespace gpd + +#endif /* IMAGE_GENERATOR_H_ */ diff --git a/include/gpd/descriptor/image_geometry.h b/include/gpd/descriptor/image_geometry.h new file mode 100644 index 0000000..3d4c3d8 --- /dev/null +++ b/include/gpd/descriptor/image_geometry.h @@ -0,0 +1,92 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, Andreas ten Pas + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * 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. + */ + +#ifndef IMAGE_GEOMETRY_H_ +#define IMAGE_GEOMETRY_H_ + +#include + +namespace gpd { +namespace descriptor { + +/** + * + * \brief Store grasp image geometry. + * + * Stores the parameters used to calculate a grasp image. + * + * Each grasp image is based on a bounding box that defines the closing region + * of the robot hand. The dimensions of the bounding box are: *outer_diameter* + * x *depth* x *height*. These parameters are analogous to those that define + * the robot hand geometry (see \class HandGeometry). + * + * \note The grasp image is assumed to be square in size (width = height). + * + */ +class ImageGeometry { + public: + /** + * \brief Constructor. + */ + ImageGeometry(); + + /** + * \brief Constructor. + * \param outer_diameter the width of the bounding box + * \param depth the depth of the bounding box + * \param height the height of the bounding box + * \param size the size of the grasp image (width and height) + * \param num_channels the number of channels of the grasp image + */ + ImageGeometry(double outer_diameter, double depth, double height, int size, + int num_channels); + + /** + * \brief Constructor that uses a given configuration file to read in the + * parameters of the grasp image. + * \param filepath the filepath to the configuration file + */ + ImageGeometry(const std::string &filepath); + + double outer_diameter_; ///< the width of the volume + double depth_; ///< the depth of the volume + double height_; ///< the height of the volume + int size_; ///< the size of the image (image is square: width = height) + int num_channels_; ///< the number of channels in the image +}; + +std::ostream &operator<<(std::ostream &stream, + const ImageGeometry &hand_geometry); + +} // namespace descriptor +} // namespace gpd + +#endif /* IMAGE_GEOMETRY_H_ */ diff --git a/include/gpd/descriptor/image_strategy.h b/include/gpd/descriptor/image_strategy.h new file mode 100644 index 0000000..dcb9d94 --- /dev/null +++ b/include/gpd/descriptor/image_strategy.h @@ -0,0 +1,194 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, Andreas ten Pas + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * 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. + */ + +#ifndef IMAGE_STRATEGY_H +#define IMAGE_STRATEGY_H + +#include + +#include + +#include +#include + +#include +#include + +typedef std::pair Matrix3XdPair; + +namespace gpd { +namespace descriptor { + +/** + * + * \brief Abstract base class for calculating grasp images/descriptors. + * + * Also offers methods to calculate various images. + * + */ +class ImageStrategy { + public: + /** + * \brief Create a strategy for calculating grasp images. + * \param image_params the grasp image parameters + * \param num_threads the number of CPU threads to be used + * \param num_orientations the number of robot hand orientations + * \param is_plotting if the images are visualized + * \return the strategy for calculating grasp images + */ + static std::unique_ptr makeImageStrategy( + const ImageGeometry &image_params, int num_threads, int num_orientations, + bool is_plotting); + + /** + * \brief Constructor. + * \param image_params the grasp image parameters + * \param num_threads the number of CPU threads to be used + * \param num_orientations the number of robot hand orientations + * \param is_plotting if the images are visualized + * \return the strategy for calculating grasp images + */ + ImageStrategy(const ImageGeometry &image_params, int num_threads, + int num_orientations, bool is_plotting) + : image_params_(image_params), + num_orientations_(num_orientations), + num_threads_(num_threads), + is_plotting_(is_plotting) {} + + virtual ~ImageStrategy() {} + + /** + * \brief Create grasp images given a list of grasp candidates. + * \param hand_set the grasp candidates + * \param nn_points the point neighborhoods used to calculate the images + * \return the grasp images + */ + virtual std::vector> createImages( + const candidate::HandSet &hand_set, + const util::PointList &nn_points) const = 0; + + /** + * \brief Return the grasp image parameters. + * \return the grasp image parameters + */ + const ImageGeometry &getImageParameters() const { return image_params_; } + + protected: + /** + * \brief Transform a given list of points to the unit image. + * \param point_list the list of points + * \param hand the grasp + * \return the transformed points and their surface normals + */ + Matrix3XdPair transformToUnitImage(const util::PointList &point_list, + const candidate::Hand &hand) const; + + /** + * \brief Find points that lie in the closing region of the robot hand. + * \param hand the grasp + * \param points the points to be checked + * \return the indices of the points that lie inside the closing region + */ + std::vector findPointsInUnitImage(const candidate::Hand &hand, + const Eigen::Matrix3Xd &points) const; + + /** + * \brief Transform points to the unit image. + * \param hand the grasp + * \param points the points + * \param indices the indices of the points to be transformed + * \return the transformed points + */ + Eigen::Matrix3Xd transformPointsToUnitImage( + const candidate::Hand &hand, const Eigen::Matrix3Xd &points, + const std::vector &indices) const; + + /** + * \brief Find the indices of the pixels that are occupied by a given list of + * points. + * \param points the points + * \return the indices occupied by the points + */ + Eigen::VectorXi findCellIndices(const Eigen::Matrix3Xd &points) const; + + /** + * \brief Create a binary image based on which pixels are occupied by the + * points. + * \param cell_indices the indices of the points in the image + * \return the image + */ + cv::Mat createBinaryImage(const Eigen::VectorXi &cell_indices) const; + + /** + * \brief Create an RGB image based on the surface normals of the points. + * \param normals the surface normals + * \param cell_indices the indices of the points in the image + * \return the image + */ + cv::Mat createNormalsImage(const Eigen::Matrix3Xd &normals, + const Eigen::VectorXi &cell_indices) const; + + /** + * \brief Create a grey value image based on the depth value of the points. + * \param points the points + * \param cell_indices the indices of the points in the image + * \return the image + */ + cv::Mat createDepthImage(const Eigen::Matrix3Xd &points, + const Eigen::VectorXi &cell_indices) const; + + /** + * \brief Create a grey value image based on the depth of the shadow points. + * \param points the shadow points + * \param cell_indices the indices of the shadow points in the image + * \return the image + */ + cv::Mat createShadowImage(const Eigen::Matrix3Xd &points, + const Eigen::VectorXi &cell_indices) const; + + ImageGeometry image_params_; ///< grasp image parameters + int num_orientations_; ///< number of hand orientations + int num_threads_; ///< number of CPU threads to be used + bool is_plotting_; ///< if the grasp images are visualized + + private: + /** + * \brief Round a floating point vector to the closest, smaller integers. + * \param a the floating point vector + * \return the vector containing the integers + */ + Eigen::VectorXi floorVector(const Eigen::VectorXd &a) const; +}; + +} // namespace descriptor +} // namespace gpd + +#endif /* IMAGE_STRATEGY_H */ diff --git a/include/gpd/grasp_detector.h b/include/gpd/grasp_detector.h index 71a506f..f0ae2fb 100644 --- a/include/gpd/grasp_detector.h +++ b/include/gpd/grasp_detector.h @@ -1,7 +1,7 @@ /* * Software License Agreement (BSD License) * - * Copyright (c) 2016, Andreas ten Pas + * Copyright (c) 2018, Andreas ten Pas * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -32,9 +32,9 @@ #ifndef GRASP_DETECTOR_H_ #define GRASP_DETECTOR_H_ - // System #include +#include #include // PCL @@ -44,167 +44,187 @@ #include #include -// ROS -#include - -// Grasp Candidates Generator -#include -#include -#include -#include -#include - -// Custom -#include "gpd/classifier.h" -#include "../gpd/clustering.h" -#include "../gpd/learning.h" +#include +#include +#include +#include +#include +#include +#include +#include +namespace gpd { -/** GraspDetector class +/** * * \brief Detect grasp poses in point clouds. * - * This class detects grasps in a point cloud by first creating a large set of grasp hypotheses, and then - * classifying each of them as a grasp or not. It also contains a function to preprocess the point cloud. + * This class detects grasp poses in a point clouds by first creating a large + * set of grasp candidates, and then classifying each of them as a grasp or not. * -*/ -class GraspDetector -{ -public: - + */ +class GraspDetector { + public: /** * \brief Constructor. * \param node ROS node handle */ - GraspDetector(ros::NodeHandle& node); + GraspDetector(const std::string &config_filename); /** - * \brief Destructor. - */ - ~GraspDetector() - { - delete candidates_generator_; - delete learning_; - delete clustering_; - } - - /** - * \brief Preprocess the point cloud. + * \brief Detect grasps in a point cloud. * \param cloud_cam the point cloud + * \return list of grasps */ - void preprocessPointCloud(CloudCamera& cloud_cam); + std::vector> detectGrasps( + const util::Cloud &cloud); /** - * \brief Detect grasps in a given point cloud. + * \brief Preprocess the point cloud. * \param cloud_cam the point cloud - * \return the list of detected grasps */ - std::vector detectGrasps(const CloudCamera& cloud_cam); + void preprocessPointCloud(util::Cloud &cloud); /** - * \brief Generate grasp candidates. - * \param cloud_cam the point cloud - * \return the list of grasp candidates + * Filter grasps based on the robot's workspace. + * \param hand_set_list list of grasp candidate sets + * \param workspace the robot's workspace as a 3D cube, centered at the origin + * \param thresh_rad the angle in radians above which grasps are filtered + * \return list of grasps after filtering */ - std::vector generateGraspCandidates(const CloudCamera& cloud_cam); + std::vector> filterGraspsWorkspace( + std::vector> &hand_set_list, + const std::vector &workspace) const; /** - * \brief Filter grasps that are outside of the robot's workspace. - * \param hand_set_list the list of grasp candidate sets - * \param workspace the robot's workspace - * \return the grasps that are inside the robot's workspace + * Filter grasps based on their approach direction. + * \param hand_set_list list of grasp candidate sets + * \param direction the direction used for filtering + * \param thresh_rad the angle in radians above which grasps are filtered + * \return list of grasps after filtering */ - std::vector filterGraspsWorkspace(const std::vector& hand_set_list, - const std::vector& workspace); - + std::vector> filterGraspsDirection( + std::vector> &hand_set_list, + const Eigen::Vector3d &direction, const double thresh_rad); + /** - * \brief Filter grasps that are half-antipodal. - * \param hand_set_list the list of grasp candidate sets - * \return the grasps that are not half-antipodal + * \brief Generate grasp candidates. + * \param cloud the point cloud + * \return the list of grasp candidates */ - std::vector filterHalfAntipodal(const std::vector& hand_set_list); + std::vector> generateGraspCandidates( + const util::Cloud &cloud); /** - * \brief Extract the valid grasps from a given list of grasp candidate sets. - * \param hand_set_list the list of grasp candidate sets - * \return the valid grasps + * \brief Create grasp images and candidates for a given point cloud. + * \param cloud the point cloud + * \param[out] hands_out the grasp candidates + * \param[out] images_out the grasp images + * \return `false` if no grasp candidates are found, `true` otherwise */ - std::vector extractHypotheses(const std::vector& hand_set_list); + bool createGraspImages( + util::Cloud &cloud, + std::vector> &hands_out, + std::vector> &images_out); /** - * \brief Match grasps with their corresponding grasp images. - * \param[in] hand_set_list list of grasp candidate sets - * \param[in] images list of grasp images - * \param[out] grasps_out the grasps corresponding to the images - * \param[out] images_out the images corresponding to the grasps + * \brief Evaluate the ground truth for a given list of grasps. + * \param cloud_gt the point cloud (typically a mesh) + * \param hands the grasps + * \return the ground truth label for each grasp */ - void extractGraspsAndImages(const std::vector& hand_set_list, const std::vector& images, - std::vector& grasps_out, std::vector& images_out); + std::vector evalGroundTruth( + const util::Cloud &cloud_gt, + std::vector> &hands); /** - * \brief Classify grasp candidates as viable grasps or not. - * \param cloud_cam the point cloud - * \param candidates the grasp candidates to be classified - * \return the classified grasps + * \brief Creates grasp images and prunes grasps below a given score. + * \param cloud the point cloud + * \param hand_set_list the grasps + * \param min_score the score below which grasps are pruned + * \return the grasps above the score */ - std::vector classifyGraspCandidates(const CloudCamera& cloud_cam, std::vector& candidates); + std::vector> pruneGraspCandidates( + const util::Cloud &cloud, + const std::vector> &hand_set_list, + double min_score); /** - * \brief Find clusters of grasps that are geometrically aligned. - * \param grasps the grasps for which to search clusters - * \return the grasps that are in clusters + * \brief Select the k highest scoring grasps. + * \param hands the grasps + * \return the k highest scoring grasps */ - std::vector findClusters(const std::vector& grasps); + std::vector> selectGrasps( + std::vector> &hands) const; /** - * \brief Compare if the score of a grasp is larger than the score of another grasp. - * \param hypothesis1 a grasp - * \param hypothesis2 another grasp - * \return true if it is larger, false otherwise + * \brief Compare the scores of two given grasps. + * \param hand1 the first grasp to be compared + * \param hand1 the second grasp to be compared + * \return `true` if \param hand1 has a larger score than \param hand2, + * `false` otherwise */ - static bool isScoreGreater(const Grasp& hypothesis1, const Grasp& hypothesis2) - { - return hypothesis1.getScore() > hypothesis2.getScore(); + static bool isScoreGreater(const std::unique_ptr &hand1, + const std::unique_ptr &hand2) { + return hand1->getScore() > hand2->getScore(); } - const HandSearch::Parameters& getHandSearchParameters() - { + /** + * \brief Return the hand search parameters. + * \return the hand search parameters + */ + const candidate::HandSearch::Parameters &getHandSearchParameters() { return candidates_generator_->getHandSearchParams(); } + /** + * \brief Return the image geometry parameters. + * \return the image geometry parameters + */ + const descriptor::ImageGeometry &getImageGeometry() const { + return image_generator_->getImageGeometry(); + } -private: + private: + void printStdVector(const std::vector &v, const std::string &name) const; - CandidatesGenerator* candidates_generator_; ///< pointer to object for grasp candidate generation - Learning* learning_; ///< pointer to object for grasp image creation - Clustering* clustering_; ///< pointer to object for clustering geometrically aligned grasps - std::shared_ptr classifier_; ///< pointer to object for classification of candidates + void printStdVector(const std::vector &v, + const std::string &name) const; - Learning::ImageParameters image_params_; // grasp image parameters + std::unique_ptr candidates_generator_; + std::unique_ptr image_generator_; + std::unique_ptr clustering_; + std::unique_ptr plotter_; + std::shared_ptr classifier_; // classification parameters - double min_score_diff_; ///< minimum classifier confidence score - bool create_image_batches_; ///< if images are created in batches (reduces memory usage) + double min_score_; ///< minimum classifier confidence score + bool create_image_batches_; ///< if images are created in batches (reduces + /// memory usage) // plotting parameters - bool plot_normals_; ///< if normals are plotted - bool plot_samples_; ///< if samples/indices are plotted - bool plot_filtered_grasps_; ///< if filtered grasps are plotted - bool plot_valid_grasps_; ///< if positive grasp instances are plotted - bool plot_clusters_; ///< if grasp clusters are plotted - bool plot_selected_grasps_; ///< if selected grasps are plotted + bool plot_normals_; ///< if normals are plotted + bool plot_samples_; ///< if samples/indices are plotted + bool plot_candidates_; ///< if grasp candidates are plotted + bool plot_filtered_candidates_; ///< if filtered grasp candidates are plotted + bool plot_valid_grasps_; ///< if valid grasps are plotted + bool plot_clustered_grasps_; ///< if clustered grasps are plotted + bool plot_selected_grasps_; ///< if selected grasps are plotted // filtering parameters - bool filter_grasps_; ///< if grasps are filtered based on the robot's workspace and the robot hand width - bool filter_half_antipodal_; ///< if grasps are filtered based on being half-antipodal - bool cluster_grasps_; ///< if grasps are clustered - double outer_diameter_; ///< the outer diameter of the robot hand - double min_aperture_; ///< the minimum opening width of the robot hand - double max_aperture_; ///< the maximum opening width of the robot hand - std::vector workspace_; ///< the workspace of the robot + bool cluster_grasps_; ///< if grasps are clustered + double min_aperture_; ///< the minimum opening width of the robot hand + double max_aperture_; ///< the maximum opening width of the robot hand + std::vector workspace_grasps_; ///< the workspace of the robot with + /// respect to hand poses + bool filter_approach_direction_; + Eigen::Vector3d direction_; + double thresh_rad_; // selection parameters - int num_selected_; ///< the number of selected grasps + int num_selected_; ///< the number of selected grasps }; +} // namespace gpd + #endif /* GRASP_DETECTOR_H_ */ diff --git a/include/gpd/grasp_image.h b/include/gpd/grasp_image.h deleted file mode 100644 index 1ad1cf1..0000000 --- a/include/gpd/grasp_image.h +++ /dev/null @@ -1,142 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2016, Andreas ten Pas - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * 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. - */ - - -#ifndef GRASP_IMAGE_H -#define GRASP_IMAGE_H - -#include - -#include - -#include -#include -#include - - -/** GraspImage class - * - * \brief Create grasp images. - * - * This class contains a set of methods to create grasp images. A grasp image is a representation of a grasp candidate - * that can be used as input for a convolutional neural network (or another classification method). - * -*/ -class GraspImage -{ -public: - - /** - * \brief Constructor. - * \param image_size the size of the image - * \param num_channels the number of channels of the image - * \param is_plotting if the image is visualized - */ - GraspImage(int image_size, int num_channels, bool is_plotting); - - /** - * \brief Virtual destructor. - */ - virtual ~GraspImage() { }; - - /** - * \brief Virtual function to calculate a grasp image. - */ - virtual cv::Mat calculateImage() = 0; - - /** - * \brief Find the indices of the pixels that are occupied by a given list of points. - * \param points the points - * \return the indices occupied by the points - */ - static Eigen::VectorXi findCellIndices(const Eigen::Matrix3Xd& points); - - /** - * \brief Create a binary image based on which pixels are occupied by the points. - * \param cell_indices the indices of the points in the image - * \return the image - */ - static cv::Mat createBinaryImage(const Eigen::VectorXi& cell_indices); - - /** - * \brief Create an RGB image based on the surface normals of the points. - * \param normals the surface normals - * \param cell_indices the indices of the points in the image - * \return the image - */ - static cv::Mat createNormalsImage(const Eigen::Matrix3Xd& normals, const Eigen::VectorXi& cell_indices); - - /** - * \brief Create a grey value image based on the depth value of the points. - * \param points the points - * \param cell_indices the indices of the points in the image - * \return the image - */ - static cv::Mat createDepthImage(const Eigen::Matrix3Xd& points, const Eigen::VectorXi& cell_indices); - - /** - * \brief Create a grey value image based on the depth of the shadow points. - * \param points the shadow points - * \param cell_indices the indices of the shadow points in the image - * \return the image - */ - static cv::Mat createShadowImage(const Eigen::Matrix3Xd& points, const Eigen::VectorXi& cell_indices); - - /** - * \brief Set the size of the image. - * \param image_size the image size - */ - static void setImageSize(int image_size) - { - image_size_ = image_size; - } - - -protected: - - int num_channels_; ///< the number of channels of the grasp image - bool is_plotting_; ///< if the grasp image is visualized - - static int image_size_; ///< the size of the grasp image (height x width) - - -private: - - /** - * \brief Round a floating point vector to the closest, smaller integers. - * \param a the floating point vector - * \return the vector containing the integers - */ - static Eigen::VectorXi floorVector(const Eigen::VectorXd& a); -}; - - -# endif /* GRASP_IMAGE_H */ diff --git a/include/gpd/grasp_image_15_channels.h b/include/gpd/grasp_image_15_channels.h deleted file mode 100644 index d8c7dd6..0000000 --- a/include/gpd/grasp_image_15_channels.h +++ /dev/null @@ -1,121 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2016, Andreas ten Pas - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * 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. - */ - - -#ifndef GRASP_IMAGE_15_CHANNELS_H_ -#define GRASP_IMAGE_15_CHANNELS_H_ - - -#include "../gpd/grasp_image.h" - - -/** - * \brief Set of images for projection of points onto plane orthogonal to one of the robot hand axes. - */ -struct Projection -{ - cv::Mat normals_image_; ///< image based on surface normals (3 channels) - cv::Mat depth_image_; ///< image based on depth (1 channel) - cv::Mat shadow_image_; ///< image based on shadow (1 channel) -}; - - -/** GraspImage15Channels class - * - * \brief Create grasp image with 15 channels. - * - * This class creates the 15 channels grasp image. The image is composed of three projections of points onto a plane - * orthogonal to one of the robot hand axes. Each of these projections contains three images of which one has - * three channels, and the other two have one channel each. - * -*/ -class GraspImage15Channels: public GraspImage -{ -public: - - /** - * \brief Constructor. - * \param image_size the image size - * \param is_plotting if the image is visualized - * \param points the points on which the image is based - * \param shadow the shadow on which the image is based - */ - GraspImage15Channels(int image_size, bool is_plotting, Eigen::Matrix3Xd* points, Eigen::Matrix3Xd* normals, - Eigen::Matrix3Xd* shadow); - - /** - * \brief Virtual destructor. - */ - virtual ~GraspImage15Channels(); - - /** - * \brief Calculate the 15 channels grasp image. - * \return the 15 channels image - */ - cv::Mat calculateImage(); - - -private: - - /** - * \brief Calculate the images for a projection of points. - * \param points the points which are projected - * \param normals the surface normals of the points - * \param shadow the shadow of the points - * \return the three images associated with the projection - */ - Projection calculateProjection(const Eigen::Matrix3Xd& points, const Eigen::Matrix3Xd& normals, - const Eigen::Matrix3Xd& shadow); - - /** - * \brief Concatenate three projections into a single 15 channels image. - * \param projection1 the first projection - * \param projection2 the second projection - * \param projection3 the third projection - * \return the 15 channels image - */ - cv::Mat concatenateProjections(const Projection& projection1, const Projection& projection2, - const Projection& projection3) const; - - /** - * \brief Visualize the 15 channels image. - * \param projections list of projections - */ - void showImage(const std::vector& projections) const; - - Eigen::Matrix3Xd* points_; ///< pointer to points matrix - Eigen::Matrix3Xd* normals_; ///< pointer to surface normals matrix - Eigen::Matrix3Xd* shadow_; ///< pointer to shadow matrix - - static const int NUM_CHANNELS; ///< the number of channels used for this image -}; - -#endif /* GRASP_IMAGE_15_CHANNELS_H_ */ diff --git a/include/gpd/learning.h b/include/gpd/learning.h deleted file mode 100644 index bbb97eb..0000000 --- a/include/gpd/learning.h +++ /dev/null @@ -1,210 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2016, Andreas ten Pas - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * 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. - */ - - -#ifndef LEARNING_H_ -#define LEARNING_H_ - -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include - -#include -#include - -#include -#include -#include - -#include -#include -#include - -#include -#include -#include -#include - -#include "../gpd/grasp_image_15_channels.h" - - -typedef std::pair Matrix3XdPair; - - -/** Learning class - * - * \brief Create images for classification. - * - * This class is used to create images for the input layer of a convolutional neural network. Each image represents a - * grasp candidate. We call these "grasp images". - * - */ -class Learning -{ - public: - - /** - * \brief Parameters for the grasp images. - */ - struct ImageParameters - { - double outer_diameter_; ///< the maximum robot hand aperture - double depth_; ///< the hand depth (length of fingers) - double height_; ///< the hand extends plus/minus this value along the hand axis - int size_; ///< the size of the image in pixels (for one side) - int num_channels_; ///< the number of channels in the grasp image - }; - - /** - * \brief Constructor. - * \param params parameters for grasp images - * \param num_threads number of CPU threads to be used - * \param is_plotting if images are visualized - * \param remove_plane if the (table) plane is removed before calculating images - */ - Learning(const ImageParameters& params, int num_threads, int num_orientations, bool is_plotting, bool remove_plane) - : image_params_(params), num_threads_(num_threads), num_orientations_(num_orientations), - is_plotting_(is_plotting), remove_plane_(remove_plane) { } - - /** - * \brief Match grasps with their corresponding grasp images. - * \param hand_set_list list of grasp candidate sets - * \param images list of grasp images - * \param[out] grasps_out list of grasps matched to images - * \param[out] images_out list of images matched to grasps - */ - void extractGraspsAndImages(const std::vector& hand_set_list, const std::vector& images, - std::vector& grasps_out, std::vector& images_out); - - /** - * \brief Create a list of grasp images for a given list of grasp candidates. - * \param cloud_cam the point cloud - * \param hand_set_list the list of grasp candidates - * \return the list of grasp images - */ - std::vector createImages(const CloudCamera& cloud_cam, const std::vector& hand_set_list) const; - - - private: - - /** - * \brief Create grasp images with one or three channels. - * \param hand_set_list the list of grasp candidate sets - * \param nn_points_list the list of point neighborhoods - * \param is_valid the list of booleans indicating for each neighborhood if it is valid or not - * \param image_dims the image dimensions - * \return the grasp images - */ - std::vector createImages1or3Channels(const std::vector& hand_set_list, - const std::vector& nn_points_list, const bool* is_valid, const Eigen::Vector3d& image_dims) const; - - /** - * \brief Create a one or three channels grasp image. - * \param point_list the point neighborhood - * \param hand the grasp - * \return the grasp image - */ - cv::Mat createImage1or3Channels(const PointList& point_list, const Grasp& hand) const; - - /** - * \brief Create grasp images with 15 channels. - * \param hand_set_list the list of grasp candidate sets - * \param nn_points_list the list of point neighborhoods - * \param is_valid the list of booleans indicating for each neighborhood if it is valid or not - * \param image_dims the image dimensions - * \return the grasp images - */ - std::vector createImages15Channels(const std::vector& hand_set_list, - const std::vector& nn_points_list, const bool* is_valid, const Eigen::Vector3d& image_dims) const; - - /** - * \brief Create a 15 channels grasp image. - * \param point_list the point neighborhood - * \param shadow the shadow of the point neighborhood - * \param hand the grasp - * \return the grasp image - */ - cv::Mat createImage15Channels(const PointList& point_list, const Eigen::Matrix3Xd& shadow, const Grasp& hand) - const; - - /** - * \brief Transform a given list of points to the unit image. - * \param point_list the list of points - * \param hand the grasp - * \return the transformed points and their surface normals - */ - Matrix3XdPair transformToUnitImage(const PointList& point_list, const Grasp& hand) const; - - /** - * \brief Find points that lie in the closing region of the robot hand. - * \param hand the grasp - * \param points the points to be checked - * \return the indices of the points that lie inside the closing region - */ - std::vector findPointsInUnitImage(const Grasp& hand, const Eigen::Matrix3Xd& points) const; - - /** - * \brief Transform points to the unit image. - * \param hand the grasp - * \param points the points - * \param indices the indices of the points to be transformed - * \return the transformed points - */ - Eigen::Matrix3Xd transformPointsToUnitImage(const Grasp& hand, const Eigen::Matrix3Xd& points, - const std::vector& indices) const; - - /** - * \brief Transform a list of points into a frame. - * \param point_list the points to be transformed - * \param centroid the origin of the frame - * \param rotation the orientation of the frame - * \return the transformed points and their surface normals - */ - Matrix3XdPair transformToHandFrame(const PointList& point_list, const Eigen::Vector3d& centroid, - const Eigen::Matrix3d& rotation) const; - - int num_threads_; ///< number of threads - int num_orientations_; ///< number of hand orientations - ImageParameters image_params_; ///< parameters of the grasp image - bool is_plotting_; ///< if grasp images are visualized - bool remove_plane_; ///< if the largest plane is removed from the point cloud -}; - -#endif /* LEARNING_H_ */ diff --git a/include/gpd/caffe_classifier.h b/include/gpd/net/caffe_classifier.h similarity index 57% rename from include/gpd/caffe_classifier.h rename to include/gpd/net/caffe_classifier.h index 908bc88..9574e38 100644 --- a/include/gpd/caffe_classifier.h +++ b/include/gpd/net/caffe_classifier.h @@ -1,7 +1,7 @@ /* * Software License Agreement (BSD License) * - * Copyright (c) 2016, Andreas ten Pas + * Copyright (c) 2018, Andreas ten Pas * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -29,7 +29,6 @@ * POSSIBILITY OF SUCH DAMAGE. */ - #ifndef CAFFE_CLASSIFIER_H_ #define CAFFE_CLASSIFIER_H_ @@ -45,44 +44,51 @@ // OpenCV #include -#include "gpd/classifier.h" +#include + +namespace gpd { +namespace net { -/** CaffeClassifier class +/** * - * \brief Classify grasp candidates as viable grasps or not + * \brief Classify grasp candidates as viable grasps or not with Caffe * - * This class classifies grasps as viable or not using a convolutional neural network (CNN) in the Caffe framework. + * Classifies grasps as viable or not using a convolutional neural network (CNN) + * with the Caffe framework. * */ -class CaffeClassifier : public Classifier -{ - public: - - /** - * \brief Constructor. - * \param model_file the location of the file that describes the network model - * \param weights_file the location of the file that contains the network weights - */ - CaffeClassifier(const std::string& model_file, const std::string& weights_file, Classifier::Device device); - - /** - * \brief Classify grasp candidates as viable grasps or not. - * \param image_list the list of grasp images - * \return the classified grasp candidates - */ - std::vector classifyImages(const std::vector& image_list); - - int getBatchSize() const - { - return input_layer_->batch_size(); - } +class CaffeClassifier : public Classifier { + public: + /** + * \brief Constructor. + * \param model_file the location of the file that describes the network model + * \param weights_file the location of the file that contains the network + * weights + */ + CaffeClassifier(const std::string &model_file, + const std::string &weights_file, Classifier::Device device, + int batch_size); + /** + * \brief Classify grasp candidates as viable grasps or not. + * \param image_list the list of grasp images + * \return the classified grasp candidates + */ + std::vector classifyImages( + const std::vector> &image_list); - private: + /** + * \brief Return the batch size. + * \return the batch size + */ + int getBatchSize() const { return input_layer_->batch_size(); } - boost::shared_ptr > net_; ///< the network - boost::shared_ptr > input_layer_; ///< the input layer of the network + private: + boost::shared_ptr> net_; + boost::shared_ptr> input_layer_; }; +} // namespace net +} // namespace gpd #endif /* CAFFE_CLASSIFIER_H_ */ diff --git a/include/gpd/classifier.h b/include/gpd/net/classifier.h similarity index 55% rename from include/gpd/classifier.h rename to include/gpd/net/classifier.h index 447df0b..c3bc3bf 100644 --- a/include/gpd/classifier.h +++ b/include/gpd/net/classifier.h @@ -1,7 +1,7 @@ /* * Software License Agreement (BSD License) * - * Copyright (c) 2018 Intel Corporation + * Copyright (c) 2018, Andreas ten Pas * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -40,41 +40,47 @@ // OpenCV #include -/** Classifier class +namespace gpd { +namespace net { + +/** * - * \brief Abstract base class for classifier that Classify grasp candidates as viable grasps or not + * \brief Abstract base class for classifier that classifies grasp candidates as + * viable grasps or not. * */ -class Classifier -{ - public: - - enum class Device : uint8_t { - eCPU = 0, - eGPU = 1, - eVPU = 2, - eFPGA = 3 - }; +class Classifier { + public: + enum class Device : uint8_t { eCPU = 0, eGPU = 1, eVPU = 2, eFPGA = 3 }; - /** - * \brief Create Classifier per build option. - * - If "USE_OPENVINO", an OpenVINO classifier will be created - * - Otherwise a Caffe classifier will be created - * \param model_file The location of the file that describes the network model - * \param weights_file The location of the file that contains the network weights - * \param device The target device where the network computation executes - */ - static std::shared_ptr create(const std::string& model_file, const std::string& weights_file, Device device = Device::eCPU); + /** + * \brief Create a classifier dependent on build options. + * \param model_file filepath to the network model + * \param weights_file filepath to the network parameters + * \param device target device on which the network is run + * \return the classifier + */ + static std::shared_ptr create(const std::string &model_file, + const std::string &weights_file, + Device device = Device::eCPU, + int batch_size = 1); - /** - * \brief Classify grasp candidates as viable grasps or not. - * \param image_list the list of grasp images - * \return the classified grasp candidates - */ - virtual std::vector classifyImages(const std::vector& image_list) = 0; + /** + * \brief Classify grasp candidates as viable grasps or not. + * \param image_list the list of grasp images + * \return the classified grasp candidates + */ + virtual std::vector classifyImages( + const std::vector> &image_list) = 0; - virtual int getBatchSize() const = 0; + /** + * \brief Return the batch size. + * \return the batch size + */ + virtual int getBatchSize() const = 0; }; +} // namespace net +} // namespace gpd #endif /* CLASSIFIER_H_ */ diff --git a/include/gpd/net/conv_layer.h b/include/gpd/net/conv_layer.h new file mode 100644 index 0000000..3ebacda --- /dev/null +++ b/include/gpd/net/conv_layer.h @@ -0,0 +1,122 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Andreas ten Pas + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * 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. + */ + +#ifndef CONV_LAYER_H +#define CONV_LAYER_H + +#include + +#include +#include + +#include + +namespace gpd { +namespace net { + +/** + * + * \brief Convolutional layer. + * + * A convolutional layer for a neural network for the `EigenClassifier`. + * + */ +class ConvLayer : public Layer { + public: + /** + * \brief Constructor. + * \param width input width + * \param height input height + * \param depth input depth + * \param num_filters number of convolutional filters + * \param spatial_extent size of each filter + * \param padding how much zero-padding is used + */ + ConvLayer(int width, int height, int depth, int num_filters, + int spatial_extent, int stride, int padding); + + /** + * \brief Forward pass. + * \param x input + * \return output of forward pass + */ + Eigen::MatrixXf forward(const std::vector &x) const; + + private: + typedef Eigen::Map> + RowMajorMatrixMap; + + /** + * \brief Calculate the forward pass. + * \param W weight matrix + * \param b bias vector + * \param X input matrix + * \return output of forward pass + */ + Eigen::MatrixXf forward(const Eigen::MatrixXf &W, const Eigen::VectorXf &b, + const Eigen::MatrixXf &X) const; + + bool is_a_ge_zero_and_a_lt_b(int a, int b) const; + + /** + * \brief Convert image to array. Arranges image slices into columns so that + * the forward pass can be expressed as a + * as a matrix multiplication. + * \param data_im the image to be converted + * \param channels number of image channels + * \param height image height + * \param width image width + * \param num_kernels number of filters to be used in the convolution + * \param kernel_h filter height + * \param kernel_w filter width + * \param stride_h stride height + * \param stride_w stride width + * \param[out] data_col the array + */ + void imageToColumns(const float *data_im, const int channels, + const int height, const int width, const int num_kernels, + const int kernel_h, const int kernel_w, + const int stride_h, const int stride_w, + float *data_col) const; + + int w1, h1, d1; // size of input volume: w1 x h1 x d1 + int w2, h2, d2; // size of output volume: w2 x h2 x d2 + int k, f, s, p; // number of filters, their spatial extent, stride, amount of + // zero padding + int W_row_r, W_row_c; // number of rows and columns in matrix W_row + int X_col_r, X_col_c; // number of rows and columns in matrix X_col +}; + +} // namespace net +} // namespace gpd + +#endif /* CONV_LAYER_H */ diff --git a/src/gpd/classifier.cpp b/include/gpd/net/dense_layer.h similarity index 63% rename from src/gpd/classifier.cpp rename to include/gpd/net/dense_layer.h index e23905d..efb40ed 100644 --- a/src/gpd/classifier.cpp +++ b/include/gpd/net/dense_layer.h @@ -1,7 +1,7 @@ /* * Software License Agreement (BSD License) * - * Copyright (c) 2018 Intel Corporation + * Copyright (c) 2017, Andreas ten Pas * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -29,18 +29,46 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include "gpd/classifier.h" -#ifdef USE_OPENVINO -#include "gpd/openvino_classifier.h" -#else -#include "gpd/caffe_classifier.h" -#endif - -std::shared_ptr Classifier::create(const std::string& model_file, const std::string& weights_file, Classifier::Device device) -{ -#ifdef USE_OPENVINO - return std::make_shared(device); -#else - return std::make_shared(model_file, weights_file, device); -#endif -} +#ifndef DENSE_LAYER_H +#define DENSE_LAYER_H + +#include + +#include +#include + +#include + +namespace gpd { +namespace net { + +/** + * + * \brief Dense (fully connected) layer. + * + * A dense (fully connected) layer for a neural network for the + * `EigenClassifier`. + * + */ +class DenseLayer : public Layer { + public: + /** + * \brief Contructor. + * \param num_units number of units/neurons in this layer + */ + DenseLayer(int num_units) : num_units_(num_units) {} + + /** + * \brief Forward pass. + * \return output of forward pass + */ + Eigen::MatrixXf forward(const std::vector &x) const; + + private: + int num_units_; ///< the number of units +}; + +} // namespace net +} // namespace gpd + +#endif /* DENSE_LAYER_H */ diff --git a/include/gpd/net/eigen_classifier.h b/include/gpd/net/eigen_classifier.h new file mode 100644 index 0000000..2643579 --- /dev/null +++ b/include/gpd/net/eigen_classifier.h @@ -0,0 +1,120 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, Andreas ten Pas + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * 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. + */ + +#ifndef EIGEN_CLASSIFIER_H_ +#define EIGEN_CLASSIFIER_H_ + +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace gpd { +namespace net { + +/** + * + * \brief Classify grasp candidates as viable grasps or not with Eigen + * + * Classifies grasps as viable or not using a custom neural network framework + * based on the Eigen library. + * + */ +class EigenClassifier : public Classifier { + public: + /** + * \brief Constructor. + * \param model_file the location of the file that describes the network model + * \param weights_file the location of the file that contains the network + * weights + */ + EigenClassifier(const std::string &model_file, + const std::string &weights_file, Classifier::Device device, + int batch_size); + + /** + * \brief Classify grasp candidates as viable grasps or not. + * \param image_list the list of grasp images + * \return the classified grasp candidates + */ + std::vector classifyImages( + const std::vector> &image_list); + + int getBatchSize() const { return 0; } + + private: + /** + * \brief Forward pass of the network. + * \param x input to the network + * \return output of the network + */ + std::vector forward(const std::vector &x); + + /** + * \brief Convert an image to an array (std::vector) so that it can be used as + * input for a layer. + * \param img the image to be converted + * \return the array + */ + std::vector imageToArray(const cv::Mat &img) const; + + /** + * \brief Forward pass for a max pooling layer. + * \param X input + * \param filter_size the size of the filter + * \param stride the stride at which to apply the filter + */ + Eigen::MatrixXf poolForward(const Eigen::MatrixXf &X, int filter_size, + int stride) const; + + /** + * \brief Read a binary file into a vector. + * \param location path to the binary file + */ + std::vector readBinaryFileIntoVector(const std::string &location); + + std::unique_ptr conv1_; ///< 1st conv layer + std::unique_ptr conv2_; ///< 2nd conv layer + std::unique_ptr dense1_; ///< 1st dense layer + std::unique_ptr dense2_; ///< 2nd dense layer + std::vector x_dense1_, x_dense2_, x_conv2_; ///< inputs for layers + int num_threads_{1}; +}; + +} // namespace net +} // namespace gpd + +#endif /* EIGEN_CLASSIFIER_H_ */ diff --git a/include/gpd/net/layer.h b/include/gpd/net/layer.h new file mode 100644 index 0000000..af8929f --- /dev/null +++ b/include/gpd/net/layer.h @@ -0,0 +1,79 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Andreas ten Pas + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * 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. + */ + +#ifndef LAYER_H +#define LAYER_H + +#include + +#include + +namespace gpd { +namespace net { + +/** + * + * \brief Abstract base class for neural network layers in the custom framework. + * + */ +class Layer { + public: + /** + * \brief Constructor. + */ + virtual ~Layer() {} + + /** + * \brief Forward pass for the layer. + * \return output of forward pass + */ + virtual Eigen::MatrixXf forward(const std::vector &x) const = 0; + + /** + * \brief Set the parameters of the layer. + * \param weights the weights + * \param biases the biases + */ + void setWeightsAndBiases(const std::vector &weights, + const std::vector &biases) { + weights_ = weights; + biases_ = biases; + } + + protected: + std::vector weights_; + std::vector biases_; +}; + +} // namespace net +} // namespace gpd + +#endif /* LAYER_H */ diff --git a/include/gpd/openvino_classifier.h.in b/include/gpd/net/openvino_classifier.h.in similarity index 53% rename from include/gpd/openvino_classifier.h.in rename to include/gpd/net/openvino_classifier.h.in index 1ed53ce..1cc8832 100644 --- a/include/gpd/openvino_classifier.h.in +++ b/include/gpd/net/openvino_classifier.h.in @@ -1,7 +1,7 @@ /* * Software License Agreement (BSD License) * - * Copyright (c) 2018 Intel Corporation + * Copyright (c) 2018, Andreas ten Pas * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -42,39 +42,58 @@ // OpenCV #include -#include +#include #define MODELS_DIR "@MODELS_DIR@" -/** CaffeClassifier class +namespace gpd { +namespace net { + +/** * - * \brief Classify grasp candidates as viable grasps or not + * \brief Classify grasp candidates as viable grasps or not with OpenVINO * - * This class classifies grasps as viable or not using a convolutional neural network (CNN) in the Caffe framework. + * Classifies grasps as viable or not using a convolutional neural network + * (CNN) with the OpenVINO framework. * */ -class OpenVINOClassifier : public Classifier -{ - public: - - /** - * \brief Constructor. - * \param model_dir The directory that contains the openvino network model - * \param device The target device where the computation executes - */ - OpenVINOClassifier(Classifier::Device device); +class OpenVinoClassifier : public Classifier { + public: + /** + * \brief Constructor. + * \param model_dir The directory that contains the openvino network model + * \param device The target device where the computation executes + */ + OpenVinoClassifier(const std::string& model_file, + const std::string& weights_file, + Classifier::Device device, + int batch_size); - std::vector classifyImages(const std::vector& image_list); + /** + * \brief Classify grasp candidates as viable grasps or not. + * \param image_list the list of grasp images + * \return the classified grasp candidates + */ + std::vector classifyImages( + const std::vector>& image_list); - int getBatchSize() const; + /** + * \brief Return the batch size. + * \return the batch size + */ + int getBatchSize() const; - private: - static std::map device_map_; - std::vector batch_image_list_; - InferenceEngine::Blob::Ptr input_blob_, output_blob_; - InferenceEngine::CNNNetwork network_; - InferenceEngine::InferRequest infer_request_; - InferenceEngine::InferencePlugin plugin_; + private: + static std::map + device_map_; + std::vector batch_image_list_; + InferenceEngine::Blob::Ptr input_blob_, output_blob_; + InferenceEngine::CNNNetwork network_; + InferenceEngine::InferRequest infer_request_; + InferenceEngine::InferencePlugin plugin_; }; +} // namespace net +} // namespace gpd + #endif /* OPENVINO_CLASSIFIER_H_ */ diff --git a/include/gpd/sequential_importance_sampling.h b/include/gpd/sequential_importance_sampling.h index 9a062d6..755c787 100644 --- a/include/gpd/sequential_importance_sampling.h +++ b/include/gpd/sequential_importance_sampling.h @@ -1,7 +1,7 @@ /* * Software License Agreement (BSD License) * - * Copyright (c) 2016, Andreas ten Pas + * Copyright (c) 2018, Andreas ten Pas * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -29,151 +29,117 @@ * POSSIBILITY OF SUCH DAMAGE. */ - #ifndef SEQUENTIAL_IMPORTANCE_SAMPLING_H #define SEQUENTIAL_IMPORTANCE_SAMPLING_H - -#include - -#include #include +#include +#include #include -#include -#include - -#include "../gpd/grasp_detector.h" +#include +#include +namespace gpd { typedef pcl::PointCloud PointCloudRGB; -typedef boost::variate_generator > Gaussian; - - -/** SequentialImportanceSampling class +/** * - * \brief Use sequential importance sampling to focus the sampling of grasp candidates. + * \brief Grasp pose detection with the Cross Entropy Method. * - * This class uses sequential importance sampling to focus the grasp candidate generation on areas of the point cloud - * where more grasp candidates are expected to be found. + * This class uses the Cross Entropy Method to focus the grasp candidate + * generation on areas of the point cloud where more grasp candidates are + * expected to be found. * */ -class SequentialImportanceSampling -{ -public: - +class SequentialImportanceSampling { + public: /** * \brief Constructor. * \param node ROS node handle */ - SequentialImportanceSampling(ros::NodeHandle& node); - - /** - * \brief Destructor. - */ - ~SequentialImportanceSampling() - { - delete grasp_detector_; - } + SequentialImportanceSampling(const std::string &config_filename); /** * \brief Detect grasps. - * \param cloud_cam_in the point cloud + * \param cloud_in the point cloud * \return the list of grasps */ - std::vector detectGrasps(const CloudCamera& cloud_cam_in); - - /** - * Preprocess the point cloud (workspace filtering, voxelization, surface normals). - * \param cloud_cam the point cloud - */ - void preprocessPointCloud(CloudCamera& cloud_cam); + std::vector> detectGrasps( + util::Cloud &cloud); /** * \brief Compare if two grasps are equal based on their position. * \param h1 the first grasp * \param h2 the second grasp */ - struct compareGraspPositions - { - bool operator()(const Grasp& h1, const Grasp& h2) - { + struct compareGraspPositions { + bool operator()(const candidate::Hand &h1, const candidate::Hand &h2) { double position_thresh = 0.001; double approach_thresh = 0.99; - return (h1.getApproach().dot(h2.getApproach()) < approach_thresh) - && ((h1.getGraspBottom() - h2.getGraspBottom()).norm() > position_thresh); + return (h1.getApproach().dot(h2.getApproach()) < approach_thresh) && + ((h1.getPosition() - h2.getPosition()).norm() > position_thresh); } }; - -private: - + private: /** * \brief Draw (x,y,z) grasp samples from sum of Gaussians. * \param hands the list of grasp candidate sets - * \param generator Gaussian random number generator * \param sigma standard deviation of the Gaussian * \param num_gauss_samples number of samples to be drawn * \return the samples drawn from the sum of Gaussians */ - void drawSamplesFromSumOfGaussians(const std::vector& hands, Gaussian& generator, double sigma, - int num_gauss_samples, Eigen::Matrix3Xd& samples_out); + void drawSamplesFromSumOfGaussians( + const std::vector> &hand_sets, + double sigma, int num_gauss_samples, Eigen::Matrix3Xd &samples_out); /** * \brief Draw (x,y,z) grasp samples from max of Gaussians. * \param hands the list of grasp candidate sets - * \param generator Gaussian random number generator * \param sigma standard deviation of the Gaussian * \param num_gauss_samples number of samples to be drawn * \return the samples drawn from the sum of Gaussians */ - void drawSamplesFromMaxOfGaussians(const std::vector& hands, Gaussian& generator, double sigma, - int num_gauss_samples, Eigen::Matrix3Xd& samples_out, double term); + void drawSamplesFromMaxOfGaussians( + const std::vector> &hands, + double sigma, int num_gauss_samples, Eigen::Matrix3Xd &samples_out, + double term); - /** - * \brief Draw weighted (x,y,z) grasp samples from set of Gaussians. - * \param hands the list of grasp candidate sets - * \param generator Gaussian random number generator - * \param sigma standard deviation of the Gaussian - * \param num_gauss_samples number of samples to be drawn - * \param[out] samples_out the samples drawn from the set of Gaussians - */ - void drawWeightedSamples(const std::vector& hands, Gaussian& generator, double sigma, int num_gauss_samples, - Eigen::Matrix3Xd& samples_out); + void drawUniformSamples(const util::Cloud &cloud, int num_samples, + int start_idx, Eigen::Matrix3Xd &samples); - GraspDetector* grasp_detector_; ///< pointer to object for grasp detection + std::unique_ptr grasp_detector_; + std::unique_ptr clustering_; // sequential importance sampling parameters - int num_iterations_; ///< number of iterations of Sequential Importance Sampling - int num_samples_; ///< number of samples to use in each iteration - int num_init_samples_; ///< number of initial samples - double prob_rand_samples_; ///< probability of random samples - double radius_; ///< radius - int sampling_method_; ///< what sampling method is used (sum, max, weighted) + int num_iterations_; ///< number of iterations of CEM + int num_samples_; ///< number of samples to use in each iteration + int num_init_samples_; ///< number of initial samples + double prob_rand_samples_; ///< probability of random samples + double radius_; ///< standard deviation of Gaussian distribution + int sampling_method_; ///< what sampling method is used (sum, max, weighted) + double min_score_; ///< minimum score to consider a candidate as a grasp // visualization parameters - bool visualize_rounds_; ///< if all iterations are visualized - bool visualize_steps_; ///< if all grasp candidates and all valid grasps are visualized - bool visualize_results_; ///< if the final results are visualized + bool visualize_rounds_; ///< if all iterations are visualized + bool visualize_steps_; ///< if all grasp candidates and all valid grasps are + /// visualized + bool visualize_results_; ///< if the final results are visualized // grasp filtering parameters - bool filter_grasps_; ///< if grasps are filtered based on the robot's workspace - std::vector workspace_; ///< the robot's workspace - std::vector workspace_grasps_; ///< the robot's workspace - - int num_threads_; ///< number of CPU threads used in grasp detection - - // standard parameters - static const int NUM_ITERATIONS; - static const int NUM_SAMPLES; - static const int NUM_INIT_SAMPLES; - static const double PROB_RAND_SAMPLES; - static const double RADIUS; - static const bool VISUALIZE_STEPS; - static const bool VISUALIZE_RESULTS; - static const int SAMPLING_METHOD; + std::vector workspace_; ///< the robot's workspace + std::vector workspace_grasps_; ///< the robot's workspace + + bool filter_approach_direction_; + Eigen::Vector3d direction_; + double thresh_rad_; + + int num_threads_; ///< number of CPU threads used in grasp detection }; +} // namespace gpd + #endif /* SEQUENTIAL_IMPORTANCE_SAMPLING_H */ diff --git a/include/gpd/util/cloud.h b/include/gpd/util/cloud.h new file mode 100644 index 0000000..023c480 --- /dev/null +++ b/include/gpd/util/cloud.h @@ -0,0 +1,393 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, Andreas ten Pas + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * 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. + */ + +#ifndef CLOUD_H_ +#define CLOUD_H_ + +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#if defined(USE_PCL_GPU) // switch include ordering if compilation fails +#include +#include +#endif + +#include + +namespace gpd { +namespace util { + +typedef pcl::PointCloud PointCloudXYZ; +typedef pcl::PointCloud PointCloudRGB; +typedef pcl::PointCloud PointCloudPointNormal; +typedef pcl::PointCloud PointCloudNormal; + +/** + * + * \brief Multi-view point cloud + * + * Stores and processes a point cloud that has been observed from one or + * multiple camera view points. The raw point cloud is stored in + * `cloud_original_` and the processed point cloud in `cloud_processed_`. + * + */ +class Cloud { + public: + /** + * \brief Comparator for checking uniqueness of two 3D-vectors. + */ + struct UniqueVectorComparator { + /** + * \brief Compares two 3D-vectors for uniqueness. + * \param a the first 3D-vector + * \param b the second 3D-vector + * \return true if they differ in at least one element, false if all + * elements are equal + */ + bool operator()(const Eigen::Vector3i &a, const Eigen::Vector3i &b) { + for (int i = 0; i < a.size(); i++) { + if (a(i) != b(i)) { + return a(i) < b(i); + } + } + + return false; + } + }; + + /** + * \brief Comparator for checking uniqueness of two 4D-vectors. + */ + struct UniqueVector4First3Comparator { + /** + * \brief Compares two 4D-vectors for uniqueness (ignores the last element). + * \param a the first 4D-vector + * \param b the second 4D-vector + * \return true if they differ in at least one of the first three elements, + * false otherwise + */ + bool operator()(const Eigen::Vector4i &a, const Eigen::Vector4i &b) { + for (int i = 0; i < a.size() - 1; i++) { + if (a(i) != b(i)) { + return true; + } + } + + return false; + } + }; + + /** + * \brief Constructor. + */ + Cloud(); + + /** + * \brief Constructor. + * \param cloud the point cloud (of size n) + * \param camera_source the camera source for each point in the cloud (size: k + * x n) + * \param view_points the origins of the cameras (size: 3 x k) + */ + Cloud(const PointCloudRGB::Ptr &cloud, const Eigen::MatrixXi &camera_source, + const Eigen::Matrix3Xd &view_points); + + /** + * \brief Constructor. + * \param cloud the point cloud with surface normals (of size n) + * \param camera_source the camera source for each point in the cloud (size: k + * x n) + * \param view_points the origins of the cameras (size: 3 x k) + */ + Cloud(const PointCloudPointNormal::Ptr &cloud, + const Eigen::MatrixXi &camera_source, + const Eigen::Matrix3Xd &view_points); + + /** + * \brief Constructor for a two camera setup (left and right camera). + * \param cloud the point cloud (of size n) + * \param size_left_cloud the size of the point cloud from the left camera + * \param view_points the origins of the cameras (size: 3 x k) + */ + Cloud(const PointCloudRGB::Ptr &cloud, int size_left_cloud, + const Eigen::Matrix3Xd &view_points); + + /** + * \brief Constructor for a two cameras setup (left and right camera). + * \param cloud the point cloud with surface normals (of size n) + * \param size_left_cloud the size of the point cloud from the left camera + * \param view_points the origins of the cameras (size: 3 x k) + */ + Cloud(const PointCloudPointNormal::Ptr &cloud, int size_left_cloud, + const Eigen::Matrix3Xd &view_points); + + /** + * \brief Constructor for point cloud files (*.pcd). + * \param filename the location of the point cloud file + * \param view_points the origins of the cameras (size: 3 x k) + */ + Cloud(const std::string &filename, const Eigen::Matrix3Xd &view_points); + + /** + * \brief Constructor for point cloud files (*.pcd) from a two cameras setup. + * \param filename_left the location of the point cloud file from the first + * camera + * \param filename_right the location of the point cloud file from the second + * camera + * \param view_points the origins of the cameras (size: 3 x k) + */ + Cloud(const std::string &filename_left, const std::string &filename_right, + const Eigen::Matrix3Xd &view_points); + + /** + * \brief Remove NANs from the point cloud. + */ + void removeNans(); + + /** + * \brief Remove statistical outliers from the point cloud. + */ + void removeStatisticalOutliers(); + + void refineNormals(int k = 10); + + /** + * \brief Filter out points in the point cloud that lie outside the workspace + * dimensions. + * \param[in] workspace a 6-D vector containing the workspace limits: [minX, + * maxX, minY, maxY, minZ, maxZ] + */ + void filterWorkspace(const std::vector &workspace); + + /** + * \brief Filter out samples that lie outside the workspace dimensions. + * \param[in] workspace a 6-D vector containing the workspace limits: [minX, + * maxX, minY, maxY, minZ, maxZ] + */ + void filterSamples(const std::vector &workspace); + + /** + * \brief Voxelize the point cloud and keep track of the camera source for + * each voxel. + * \param[in] cell_size the size of each voxel + */ + void voxelizeCloud(float cell_size); + + /** + * \brief Subsample the point cloud according to the uniform distribution. + * \param[in] num_samples the number of samples to draw from the point cloud + */ + void subsampleUniformly(int num_samples); + + /** + * \brief Subsample the samples according to the uniform distribution. + * \param[in] num_samples the number of samples to draw from the samples + */ + void subsampleSamples(int num_samples); + + /** + * \brief Subsample the sample indices according to the uniform distribution. + * \param[in] num_samples the number of samples to draw from the indices + */ + void subsampleSampleIndices(int num_samples); + + /** + * \brief Sample above the support plane. + */ + void sampleAbovePlane(); + + /** + * \brief Subsample the cloud. + * + * A helper function that subsamples the cloud, the samples, or the sample + * indices depending on which component is available. + * \param[in] num_samples the number of samples to draw + */ + void subsample(int num_samples); + + /** + * \brief Estimate surface normals for the point cloud. + * Switches the normals estimation method based on if the point cloud is + * organized or not. \param[in] num_threads the number of CPU threads to be + * used in the calculation + */ + void calculateNormals(int num_threads, double radius); + + /** + * \brief Calculate surface normals for an organized point cloud. + */ + void calculateNormalsOrganized(); + + /** + * \brief Calculate surface normals for an unorganized point cloud. + * \param[in] num_threads the number of CPU threads to be used in the + * calculation + */ + void calculateNormalsOMP(int num_threads, double radius); + +#if defined(USE_PCL_GPU) + void calculateNormalsGPU(); +#endif + + /** + * \brief Reverse direction of surface normals (if a normal does not point to + * at least one camera). + */ + void reverseNormals(); + + /** + * \brief Set surface normals from a file. + * \param[in] filename the location of the file + */ + void setNormalsFromFile(const std::string &filename); + + /** + * \brief Write surface normals to a file. + * \param filename the location of the file + * \param normals the surface normals + */ + void writeNormalsToFile(const std::string &filename, + const Eigen::Matrix3Xd &normals); + + /** + * \brief Return the camera source matrix. + * \return the camera source matrix (k x n) + */ + const Eigen::MatrixXi &getCameraSource() const { return camera_source_; } + + /** + * \brief Return the preprocessed point cloud. + * \return the point cloud + */ + const PointCloudRGB::Ptr &getCloudProcessed() const { + return cloud_processed_; + } + + /** + * \brief Return the original point cloud. + * \return the point cloud + */ + const PointCloudRGB::Ptr &getCloudOriginal() const { return cloud_original_; } + + /** + * \brief Return the original point cloud. + * \return the point cloud + */ + const std::vector &getSampleIndices() const { return sample_indices_; } + + /** + * \brief Return the surface normals. + * \return the surface normals (size: 3 x n) + */ + const Eigen::Matrix3Xd &getNormals() const { return normals_; } + + /** + * \brief Return the samples. + * \return the samples (size: 3 x n) + */ + const Eigen::Matrix3Xd &getSamples() const { return samples_; } + + /** + * \brief Return the sample indices. + * \return the sample indices (size: n) + */ + void setSampleIndices(const std::vector &sampleIndices) { + sample_indices_ = sampleIndices; + } + + /** + * \brief Set the samples. + * \return the samples (size: 3 x n) + */ + void setSamples(const Eigen::Matrix3Xd &samples); + + /** + * \brief Set the surface normals. + * \return the surface normals (size: 3 x n) + */ + void setNormals(const Eigen::Matrix3Xd &normals) { normals_ = normals; } + + /** + * \brief Get the view points of the cameras. + * \return the origins of the cameras (size: 3 x k) + */ + const Eigen::Matrix3Xd &getViewPoints() const { return view_points_; } + + /** + * \brief Set the camera view points. + * \return the origins of the cameras (size: 3 x k) + */ + void setViewPoints(const Eigen::Matrix3Xd &view_points) { + view_points_ = view_points; + } + + /** + * \brief Load a point cloud from a file. + * \param filename the location of the file + * \return the point cloud + */ + PointCloudRGB::Ptr loadPointCloudFromFile(const std::string &filename) const; + + private: + std::vector> convertCameraSourceMatrixToLists(); + + PointCloudRGB::Ptr cloud_processed_; + PointCloudRGB::Ptr cloud_original_; + + // binary matrix: (i,j) = 1 if point j is seen by camera i, 0 otherwise + Eigen::MatrixXi camera_source_; + Eigen::Matrix3Xd normals_; + Eigen::Matrix3Xd view_points_; + + std::vector sample_indices_; + Eigen::Matrix3Xd samples_; +}; + +} // namespace util +} // namespace gpd + +#endif /* CLOUD_H_ */ diff --git a/include/gpd/util/config_file.h b/include/gpd/util/config_file.h new file mode 100644 index 0000000..973cbcf --- /dev/null +++ b/include/gpd/util/config_file.h @@ -0,0 +1,173 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, Andreas ten Pas + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * 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. + */ + +#ifndef CONFIG_FILE_H_ +#define CONFIG_FILE_H_ + +#include +#include +#include +#include +#include +#include +#include + +namespace gpd { +namespace util { + +/** + * + * \brief Configuration file + * + * Reads parameters from a configuration file (`*.cfg`). The configuration file + * is a key-value storage. + * + */ +class ConfigFile { + public: + /** + * \brief Constructor. + * \param fName the location of the configuration file + */ + ConfigFile(const std::string &fName); + + /** + * \brief Extract all keys. + * \return `false` if the configuration file cannot be found, `true` otherwise + */ + bool ExtractKeys(); + + /** + * \brief Check if a key exists. + * \param key the key + * \return `false` if the configuration file cannot be found, `true` otherwise + */ + bool keyExists(const std::string &key) const; + + /** + * \brief Return the value at a given key. + * \param key the key associated to the value + * \param defaultValue default value of the given key + * \return the value associated to the key + */ + template + ValueType getValueOfKey(const std::string &key, + ValueType const &defaultValue) const { + if (!keyExists(key)) return defaultValue; + + return string_to_T(contents.find(key)->second); + } + + /** + * \brief Return the value at a given key as a `string`. + * \param key the key + * \param defaultValue default value of the given key + * \return the value as a `string` + */ + std::string getValueOfKeyAsString(const std::string &key, + const std::string &defaultValue); + + /** + * \brief Return the value at a given key as a `std::vector`. + * \param key the key + * \param defaultValue default value of the given key + * \return the value as a `std::vector` + */ + std::vector getValueOfKeyAsStdVectorDouble( + const std::string &key, const std::string &defaultValue); + + /** + * \brief Return the value at a given key as a `std::vector`. + * \param key the key + * \param defaultValue default value of the given key + * \return the value as a `std::vector` + */ + std::vector getValueOfKeyAsStdVectorInt(const std::string &key, + const std::string &defaultValue); + + /** + * \brief Convert value of type `T` to `string`. + * \param value the value to be converted + * \return the value as a `string` + */ + template + std::string T_to_string(T const &val) const { + std::ostringstream ostr; + ostr << val; + + return ostr.str(); + } + + /** + * \brief Convert value of type `string` to type `T`. + * \param value the value to be converted + * \return the value as type `T` + */ + template + T string_to_T(std::string const &val) const { + std::istringstream istr(val); + T returnVal; + if (!(istr >> returnVal)) + std::cout << "CFG: Not a valid " + (std::string) typeid(T).name() + + " received!\n"; + + return returnVal; + } + + private: + void removeComment(std::string &line) const; + + bool onlyWhitespace(const std::string &line) const; + + bool validLine(const std::string &line) const; + + void extractKey(std::string &key, size_t const &sepPos, + const std::string &line) const; + + void extractValue(std::string &value, size_t const &sepPos, + const std::string &line) const; + + void extractContents(const std::string &line); + + void parseLine(const std::string &line, size_t const lineNo); + + std::vector stringToDouble(const std::string &str); + + std::vector stringToInt(const std::string &str); + + std::map contents; + std::string fName; +}; + +} // namespace util +} // namespace gpd + +#endif /* CONFIG_FILE_H_ */ diff --git a/include/gpd/util/eigen_utils.h b/include/gpd/util/eigen_utils.h new file mode 100644 index 0000000..0ef520d --- /dev/null +++ b/include/gpd/util/eigen_utils.h @@ -0,0 +1,81 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, Andreas ten Pas + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * 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. + */ + +#ifndef EIGEN_UTILS_H_ +#define EIGEN_UTILS_H_ + +#include + +#include + +namespace gpd { +namespace util { + +/** + * + * \brief Utility functions for the Eigen matrix library + * + * This namespace contains utility functions for the Eigen matrix library such + * as slicing matrices and rounding vectors. + * + */ +namespace EigenUtils { +/** + * \brief Slice a given matrix given a set of column indices. + * \param mat the matrix to be sliced + * \param indices set of column indices + * \return the columns of the given matrix contained in the indices set + */ +Eigen::Matrix3Xd sliceMatrix(const Eigen::Matrix3Xd &mat, + const std::vector &indices); + +/** + * \brief Slice a given matrix given a set of column indices. + * \param mat the matrix to be sliced + * \param indices set of column indices + * \return the columns of the given matrix contained in the indices set + */ +Eigen::MatrixXi sliceMatrix(const Eigen::MatrixXi &mat, + const std::vector &indices); + +/** + * \brief Round the elements of a floating point vector to the nearest, smaller + * integers. + * \param a the vector to be rounded down + * \return the vector containing the rounded down elements + */ +Eigen::Vector3i floorVector(const Eigen::Vector3f &a); +} // namespace EigenUtils + +} // namespace util +} // namespace gpd + +#endif /* EIGEN_UTILS_H_ */ diff --git a/include/gpd/util/plot.h b/include/gpd/util/plot.h new file mode 100644 index 0000000..9b4b685 --- /dev/null +++ b/include/gpd/util/plot.h @@ -0,0 +1,333 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, Andreas ten Pas + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * 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. + */ + +#ifndef PLOT_H +#define PLOT_H + +#include + +#include +#include +#include +#include +#include + +namespace gpd { +namespace util { + +typedef pcl::PointCloud PointCloudRGBA; +typedef pcl::PointCloud PointCloudPointNormal; + +typedef boost::shared_ptr PCLVisualizer; + +/** + * + * \brief Visualization utilities + * + * Provides visualization methods that use the PCL Visualizer. Allows to + * visualize samples, surface normals, grasps, and point clouds. + * + */ +class Plot { + public: + /** + * \brief Constructor. + * \param num_axes the number of orientation axes + * \param num_orientations the number of hand orientations + */ + Plot(int num_axes, int num_orientations) + : num_axes_(num_axes), num_orientations_(num_orientations) {} + + void plotHandGeometry(const candidate::Hand &hand, + const PointCloudRGBA::Ptr &cloud, + const candidate::HandGeometry &hand_geom, + const descriptor::ImageGeometry &image_geom); + + /** + * \brief Plot a list of hand sets and their associated volumes. + * \param hand_set_list the list of grasp sets + * \param cloud the point cloud to be plotted + * \param str the title of the plot window + * \param outer_diameter the outer diameter of the robot hand + * \param finger_width the width of the robot fingers + * \param hand_depth the depth of the robot hand + * \param hand_height the height of the robot hand + */ + void plotVolumes3D( + const std::vector> &hand_set_list, + const PointCloudRGBA::Ptr &cloud, std::string str, double outer_diameter, + double finger_width, double hand_depth, double hand_height, + double volume_width, double volume_depth, double volume_height); + + /** + * \brief Plot a list of hands and their associated volumes. + * \param hand_set_list the list of grasp sets + * \param cloud the point cloud to be plotted + * \param str the title of the plot window + * \param outer_diameter the outer diameter of the robot hand + * \param finger_width the width of the robot fingers + * \param hand_depth the depth of the robot hand + * \param hand_height the height of the robot hand + */ + void plotVolumes3D( + const std::vector> &hand_set_list, + const PointCloudRGBA::Ptr &cloud, std::string str, double outer_diameter, + double finger_width, double hand_depth, double hand_height, + double volume_width, double volume_depth, double volume_height); + + void plotFingers3D( + const std::vector> &hand_set_list, + const PointCloudRGBA::Ptr &cloud, std::string str, + const candidate::HandGeometry &geometry, bool draw_all = false, + bool draw_frame = false); + + /** + * \brief Plot a list of grasp sets with 3D cubes. + * \param hand_set_list the list of grasp sets + * \param cloud the point cloud to be plotted + * \param str the title of the plot window + * \param outer_diameter the outer diameter of the robot hand + * \param finger_width the width of the robot fingers + * \param hand_depth the depth of the robot hand + * \param hand_height the height of the robot hand + */ + void plotFingers3D(const std::vector &hand_set_list, + const PointCloudRGBA::Ptr &cloud, std::string str, + double outer_diameter, double finger_width, + double hand_depth, double hand_height, + bool draw_all = false, int num_axes = 1, + int num_orientations = 8); + + void plotFingers3D( + const std::vector> &hand_list, + const PointCloudRGBA::Ptr &cloud, const std::string &str, + const candidate::HandGeometry &geometry, bool use_same_color = true); + + void plotAntipodalHands( + const std::vector> &hand_list, + const PointCloudRGBA::Ptr &cloud, const std::string &str, + const candidate::HandGeometry &geometry); + + void plotValidHands( + const std::vector> &hand_list, + const PointCloudRGBA::Ptr &cloud, const PointCloudRGBA::Ptr &mesh, + const std::string &str, const candidate::HandGeometry &geometry); + + /** + * \brief Plot a list of grasps with 3D cubes. + * \param hand_list the list of grasps + * \param cloud the point cloud to be plotted + * \param str the title of the plot window + * \param outer_diameter the outer diameter of the robot hand + * \param finger_width the width of the robot fingers + * \param hand_depth the depth of the robot hand + * \param hand_height the height of the robot hand + */ + void plotFingers3D(const std::vector &hand_list, + const PointCloudRGBA::Ptr &cloud, std::string str, + double outer_diameter, double finger_width, + double hand_depth, double hand_height, + bool draw_all = false); + + /** + * \brief Plot a list of samples. + * \param index_list the list of samples (indices into the point cloud) + * \param cloud the point cloud to be plotted + */ + void plotSamples(const std::vector &index_list, + const PointCloudRGBA::Ptr &cloud); + + /** + * \brief Plot a list of samples. + * \param samples the list of samples (indices into the point cloud) + * \param cloud the point cloud to be plotted + */ + void plotSamples(const Eigen::Matrix3Xd &samples, + const PointCloudRGBA::Ptr &cloud); + + /** + * \brief Plot a point cloud that contains samples. + * \param samples_cloud the point cloud that contains the samples + * \param cloud the point cloud to be plotted + */ + void plotSamples(const PointCloudRGBA::Ptr &samples_cloud, + const PointCloudRGBA::Ptr &cloud); + + void plotNormals(const util::Cloud &cloud_cam, bool draw_camera_cone = false); + + void plotNormals(const PointCloudRGBA::Ptr &cloud, + const PointCloudRGBA::Ptr &cloud_samples, + const Eigen::Matrix3Xd &normals); + + /** + * \brief Plot a list of normals. + * \param cloud the point cloud to be plotted + * \param normals the normals to be plotted + */ + void plotNormals(const PointCloudRGBA::Ptr &cloud, + const Eigen::Matrix3Xd &normals); + + /** + * \brief Plot a list of points and their normals. + * \param pts the list of points to be plotted + * \param normals the normals to be plotted + */ + void plotNormals(const Eigen::Matrix3Xd &pts, + const Eigen::Matrix3Xd &normals); + + /** + * \brief Plot a list of local reference frames. + * \param frame_list the list of frames to be plotted + * \param cloud the point cloud to be plotted + */ + void plotLocalAxes(const std::vector &frames, + const PointCloudRGBA::Ptr &cloud); + + /** + * \brief Plot the camera source for each point in the point cloud. + * \param pts_cam_source_in the camera source for each point in the point + * cloud + * \param cloud the point cloud to be plotted + */ + void plotCameraSource(const Eigen::VectorXi &pts_cam_source_in, + const PointCloudRGBA::Ptr &cloud); + + /** + * \brief Plot a point cloud. + * \param cloud_rgb the point cloud to be plotted + * \param str the title of the plot window + */ + void plotCloud(const PointCloudRGBA::Ptr &cloud_rgb, + const std::string &title); + + private: + void addDimensions(const Eigen::Vector3d ¢er, const Eigen::Matrix3d &rot, + const Eigen::Vector3d &dimensions, + const Eigen::Matrix3d &colors, + const std::vector &labels, + PCLVisualizer &viewer); + + void addDoubleArrow(const Eigen::Vector3d &start, const Eigen::Vector3d &end, + const std::string &label, const Eigen::Vector3d &rgb, + PCLVisualizer &viewer, bool is_label_at_start = false); + + void plotHand3D(PCLVisualizer &viewer, const candidate::Hand &hand, + const candidate::HandGeometry &geometry, int idx, + const Eigen::Vector3d &rgb); + + /** + * \brief Plot a grasp. + * \param viewer viewer the PCL visualizer in which the grasp is plotted + * \param hand the grasp + * \param outer_diameter the outer diameter of the robot hand + * \param finger_width the width of the robot fingers + * \param hand_depth the depth of the robot hand + * \param hand_height the height of the robot hand + * \param idx the ID of the grasp in the viewer + */ + void plotHand3D(PCLVisualizer &viewer, const candidate::Hand &hand, + double outer_diameter, double finger_width, double hand_depth, + double hand_height, int idx, const Eigen::Vector3d &rgb); + + /** + * \brief Plot a cube. + * \param viewer viewer the PCL visualizer in which the grasp is plotted + * \param position the center of the cube + * \param rotation the orientation of the cube + * \param width the width of the cube + * \param height the height of the cube + * \param depth the depth of the cube + * \param name the name of the cube in the viewer + */ + void plotCube(PCLVisualizer &viewer, const Eigen::Vector3d &position, + const Eigen::Quaterniond &rotation, double width, double height, + double depth, const std::string &name, + const Eigen::Vector3d &rgb); + + void plotFrame(PCLVisualizer &viewer, const Eigen::Vector3d &translation, + const Eigen::Matrix3d &rotation, const std::string &id, + double axis_length = 0.02); + /** + * \brief Create a point cloud that stores the visual representations of the + * grasps. + * \param hand_list the list of grasps to be be stored in the point cloud + * \param outer_diameter the outer diameter of the visual grasp representation + */ + PointCloudRGBA::Ptr createFingersCloud( + const std::vector> &hand_list, + double outer_diameter); + + /** + * \brief Convert an Eigen vector to a PCL point. + * \param v the Eigen vector to be converted + */ + pcl::PointXYZRGBA eigenVector3dToPointXYZRGBA(const Eigen::Vector3d &v); + + /** + * \brief Add a point cloud with normals to a PCL visualizer. + * \param viewer the PCL visualizer that the cloud is added to + * \param cloud the cloud to be added + * \param line_width the line width for drawing normals + * \param color_cloud the color that is used to draw the cloud + * \param color_normals the color that is used to draw the normals + * \param cloud_name an identifier string for the cloud + * \param normals_name an identifier string for the normals + */ + void addCloudNormalsToViewer(PCLVisualizer &viewer, + const PointCloudPointNormal::Ptr &cloud, + double line_width, double *color_cloud, + double *color_normals, + const std::string &cloud_name, + const std::string &normals_name); + + /** + * \brief Run/show a PCL visualizer until an escape key is hit. + * \param viewer the PCL visualizer to be shown + */ + void runViewer(PCLVisualizer &viewer); + + /** + * \brief Create a PCL visualizer. + * \param title the title of the visualization window + */ + PCLVisualizer createViewer(std::string title); + + void keyboardEventOccurred(const pcl::visualization::KeyboardEvent &event, + void *viewer_void); + + int num_orientations_; + int num_axes_; +}; + +} // namespace util +} // namespace gpd + +#endif /* PLOT_H */ diff --git a/include/gpd/util/point_list.h b/include/gpd/util/point_list.h new file mode 100644 index 0000000..21649fa --- /dev/null +++ b/include/gpd/util/point_list.h @@ -0,0 +1,189 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, Andreas ten Pas + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * 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. + */ + +#ifndef POINT_LIST_H_ +#define POINT_LIST_H_ + +#include + +#include + +#include + +namespace gpd { +namespace util { + +/** + * + * \brief List of points + * + * Stores a list of *n* points (3 x n matrix) together with their surface + * normals + * (3 x n matrix). Also keeps information about which camera sees which point + * by storing the view points (3 x k matrix), i.e., locations, of *k* + * cameras, and for each of the *n* points, if the point is seen or not from + * a camera (k x n matrix). If point *i* is seen from camera *j*, then + * `cam_source(i,j)` = 1. Otherwise, `cam_source(i,j)` = 0. + * + */ +class PointList { + public: + /** + * \brief Default constructor. + */ + PointList() {} + + /** + * \brief Construct a list of n points. + * \param points the points (3 x n) + * \param normals the surface normals associated with the points (3 x n) + * \param cam_source the camera source for each point (k x n) + * \param view_points the origins of the cameras that saw the points (3 x k) + */ + PointList(const Eigen::Matrix3Xd &points, const Eigen::Matrix3Xd &normals, + const Eigen::MatrixXi &cam_source, + const Eigen::Matrix3Xd &view_points) + : points_(points), + normals_(normals), + cam_source_(cam_source), + view_points_(view_points) {} + + /** + * \brief Constructor. + * \param size number of points + * \param num_cams number of cameras that observed the points + */ + PointList(int size, int num_cams); + + /** + * \brief Slice the point list given a set of indices. + * \param indices the indices to be sliced + * \return the point list containing the points given by the indices + */ + PointList slice(const std::vector &indices) const; + + /** + * \brief Transform a point list to a robot hand frame. + * \param centroid the origin of the frame + * \param rotation the orientation of the frame (3 x 3 rotation matrix) + * \return the point list transformed into the hand frame + */ + PointList transformToHandFrame(const Eigen::Vector3d ¢roid, + const Eigen::Matrix3d &rotation) const; + + /** + * \brief Rotate a point list. + * \param rotation the 3 x 3 rotation matrix + * \return the rotated point list + */ + PointList rotatePointList(const Eigen::Matrix3d &rotation) const; + + /** + * \brief Crop the points by the height of the robot hand. + * \param height the robot hand height + * \param dim the dimension of the points corresponding to the height + */ + PointList cropByHandHeight(double height, int dim = 2) const; + + /** + * \brief Return the camera source matrix. + * \return the camera source matrix (size: k x n) + */ + const Eigen::MatrixXi &getCamSource() const { return cam_source_; } + + /** + * \brief Set the camera source matrix. + * \param cam_source the camera source matrix (size: k x n) + */ + void setCamSource(const Eigen::MatrixXi &cam_source) { + cam_source_ = cam_source; + } + + /** + * \brief Return the surface normals. + * \return the surface normals (size: 3 x n) + */ + const Eigen::Matrix3Xd &getNormals() const { return normals_; } + + /** + * \brief Set the surface normals. + * \param normals the surface normals (size: 3 x n) + */ + void setNormals(const Eigen::Matrix3Xd &normals) { normals_ = normals; } + + /** + * \brief Return the points. + * \return the points (size: 3 x n) + */ + const Eigen::Matrix3Xd &getPoints() const { return points_; } + + /** + * \brief Set the points. + * \param points the points (size: 3 x n) + */ + void setPoints(const Eigen::Matrix3Xd &points) { points_ = points; } + + /** + * \brief Return the size of the list. + * \return the number of points in the list + */ + int size() const { return points_.cols(); } + + /** + * \brief Return the view points of the cameras. + * \return the view points (size: 3 x k) + */ + const Eigen::Matrix3Xd &getViewPoints() const { return view_points_; } + + /** + * \brief Set the view points of the cameras. + * \param points the view points (size: 3 x k) + */ + void setViewPoints(const Eigen::Matrix3Xd &view_points) { + view_points_ = view_points; + } + + // The following macro makes sure that pointers are aligned correctly. + // See + // https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html. + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + private: + Eigen::Matrix3Xd points_; + Eigen::Matrix3Xd normals_; + Eigen::MatrixXi cam_source_; // camera source (k x n matrix of 1s and 0s) + Eigen::Matrix3Xd view_points_; +}; + +} // namespace util +} // namespace gpd + +#endif /* POINT_LIST_H_ */ diff --git a/include/nodes/grasp_detection_node.h b/include/nodes/grasp_detection_node.h deleted file mode 100644 index 5a543bc..0000000 --- a/include/nodes/grasp_detection_node.h +++ /dev/null @@ -1,219 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2015, Andreas ten Pas - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * 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. - */ - -#ifndef GRASP_DETECTION_NODE_H_ -#define GRASP_DETECTION_NODE_H_ - -// system -#include -#include - -// ROS -#include -#include -#include -#include -#include -#include - -// PCL -#include -#include -#include - -// GPG -#include - -// this project (messages) -#include -#include -#include -#include -#include -#include - -// this project (headers) -#include "../gpd/grasp_detector.h" -#include "../gpd/sequential_importance_sampling.h" - -#include - -typedef pcl::PointCloud PointCloudRGBA; -typedef pcl::PointCloud PointCloudPointNormal; - - -/** GraspDetectionNode class - * - * \brief A ROS node that can detect grasp poses in a point cloud. - * - * This class is a ROS node that handles all the ROS topics. - * -*/ -class GraspDetectionNode -{ -public: - - /** - * \brief Constructor. - * \param node the ROS node - */ - GraspDetectionNode(ros::NodeHandle& node); - - /** - * \brief Destructor. - */ - ~GraspDetectionNode() - { - delete cloud_camera_; - - if (use_importance_sampling_) - { - delete importance_sampling_; - } - - delete grasp_detector_; - } - - /** - * \brief Run the ROS node. Loops while waiting for incoming ROS messages. - */ - void run(); - - /** - * \brief Detect grasp poses in a point cloud received from a ROS topic. - * \return the list of grasp poses - */ - std::vector detectGraspPosesInTopic(); - - -private: - - /** - * \brief Find the indices of the points within a ball around a given point in the cloud. - * \param cloud the point cloud - * \param centroid the centroid of the ball - * \param radius the radius of the ball - * \return the indices of the points in the point cloud that lie within the ball - */ - std::vector getSamplesInBall(const PointCloudRGBA::Ptr& cloud, const pcl::PointXYZRGBA& centroid, float radius); - - /** - * \brief Callback function for the ROS topic that contains the input point cloud. - * \param msg the incoming ROS message - */ - void cloud_callback(const sensor_msgs::PointCloud2& msg); - - /** - * \brief Callback function for the ROS topic that contains the input point cloud and a list of indices. - * \param msg the incoming ROS message - */ - void cloud_indexed_callback(const gpd::CloudIndexed& msg); - - /** - * \brief Callback function for the ROS topic that contains the input point cloud and a list of (x,y,z) samples. - * \param msg the incoming ROS message - */ - void cloud_samples_callback(const gpd::CloudSamples& msg); - - /** - * \brief Callback function for the ROS service that reloads the rosparams. - * \param req, resp the service request and response - */ - bool set_params_callback(gpd::SetParameters::Request &req, gpd::SetParameters::Response &resp); - - /** - * \brief Initialize the object given a message. - * \param msg the message - */ - void initCloudCamera(const gpd::CloudSources& msg); - - /** - * \brief Callback function for the ROS topic that contains the input samples. - * \param msg the incoming ROS message - */ - void samples_callback(const gpd::SamplesMsg& msg); - - /** - * \brief Create a ROS message that contains a list of grasp poses from a list of handles. - * \param hands the list of grasps - * \return the ROS message that contains the grasp poses - */ - gpd::GraspConfigList createGraspListMsg(const std::vector& hands); - - gpd::GraspConfig convertToGraspMsg(const Grasp& hand); - - visualization_msgs::MarkerArray convertToVisualGraspMsg(const std::vector& hands, double outer_diameter, - double hand_depth, double finger_width, double hand_height, const std::string& frame_id); - - visualization_msgs::Marker createFingerMarker(const Eigen::Vector3d& center, const Eigen::Matrix3d& frame, - double length, double width, double height, int id, const std::string& frame_id); - - visualization_msgs::Marker createHandBaseMarker(const Eigen::Vector3d& start, const Eigen::Vector3d& end, - const Eigen::Matrix3d& frame, double length, double height, int id, const std::string& frame_id); - - Eigen::Matrix3Xd fillMatrixFromFile(const std::string& filename, int num_normals); - - Eigen::Vector3d view_point_; ///< (input) view point of the camera onto the point cloud - - CloudCamera* cloud_camera_; ///< stores point cloud with (optional) camera information and surface normals - std_msgs::Header cloud_camera_header_; ///< stores header of the point cloud - - int size_left_cloud_; ///< (input) size of the left point cloud (when using two point clouds as input) - bool has_cloud_, has_normals_, has_samples_; ///< status variables for received (input) messages - std::string frame_; ///< point cloud frame - - ros::NodeHandle nh_; ///< ROS node handle - ros::Subscriber cloud_sub_; ///< ROS subscriber for point cloud messages - ros::Subscriber samples_sub_; ///< ROS subscriber for samples messages - ros::Publisher grasps_pub_; ///< ROS publisher for grasp list messages - ros::Publisher grasps_rviz_pub_; ///< ROS publisher for grasps in rviz (visualization) - ros::ServiceServer srv_set_params_; ///< ROS service server for setting params - - bool use_importance_sampling_; ///< if importance sampling is used - bool filter_grasps_; ///< if grasps are filtered on workspace and gripper aperture - bool filter_half_antipodal_; ///< if half-antipodal grasps are filtered - bool plot_filtered_grasps_; ///< if filtered grasps are plotted - bool plot_selected_grasps_; ///< if selected grasps are plotted - bool plot_normals_; ///< if normals are plotted - bool plot_samples_; ///< if samples/indices are plotted - bool use_rviz_; ///< if rviz is used for visualization instead of PCL - std::vector workspace_; ///< workspace limits - - GraspDetector* grasp_detector_; ///< used to run the grasp pose detection - SequentialImportanceSampling* importance_sampling_; ///< sequential importance sampling variation of grasp pose detection - - /** constants for input point cloud types */ - static const int POINT_CLOUD_2; ///< sensor_msgs/PointCloud2 - static const int CLOUD_INDEXED; ///< gpd/CloudIndexed - static const int CLOUD_SAMPLES; ///< gpd/CloudSamples -}; - -#endif /* GRASP_DETECTION_NODE_H_ */ diff --git a/launch/baxter_15_channels.launch b/launch/baxter_15_channels.launch deleted file mode 100644 index ae90a64..0000000 --- a/launch/baxter_15_channels.launch +++ /dev/null @@ -1,68 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - [0.51, 0.9, -0.43, 0.15, -0.29, 1.0] - [0, 0, 0.2] - - - - - - - - - - - - [0.55, 1.0, -0.41, 0.13, -0.29, 1.0] - - - - - - - - - - - - - - - - - - diff --git a/launch/baxter_3_channels.launch b/launch/baxter_3_channels.launch deleted file mode 100644 index 3e5bfc0..0000000 --- a/launch/baxter_3_channels.launch +++ /dev/null @@ -1,68 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - [0.51, 0.9, -0.43, 0.05, -0.29, 0.8] - [0, 0, 0.2] - - - - - - - - - - - - - [0.55, 1.0, -0.41, 0.03, -0.29, 1.0] - - - - - - - - - - - - - - - - - diff --git a/launch/caffe/classifier_15channels.launch b/launch/caffe/classifier_15channels.launch deleted file mode 100644 index cc51497..0000000 --- a/launch/caffe/classifier_15channels.launch +++ /dev/null @@ -1,18 +0,0 @@ - - - - - - - - - - - - - - - - - - diff --git a/launch/caffe/classifier_3channels.launch b/launch/caffe/classifier_3channels.launch deleted file mode 100644 index c4964a2..0000000 --- a/launch/caffe/classifier_3channels.launch +++ /dev/null @@ -1,17 +0,0 @@ - - - - - - - - - - - - - - - - - diff --git a/launch/caffe/ur5_classifier_15channels.launch b/launch/caffe/ur5_classifier_15channels.launch deleted file mode 100644 index 4380b29..0000000 --- a/launch/caffe/ur5_classifier_15channels.launch +++ /dev/null @@ -1,17 +0,0 @@ - - - - - - - - - - - - - - - - - diff --git a/launch/classify_candidates_file_15_channels.launch b/launch/classify_candidates_file_15_channels.launch deleted file mode 100644 index 1678804..0000000 --- a/launch/classify_candidates_file_15_channels.launch +++ /dev/null @@ -1,71 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - [-1, 1, -1, 1, -1, 1] - - [0, 0, 0] - - - - - - - - - - - - - - [-1, 1, -1, 1, -1, 1] - - - - - - - - - - - - - - - diff --git a/launch/classify_candidates_file_3_channels.launch b/launch/classify_candidates_file_3_channels.launch deleted file mode 100644 index 92b3fd7..0000000 --- a/launch/classify_candidates_file_3_channels.launch +++ /dev/null @@ -1,67 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - [-1, 1, -1, 1, -1, 1] - [0, 0, 0] - - - - - - - - - - - - - [0.6, 1.0, -0.33, 0.13, -0.3, 1.0] - - - - - - - - - - - - - - - - - diff --git a/launch/create_grasp_images.launch b/launch/create_grasp_images.launch deleted file mode 100644 index f446cf0..0000000 --- a/launch/create_grasp_images.launch +++ /dev/null @@ -1,61 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - [-1, 1, -1, 1, -1, 1] - [0, 0, 0] - - - - - - - - - - - - - - - [0.6, 1.0, -0.33, 0.13, -0.3, 1.0] - - - - - - - - - - - - - - - diff --git a/launch/create_training_data.launch b/launch/create_training_data.launch deleted file mode 100644 index 1880676..0000000 --- a/launch/create_training_data.launch +++ /dev/null @@ -1,62 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - [-1, 1, -1, 1, -1, 1] - [0, 0, 0] - - - - - - - - - - - - - - - [0.6, 1.0, -0.33, 0.13, -0.3, 1.0] - - - - - - - - - - - - - - - diff --git a/launch/hand_geometry.launch b/launch/hand_geometry.launch deleted file mode 100644 index 05f5f48..0000000 --- a/launch/hand_geometry.launch +++ /dev/null @@ -1,17 +0,0 @@ - - - - - - - - - - - - - diff --git a/launch/importance_sampling_baxter_15_channels.launch b/launch/importance_sampling_baxter_15_channels.launch deleted file mode 100644 index 860a655..0000000 --- a/launch/importance_sampling_baxter_15_channels.launch +++ /dev/null @@ -1,78 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - [0.51, 0.9, -0.43, 0.05, -0.29, 0.8] - [0, 0, 0.2] - - - - - - - - - - - - - [0.55, 1.0, -0.41, 0.03, -0.29, 1.0] - - - - - - - - - - - - - - - - - diff --git a/launch/importance_sampling_baxter_3_channels.launch b/launch/importance_sampling_baxter_3_channels.launch deleted file mode 100644 index ace481f..0000000 --- a/launch/importance_sampling_baxter_3_channels.launch +++ /dev/null @@ -1,78 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - [0.51, 0.9, -0.43, 0.05, -0.29, 0.8] - [0, 0, 0.2] - - - - - - - - - - - - - [0.55, 1.0, -0.41, 0.03, -0.29, 1.0] - - - - - - - - - - - - - - - - - diff --git a/launch/importance_sampling_file_15_channels.launch b/launch/importance_sampling_file_15_channels.launch deleted file mode 100644 index 3e2ddeb..0000000 --- a/launch/importance_sampling_file_15_channels.launch +++ /dev/null @@ -1,68 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - [0.5, 0.9, -0.4, 0.25, -0.28, 1] - [0, 0, 0] - - - - - - - - - - - - - [0.57, 0.87, -0.38, 0.23, -0.26, 1] - - - - - - - - - - - - - - - diff --git a/launch/tutorial0.launch b/launch/tutorial0.launch deleted file mode 100644 index a17389c..0000000 --- a/launch/tutorial0.launch +++ /dev/null @@ -1,65 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - [-1, 1, -1, 1, -1, 1] - [0, 0, 0] - - - - - - - - - - - - - - [-1, 1, -1, 1, -1, 1] - - - - - - - - - - - - - - - diff --git a/launch/tutorial1.launch b/launch/tutorial1.launch deleted file mode 100644 index ebe928b..0000000 --- a/launch/tutorial1.launch +++ /dev/null @@ -1,74 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - [-1, 1, -1, 1, -1, 1] - [0, 0, 0] - - - - - - - - - - - - [0.55, 1.0, -0.41, 0.03, -0.29, 1.0] - - - - - - - - - - - - - - - - - - diff --git a/launch/tutorial2.launch b/launch/tutorial2.launch deleted file mode 100644 index 82a2cd4..0000000 --- a/launch/tutorial2.launch +++ /dev/null @@ -1,69 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - [-1, 1, -1, 1, -1, 1] - [0, 0, 0] - - - - - - - - - - - - [0.55, 1.0, -0.41, 0.03, -0.29, 1.0] - - - - - - - - - - - - - - - - - - diff --git a/launch/ur5_15_channels.launch b/launch/ur5_15_channels.launch deleted file mode 100644 index 98fe1a8..0000000 --- a/launch/ur5_15_channels.launch +++ /dev/null @@ -1,70 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - [-1, 1, -1, 1, -1, 1] - [0, 0, 0] - - - - - - - - - - - - [0.30, 0.90, -0.30, 0.30, -0.12, 0.40] - - - - - - - - - - - - - - - - - - diff --git a/launch/ur5_hand_geometry.launch b/launch/ur5_hand_geometry.launch deleted file mode 100644 index 251f9ad..0000000 --- a/launch/ur5_hand_geometry.launch +++ /dev/null @@ -1,16 +0,0 @@ - - - - - - - - - - - - diff --git a/caffe/15channels/lenet_15_channels.prototxt b/models/caffe/15channels/lenet_15_channels.prototxt similarity index 100% rename from caffe/15channels/lenet_15_channels.prototxt rename to models/caffe/15channels/lenet_15_channels.prototxt diff --git a/caffe/15channels/lenet_15_channels_train_val.prototxt b/models/caffe/15channels/lenet_15_channels_train_val.prototxt similarity index 100% rename from caffe/15channels/lenet_15_channels_train_val.prototxt rename to models/caffe/15channels/lenet_15_channels_train_val.prototxt diff --git a/caffe/15channels/lenet_solver_15_channels.prototxt b/models/caffe/15channels/lenet_solver_15_channels.prototxt similarity index 100% rename from caffe/15channels/lenet_solver_15_channels.prototxt rename to models/caffe/15channels/lenet_solver_15_channels.prototxt diff --git a/caffe/15channels/single_view_15_channels.caffemodel b/models/caffe/15channels/single_view_15_channels.caffemodel similarity index 100% rename from caffe/15channels/single_view_15_channels.caffemodel rename to models/caffe/15channels/single_view_15_channels.caffemodel diff --git a/caffe/15channels/two_views_15_channels_53_deg.caffemodel b/models/caffe/15channels/two_views_15_channels_53_deg.caffemodel similarity index 100% rename from caffe/15channels/two_views_15_channels_53_deg.caffemodel rename to models/caffe/15channels/two_views_15_channels_53_deg.caffemodel diff --git a/caffe/15channels/two_views_15_channels_90_deg.caffemodel b/models/caffe/15channels/two_views_15_channels_90_deg.caffemodel similarity index 100% rename from caffe/15channels/two_views_15_channels_90_deg.caffemodel rename to models/caffe/15channels/two_views_15_channels_90_deg.caffemodel diff --git a/caffe/15channels/two_views_15_channels_90_deg_no_flipping.caffemodel b/models/caffe/15channels/two_views_15_channels_90_deg_no_flipping.caffemodel similarity index 100% rename from caffe/15channels/two_views_15_channels_90_deg_no_flipping.caffemodel rename to models/caffe/15channels/two_views_15_channels_90_deg_no_flipping.caffemodel diff --git a/caffe/3channels/bottles_boxes_cans_5xNeg.caffemodel b/models/caffe/3channels/bottles_boxes_cans_5xNeg.caffemodel similarity index 100% rename from caffe/3channels/bottles_boxes_cans_5xNeg.caffemodel rename to models/caffe/3channels/bottles_boxes_cans_5xNeg.caffemodel diff --git a/caffe/3channels/lenet_3_channels.prototxt b/models/caffe/3channels/lenet_3_channels.prototxt similarity index 100% rename from caffe/3channels/lenet_3_channels.prototxt rename to models/caffe/3channels/lenet_3_channels.prototxt diff --git a/caffe/labels.txt b/models/caffe/labels.txt similarity index 100% rename from caffe/labels.txt rename to models/caffe/labels.txt diff --git a/models/lenet/15channels/params/conv1_biases.bin b/models/lenet/15channels/params/conv1_biases.bin new file mode 100644 index 0000000..4f24a72 --- /dev/null +++ b/models/lenet/15channels/params/conv1_biases.bin @@ -0,0 +1 @@ +^ÿº¼ï<07=ñ¶o< ‘=ñǽh º»‹ ˜½~[œ<áåý¼þ==ɽ@<ÒƽÀò =?2=ÿ[—=xMz=ÊXk=âùj½ª²» \ No newline at end of file diff --git a/models/lenet/15channels/params/conv1_weights.bin b/models/lenet/15channels/params/conv1_weights.bin new file mode 100644 index 0000000..0c56106 Binary files /dev/null and b/models/lenet/15channels/params/conv1_weights.bin differ diff --git a/models/lenet/15channels/params/conv2_biases.bin b/models/lenet/15channels/params/conv2_biases.bin new file mode 100644 index 0000000..e9b8db0 Binary files /dev/null and b/models/lenet/15channels/params/conv2_biases.bin differ diff --git a/models/lenet/15channels/params/conv2_weights.bin b/models/lenet/15channels/params/conv2_weights.bin new file mode 100644 index 0000000..16fbc45 Binary files /dev/null and b/models/lenet/15channels/params/conv2_weights.bin differ diff --git a/models/lenet/15channels/params/ip1_biases.bin b/models/lenet/15channels/params/ip1_biases.bin new file mode 100644 index 0000000..7359409 Binary files /dev/null and b/models/lenet/15channels/params/ip1_biases.bin differ diff --git a/models/lenet/15channels/params/ip1_weights.bin b/models/lenet/15channels/params/ip1_weights.bin new file mode 100644 index 0000000..425ee17 Binary files /dev/null and b/models/lenet/15channels/params/ip1_weights.bin differ diff --git a/models/lenet/15channels/params/ip2_biases.bin b/models/lenet/15channels/params/ip2_biases.bin new file mode 100644 index 0000000..2852461 --- /dev/null +++ b/models/lenet/15channels/params/ip2_biases.bin @@ -0,0 +1 @@ +ýº;=õº;½ \ No newline at end of file diff --git a/models/lenet/15channels/params/ip2_weights.bin b/models/lenet/15channels/params/ip2_weights.bin new file mode 100644 index 0000000..b109bb8 Binary files /dev/null and b/models/lenet/15channels/params/ip2_weights.bin differ diff --git a/models/lenet/3channels/params/conv1_biases.bin b/models/lenet/3channels/params/conv1_biases.bin new file mode 100644 index 0000000..43093e0 --- /dev/null +++ b/models/lenet/3channels/params/conv1_biases.bin @@ -0,0 +1,3 @@ + +1½ê°é½µ9~=‹ Û;·{º½Ú§˜=¿„‡»¼¸˜½½ø*¢½ã<<‚ü½{j8½ ›v=<œÿ¼{Þ<çð:=è™g; +6(=‡®ƒ¼ \ No newline at end of file diff --git a/models/lenet/3channels/params/conv1_weights.bin b/models/lenet/3channels/params/conv1_weights.bin new file mode 100644 index 0000000..65c3670 Binary files /dev/null and b/models/lenet/3channels/params/conv1_weights.bin differ diff --git a/models/lenet/3channels/params/conv2_biases.bin b/models/lenet/3channels/params/conv2_biases.bin new file mode 100644 index 0000000..11bcc05 Binary files /dev/null and b/models/lenet/3channels/params/conv2_biases.bin differ diff --git a/models/lenet/3channels/params/conv2_weights.bin b/models/lenet/3channels/params/conv2_weights.bin new file mode 100644 index 0000000..0ffc94e Binary files /dev/null and b/models/lenet/3channels/params/conv2_weights.bin differ diff --git a/models/lenet/3channels/params/ip1_biases.bin b/models/lenet/3channels/params/ip1_biases.bin new file mode 100644 index 0000000..dbcfd76 Binary files /dev/null and b/models/lenet/3channels/params/ip1_biases.bin differ diff --git a/models/lenet/3channels/params/ip1_weights.bin b/models/lenet/3channels/params/ip1_weights.bin new file mode 100644 index 0000000..885662a Binary files /dev/null and b/models/lenet/3channels/params/ip1_weights.bin differ diff --git a/models/lenet/3channels/params/ip2_biases.bin b/models/lenet/3channels/params/ip2_biases.bin new file mode 100644 index 0000000..b2423bb --- /dev/null +++ b/models/lenet/3channels/params/ip2_biases.bin @@ -0,0 +1 @@ +l2>c2¾ \ No newline at end of file diff --git a/models/lenet/3channels/params/ip2_weights.bin b/models/lenet/3channels/params/ip2_weights.bin new file mode 100644 index 0000000..f620099 Binary files /dev/null and b/models/lenet/3channels/params/ip2_weights.bin differ diff --git a/models/openvino/two_views_12_channels_curv_axis.bin b/models/openvino/two_views_12_channels_curv_axis.bin new file mode 100644 index 0000000..0ca9221 Binary files /dev/null and b/models/openvino/two_views_12_channels_curv_axis.bin differ diff --git a/models/openvino/two_views_12_channels_curv_axis.xml b/models/openvino/two_views_12_channels_curv_axis.xml new file mode 100644 index 0000000..e890724 --- /dev/null +++ b/models/openvino/two_views_12_channels_curv_axis.xml @@ -0,0 +1,247 @@ + + + + + + + 1 + 12 + 60 + 60 + + + + + + + + 1 + 12 + 60 + 60 + + + + + 1 + 20 + 56 + 56 + + + + + + + + + + + 1 + 20 + 56 + 56 + + + + + 1 + 20 + 56 + 56 + + + + + + + + 1 + 20 + 56 + 56 + + + + + 1 + 20 + 28 + 28 + + + + + + + + 1 + 20 + 28 + 28 + + + + + 1 + 50 + 24 + 24 + + + + + + + + + + + 1 + 50 + 24 + 24 + + + + + 1 + 50 + 24 + 24 + + + + + + + + 1 + 50 + 24 + 24 + + + + + 1 + 50 + 12 + 12 + + + + + + + + 1 + 50 + 12 + 12 + + + + + 1 + 7200 + + + + + + + + 1 + 7200 + + + + + 1 + 500 + + + + + + + + + + + 1 + 500 + + + + + 1 + 500 + + + + + + + + 1 + 500 + + + + + 1 + 2 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/models/openvino/two_views_15_channels_53_deg.bin b/models/openvino/two_views_15_channels_53_deg.bin new file mode 100644 index 0000000..af2de05 Binary files /dev/null and b/models/openvino/two_views_15_channels_53_deg.bin differ diff --git a/models/openvino/two_views_15_channels_53_deg.xml b/models/openvino/two_views_15_channels_53_deg.xml new file mode 100644 index 0000000..cabb9b0 --- /dev/null +++ b/models/openvino/two_views_15_channels_53_deg.xml @@ -0,0 +1,163 @@ + + + + + + + 100 + 15 + 60 + 60 + + + + + + + + 100 + 15 + 60 + 60 + + + + + 100 + 20 + 56 + 56 + + + + + + + + + + + + 100 + 20 + 56 + 56 + + + + + 100 + 20 + 28 + 28 + + + + + + + + 100 + 20 + 28 + 28 + + + + + 100 + 50 + 24 + 24 + + + + + + + + + + + + 100 + 50 + 24 + 24 + + + + + 100 + 50 + 12 + 12 + + + + + + + + 100 + 50 + 12 + 12 + + + + + 100 + 500 + + + + + + + + + + + + 100 + 500 + + + + + 100 + 500 + + + + + + + + 100 + 500 + + + + + 100 + 2 + + + + + + + + + + + + + + + + + + diff --git a/models/openvino/two_views_15_channels_90_deg.bin b/models/openvino/two_views_15_channels_90_deg.bin new file mode 100644 index 0000000..a783ef8 Binary files /dev/null and b/models/openvino/two_views_15_channels_90_deg.bin differ diff --git a/models/openvino/two_views_15_channels_90_deg.xml b/models/openvino/two_views_15_channels_90_deg.xml new file mode 100644 index 0000000..cabb9b0 --- /dev/null +++ b/models/openvino/two_views_15_channels_90_deg.xml @@ -0,0 +1,163 @@ + + + + + + + 100 + 15 + 60 + 60 + + + + + + + + 100 + 15 + 60 + 60 + + + + + 100 + 20 + 56 + 56 + + + + + + + + + + + + 100 + 20 + 56 + 56 + + + + + 100 + 20 + 28 + 28 + + + + + + + + 100 + 20 + 28 + 28 + + + + + 100 + 50 + 24 + 24 + + + + + + + + + + + + 100 + 50 + 24 + 24 + + + + + 100 + 50 + 12 + 12 + + + + + + + + 100 + 50 + 12 + 12 + + + + + 100 + 500 + + + + + + + + + + + + 100 + 500 + + + + + 100 + 500 + + + + + + + + 100 + 500 + + + + + 100 + 2 + + + + + + + + + + + + + + + + + + diff --git a/models/openvino/two_views_3_channels_curv_axis.bin b/models/openvino/two_views_3_channels_curv_axis.bin new file mode 100644 index 0000000..c32d6e7 Binary files /dev/null and b/models/openvino/two_views_3_channels_curv_axis.bin differ diff --git a/models/openvino/two_views_3_channels_curv_axis.xml b/models/openvino/two_views_3_channels_curv_axis.xml new file mode 100644 index 0000000..1001892 --- /dev/null +++ b/models/openvino/two_views_3_channels_curv_axis.xml @@ -0,0 +1,216 @@ + + + + + + + 1 + 3 + 60 + 60 + + + + + + + + 1 + 3 + 60 + 60 + + + + + 1 + 20 + 56 + 56 + + + + + + + + + + + 1 + 20 + 56 + 56 + + + + + 1 + 20 + 56 + 56 + + + + + + + + 1 + 20 + 56 + 56 + + + + + 1 + 20 + 28 + 28 + + + + + + + + 1 + 20 + 28 + 28 + + + + + 1 + 50 + 24 + 24 + + + + + + + + + + + 1 + 50 + 24 + 24 + + + + + 1 + 50 + 24 + 24 + + + + + + + + 1 + 50 + 24 + 24 + + + + + 1 + 50 + 12 + 12 + + + + + + + + 1 + 50 + 12 + 12 + + + + + 1 + 7200 + + + + + + + + 1 + 7200 + + + + + 1 + 500 + + + + + + + + + + + 1 + 500 + + + + + 1 + 500 + + + + + + + + 1 + 500 + + + + + 1 + 2 + + + + + + + + + + + + + + + + + + + + + diff --git a/msg/CloudIndexed.msg b/msg/CloudIndexed.msg deleted file mode 100644 index 4f88f33..0000000 --- a/msg/CloudIndexed.msg +++ /dev/null @@ -1,8 +0,0 @@ -# This message holds a point cloud and a list of indices into the point cloud -# at which to sample grasp candidates. - -# The point cloud. -gpd/CloudSources cloud_sources - -# The indices into the point cloud at which to sample grasp candidates. -std_msgs/Int64[] indices diff --git a/msg/CloudSamples.msg b/msg/CloudSamples.msg deleted file mode 100644 index 15e4df7..0000000 --- a/msg/CloudSamples.msg +++ /dev/null @@ -1,8 +0,0 @@ -# This message holds a point cloud and a list of samples at which the grasp -# detector should search for grasp candidates. - -# The point cloud. -gpd/CloudSources cloud_sources - -# The samples, as (x,y,z) points, at which to search for grasp candidates. -geometry_msgs/Point[] samples diff --git a/msg/CloudSources.msg b/msg/CloudSources.msg deleted file mode 100644 index d2d3be0..0000000 --- a/msg/CloudSources.msg +++ /dev/null @@ -1,12 +0,0 @@ -# This message holds a point cloud that can be a combination of point clouds -# from different camera sources (at least one). For each point in the cloud, -# this message also stores the index of the camera that produced the point. - -# The point cloud. -sensor_msgs/PointCloud2 cloud - -# For each point in the cloud, the index of the camera that acquired the point. -std_msgs/Int64[] camera_source - -# A list of camera positions at which the point cloud was acquired. -geometry_msgs/Point[] view_points \ No newline at end of file diff --git a/msg/GraspConfig.msg b/msg/GraspConfig.msg deleted file mode 100644 index 20185fe..0000000 --- a/msg/GraspConfig.msg +++ /dev/null @@ -1,19 +0,0 @@ -# This message describes a 2-finger grasp configuration by its 6-DOF pose, -# consisting of a 3-DOF position and 3-DOF orientation, and the opening -# width of the robot hand. - -# Position -geometry_msgs/Point bottom # centered bottom/base of the robot hand) -geometry_msgs/Point top # centered top/fingertip of the robot hand) -geometry_msgs/Point surface # centered position on object surface - -# Orientation represented as three axis (R = [approach binormal axis]) -geometry_msgs/Vector3 approach # The grasp approach direction -geometry_msgs/Vector3 binormal # The binormal -geometry_msgs/Vector3 axis # The hand axis - -geometry_msgs/Point sample # Point at which the grasp was found - -std_msgs/Float32 width # Required aperture (opening width) of the robot hand - -std_msgs/Float32 score # Score assigned to the grasp by the classifier diff --git a/msg/GraspConfigList.msg b/msg/GraspConfigList.msg deleted file mode 100644 index 5f19ca4..0000000 --- a/msg/GraspConfigList.msg +++ /dev/null @@ -1,7 +0,0 @@ -# This message stores a list of grasp configurations. - -# The time of acquisition, and the coordinate frame ID. -Header header - -# The list of grasp configurations. -gpd/GraspConfig[] grasps diff --git a/msg/SamplesMsg.msg b/msg/SamplesMsg.msg deleted file mode 100644 index 7852c6e..0000000 --- a/msg/SamplesMsg.msg +++ /dev/null @@ -1,7 +0,0 @@ -# This message describes a set of point samples at which to detect grasps. - -# Header -Header header - -# The samples, as (x,y,z) points, at which to search for grasp candidates. -geometry_msgs/Point[] samples diff --git a/package.xml b/package.xml deleted file mode 100644 index d135748..0000000 --- a/package.xml +++ /dev/null @@ -1,62 +0,0 @@ - - - gpd - 1.0.0 - This package detects 6-DOF grasp poses for a parallel jaw gripper in 3D point clouds. - - - - - Andreas ten Pas - - - - - BSD - - - - - - - - - - - - - - - - - cmake_modules - eigen_conversions - geometry_msgs - libpcl-all-dev - message_generation - roscpp - sensor_msgs - std_msgs - - - cmake_modules - eigen_conversions - geometry_msgs - libpcl-all - message_runtime - roscpp - sensor_msgs - std_msgs - - - - - catkin - - - - - - - - diff --git a/pytorch/.torch_to_onxx.py.swp b/pytorch/.torch_to_onxx.py.swp new file mode 100644 index 0000000..85a9c63 Binary files /dev/null and b/pytorch/.torch_to_onxx.py.swp differ diff --git a/pytorch/hdf5_dataset.py b/pytorch/hdf5_dataset.py new file mode 100644 index 0000000..74f144e --- /dev/null +++ b/pytorch/hdf5_dataset.py @@ -0,0 +1,24 @@ +import h5py +import h5py_cache as h5c +import torch +import torch.utils.data as torchdata + +CHUNK_CACHE_MEM_SIZE = 1024**2*4000 + +class H5Dataset(torchdata.Dataset): + def __init__(self, file_path, start=0, end=None): + super(H5Dataset, self).__init__() + with h5c.File(file_path, 'r', chunk_cache_mem_size=CHUNK_CACHE_MEM_SIZE) as f: + self.images = torch.from_numpy(f['images'][start : end]) + self.labels = torch.from_numpy(f['labels'][start : end]).to(torch.int32) + print("Loaded images of shape {}, type {}, and labels of shape {}, type {}.".format(self.images.shape, self.images.dtype, self.labels.shape, self.labels.dtype)) + + def __getitem__(self, index): + image = self.images[index,:,:].to(torch.float32) * 1/256.0 + # Pytorch uses NCHW format + image = image.permute(2, 0, 1) + label = self.labels[index,:][0] + return (image, label) + + def __len__(self): + return self.labels.shape[0] diff --git a/pytorch/hdf5_loader.py b/pytorch/hdf5_loader.py new file mode 100644 index 0000000..879c34d --- /dev/null +++ b/pytorch/hdf5_loader.py @@ -0,0 +1,34 @@ +import h5py +import torch +import sys +import torch.utils.data as torchdata +import torch.multiprocessing + +torch.multiprocessing.set_start_method('spawn') + +class H5Dataset(torchdata.Dataset): + + def __init__(self, file_path): + super(H5Dataset, self).__init__() + h5_file = h5py.File(file_path) + self.data = h5_file.get('images') + self.target = h5_file.get('labels') + + def __getitem__(self, index): + return (torch.from_numpy(self.data[index]).float(), + torch.from_numpy(self.target[index]).int()) + + def __len__(self): + return self.data.shape[0] + +train_dset = H5Dataset(sys.argv[1]) +train_loader = torchdata.DataLoader(train_dset, batch_size=64, shuffle=True, num_workers=0) + +print('Iterating over dataset ...') + +for i_batch, sample_batched in enumerate(train_loader): + print(i_batch, sample_batched[0].size(), sample_batched[0].size()) + + # observe 4th batch and stop. + if i_batch == 3: + break diff --git a/pytorch/hdf5_to_lmdb.py b/pytorch/hdf5_to_lmdb.py new file mode 100644 index 0000000..7d93531 --- /dev/null +++ b/pytorch/hdf5_to_lmdb.py @@ -0,0 +1,22 @@ +import h5py +import lmdb +import numpy as np +import sys + +h5_file = h5py.File(sys.argv[1]) +data = h5_file.get('images') +target = h5_file.get('labels') + +num = int(sys.argv[3]) +data = data[:num] +target = target[:num] + +map_size = data.nbytes * 10 +env = lmdb.open(sys.argv[2], map_size=map_size) + +for i in range(data.shape[0]): + with env.begin(write=True) as txn: + txn.put('X_' + str(i), data[i]) + txn.put('y_' + str(i), target[i]) + if i % 1000 == 0: + print i, data.shape[0] diff --git a/pytorch/hdf5_to_zarr.py b/pytorch/hdf5_to_zarr.py new file mode 100644 index 0000000..0529c8b --- /dev/null +++ b/pytorch/hdf5_to_zarr.py @@ -0,0 +1,15 @@ +import h5py +import numpy as np +import sys +import zarr + +print('Loading database ...') +h5_file = h5py.File(sys.argv[1]) +images = h5_file.get('images') +labels = h5_file.get('labels') + +root = zarr.open(sys.argv[2], mode='w') +print('Writing images of shape', images.shape, '...') +images_out = root.create_dataset('images', data=images, dtype=images.dtype) #, chunks=(5000,) + images.shape[1:]) +print('Writing labels of shape', labels.shape, '...') +labels_out = root.create_dataset('labels', data=labels, dtype=labels.dtype) diff --git a/pytorch/multiproc.py b/pytorch/multiproc.py new file mode 100644 index 0000000..b4e9172 --- /dev/null +++ b/pytorch/multiproc.py @@ -0,0 +1,17 @@ +import torch.multiprocessing as mp + +# mp.set_start_method('spawn') + +def train(model): + pass + +model = 1 + +num_processes = 4 +processes = [] +for rank in range(num_processes): + p = mp.Process(target=train, args=(model,)) + p.start() + processes.append(p) +for p in processes: + p.join() diff --git a/pytorch/network.py b/pytorch/network.py new file mode 100644 index 0000000..e428e8e --- /dev/null +++ b/pytorch/network.py @@ -0,0 +1,88 @@ +import torch +import torch.nn as nn +import torch.nn.functional as F + +CHANNELS = [20, 50, 500] +#CHANNELS = [40, 100, 500] +#CHANNELS = [10, 20, 100] + +#CHANNELS = [6, 16, 120, 84] +#CHANNELS = [12, 32, 120, 84] +#CHANNELS = [32, 32, 120, 84] + +class NetCCFFF(nn.Module): + def __init__(self, input_channels): + super(NetCCFFF, self).__init__() + self.conv1 = nn.Conv2d(input_channels, CHANNELS[0], 5) + self.pool = nn.MaxPool2d(2, 2) + self.conv2 = nn.Conv2d(CHANNELS[0], CHANNELS[1], 5) + self.fc1 = nn.Linear(CHANNELS[1] * 12 * 12, CHANNELS[2]) + self.fc2 = nn.Linear(CHANNELS[2], CHANNELS[3]) + self.fc3 = nn.Linear(CHANNELS[3], 2) + + def forward(self, x): + x = self.pool(F.relu(self.conv1(x))) + x = self.pool(F.relu(self.conv2(x))) + x = x.view(-1, x.shape[1] * x.shape[2] * x.shape[3]) + x = F.relu(self.fc1(x)) + x = F.relu(self.fc2(x)) + x = self.fc3(x) + return x + +class Net(nn.Module): + def __init__(self, input_channels): + super(Net, self).__init__() + self.conv1 = nn.Conv2d(input_channels, CHANNELS[0], 5) + self.pool = nn.MaxPool2d(2, 2) + self.conv2 = nn.Conv2d(CHANNELS[0], CHANNELS[1], 5) + self.fc1 = nn.Linear(CHANNELS[1] * 12 * 12, CHANNELS[2]) + self.fc2 = nn.Linear(CHANNELS[2], 2) + + def forward(self, x): + x = self.pool(F.relu(self.conv1(x))) + x = self.pool(F.relu(self.conv2(x))) + x = x.view(-1, x.shape[1] * x.shape[2] * x.shape[3]) + x = F.relu(self.fc1(x)) + x = self.fc2(x) + return x + +def train(model, criterion, optimizer, data, device): + # Get the inputs and transfer them to the CPU/GPU. + inputs, labels = data + inputs = inputs.to(device) + labels = labels.to(device) + + # Reset the parameter gradients. + optimizer.zero_grad() + + # Forward + backward + optimize. + outputs = model(inputs) + loss = criterion(outputs, labels.long()) + loss.backward() + optimizer.step() + + return loss + +def eval(model, test_loader, device): + model.eval() + correct = 0 + total = 0 + print('Testing the network on the test data ...') + + with torch.no_grad(): + for data in test_loader: + inputs, labels = data + inputs = inputs.to(device) + labels = labels.to(device) + + # Forward + outputs = model(inputs) + _, predicted = torch.max(outputs.data, 1) + total += labels.size(0) + correct += (predicted == labels.long()).sum().item() + + accuracy = 100.0 * float(correct) / float(total) + print('Accuracy of the network on the test set: %.3f%%' % ( + accuracy)) + + return accuracy diff --git a/pytorch/overfit.py b/pytorch/overfit.py new file mode 100644 index 0000000..d6a254a --- /dev/null +++ b/pytorch/overfit.py @@ -0,0 +1,144 @@ +# Use tensors to speed up loading data onto the GPU during training. + +import h5py +import numpy as np +import torch +import torch.nn as nn +import torch.nn.functional as F +import torch.optim as optim +import torch.utils.data as torchdata +import sys + +class H5Dataset(torchdata.Dataset): + def __init__(self, file_path, start_idx, end_idx): + super(H5Dataset, self).__init__() + with h5py.File(file_path, 'r') as h5_file: + self.data = torch.from_numpy(np.array(h5_file.get('images')[start_idx : end_idx]).astype('float32')) + self.target = torch.from_numpy(np.array(h5_file.get('labels')[start_idx : end_idx]).astype('int32')) + print("Loaded data") + + def __getitem__(self, index): + image = self.data[index,:,:] + # ptorch uses NCHW format + image = image.reshape((image.shape[2], image.shape[0], image.shape[1])) + target = self.target[index,:][0] + return (image, target) + + def __len__(self): + return self.data.shape[0] + +class Net(nn.Module): + def __init__(self, input_channels): + super(Net, self).__init__() + self.conv1 = nn.Conv2d(input_channels, 40, 5) + self.pool = nn.MaxPool2d(2, 2) + self.conv2 = nn.Conv2d(40, 80, 5) + self.fc1 = nn.Linear(80 * 12 * 12, 1000) + self.fc2 = nn.Linear(1000, 2) + # self.fc2 = nn.Linear(120, 84) + # self.fc3 = nn.Linear(84, 2) + + def forward(self, x): + x = self.pool(F.relu(self.conv1(x))) + x = self.pool(F.relu(self.conv2(x))) + x = x.view(-1, 80 * x.shape[2] * x.shape[3]) + x = F.relu(self.fc1(x)) + x = self.fc2(x) + # x = F.relu(self.fc2(x)) + # x = self.fc3(x) + return x + +with h5py.File(sys.argv[1], 'r') as db: + num_train = len(db['images']) +print('Have', num_train, 'total training examples') +num_epochs = 60 +max_in_memory = 10000 + +# Use GPU. +device = torch.device("cuda:0" if torch.cuda.is_available() else "cpu") +print(device) + +# Load the training data. +print('Loading data ...') + +# Create the network. +input_channels = int(sys.argv[3]) +net = Net(input_channels) +print(net) + +print('Copying network to GPU ...') +net.to(device) + +# Define the loss function and optimizer. +criterion = nn.CrossEntropyLoss() +# optimizer = optim.SGD(net.parameters(), lr=0.1, momentum=0.9) +# optimizer = optim.SGD(net.parameters(), lr=0.01, momentum=0.9) +# optimizer = optim.SGD(net.parameters(), lr=0.001, momentum=0.9, weight_decay=0.01) +# optimizer = optim.SGD(net.parameters(), lr=0.001, momentum=0.9, weight_decay=0.05) +# optimizer = optim.SGD(net.parameters(), lr=0.001, momentum=0.9, weight_decay=0.1) +# optimizer = optim.SGD(net.parameters(), lr=0.0001, momentum=0.9, weight_decay=0.01) +# optimizer = optim.SGD(net.parameters(), lr=0.0001, momentum=0.9, weight_decay=0.05) +# optimizer = optim.SGD(net.parameters(), lr=0.00001, momentum=0.9, weight_decay=0.05) +# optimizer = optim.SGD(net.parameters(), lr=0.000001, momentum=0.9, weight_decay=0.01) + +# optimizer = optim.Adam(net.parameters(), weight_decay=0.05) +# optimizer = optim.Adam(net.parameters(), weight_decay=0.01) +optimizer = optim.Adam(net.parameters(), weight_decay=0.005) + +#scheduler = torch.optim.lr_scheduler.ExponentialLR(optimizer, gamma=0.96) + +dset = H5Dataset(sys.argv[1], 0, max_in_memory) +#train_loader = torchdata.DataLoader(dset, batch_size=64, shuffle=True, num_workers=2) +train_loader = torchdata.DataLoader(dset, batch_size=64, shuffle=True) + +print('Training ...') + +for epoch in range(num_epochs): + print('epoch: %d/%d' % (epoch, num_epochs)) + # scheduler.step() + for param_group in optimizer.param_groups: + print('learning rate:', param_group['lr']) + + running_loss = 0.0 + + for i, data in enumerate(train_loader): + # get the inputs + inputs, labels = data + inputs = inputs.to(device) + labels = labels.to(device) + + # zero the parameter gradients + optimizer.zero_grad() + + # forward + backward + optimize + outputs = net(inputs) + loss = criterion(outputs, labels.long()) + loss.backward() + optimizer.step() + + # print statistics + running_loss += loss.item() + if i % 100 == 99: # print every 10 mini-batches + print('epoch: %d, batch: %5d, loss: %.5f' % + (epoch + 1, i + 1, running_loss / 100)) + +print('Finished Training') + +# Test the network on the test data. +test_set = H5Dataset(sys.argv[2], 0, 20000) +test_loader = torchdata.DataLoader(test_set, batch_size=64, shuffle=True) +correct = 0 +total = 0 +print('Testing the network on the test data ...') +with torch.no_grad(): + for data in test_loader: + inputs, labels = data + inputs = inputs.to(device) + labels = labels.to(device) + outputs = net(inputs) + _, predicted = torch.max(outputs.data, 1) + total += labels.size(0) + correct += (predicted == labels.long()).sum().item() + +print('Accuracy of the network on the 20000 test images: %d %%' % ( + 100 * correct / total)) diff --git a/pytorch/parallel_test.py b/pytorch/parallel_test.py new file mode 100644 index 0000000..b3485be --- /dev/null +++ b/pytorch/parallel_test.py @@ -0,0 +1,11 @@ +from mpi4py import MPI +import h5py + +rank = MPI.COMM_WORLD.rank # The process ID (integer 0-3 for 4-process run) + +f = h5py.File('parallel_test.hdf5', 'w', driver='mpio', comm=MPI.COMM_WORLD) + +dset = f.create_dataset('test', (4,), dtype='i') +dset[rank] = rank + +f.close() diff --git a/pytorch/plot_loss_stats.py b/pytorch/plot_loss_stats.py new file mode 100644 index 0000000..d7d9cb5 --- /dev/null +++ b/pytorch/plot_loss_stats.py @@ -0,0 +1,14 @@ +import matplotlib.pyplot as plt +import numpy as np +import sys + +with open(sys.argv[1], 'r') as f: + losses = [line.rstrip() for line in f] +losses = np.array([float(x) for x in losses]) + +iter = np.arange(losses.shape[0]) + +fig, ax = plt.subplots() +ax.plot(iter, losses/100) +# plt.xticks(np.arange(0, 1, step=0.2)) +plt.show() diff --git a/pytorch/reshape_hdf5.py b/pytorch/reshape_hdf5.py new file mode 100644 index 0000000..01ddd1a --- /dev/null +++ b/pytorch/reshape_hdf5.py @@ -0,0 +1,33 @@ +import h5py +import numpy as np +import torch.nn as nn +import torch.nn.functional as F +import torch.optim as optim +import torch.utils.data as data +import sys + +class H5Dataset(data.Dataset): + def __init__(self, file_path): + super(H5Dataset, self).__init__() + # with h5py.File(file_path, 'r') as h5_file: + h5_file = h5py.File(file_path, 'r') + self.data = h5_file.get('images') + self.target = h5_file.get('labels') + + def __getitem__(self, index): + return (torch.from_numpy(self.data[index,:,:,:]).float(), + torch.from_numpy(self.target[index,:,:,:]).float()) + + def __len__(self): + return self.data.shape[0] + +dset = H5Dataset(sys.argv[1]) +num = int(sys.argv[3]) + +with h5py.File(sys.argv[2]) as h5_file_out: + print ('Storing images ...') + dset_small = h5_file_out.create_dataset('images', data=dset.data[:num], compression="gzip", compression_opts=4) + print ('Storing labels ...') + dset_small = h5_file_out.create_dataset('labels', data=dset.target[:num], compression="gzip", compression_opts=4) + print(len(dset)) + print(len(dset_small)) diff --git a/pytorch/reshape_hdf5_mem.py b/pytorch/reshape_hdf5_mem.py new file mode 100644 index 0000000..a0a23a7 --- /dev/null +++ b/pytorch/reshape_hdf5_mem.py @@ -0,0 +1,37 @@ +import h5py +import numpy as np +import torch.nn as nn +import torch.nn.functional as F +import torch.optim as optim +import torch.utils.data as data +import sys + +h5_file = h5py.File(sys.argv[1], 'r') +images = h5_file['images'] +labels = h5_file['labels'] + +num = int(sys.argv[3]) + +max_in_memory = 100000 +indices = np.arange(0, num, max_in_memory) +indices = list(indices) + [num] +iters = int(np.ceil(num / float(max_in_memory))) +print('indices:', indices) +print('iters:', iters) +print(images.shape) +print(labels.shape) +print(images[0,0,0].shape) +#exit(-1) + +with h5py.File(sys.argv[2]) as h5_file_out: + print ('Creating HDF5 datasets ...') + dset_images = h5_file_out.create_dataset('images', [num] + list(images.shape[1:]) + [images[0,0,0].shape[0]], compression="gzip", compression_opts=4) + dset_labels = h5_file_out.create_dataset('labels', [num] + list(labels.shape[1:]), compression="gzip", compression_opts=4) + + for i in range(iters): + print ('i: %d' % i) + print ('Copying %d images ...' % (indices[i + 1] - indices[i])) + dset_images[indices[i] : indices[i + 1], :, :] = images[indices[i] : indices[i + 1], :, :] + print ('Copying %d labels ...' % (indices[i + 1] - indices[i])) + dset_labels[indices[i] : indices[i + 1], :] = labels[indices[i] : indices[i + 1], :] + diff --git a/pytorch/shuffle_hdf5.py b/pytorch/shuffle_hdf5.py new file mode 100644 index 0000000..a06e663 --- /dev/null +++ b/pytorch/shuffle_hdf5.py @@ -0,0 +1,48 @@ +import h5py +import numpy as np +import torch.nn as nn +import torch.nn.functional as F +import torch.optim as optim +import torch.utils.data as data +import sys + +max_in_memory = 200000 + +with h5py.File(sys.argv[1], 'r') as h5_file: + labels = h5_file['labels'] + images_shape = h5_file['images'].shape + labels_shape = h5_file['labels'].shape + n = len(labels) + +# n = 300 +idx = np.arange(n) +np.random.shuffle(idx) + +subIdx = np.arange(0, n, max_in_memory) +subIdx = list(subIdx) + [n] +print('subIdx:', subIdx) + +with h5py.File(sys.argv[2], 'w') as f: + images_dset = f.create_dataset("images", images_shape, dtype='uint8', compression="gzip", compression_opts=4) + labels_dset = f.create_dataset("labels", labels_shape, dtype='uint8', compression="gzip", compression_opts=4) + + for i in range(len(subIdx) - 1): + print('Block %d/%d ...' % (i + 1, len(subIdx) - 1)) + images = np.empty((0, images_shape[1], images_shape[2], images_shape[3]), np.uint8) + labels = np.empty((0, labels_shape[1]), np.uint8) + idx2 = idx[subIdx[i] : subIdx[i+1]] + + for j in range(len(subIdx) - 1): + print(' Loading images and labels from block %d/%d ...' % (j + 1, len(subIdx) - 1)) + with h5py.File(sys.argv[1], 'r') as h5_file: + images_in = np.array(h5_file['images'][subIdx[j] : subIdx[j+1]]) + labels_in = np.array(h5_file['labels'][subIdx[j] : subIdx[j+1]]) + print(' Extracting images and labels at indices in this block ...') + indices = idx2[np.where(np.logical_and(idx2 >= subIdx[j], idx2 < subIdx[j+1]))[0]] + indices = indices - subIdx[j] + images = np.vstack((images, images_in[indices])) + labels = np.vstack((labels, labels_in[indices])) + print(' images, labels:', images.shape, labels.shape) + print(' Storing images and labels ...') + images_dset[subIdx[i] : subIdx[i+1]] = images + labels_dset[subIdx[i] : subIdx[i+1]] = labels diff --git a/pytorch/shuffle_hdf5_mem.py b/pytorch/shuffle_hdf5_mem.py new file mode 100644 index 0000000..8925206 --- /dev/null +++ b/pytorch/shuffle_hdf5_mem.py @@ -0,0 +1,72 @@ +import h5py +import h5py_cache as h5c +import numpy as np +import os +import sys + +CHUNK_CACHE = 1024**2*4000 + +if len(sys.argv) < 4: + print('Error: not enough input arguments!') + print('Usage: python3 shuffle_h5_mem_2pass.py IN_H5 OUT_H5 STEPS') + exit(-1) + +fin = sys.argv[1] +fout = sys.argv[2] +steps = int(sys.argv[3]) + +with h5c.File(fin, 'r', chunk_cache_mem_size=CHUNK_CACHE) as f: + images_shape = f['images'].shape + labels_shape = f['labels'].shape + n = labels_shape[0] + max_in_memory = int(np.floor(n/steps)) + blocks = list(np.arange(0, n, max_in_memory)) + [labels_shape[0]] + blocks = blocks[:-1] + blocks[-1] = n + m = len(blocks) - 1 + print('n:', n, 'm:', m, 'blocks:', blocks) + chunk_shape = (1000,) + images_shape[1:] + db_idx = np.random.permutation(np.repeat(np.arange(m), max_in_memory)) + print('db_idx:', len(db_idx)) + offsets = [0]*m + + # 1st pass: Assign instances to randomly chosen bins. + print('1st pass: assign instances to randomly chosen bins ...') + with h5c.File('temp.h5', 'w', chunk_cache_mem_size=CHUNK_CACHE) as db: + for i in range(m): + db.create_dataset('images' + str(i), (max_in_memory,) + images_shape[1:], dtype='uint8', chunks=chunk_shape) + db.create_dataset('labels' + str(i), (max_in_memory,) + labels_shape[1:], dtype='uint8') + + for i in range(m): + print('%d/%d: %d to %d' % (i+1, m, blocks[i], blocks[i+1])) + images_in = f['images'][blocks[i] : blocks[i+1]] + labels_in = f['labels'][blocks[i] : blocks[i+1]] + idx = db_idx[blocks[i] : blocks[i+1]] + print(' %d indices in this block' % len(idx)) + for j in range(m): + in_bin = np.where(idx == j)[0] + start = offsets[j] + end = offsets[j] + len(in_bin) + offsets[j] = end + print(' %d indices in bin %d' % (len(in_bin), j)) + print(' start: %d, end: %d' % (start, end)) + db['images' + str(j)][start:end] = np.array(images_in)[in_bin] + db['labels' + str(j)][start:end] = np.array(labels_in)[in_bin] + + # 2nd pass: Shuffle each bin. + print('--------------------------------------------------------') + print('2nd pass: shuffle each bin ...') + with h5c.File(fout, 'w', chunk_cache_mem_size=CHUNK_CACHE) as fout: + images_out = fout.create_dataset('images', images_shape, dtype='uint8', chunks=chunk_shape, compression='lzf') + labels_out = fout.create_dataset('labels', labels_shape, dtype='uint8') + + with h5c.File('temp.h5', 'r', chunk_cache_mem_size=CHUNK_CACHE) as fin: + for i in range(m): + images_in = np.array(fin['images' + str(i)]) + labels_in = np.array(fin['labels' + str(i)]) + p = np.random.permutation(len(labels_in)) + k = len(p) + print('%d/%d: %d to %d. k: %d' % (i+1, m, blocks[i], blocks[i+1], k)) + images_out[blocks[i] : blocks[i] + k] = images_in[p] + labels_out[blocks[i] : blocks[i] + k] = labels_in[p] + diff --git a/pytorch/torch_to_onnx.py b/pytorch/torch_to_onnx.py new file mode 100644 index 0000000..9a191ab --- /dev/null +++ b/pytorch/torch_to_onnx.py @@ -0,0 +1,25 @@ +import torch +import torch.nn as nn +import torch.nn.functional as F +import sys +from collections import OrderedDict +from network import Net, NetCCFFF + +if len(sys.argv) < 4: + print('ERROR: Not enough input arguments!') + print('Usage: python torch_to_onxx.py pathToPytorchModel.pwf pathToONNXModel.onnx num_channels') + exit(-1) + +state_dict = torch.load(sys.argv[1]) +#new_state_dict = OrderedDict() +#for k, v in state_dict.items(): +# name = k[7:] +# new_state_dict[name] = v + +input_channels = int(sys.argv[3]) +net = Net(input_channels) +net.load_state_dict(state_dict) +print(net) + +dummy_input = torch.randn(1, input_channels, 60, 60) +torch.onnx.export(net, dummy_input, sys.argv[2], verbose=True) diff --git a/pytorch/train_net.py b/pytorch/train_net.py new file mode 100644 index 0000000..f2786c0 --- /dev/null +++ b/pytorch/train_net.py @@ -0,0 +1,106 @@ +import h5py +import numpy as np +import torch +import torch.nn as nn +import torch.nn.functional as F +import torch.optim as optim +import torch.utils.data as data +import sys + +class H5Dataset(data.Dataset): + def __init__(self, file_path): + super(H5Dataset, self).__init__() + h5_file = h5py.File(file_path) + self.data = h5_file.get('images') + self.target = h5_file.get('labels') + + def __getitem__(self, index): + data = self.data[index,:,:].astype('float32') + # ptorch uses NCHW format + data = data.reshape((data.shape[2], data.shape[0], data.shape[1])) + target = self.target[index,:].astype('int32')[0] + return (data, target) + + def __len__(self): + return self.data.shape[0] + +class Net(nn.Module): + def __init__(self, input_channels): + super(Net, self).__init__() + self.conv1 = nn.Conv2d(input_channels, 20, 5) + self.pool = nn.MaxPool2d(2, 2) + self.conv2 = nn.Conv2d(20, 50, 5) + self.fc1 = nn.Linear(50 * 12 * 12, 500) + self.fc2 = nn.Linear(500, 2) + # self.fc2 = nn.Linear(120, 84) + # self.fc3 = nn.Linear(84, 2) + + def forward(self, x): + x = self.pool(F.relu(self.conv1(x))) + x = self.pool(F.relu(self.conv2(x))) + x = x.view(-1, 50 * x.shape[2] * x.shape[3]) + x = F.relu(self.fc1(x)) + x = self.fc2(x) + # x = F.relu(self.fc2(x)) + # x = self.fc3(x) + return x + +# Load the training data. +dset = H5Dataset(sys.argv[1]) +train_loader = data.DataLoader(dset, batch_size=64, shuffle=True) + +# Create the network. +input_channels = int(sys.argv[3]) +net = Net(input_channels) +print net + +device = torch.device("cuda:0" if torch.cuda.is_available() else "cpu") +print(device) +net.to(device) + +# Define the loss function and optimizer. +criterion = nn.CrossEntropyLoss() +optimizer = optim.SGD(net.parameters(), lr=0.00001, momentum=0.9) + +num_epochs = 1 + +for epoch in range(num_epochs): # loop over the dataset multiple times + running_loss = 0.0 + for i, data in enumerate(train_loader, 0): + # get the inputs + inputs, labels = data + inputs, labels = inputs.to(device), labels.to(device) + + # zero the parameter gradients + optimizer.zero_grad() + + # forward + backward + optimize + outputs = net(inputs) + loss = criterion(outputs, labels.long()) + loss.backward() + optimizer.step() + + # print statistics + running_loss += loss.item() + if i % 10 == 9: # print every 10 mini-batches + print('[%d, %5d] loss: %.5f' % + (epoch + 1, i + 1, running_loss / 10)) + running_loss = 0.0 + +print('Finished Training') + +# Test the network on the test data. +test_set = H5Dataset(sys.argv[2]) +test_loader = data.DataLoader(test_set, batch_size=64, shuffle=True) +correct = 0 +total = 0 +with torch.no_grad(): + for data in testloader: + images, labels = data + outputs = net(images) + _, predicted = torch.max(outputs.data, 1) + total += labels.size(0) + correct += (predicted == labels).sum().item() + +print('Accuracy of the network on the 10000 test images: %d %%' % ( + 100 * correct / total)) diff --git a/pytorch/train_net2.py b/pytorch/train_net2.py new file mode 100644 index 0000000..5e7bdde --- /dev/null +++ b/pytorch/train_net2.py @@ -0,0 +1,165 @@ +# Use tensors to speed up loading data onto the GPU during training. + +import h5py +import numpy as np +import torch +import torch.nn as nn +import torch.nn.functional as F +import torch.optim as optim +import torch.utils.data as torchdata +import torch.multiprocessing +import sys + +#torch.multiprocessing.set_start_method('spawn') + +class H5Dataset(torchdata.Dataset): + def __init__(self, file_path, start_idx, end_idx): + super(H5Dataset, self).__init__() + with h5py.File(file_path, 'r') as h5_file: + self.data = torch.from_numpy(np.array(h5_file.get('images')[start_idx : end_idx]).astype('float32')) + self.target = torch.from_numpy(np.array(h5_file.get('labels')[start_idx : end_idx]).astype('int32')) + print("Loaded data") + + def __getitem__(self, index): + image = self.data[index,:,:] + # ptorch uses NCHW format + image = image.reshape((image.shape[2], image.shape[0], image.shape[1])) + target = self.target[index,:][0] + return (image, target) + + def __len__(self): + return self.data.shape[0] + +class Net(nn.Module): + def __init__(self, input_channels): + super(Net, self).__init__() + self.conv1 = nn.Conv2d(input_channels, 20, 5) + self.pool = nn.MaxPool2d(2, 2) + self.conv2 = nn.Conv2d(20, 50, 5) + self.fc1 = nn.Linear(50 * 12 * 12, 500) + self.fc2 = nn.Linear(500, 2) + # self.fc2 = nn.Linear(120, 84) + # self.fc3 = nn.Linear(84, 2) + + def forward(self, x): + x = self.pool(F.relu(self.conv1(x))) + x = self.pool(F.relu(self.conv2(x))) + x = x.view(-1, 50 * x.shape[2] * x.shape[3]) + x = F.relu(self.fc1(x)) + x = self.fc2(x) + # x = F.relu(self.fc2(x)) + # x = self.fc3(x) + return x + +with h5py.File(sys.argv[1], 'r') as db: + num_train = len(db['images']) +print('Have', num_train, 'total training examples') +num_epochs = 10 +max_in_memory = 50000 +repeats = 2 +early_stop_loss = 0.15 +start_idx = 0 +end_idx = max_in_memory +iter_per_epoch = int(np.ceil(num_train / float(max_in_memory))) +indices = np.arange(0, num_train, max_in_memory) +indices = list(indices) + [num_train] +print('iter_per_epoch:', iter_per_epoch) +print(indices) + +# Use GPU. +device = torch.device("cuda:0" if torch.cuda.is_available() else "cpu") +print(device) + +# Load the training data. +print('Loading data ...') + +# Create the network. +input_channels = int(sys.argv[3]) +net = Net(input_channels) +print(net) + +print('Copying network to GPU ...') +net.to(device) + +# Define the loss function and optimizer. +criterion = nn.CrossEntropyLoss() +# optimizer = optim.SGD(net.parameters(), lr=0.1, momentum=0.9) +# optimizer = optim.SGD(net.parameters(), lr=0.01, momentum=0.9) +# optimizer = optim.SGD(net.parameters(), lr=0.001, momentum=0.9, weight_decay=0.01) +# optimizer = optim.SGD(net.parameters(), lr=0.001, momentum=0.9, weight_decay=0.05) +# optimizer = optim.SGD(net.parameters(), lr=0.001, momentum=0.9, weight_decay=0.1) +optimizer = optim.SGD(net.parameters(), lr=0.0001, momentum=0.9, weight_decay=0.01) +# optimizer = optim.SGD(net.parameters(), lr=0.0001, momentum=0.9, weight_decay=0.05) +# optimizer = optim.SGD(net.parameters(), lr=0.00001, momentum=0.9, weight_decay=0.05) +# optimizer = optim.SGD(net.parameters(), lr=0.000001, momentum=0.9, weight_decay=0.01) + +early_stop = False +print('Training ...') + +for epoch in range(num_epochs): + print('epoch: %d/%d' % (epoch, num_epochs)) + + for j in range(iter_per_epoch): + print('iter: %d/%d' % (j, iter_per_epoch)) + dset = H5Dataset(sys.argv[1], indices[j], indices[j + 1]) + #train_loader = torchdata.DataLoader(dset, batch_size=64, shuffle=True, num_workers=2) + train_loader = torchdata.DataLoader(dset, batch_size=64, shuffle=True) + + running_loss = 0.0 + + for r in range(repeats): + for i, data in enumerate(train_loader): + # get the inputs + inputs, labels = data + inputs = inputs.to(device) + labels = labels.to(device) + + # zero the parameter gradients + optimizer.zero_grad() + + # forward + backward + optimize + outputs = net(inputs) + loss = criterion(outputs, labels.long()) + loss.backward() + optimizer.step() + + # print statistics + running_loss += loss.item() + if i % 100 == 99: # print every 10 mini-batches + print('epoch: %d, batch: %5d, loss: %.5f' % + (epoch + 1, i + 1, running_loss / 100)) + if running_loss / 100 < early_stop_loss: + print('reached loss threshold for early stopping: %.5f', early_stop_loss) + early_stop = True + break + running_loss = 0.0 + if early_stop: + break + if early_stop: + break + if early_stop: + break + +print('Finished Training') + +model_path = raw_input("Enter the filename/path for the trained model: ") +torch.save(net.state_dict(), model_path) + +# Test the network on the test data. +test_set = H5Dataset(sys.argv[2], 0, 20000) +test_loader = torchdata.DataLoader(test_set, batch_size=64, shuffle=True) +correct = 0 +total = 0 +print('Testing the network on the test data ...') +with torch.no_grad(): + for data in test_loader: + inputs, labels = data + inputs = inputs.to(device) + labels = labels.to(device) + outputs = net(inputs) + _, predicted = torch.max(outputs.data, 1) + total += labels.size(0) + correct += (predicted == labels.long()).sum().item() + +print('Accuracy of the network on the 20000 test images: %d %%' % ( + 100 * correct / total)) diff --git a/pytorch/train_net3.py b/pytorch/train_net3.py new file mode 100644 index 0000000..bdcb92d --- /dev/null +++ b/pytorch/train_net3.py @@ -0,0 +1,181 @@ +# Use tensors to speed up loading data onto the GPU during training. + +import h5py +import h5py_cache as h5c +import numpy as np +import torch +import torch.nn as nn +import torch.nn.functional as F +import torch.optim as optim +import torch.utils.data as torchdata +import torch.multiprocessing +import sys + +#torch.multiprocessing.set_start_method('spawn') + +from hdf5_dataset import H5Dataset +from network import Net, NetCCFFF + +def train(model, criterion, optimizer, data, device): + # Get the inputs and transfer them to the CPU/GPU. + inputs, labels = data + inputs = inputs.to(device) + labels = labels.to(device) + + # Reset the parameter gradients. + optimizer.zero_grad() + + # Forward + backward + optimize. + outputs = model(inputs) + loss = criterion(outputs, labels.long()) + loss.backward() + optimizer.step() + + return loss + +def eval(model, test_loader, device): + model.eval() + correct = 0 + total = 0 + print('Testing the network on the test data ...') + + with torch.no_grad(): + for data in test_loader: + inputs, labels = data + inputs = inputs.to(device) + labels = labels.to(device) + + # Forward + outputs = model(inputs) + _, predicted = torch.max(outputs.data, 1) + total += labels.size(0) + correct += (predicted == labels.long()).sum().item() + + accuracy = 100.0 * float(correct) / float(total) + print('Accuracy of the network on the test set: %.3f%%' % ( + accuracy)) + + return accuracy + +if len(sys.argv) < 3: + print('ERROR: Not enough input arguments!') + print('Usage: python train_net3.py pathToTrainingSet.h5') + exit(-1) + +with h5py.File(sys.argv[1], 'r') as db: + num_train = len(db['images']) +print('Have', num_train, 'total training examples') +num_epochs = 10 +max_in_memory = 80000 +print_step = 250 +repeats = 1 +early_stop_loss = 0.0000001 +start_idx = 0 +end_idx = max_in_memory +iter_per_epoch = int(np.ceil(num_train / float(max_in_memory))) +indices = np.arange(0, num_train, max_in_memory) +indices = list(indices) + [num_train] +print('iter_per_epoch:', iter_per_epoch) +print(indices) + +# Use GPU. +device = torch.device("cuda:0" if torch.cuda.is_available() else "cpu") +print(device) + +# Load the test data. +print('Loading test data ...') +test_set = H5Dataset(sys.argv[2], 0, 10000) +test_loader = torchdata.DataLoader(test_set, batch_size=64, shuffle=True) + +# Create the network. +input_channels = test_set.images.shape[-1] +#net = NetCCFFF(input_channels) +net = Net(input_channels) +print(net) + +print('Copying network to GPU ...') +if torch.cuda.device_count() > 1: + print("Let's use", torch.cuda.device_count(), "GPUs.") + net = nn.DataParallel(net) +net.to(device) + +# Define the loss function and optimizer. +#LR = 0.1 +#LR = 0.01 +LR = 0.001 +criterion = nn.CrossEntropyLoss() +#optimizer = optim.SGD(net.parameters(), lr=LR, momentum=0.9, weight_decay=0.0005) + +optimizer = optim.Adam(net.parameters(), lr=LR, weight_decay=0.0005) + +#scheduler = torch.optim.lr_scheduler.ExponentialLR(optimizer, gamma=0.9) +accuracy = eval(net, test_loader, device) +accuracies = [] +accuracies.append(accuracy) + +early_stop = False +losses = [] +loss = None +print('Training ...') + +for epoch in range(num_epochs): + print('epoch: %d/%d' % (epoch + 1, num_epochs)) + net.train() + + for param_group in optimizer.param_groups: + print('learning rate:', param_group['lr']) + + for j in range(iter_per_epoch): + print('iter: %d/%d' % (j + 1, iter_per_epoch)) + print('Loading data block [%d, %d] ...' % (indices[j], indices[j + 1])) + dset = [] + train_loader = [] + dset = H5Dataset(sys.argv[1], indices[j], indices[j + 1]) + #train_loader = torchdata.DataLoader(dset, batch_size=64, shuffle=True, num_workers=2) + train_loader = torchdata.DataLoader(dset, batch_size=64, shuffle=True) + + running_loss = 0.0 + + for r in range(repeats): + if r > 1: + print('repeat: %d/%d' % (r + 1, repeats)) + for i, data in enumerate(train_loader): + loss = train(net, criterion, optimizer, data, device) + + # print statistics + running_loss += loss.item() + if i % print_step == print_step - 1: + print('epoch: {}, batch: {}, loss: {:.4f}'.format(epoch + 1, i + 1, running_loss / print_step)) + losses.append(running_loss) + running_loss = 0.0 + # Evaluate the network on the test dataset. + accuracy = eval(net, test_loader, device) + accuracies.append(accuracy) + model_path = 'model_' + str(accuracy) + '.pwf' + torch.save(net.state_dict(), model_path) + net.train() + if early_stop: + break + if early_stop: + break + if early_stop: + break + + # Evaluate the network on the test dataset. + accuracy = eval(net, test_loader, device) + accuracies.append(accuracy) + model_path = 'model_' + str(accuracy) + '.pwf' + torch.save(net.state_dict(), model_path) + #scheduler.step() + +print('Finished Training') + +model_path = 'model.pwf' +torch.save(net.state_dict(), model_path) + +with open('loss_stats.txt', 'w') as f: + for l in losses: + f.write("%s\n" % str(l)) +with open('accuracy_stats.txt', 'w') as f: + for a in accuracies: + f.write("%s\n" % str(a)) diff --git a/pytorch/train_net4.py b/pytorch/train_net4.py new file mode 100644 index 0000000..cca7515 --- /dev/null +++ b/pytorch/train_net4.py @@ -0,0 +1,164 @@ +# Use tensors to speed up loading data onto the GPU during training. + +import h5py +import numpy as np +import torch +import torch.nn as nn +import torch.nn.functional as F +import torch.optim as optim +import torch.utils.data as torchdata +import torch.multiprocessing +import sys + +torch.multiprocessing.set_start_method('spawn') + +class H5Dataset(torchdata.Dataset): + def __init__(self, file_path): + super(H5Dataset, self).__init__() + self.file_path = file_path + self.file = None + with h5py.File(self.file_path, 'r') as f: + self.length = f['labels'].shape[0] + + def __getitem__(self, index): + if self.file == None: + self.file = h5py.File(self.file_path, 'r') + print('Accessing image ...') + image = torch.from_numpy(np.array(self.file['images'][index])).to(torch.float32) + # ptorch uses NCHW format + image = image.reshape((image.shape[2], image.shape[0], image.shape[1])) + print('Accessing label ...') + label = torch.from_numpy(np.array(self.file['labels'][index][0])).to(torch.int32) + return (image, label) + + def __len__(self): + return self.length + +class Net(nn.Module): + def __init__(self, input_channels): + super(Net, self).__init__() + self.conv1 = nn.Conv2d(input_channels, 20, 5) + self.pool = nn.MaxPool2d(2, 2) + self.conv2 = nn.Conv2d(20, 50, 5) + self.fc1 = nn.Linear(50 * 12 * 12, 500) + self.fc2 = nn.Linear(500, 2) + # self.fc2 = nn.Linear(120, 84) + # self.fc3 = nn.Linear(84, 2) + + def forward(self, x): + x = self.pool(F.relu(self.conv1(x))) + x = self.pool(F.relu(self.conv2(x))) + x = x.view(-1, 50 * x.shape[2] * x.shape[3]) + x = F.relu(self.fc1(x)) + x = self.fc2(x) + # x = F.relu(self.fc2(x)) + # x = self.fc3(x) + return x + +with h5py.File(sys.argv[1], 'r') as db: + num_train = len(db['images']) +print('Have', num_train, 'total training examples') +num_epochs = 10 +repeats = 1 +early_stop_loss = 0.05 +start_idx = 0 + +# Use GPU. +device = torch.device("cuda:0" if torch.cuda.is_available() else "cpu") +print(device) + +# Load the training data. +print('Loading data ...') + +# Create the network. +input_channels = int(sys.argv[3]) +net = Net(input_channels) +print(net) + +print('Copying network to GPU ...') +net.to(device) + +# Define the loss function and optimizer. +criterion = nn.CrossEntropyLoss() +# optimizer = optim.SGD(net.parameters(), lr=0.1, momentum=0.9) +# optimizer = optim.SGD(net.parameters(), lr=0.01, momentum=0.9) +# optimizer = optim.SGD(net.parameters(), lr=0.001, momentum=0.9, weight_decay=0.01) +# optimizer = optim.SGD(net.parameters(), lr=0.001, momentum=0.9, weight_decay=0.05) +# optimizer = optim.SGD(net.parameters(), lr=0.001, momentum=0.9, weight_decay=0.1) +# optimizer = optim.SGD(net.parameters(), lr=0.0001, momentum=0.9, weight_decay=0.01) +# optimizer = optim.SGD(net.parameters(), lr=0.0001, momentum=0.9, weight_decay=0.05) +# optimizer = optim.SGD(net.parameters(), lr=0.00001, momentum=0.9, weight_decay=0.05) +# optimizer = optim.SGD(net.parameters(), lr=0.000001, momentum=0.9, weight_decay=0.01) + +optimizer = optim.Adam(net.parameters(), weight_decay=0.01) +# optimizer = optim.Adam(net.parameters(), weight_decay=0.05) + +#scheduler = torch.optim.lr_scheduler.ExponentialLR(optimizer, gamma=0.96) + +dset = H5Dataset(sys.argv[1]) +train_loader = torchdata.DataLoader(dset, batch_size=64, shuffle=True, num_workers=4) + +early_stop = False +print('Training ...') + +for epoch in range(num_epochs): + print('epoch: %d/%d' % (epoch, num_epochs)) + # scheduler.step() + for param_group in optimizer.param_groups: + print('learning rate:', param_group['lr']) + + running_loss = 0.0 + + for i, data in enumerate(train_loader): + # get the inputs + inputs, labels = data + inputs = inputs.to(device) + labels = labels.to(device) + + # zero the parameter gradients + optimizer.zero_grad() + + # forward + backward + optimize + outputs = net(inputs) + loss = criterion(outputs, labels.long()) + loss.backward() + optimizer.step() + + # print statistics + running_loss += loss.item() + if i % 100 == 99: # print every 10 mini-batches + print('epoch: %d, batch: %5d, loss: %.5f' % + (epoch + 1, i + 1, running_loss / 100)) +# if running_loss / 100 < early_stop_loss: +# print('reached loss threshold for early stopping: %.5f', early_stop_loss) +# early_stop = True +# break +# running_loss = 0.0 +# if early_stop: +# break +# if early_stop: +# break + +print('Finished Training') + +model_path = raw_input("Enter the filename/path for the trained model: ") +torch.save(net.state_dict(), model_path) + +# Test the network on the test data. +test_set = H5Dataset(sys.argv[2], 0, 20000) +test_loader = torchdata.DataLoader(test_set, batch_size=64, shuffle=True) +correct = 0 +total = 0 +print('Testing the network on the test data ...') +with torch.no_grad(): + for data in test_loader: + inputs, labels = data + inputs = inputs.to(device) + labels = labels.to(device) + outputs = net(inputs) + _, predicted = torch.max(outputs.data, 1) + total += labels.size(0) + correct += (predicted == labels.long()).sum().item() + +print('Accuracy of the network on the 20000 test images: %d %%' % ( + 100 * correct / total)) diff --git a/pytorch/train_net_multiple_workers.py b/pytorch/train_net_multiple_workers.py new file mode 100644 index 0000000..5e7bdde --- /dev/null +++ b/pytorch/train_net_multiple_workers.py @@ -0,0 +1,165 @@ +# Use tensors to speed up loading data onto the GPU during training. + +import h5py +import numpy as np +import torch +import torch.nn as nn +import torch.nn.functional as F +import torch.optim as optim +import torch.utils.data as torchdata +import torch.multiprocessing +import sys + +#torch.multiprocessing.set_start_method('spawn') + +class H5Dataset(torchdata.Dataset): + def __init__(self, file_path, start_idx, end_idx): + super(H5Dataset, self).__init__() + with h5py.File(file_path, 'r') as h5_file: + self.data = torch.from_numpy(np.array(h5_file.get('images')[start_idx : end_idx]).astype('float32')) + self.target = torch.from_numpy(np.array(h5_file.get('labels')[start_idx : end_idx]).astype('int32')) + print("Loaded data") + + def __getitem__(self, index): + image = self.data[index,:,:] + # ptorch uses NCHW format + image = image.reshape((image.shape[2], image.shape[0], image.shape[1])) + target = self.target[index,:][0] + return (image, target) + + def __len__(self): + return self.data.shape[0] + +class Net(nn.Module): + def __init__(self, input_channels): + super(Net, self).__init__() + self.conv1 = nn.Conv2d(input_channels, 20, 5) + self.pool = nn.MaxPool2d(2, 2) + self.conv2 = nn.Conv2d(20, 50, 5) + self.fc1 = nn.Linear(50 * 12 * 12, 500) + self.fc2 = nn.Linear(500, 2) + # self.fc2 = nn.Linear(120, 84) + # self.fc3 = nn.Linear(84, 2) + + def forward(self, x): + x = self.pool(F.relu(self.conv1(x))) + x = self.pool(F.relu(self.conv2(x))) + x = x.view(-1, 50 * x.shape[2] * x.shape[3]) + x = F.relu(self.fc1(x)) + x = self.fc2(x) + # x = F.relu(self.fc2(x)) + # x = self.fc3(x) + return x + +with h5py.File(sys.argv[1], 'r') as db: + num_train = len(db['images']) +print('Have', num_train, 'total training examples') +num_epochs = 10 +max_in_memory = 50000 +repeats = 2 +early_stop_loss = 0.15 +start_idx = 0 +end_idx = max_in_memory +iter_per_epoch = int(np.ceil(num_train / float(max_in_memory))) +indices = np.arange(0, num_train, max_in_memory) +indices = list(indices) + [num_train] +print('iter_per_epoch:', iter_per_epoch) +print(indices) + +# Use GPU. +device = torch.device("cuda:0" if torch.cuda.is_available() else "cpu") +print(device) + +# Load the training data. +print('Loading data ...') + +# Create the network. +input_channels = int(sys.argv[3]) +net = Net(input_channels) +print(net) + +print('Copying network to GPU ...') +net.to(device) + +# Define the loss function and optimizer. +criterion = nn.CrossEntropyLoss() +# optimizer = optim.SGD(net.parameters(), lr=0.1, momentum=0.9) +# optimizer = optim.SGD(net.parameters(), lr=0.01, momentum=0.9) +# optimizer = optim.SGD(net.parameters(), lr=0.001, momentum=0.9, weight_decay=0.01) +# optimizer = optim.SGD(net.parameters(), lr=0.001, momentum=0.9, weight_decay=0.05) +# optimizer = optim.SGD(net.parameters(), lr=0.001, momentum=0.9, weight_decay=0.1) +optimizer = optim.SGD(net.parameters(), lr=0.0001, momentum=0.9, weight_decay=0.01) +# optimizer = optim.SGD(net.parameters(), lr=0.0001, momentum=0.9, weight_decay=0.05) +# optimizer = optim.SGD(net.parameters(), lr=0.00001, momentum=0.9, weight_decay=0.05) +# optimizer = optim.SGD(net.parameters(), lr=0.000001, momentum=0.9, weight_decay=0.01) + +early_stop = False +print('Training ...') + +for epoch in range(num_epochs): + print('epoch: %d/%d' % (epoch, num_epochs)) + + for j in range(iter_per_epoch): + print('iter: %d/%d' % (j, iter_per_epoch)) + dset = H5Dataset(sys.argv[1], indices[j], indices[j + 1]) + #train_loader = torchdata.DataLoader(dset, batch_size=64, shuffle=True, num_workers=2) + train_loader = torchdata.DataLoader(dset, batch_size=64, shuffle=True) + + running_loss = 0.0 + + for r in range(repeats): + for i, data in enumerate(train_loader): + # get the inputs + inputs, labels = data + inputs = inputs.to(device) + labels = labels.to(device) + + # zero the parameter gradients + optimizer.zero_grad() + + # forward + backward + optimize + outputs = net(inputs) + loss = criterion(outputs, labels.long()) + loss.backward() + optimizer.step() + + # print statistics + running_loss += loss.item() + if i % 100 == 99: # print every 10 mini-batches + print('epoch: %d, batch: %5d, loss: %.5f' % + (epoch + 1, i + 1, running_loss / 100)) + if running_loss / 100 < early_stop_loss: + print('reached loss threshold for early stopping: %.5f', early_stop_loss) + early_stop = True + break + running_loss = 0.0 + if early_stop: + break + if early_stop: + break + if early_stop: + break + +print('Finished Training') + +model_path = raw_input("Enter the filename/path for the trained model: ") +torch.save(net.state_dict(), model_path) + +# Test the network on the test data. +test_set = H5Dataset(sys.argv[2], 0, 20000) +test_loader = torchdata.DataLoader(test_set, batch_size=64, shuffle=True) +correct = 0 +total = 0 +print('Testing the network on the test data ...') +with torch.no_grad(): + for data in test_loader: + inputs, labels = data + inputs = inputs.to(device) + labels = labels.to(device) + outputs = net(inputs) + _, predicted = torch.max(outputs.data, 1) + total += labels.size(0) + correct += (predicted == labels.long()).sum().item() + +print('Accuracy of the network on the 20000 test images: %d %%' % ( + 100 * correct / total)) diff --git a/pytorch/train_net_zarr.py b/pytorch/train_net_zarr.py new file mode 100644 index 0000000..5cb6bcf --- /dev/null +++ b/pytorch/train_net_zarr.py @@ -0,0 +1,206 @@ +# Use tensors to speed up loading data onto the GPU during training. + +import zarr +import numpy as np +import torch +import torch.nn as nn +import torch.nn.functional as F +import torch.optim as optim +import torch.utils.data as torchdata +import torch.multiprocessing +import sys + +#torch.multiprocessing.set_start_method('spawn') + +class ZarrDataset(torchdata.Dataset): + def __init__(self, file_path, start_idx, end_idx): + super(ZarrDataset, self).__init__() + f = zarr.open(file_path, 'r') + self.data = torch.from_numpy(np.array(f.get('images')[start_idx : end_idx])) + print(self.data.dtype) + self.target = torch.from_numpy(np.array(f.get('labels')[start_idx : end_idx])).to(torch.int32) #.astype('int32')) + print("Loaded data") + + def __getitem__(self, index): + image = self.data[index,:,:].to(torch.float32) * 1/256.0 + # Pytorch uses NCHW format + image = image.reshape((image.shape[2], image.shape[0], image.shape[1])) + target = self.target[index,:][0] + return (image, target) + + def __len__(self): + return self.data.shape[0] + +class Net(nn.Module): + def __init__(self, input_channels): + super(Net, self).__init__() + self.conv1 = nn.Conv2d(input_channels, 20, 5) + self.pool = nn.MaxPool2d(2, 2) + self.conv2 = nn.Conv2d(20, 50, 5) +# self.drop2d = nn.Dropout2d(p=0.2) + self.fc1 = nn.Linear(50 * 12 * 12, 500) +# self.drop1 = nn.Dropout(p=0.5) + self.fc2 = nn.Linear(500, 2) + + def forward(self, x): + x = self.pool(F.relu(self.conv1(x))) + x = self.pool(F.relu(self.conv2(x))) +# x = self.pool(F.relu(self.drop2d(self.conv2(x)))) + x = x.view(-1, x.shape[1] * x.shape[2] * x.shape[3]) + x = F.relu(self.fc1(x)) +# x = self.drop1(x) + x = self.fc2(x) + return x + +def train(model, criterion, optimizer, data, device): + # Get the inputs and transfer them to the CPU/GPU. + inputs, labels = data + inputs = inputs.to(device) + labels = labels.to(device) + + # Reset the parameter gradients. + optimizer.zero_grad() + + # Forward + backward + optimize. + outputs = model(inputs) + loss = criterion(outputs, labels.long()) + loss.backward() + optimizer.step() + + return loss + +def eval(model, test_loader, device): + model.eval() + correct = 0 + total = 0 + print('Testing the network on the test data ...') + + with torch.no_grad(): + for data in test_loader: + inputs, labels = data + inputs = inputs.to(device) + labels = labels.to(device) + + # Forward + outputs = model(inputs) + _, predicted = torch.max(outputs.data, 1) + total += labels.size(0) + correct += (predicted == labels.long()).sum().item() + + accuracy = 100.0 * float(correct) / float(total) + print('Accuracy of the network on the test set: %.3f%%' % ( + accuracy)) + + return accuracy + +if len(sys.argv) < 3: + print('ERROR: Not enough input arguments!') + print('Usage: python train_net3.py pathToTrainingSet.h5 pathToTestSet.h5') + exit(-1) + +f = zarr.open(sys.argv[1], 'r') +num_train = len(f['images']) +num_epochs = 5 +max_in_memory = 120000 +repeats = 1 +early_stop_loss = 0.0000001 +start_idx = 0 +end_idx = max_in_memory +iter_per_epoch = int(np.ceil(num_train / float(max_in_memory))) +indices = np.arange(0, num_train, max_in_memory) +indices = list(indices) + [num_train] +print('iter_per_epoch:', iter_per_epoch) +print(indices) + +test_set = ZarrDataset(sys.argv[2], 0, 20000) +test_loader = torchdata.DataLoader(test_set, batch_size=64, shuffle=True, num_workers=4) + +# Use GPU. +device = torch.device("cuda:0" if torch.cuda.is_available() else "cpu") +print('device:', device) +# Create the network. +net = Net(test_set.data.shape[-1]) +print('Copying network to GPU ...') +net.to(device) +print(net) + +# Define the loss function and optimizer. +criterion = nn.CrossEntropyLoss() +optimizer = optim.SGD(net.parameters(), lr=0.0001, momentum=0.9, weight_decay=0.0005) +# optimizer = optim.Adam(net.parameters(), lr=0.001, weight_decay=0.001) + +# scheduler = torch.optim.lr_scheduler.ExponentialLR(optimizer, gamma=0.96) + +accuracy = eval(net, test_loader, device) +accuracies = [] +accuracies.append(accuracy) + +early_stop = False +losses = [] +loss = None +print('Training ...') + +for epoch in range(num_epochs): + print('epoch: %d/%d' % (epoch + 1, num_epochs)) + net.train() + + # scheduler.step() + for param_group in optimizer.param_groups: + print('learning rate:', param_group['lr']) + + for j in range(iter_per_epoch): + print('Iteration: %d/%d' % (j + 1, iter_per_epoch)) + print('Loading data block [%d, %d] ...' % (indices[j], indices[j + 1])) + dset = [] + train_loader = [] + dset = ZarrDataset(sys.argv[1], indices[j], indices[j + 1]) + train_loader = torchdata.DataLoader(dset, batch_size=64, shuffle=True, num_workers=4) + running_loss = 0.0 + + for r in range(repeats): + if r > 1: + print('Repeat: %d/%d' % (r + 1, repeats)) + for i, data in enumerate(train_loader): + loss = train(net, criterion, optimizer, data, device) + + # print statistics + running_loss += loss.item() + if i % 200 == 199: + print('Epoch: %d, batch: %5d, loss: %.5f' % + (epoch + 1, i + 1, running_loss / 1000)) + losses.append(running_loss) + if running_loss / 1000 < early_stop_loss: + print('Reached loss threshold for early stopping: %.5f', early_stop_loss) + early_stop = True + break + running_loss = 0.0 + # Evaluate the network on the test dataset. + accuracy = eval(net, test_loader, device) + accuracies.append(accuracy) + model_path = 'model_' + str(accuracy) + '.pwf' + torch.save(net.state_dict(), model_path) + net.train() + if early_stop: + break + if early_stop: + break + if early_stop: + break + + # Evaluate the network on the test dataset. + accuracy = eval(net, test_loader, device) + accuracies.append(accuracy) + model_path = 'model_' + str(accuracy) + '.pwf' + torch.save(net.state_dict(), model_path) + +print('Finished Training') + +model_path = 'model.pwf' +torch.save(net.state_dict(), model_path) + +with open('loss_stats.txt', 'w') as f: + for l in losses: + f.write("%s\n" % str(l)) +with open('accuracy_stats.txt', 'w') as f: + for a in accuracies: + f.write("%s\n" % str(a)) diff --git a/pytorch/zarr_loader.py b/pytorch/zarr_loader.py new file mode 100644 index 0000000..2412e31 --- /dev/null +++ b/pytorch/zarr_loader.py @@ -0,0 +1,40 @@ +import zarr +import torch +import sys +import torch.utils.data as torchdata +#import torch.multiprocessing +import numpy as np + +#torch.multiprocessing.set_start_method('spawn') + +class ZarrDataset(torchdata.Dataset): + + def __init__(self, file_path): + super(ZarrDataset, self).__init__() + root = zarr.open(file_path, mode='r') + self.images = root['images'] + self.labels = root['labels'] + + def __getitem__(self, index): + return (torch.from_numpy(np.array(self.images[index])).float(), + torch.from_numpy(np.array(self.labels[index])).int()) + + def __len__(self): + return self.images.shape[0] + +train_dset = ZarrDataset(sys.argv[1]) +train_loader = torchdata.DataLoader(train_dset, batch_size=64, shuffle=True, num_workers=4) + +device = torch.device("cuda:0" if torch.cuda.is_available() else "cpu") +print('device:', device) + +print('Iterating over dataset ...') + +for i_batch, sample_batched in enumerate(train_loader): + print(i_batch, sample_batched[0].size(), sample_batched[1].size()) + sample_batched[0] = sample_batched[0].to(device) + sample_batched[1] = sample_batched[1].to(device) + + # observe 4th batch and stop. + if i_batch == 100: + break diff --git a/readme/clutter.png b/readme/clutter.png new file mode 100644 index 0000000..1aad306 Binary files /dev/null and b/readme/clutter.png differ diff --git a/readme/file.png b/readme/file.png index 131bebb..b90b0fd 100644 Binary files a/readme/file.png and b/readme/file.png differ diff --git a/readme/hand_frame.png b/readme/hand_frame.png new file mode 100644 index 0000000..200d356 Binary files /dev/null and b/readme/hand_frame.png differ diff --git a/readme/image_15channels.png b/readme/image_15channels.png new file mode 100644 index 0000000..ef7b8fb Binary files /dev/null and b/readme/image_15channels.png differ diff --git a/readme/scene7_12channels.png b/readme/scene7_12channels.png new file mode 100644 index 0000000..5660185 Binary files /dev/null and b/readme/scene7_12channels.png differ diff --git a/readme/scene7_15channels.png b/readme/scene7_15channels.png new file mode 100644 index 0000000..f313cdf Binary files /dev/null and b/readme/scene7_15channels.png differ diff --git a/readme/scene7_3channels.png b/readme/scene7_3channels.png new file mode 100644 index 0000000..30f861b Binary files /dev/null and b/readme/scene7_3channels.png differ diff --git a/readme/ur5_video.jpg b/readme/ur5_video.jpg new file mode 100644 index 0000000..899ae4c Binary files /dev/null and b/readme/ur5_video.jpg differ diff --git a/scripts/get_grasps.py b/scripts/get_grasps.py deleted file mode 100644 index c3c33f1..0000000 --- a/scripts/get_grasps.py +++ /dev/null @@ -1,26 +0,0 @@ -import rospy -from grasp_candidates_classifier.msg import GraspConfigList - - -grasps = [] - - -def callback(msg): - global grasps - grasps = msg.grasps - - -# Create a ROS node. -rospy.init_node('get_grasps') - -# Subscribe to the ROS topic that contains the grasps. -sub = rospy.Subscriber('/detect_grasps/clustered_grasps', GraspConfigList, callback) - -# Wait for grasps to arrive. -rate = rospy.Rate(1) - -while not rospy.is_shutdown(): - if len(grasps) > 0: - rospy.loginfo('Received %d grasps.', len(grasps)) - break - rate.sleep() diff --git a/scripts/object_grasps.py b/scripts/object_grasps.py deleted file mode 100644 index 62c3559..0000000 --- a/scripts/object_grasps.py +++ /dev/null @@ -1,104 +0,0 @@ -import cv2 -from cv_bridge import CvBridge, CvBridgeError -import rospy -from sensor_msgs.msg import Image - -from grasp_candidates_classifier.msg import GraspConfigList - -import matplotlib.pyplot as plt -import numpy as np -import os -import sys -caffe_root = '/home/andreas/software/caffe/' # replace this with the location of your Caffe folder -sys.path.insert(0, caffe_root + 'python') -import caffe - - -# global variable to store grasps -grasps = [] - -# global variable to store RGB image -image = None - - -# Callback function to receive grasps. -def graspsCallback(msg): - global grasps - grasps = msg.grasps - - -# Callback function to receive images. -def imageCallback(msg): - global image - bridge = CvBridge() - try: - image = bridge.imgmsg_to_cv2(msg, "rgb8") - except CvBridgeError, e: - print(e) - - -# ==================== MAIN ==================== -# Create a ROS node. -rospy.init_node('get_grasps') - -# Subscribe to the ROS topic that contains the grasps. -grasps_sub = rospy.Subscriber('/detect_grasps/clustered_grasps', GraspConfigList, graspsCallback) - -# Subscribe to the ROS topic that contains the RGB images. -image_sub = rospy.Subscriber("/camera/rgb/image_raw", Image, imageCallback) - -# Wait for grasps to arrive. -rate = rospy.Rate(1) - -while not rospy.is_shutdown(): - print '.' - if len(grasps) > 0 and image != None: - rospy.loginfo('Received %d grasps.', len(grasps)) - break - rate.sleep() - -# Detect mug. -if os.path.isfile(caffe_root + 'models/bvlc_reference_caffenet/bvlc_reference_caffenet.caffemodel'): - print 'CaffeNet found.' - -caffe.set_mode_cpu() - -# Load the network and its weights. -model_def = caffe_root + 'models/bvlc_reference_caffenet/deploy.prototxt' -model_weights = caffe_root + 'models/bvlc_reference_caffenet/bvlc_reference_caffenet.caffemodel' -net = caffe.Net(model_def, model_weights, caffe.TEST) - -# Load the mean ImageNet image (as distributed with Caffe) for subtraction -mu = np.load(caffe_root + 'python/caffe/imagenet/ilsvrc_2012_mean.npy') -mu = mu.mean(1).mean(1) # average over pixels to obtain the mean (BGR) pixel values -print 'mean-subtracted values:', zip('BGR', mu) - -# create transformer for the input called 'data' -transformer = caffe.io.Transformer({'data': net.blobs['data'].data.shape}) - -transformer.set_transpose('data', (2,0,1)) # move image channels to outermost dimension -transformer.set_mean('data', mu) # subtract the dataset-mean value in each channel -transformer.set_raw_scale('data', 255) # rescale from [0, 1] to [0, 255] -transformer.set_channel_swap('data', (2,1,0)) # swap channels from RGB to BGR - -# image = caffe.io.load_image(caffe_root + 'examples/images/cat.jpg') -transformed_image = transformer.preprocess('data', image) -plt.imshow(image) -plt.show() - -# copy the image data into the memory allocated for the net -net.blobs['data'].data[...] = transformed_image - -### perform classification -output = net.forward() - -output_prob = output['prob'][0] # the output probability vector for the first image in the batch - -print 'predicted class is:', output_prob.argmax() - -# load ImageNet labels -labels_file = caffe_root + 'data/ilsvrc12/synset_words.txt' - -labels = np.loadtxt(labels_file, str, delimiter='\t') - -print 'output label:', labels[output_prob.argmax()] \ No newline at end of file diff --git a/scripts/select_grasp.py b/scripts/select_grasp.py deleted file mode 100644 index 7a3f27c..0000000 --- a/scripts/select_grasp.py +++ /dev/null @@ -1,83 +0,0 @@ -import rospy -from sensor_msgs.msg import PointCloud2 -from sensor_msgs import point_cloud2 - -cloud = [] # global variable to store the point cloud - -def cloudCallback(msg): - global cloud - if len(cloud) == 0: - for p in point_cloud2.read_points(msg): - cloud.append([p[0], p[1], p[2]]) - - -# Create a ROS node. -rospy.init_node('select_grasp') - -# Subscribe to the ROS topic that contains the grasps. -cloud_sub = rospy.Subscriber('/cloud_pcd', PointCloud2, cloudCallback) - -# Wait for point cloud to arrive. -while len(cloud) == 0: - rospy.sleep(0.01) - - -# Extract the nonplanar indices. Uses a least squares fit AX = b. Plane equation: z = ax + by + c. -import numpy as np -from scipy.linalg import lstsq - -cloud = np.asarray(cloud) -X = cloud -A = np.c_[X[:,0], X[:,1], np.ones(X.shape[0])] -C, _, _, _ = lstsq(A, X[:,2]) -a, b, c, d = C[0], C[1], -1., C[2] # coefficients of the form: a*x + b*y + c*z + d = 0. -dist = ((a*X[:,0] + b*X[:,1] + d) - X[:,2])**2 -err = dist.sum() -idx = np.where(dist > 0.01) - - -# Publish point cloud and nonplanar indices. -from gpd.msg import CloudIndexed -from std_msgs.msg import Header, Int64 -from geometry_msgs.msg import Point - -pub = rospy.Publisher('cloud_indexed', CloudIndexed, queue_size=1) - -msg = CloudIndexed() -header = Header() -header.frame_id = "/base_link" -header.stamp = rospy.Time.now() -msg.cloud_sources.cloud = point_cloud2.create_cloud_xyz32(header, cloud.tolist()) -msg.cloud_sources.view_points.append(Point(0,0,0)) -for i in xrange(cloud.shape[0]): - msg.cloud_sources.camera_source.append(Int64(0)) -for i in idx[0]: - msg.indices.append(Int64(i)) -s = raw_input('Hit [ENTER] to publish') -pub.publish(msg) -rospy.sleep(2) -print 'Published cloud with', len(msg.indices), 'indices' - - -# Select a grasp for the robot to execute. -from gpd.msg import GraspConfigList - -grasps = [] # global variable to store grasps - -def callback(msg): - global grasps - grasps = msg.grasps - -# Subscribe to the ROS topic that contains the grasps. -grasps_sub = rospy.Subscriber('/detect_grasps/clustered_grasps', GraspConfigList, callback) - -# Wait for grasps to arrive. -rate = rospy.Rate(1) - -while not rospy.is_shutdown(): - if len(grasps) > 0: - rospy.loginfo('Received %d grasps.', len(grasps)) - break - -grasp = grasps[0] # grasps are sorted in descending order by score -print 'Selected grasp with score:', grasp.score diff --git a/src/cem_detect_grasps.cpp b/src/cem_detect_grasps.cpp new file mode 100644 index 0000000..05114e5 --- /dev/null +++ b/src/cem_detect_grasps.cpp @@ -0,0 +1,99 @@ +#include + +#include + +namespace gpd { +namespace apps { +namespace detect_grasps { + +bool checkFileExists(const std::string &file_name) { + std::ifstream file; + file.open(file_name.c_str()); + if (!file) { + std::cout << "File " + file_name + " could not be found!\n"; + return false; + } + file.close(); + return true; +} + +int DoMain(int argc, char *argv[]) { + // Read arguments from command line. + if (argc < 3) { + std::cout << "Error: Not enough input arguments!\n\n"; + std::cout + << "Usage: cem_detect_grasps CONFIG_FILE PCD_FILE [NORMALS_FILE]\n\n"; + std::cout << "Detect grasp poses for a point cloud, PCD_FILE (*.pcd), " + "using parameters from CONFIG_FILE (*.cfg).\n\n"; + std::cout << "[NORMALS_FILE] (optional) contains a surface normal for each " + "point in the cloud (*.csv).\n"; + return (-1); + } + + std::string config_filename = argv[1]; + std::string pcd_filename = argv[2]; + if (!checkFileExists(config_filename)) { + printf("Error: config file not found!\n"); + return (-1); + } + if (!checkFileExists(pcd_filename)) { + printf("Error: PCD file not found!\n"); + return (-1); + } + + // View point from which the camera sees the point cloud. + Eigen::Matrix3Xd view_points(3, 1); + view_points.setZero(); + + // Load point cloud from file + util::Cloud cloud(pcd_filename, view_points); + if (cloud.getCloudOriginal()->size() == 0) { + std::cout << "Error: Input point cloud is empty or does not exist!\n"; + return (-1); + } + + // Load surface normals from file. + if (argc > 3) { + std::string normals_filename = argv[3]; + cloud.setNormalsFromFile(normals_filename); + std::cout << "Loaded surface normals from file: " << normals_filename + << "\n"; + } + + // Read parameters from configuration file. + const double VOXEL_SIZE = 0.003; + util::ConfigFile config_file(config_filename); + config_file.ExtractKeys(); + std::vector workspace = + config_file.getValueOfKeyAsStdVectorDouble("workspace", "-1 1 -1 1 -1 1"); + int num_threads = config_file.getValueOfKey("num_threads", 1); + bool sample_above_plane = + config_file.getValueOfKey("sample_above_plane", false); + double normals_radius = + config_file.getValueOfKey("normals_radius", 0.03); + printf("num_threads: %d\n", num_threads); + printf("sample_above_plane: %d\n", sample_above_plane); + printf("normals_radius: %.3f\n", normals_radius); + + // Preprocess the point cloud. + cloud.filterWorkspace(workspace); + cloud.voxelizeCloud(VOXEL_SIZE); + cloud.calculateNormals(num_threads, normals_radius); + if (sample_above_plane) { + cloud.sampleAbovePlane(); + } + + // Detect grasp affordances. + SequentialImportanceSampling detector(config_filename); + detector.detectGrasps(cloud); + + return 0; +} + +} // namespace detect_grasps +} // namespace apps +} // namespace gpd + +int main(int argc, char *argv[]) { + return gpd::apps::detect_grasps::DoMain(argc, argv); +} diff --git a/src/detect_grasps.cpp b/src/detect_grasps.cpp new file mode 100644 index 0000000..f3bce54 --- /dev/null +++ b/src/detect_grasps.cpp @@ -0,0 +1,94 @@ +#include + +#include + +namespace gpd { +namespace apps { +namespace detect_grasps { + +bool checkFileExists(const std::string &file_name) { + std::ifstream file; + file.open(file_name.c_str()); + if (!file) { + std::cout << "File " + file_name + " could not be found!\n"; + return false; + } + file.close(); + return true; +} + +int DoMain(int argc, char *argv[]) { + // Read arguments from command line. + if (argc < 3) { + std::cout << "Error: Not enough input arguments!\n\n"; + std::cout << "Usage: detect_grasps CONFIG_FILE PCD_FILE [NORMALS_FILE]\n\n"; + std::cout << "Detect grasp poses for a point cloud, PCD_FILE (*.pcd), " + "using parameters from CONFIG_FILE (*.cfg).\n\n"; + std::cout << "[NORMALS_FILE] (optional) contains a surface normal for each " + "point in the cloud (*.csv).\n"; + return (-1); + } + + std::string config_filename = argv[1]; + std::string pcd_filename = argv[2]; + if (!checkFileExists(config_filename)) { + printf("Error: config file not found!\n"); + return (-1); + } + if (!checkFileExists(pcd_filename)) { + printf("Error: PCD file not found!\n"); + return (-1); + } + + // Read parameters from configuration file. + util::ConfigFile config_file(config_filename); + config_file.ExtractKeys(); + + // Set the camera position. Assumes a single camera view. + std::vector camera_position = + config_file.getValueOfKeyAsStdVectorDouble("camera_position", + "0.0 0.0 0.0"); + Eigen::Matrix3Xd view_points(3, 1); + view_points << camera_position[0], camera_position[1], camera_position[2]; + + // Load point cloud from file. + util::Cloud cloud(pcd_filename, view_points); + if (cloud.getCloudOriginal()->size() == 0) { + std::cout << "Error: Input point cloud is empty or does not exist!\n"; + return (-1); + } + + // Load surface normals from file. + if (argc > 3) { + std::string normals_filename = argv[3]; + cloud.setNormalsFromFile(normals_filename); + std::cout << "Loaded surface normals from file: " << normals_filename + << "\n"; + } + + GraspDetector detector(config_filename); + + // Preprocess the point cloud. + detector.preprocessPointCloud(cloud); + + // If the object is centered at the origin, reverse all surface normals. + bool centered_at_origin = + config_file.getValueOfKey("centered_at_origin", false); + if (centered_at_origin) { + printf("Reversing normal directions ...\n"); + cloud.setNormals(cloud.getNormals() * (-1.0)); + } + + // Detect grasp poses. + detector.detectGrasps(cloud); + + return 0; +} + +} // namespace detect_grasps +} // namespace apps +} // namespace gpd + +int main(int argc, char *argv[]) { + return gpd::apps::detect_grasps::DoMain(argc, argv); +} diff --git a/src/detect_grasps_python.cpp b/src/detect_grasps_python.cpp new file mode 100644 index 0000000..1909f46 --- /dev/null +++ b/src/detect_grasps_python.cpp @@ -0,0 +1,610 @@ +#include +#include + +#include +#include + +#include + +#include +#include + +namespace gpd { +namespace detect_grasps_python { + +typedef pcl::PointXYZRGBA PointT; +typedef pcl::PointCloud PointCloudRGB; + +struct Position { + double x, y, z; + + // Position(); + // + // Position(double x, double y, double z) : px(x), py(y), pz(z) { } + // + // Position(const Eigen::Vector3d& v) : px(v(0)), py(v(1)), pz(v(2)) { } +}; + +struct Quaternion { + double x, y, z, w; + + // Quaternion(); + // + // Quaternion(double x, double y, double z, double w) : qx(x), qy(y), qz(z), + // qw(w) { } + // + // Quaternion(const Eigen::Quaterniond& v) : qx(v.x()), qy(v.y()), qz(v.z()), + // qw(v.w()) { } +}; + +// struct Grasp +//{ +// Position pos; +// Quaternion orient; +// Position sample; +// double score; +// bool label; +//}; + +extern "C" struct Grasp { + double *pos; + double *orient; + double *sample; + double score; + bool label; + int *image; +}; + +extern "C" struct AugmentedCloud { + float *points; + float *normals; + int *camera_index; + float *view_points; + int size; + int num_view_points; +}; + +Eigen::Matrix3Xd viewPointsToMatrix(float *array, int n) { + Eigen::Matrix3Xd mat(3, n); + + for (int i = 0; i < n; i++) { + mat.col(i) << array[3 * i], array[3 * i + 1], array[3 * i + 2]; + } + + return mat; +} + +Eigen::MatrixXi cameraSourceToMatrix(int *array, int rows, int cols) { + Eigen::MatrixXi mat(rows, cols); + + for (int i = 0; i < cols; i++) { + for (int j = 0; j < rows; j++) { + mat(j, i) = array[i * rows + j]; + } + } + + return mat; +} + +int *cvMatToArray(const cv::Mat &image) { + printf("HELLO\n"); + printf("%d, %d, %d\n", image.rows, image.rows, image.channels()); + int *array = new int[image.rows * image.cols * image.channels()]; + int l = 0; + printf("%d, %d, %d\n", image.rows, image.rows, image.channels()); + + for (int i = 0; i < image.rows; i++) { + const uchar *ptr = image.ptr(i); + + for (int j = 0; j < image.cols; j++) { + const uchar *uc_pixel = ptr; + + for (int k = 0; k < image.channels(); k++) { + if ((int)uc_pixel[k] > 0) { + printf("%d, %d, %d\n", i, j, k); + std::cout << "uc_pixel:" << (int)uc_pixel[k] << "\n"; + } + // array[l] = uc_pixel[k]; + l++; + // printf(" %d\n", uc_pixel[k]); + // std::cout << image.at(i, j, k) << "\n"; + // image.at(i, j, k) + // array[l] = (float) image.at(i, j, k); + // printf("%.2f\n", array[l]); + l++; + } + } + } + + return array; +} + +PointCloudRGB arrayToPCLPointCloud(float *points, int num_points) { + PointCloudRGB cloud; + cloud.resize(num_points); + + for (int i = 0; i < num_points; i++) { + PointT p; + p.x = points[3 * i + 0]; + p.y = points[3 * i + 1]; + p.z = points[3 * i + 2]; + cloud.at(i) = p; + } + + return cloud; +} + +Cloud augmentedCloudToCloud(AugmentedCloud *cloud) { + PointCloudRGB::Ptr pcl_cloud(new PointCloudRGB); + *pcl_cloud = arrayToPCLPointCloud(cloud->points, cloud->size); + Eigen::MatrixXi camera_source = cameraSourceToMatrix( + cloud->camera_index, cloud->num_view_points, cloud->size); + Eigen::Matrix3Xd view_points = + viewPointsToMatrix(cloud->view_points, cloud->num_view_points); + Eigen::Matrix3Xd normals = viewPointsToMatrix(cloud->normals, cloud->size); + + Cloud cloud_cam(pcl_cloud, camera_source, view_points); + cloud_cam.setNormals(normals); + return cloud_cam; +} + +Cloud createCloud(float *points, int *camera_index, float *view_points, + int size, int num_view_points) { + PointCloudRGB::Ptr pcl_cloud(new PointCloudRGB); + *pcl_cloud = arrayToPCLPointCloud(points, size); + Eigen::MatrixXi camera_index_mat = + cameraSourceToMatrix(camera_index, num_view_points, size); + Eigen::Matrix3Xd view_points_mat = + viewPointsToMatrix(view_points, num_view_points); + + Cloud cloud_cam(pcl_cloud, camera_index_mat, view_points_mat); + + return cloud_cam; +} + +Cloud createCloudNormals(float *points, float *normals, int *camera_index, + float *view_points, int size, int num_view_points) { + PointCloudRGB::Ptr pcl_cloud(new PointCloudRGB); + *pcl_cloud = arrayToPCLPointCloud(points, size); + Eigen::Matrix3Xd normals_mat = viewPointsToMatrix(normals, size); + Eigen::MatrixXi camera_index_mat = + cameraSourceToMatrix(camera_index, num_view_points, size); + Eigen::Matrix3Xd view_points_mat = + viewPointsToMatrix(view_points, num_view_points); + + Cloud cloud_cam(pcl_cloud, camera_index_mat, view_points_mat); + cloud_cam.setNormals(normals_mat); + + return cloud_cam; +} + +Cloud createGroundTruthCloud(float *points, float *normals, int size) { + PointCloudRGB::Ptr pcl_cloud(new PointCloudRGB); + *pcl_cloud = arrayToPCLPointCloud(points, size); + Eigen::Matrix3Xd points_gt = viewPointsToMatrix(points, size); + Eigen::Matrix3Xd normals_gt = viewPointsToMatrix(normals, size); + Eigen::MatrixXi cam_sources_gt = Eigen::MatrixXi::Ones(1, size); + Eigen::Matrix3Xd cam_pos_gt = Eigen::Matrix3Xd::Zero(3, 1); + + Cloud mesh_cloud(pcl_cloud, cam_sources_gt, cam_pos_gt); + mesh_cloud.setNormals(normals_gt); + + return mesh_cloud; +} + +Position vectorToPositionStruct(const Eigen::Vector3d &v) { + Position p; + p.x = v(0); + p.y = v(1); + p.z = v(2); + return p; +} + +Quaternion matrixToQuaternionStruct(const Eigen::Matrix3d &rot) { + Eigen::Quaterniond quat_eig(rot); + Quaternion q; + q.x = quat_eig.x(); + q.y = quat_eig.y(); + q.z = quat_eig.z(); + q.w = quat_eig.w(); + return q; +} + +Cloud initCloud(char *pcd_filename, char *normals_filename, float *view_points, + int num_view_points) { + // Set view points from which the camera has taken the point cloud. + Eigen::Matrix3Xd view_points_mat = + viewPointsToMatrix(view_points, num_view_points); + + // Load point cloud from file. + Cloud cloud(pcd_filename, view_points_mat); + if (cloud.getCloudOriginal()->size() == 0) { + printf("Error: Input point cloud is empty or does not exist!\n"); + return cloud; + } + + // Optional: load surface normals from file. + if (std::string(normals_filename).size() > 0) { + cloud.setNormalsFromFile(normals_filename); + printf("Loaded surface normals from file: %s\n", normals_filename); + } + + return cloud; +} + +// Grasp* handsToGraspsStruct(const std::vector& hands) +//{ +// Grasp* grasps = new Grasp[hands.size()]; +// +// for (int i = 0; i < hands.size(); i++) +// { +// Position pos = vectorToPositionStruct(hands[i].getGraspBottom()); +// Quaternion quat = matrixToQuaternionStruct(hands[i].getFrame()); +// Position sample = vectorToPositionStruct(hands[i].getSample()); +// +// grasps[i].pos = pos; +// grasps[i].orient = quat; +// grasps[i].sample = sample; +// grasps[i].score = hands[i].getScore(); +// grasps[i].label = hands[i].isFullAntipodal(); +// } +// +// return grasps; +//} + +Grasp *handsToGraspsStruct(const std::vector &hands) { + Grasp *grasps = new Grasp[hands.size()]; + + for (int i = 0; i < hands.size(); i++) { + grasps[i].pos = new double[3]; + Eigen::Map(grasps[i].pos) = hands[i].getGraspBottom(); + grasps[i].orient = new double[4]; + Eigen::Quaterniond q(hands[i].getFrame()); + Eigen::Map(grasps[i].orient) = q; + grasps[i].sample = new double[3]; + grasps[i].score = hands[i].getScore(); + grasps[i].label = hands[i].isFullAntipodal(); + grasps[i].image = new int[0]; + grasps[i].image[0] = -1; + } + + return grasps; +} + +Grasp *handsToGraspsStruct(const std::vector &hands, + const std::vector &images) { + Grasp *grasps = new Grasp[hands.size()]; + + for (int i = 0; i < hands.size(); i++) { + grasps[i].pos = new double[3]; + Eigen::Map(grasps[i].pos) = hands[i].getGraspBottom(); + grasps[i].orient = new double[4]; + Eigen::Quaterniond q(hands[i].getFrame()); + Eigen::Map(grasps[i].orient) = q; + grasps[i].sample = new double[3]; + grasps[i].score = hands[i].getScore(); + grasps[i].label = hands[i].isFullAntipodal(); + printf("i: %d\n", i); + grasps[i].image = cvMatToArray(images[0]); // new + // int[images[i].rows*images[i].cols*images[i].channels()]; + // int* img = cvMatToArray(images[0]); + // grasps[i].image = img; + // delete[] img; + } + + return grasps; +} + +std::vector detectGrasps(const std::string &config_filename, + Cloud &cloud_cam) { + // Preprocess the point cloud. + GraspDetector detector(config_filename); + detector.preprocessPointCloud(cloud_cam); + + // Detect grasps. + std::vector hands = detector.detectGrasps(cloud_cam); + + return hands; +} + +std::vector generateGraspCandidates(const std::string &config_filename, + Cloud &cloud_cam) { + // Preprocess the point cloud. + GraspDetector detector(config_filename); + detector.preprocessPointCloud(cloud_cam); + + // Generate grasp candidates. + std::vector hand_sets = detector.generateGraspCandidates(cloud_cam); + + std::vector hands; + for (int i = 0; i < hand_sets.size(); i++) { + for (int j = 0; j < hand_sets[i].getHands().size(); j++) { + if (hand_sets[i].getIsValid()[j]) { + hands.push_back(hand_sets[i].getHands()[j]); + } + } + } + + return hands; +} + +void copyImageMatrix(const cv::Mat &src, cv::Mat &dst, int idx_in) { + for (int j = 0; j < 60; j++) { + for (int k = 0; k < 60; k++) { + for (int l = 0; l < 15; l++) { + int idx_src[3] = {j, k, l}; + int idx_dst[4] = {idx_in, j, k, l}; + dst.at(idx_dst) = src.at(idx_src); + } + } + } +} + +void handToTransform4x4(const Hand &hand, cv::Mat &tf, int idx_in) { + const Eigen::Matrix3d orient = hand.getFrame(); + + // Copy the orientation. + int idx[3] = {idx_in, 0, 0}; + + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + idx[1] = i; + idx[2] = j; + tf.at(idx) = orient(i, j); + } + } + + // Copy the position. + idx[2] = 3; + + for (int j = 0; j < 3; j++) { + idx[1] = j; + tf.at(idx) = hand.getGraspBottom()(j); + } +} + +bool calcDescriptorsHelper(Cloud &cloud, GraspDetector &detector, + char *filename_out, int compress_level) { + const std::string IMAGES_DS_NAME = "images"; + const std::string HANDS_DS_NAME = "hands"; + const std::string WIDTHS_DS_NAME = "widths"; + + // Calculate grasps and grasp descriptors. + std::vector hands; + std::vector images; + bool found_hands = detector.createGraspImages(cloud, hands, images); + + if (not found_hands) { + return false; + } + + // Store the grasps and the grasp descriptors in a HDF5 file. + int n_dims = 3; + int dsdims_images[n_dims] = {images.size(), images[0].rows, images[0].cols}; + cv::Mat images_mat(n_dims, dsdims_images, CV_8UC(images[0].channels()), + cv::Scalar(0.0)); + + int dsdims_hands[n_dims] = {hands.size(), 4, 4}; + cv::Mat hands_mat(n_dims, dsdims_hands, CV_32FC1, cv::Scalar(0.0)); + + int n_dims_widths = 1; + int dsdims_widths[n_dims_widths] = {hands.size()}; + cv::Mat gripper_widths_mat(n_dims_widths, dsdims_widths, CV_32FC1, + cv::Scalar(0.0)); + + for (int i = 0; i < images.size(); i++) { + copyImageMatrix(images[i], images_mat, i); + handToTransform4x4(hands[i], hands_mat, i); + gripper_widths_mat.at(i) = hands[i].getGraspWidth(); + + // ranges don't seem to work + // std::vector ranges; + // ranges.push_back(cv::Range(i,i+1)); + // ranges.push_back(cv::Range(0,60)); + // ranges.push_back(cv::Range(0,60)); + // data(&ranges[0]) = dataset[i].image_.clone(); + // dataset[0].image_.copyTo(data(&ranges[0])); + // printf("dataset.label: %d, labels(i): %d\n", dataset[i].label_, + // labels.at(i)); + } + + cv::Ptr h5io = cv::hdf::open(filename_out); + printf("Storing dataset %s ...\n", IMAGES_DS_NAME.c_str()); + h5io->dscreate(n_dims, dsdims_images, CV_8UC(images[0].channels()), + IMAGES_DS_NAME, compress_level); + h5io->dswrite(images_mat, IMAGES_DS_NAME); + + printf("Storing dataset %s ...\n", HANDS_DS_NAME.c_str()); + h5io->dscreate(n_dims, dsdims_hands, CV_32FC1, HANDS_DS_NAME, compress_level); + h5io->dswrite(hands_mat, HANDS_DS_NAME); + + printf("Storing dataset %s ...\n", WIDTHS_DS_NAME.c_str()); + h5io->dscreate(n_dims_widths, dsdims_widths, CV_32FC1, WIDTHS_DS_NAME, + compress_level); + h5io->dswrite(gripper_widths_mat, WIDTHS_DS_NAME); + + h5io->close(); + + return true; +} + +extern "C" int detectGraspsInCloud(char *config_filename, float *points, + int *camera_index, float *view_points, + int size, int num_view_points, + struct Grasp **grasps_out) { + Cloud cloud = + createCloud(points, camera_index, view_points, size, num_view_points); + + // Detect grasp affordances. + std::string config_filename_str = config_filename; + std::vector hands = detectGrasps(config_filename_str, cloud); + + // Convert output to array of structs. + *grasps_out = handsToGraspsStruct(hands); + int num_out = (int)hands.size(); + + return num_out; +} + +extern "C" int detectGraspsInCloudNormals(char *config_filename, float *points, + float *normals, int *camera_index, + float *view_points, int size, + int num_view_points, + struct Grasp **grasps_out) { + Cloud cloud = createCloudNormals(points, normals, camera_index, view_points, + size, num_view_points); + + // Detect grasp affordances. + std::string config_filename_str = config_filename; + std::vector hands = detectGrasps(config_filename_str, cloud); + + // Convert output to array of structs. + *grasps_out = handsToGraspsStruct(hands); + int num_out = (int)hands.size(); + + return num_out; +} + +extern "C" int detectGraspsInFile(char *config_filename, char *pcd_filename, + char *normals_filename, float *view_points, + int num_view_points, + struct Grasp **grasps_out) { + // Initialize cloud. + Cloud cloud = + initCloud(pcd_filename, normals_filename, view_points, num_view_points); + if (cloud.getCloudOriginal()->size() == 0) { + return 0; + } + + // Detect grasp affordances. + std::string config_filename_str = config_filename; + std::vector hands = detectGrasps(config_filename_str, cloud); + + // Convert output to array of structs. + *grasps_out = handsToGraspsStruct(hands); + int num_out = (int)hands.size(); + + return num_out; +} + +extern "C" int detectAndEvalGrasps(char *config_filename, float *points, + int *camera_index, float *view_points, + int size, int num_view_points, + float *points_gt, float *normals_gt, + int size_gt, struct Grasp **grasps_out) { + // Create point cloud for current view. + Cloud cloud = + createCloud(points, camera_index, view_points, size, num_view_points); + + // Create point cloud for ground truth mesh. + Cloud mesh_cloud = createGroundTruthCloud(points_gt, normals_gt, size_gt); + + // Preprocess the point cloud. + GraspDetector detector(config_filename); + detector.preprocessPointCloud(cloud); + + // Generate grasp candidates. + std::vector hands; + std::vector images; + bool found_hands = detector.createGraspImages(cloud, hands, images); + if (found_hands == false) { + printf("No grasps found!\n"); + return 0; + } + printf("Created %d grasps and %d images.\n", (int)hands.size(), + (int)images.size()); + + // Evaluate candidates against ground truth. + std::vector labeled_hands = detector.evalGroundTruth(mesh_cloud, hands); + printf("Done!!\n"); + + // Convert output to array of structs. + *grasps_out = handsToGraspsStruct(labeled_hands, images); + int num_out = (int)hands.size(); + + printf("%d\n", num_out); + + return num_out; +} + +extern "C" int generateGraspCandidatesInFile( + char *config_filename, char *pcd_filename, char *normals_filename, + float *view_points, int num_view_points, struct Grasp **grasps_out) { + // Initialize point cloud. + Cloud cloud = + initCloud(pcd_filename, normals_filename, view_points, num_view_points); + if (cloud.getCloudOriginal()->size() == 0) { + return 0; + } + + // Detect grasp affordances. + std::string config_filename_str = config_filename; + std::vector hands = generateGraspCandidates(config_filename_str, cloud); + + // Convert output to array of structs. + *grasps_out = handsToGraspsStruct(hands); + int num_out = (int)hands.size(); + + return num_out; +} + +extern "C" int calcGraspDescriptorsAtIndices( + char *config_filename, float *points, int *camera_index, float *view_points, + int size, int num_view_points, int *indices, int num_indices, + int compress_level, char *filename_out) { + // Initialize point cloud. + Cloud cloud = + createCloud(points, camera_index, view_points, size, num_view_points); + if (cloud.getCloudOriginal()->size() == 0) { + return 0; + } + + // Set indices at which to sample grasps. + std::vector sample_indices; + sample_indices.resize(num_indices); + for (int i = 0; i < num_indices; i++) { + sample_indices[i] = indices[i]; + } + cloud.setSampleIndices(sample_indices); + + // Calculate grasps and grasp descriptors. + GraspDetector detector(config_filename); + if (calcDescriptorsHelper(cloud, detector, filename_out, compress_level)) { + return 1; + } + + return 0; +} + +extern "C" int calcGraspDescriptors(char *config_filename, float *points, + int *camera_index, float *view_points, + int size, int num_view_points, + int compress_level, char *filename_out) { + Cloud cloud = + createCloud(points, camera_index, view_points, size, num_view_points); + + // Preprocess the point cloud. + GraspDetector detector(config_filename); + detector.preprocessPointCloud(cloud); + + // Calculate grasps and grasp descriptors. + if (calcDescriptorsHelper(cloud, detector, filename_out, compress_level)) { + return 1; + } + + return 0; +} + +extern "C" int freeMemoryGrasps(Grasp *in) { + delete[] in; + return 0; +} + +extern "C" int CopyAndFree(float *in, float *out, int n) { + memcpy(out, in, sizeof(float) * n); + delete[] in; + return 0; +} + +} // namespace detect_grasps_python +} // namespace gpd diff --git a/src/generate_candidates.cpp b/src/generate_candidates.cpp new file mode 100644 index 0000000..3d92e8d --- /dev/null +++ b/src/generate_candidates.cpp @@ -0,0 +1,151 @@ +// System +#include +#include +#include + +// Custom +#include +#include +#include + +namespace gpd { +namespace apps { +namespace generate_candidates { + +// function to read in a double array from a single line of a configuration file +std::vector stringToDouble(const std::string &str) { + std::vector values; + std::stringstream ss(str); + double v; + + while (ss >> v) { + values.push_back(v); + if (ss.peek() == ' ') { + ss.ignore(); + } + } + + return values; +} + +int DoMain(int argc, char *argv[]) { + // Read arguments from command line. + if (argc < 3) { + std::cout << "Error: Not enough input arguments!\n\n"; + std::cout + << "Usage: generate_candidates CONFIG_FILE PCD_FILE [NORMALS_FILE]\n\n"; + std::cout << "Generate grasp candidates for a point cloud, PCD_FILE " + "(*.pcd), using parameters from CONFIG_FILE (*.cfg).\n\n"; + std::cout << "[NORMALS_FILE] (optional) contains a surface normal for each " + "point in the cloud (*.csv).\n"; + return (-1); + } + + // Read parameters from configuration file. + util::ConfigFile config_file(argv[1]); + + candidate::HandGeometry hand_geom; + hand_geom.finger_width_ = + config_file.getValueOfKey("finger_width", 0.01); + hand_geom.outer_diameter_ = + config_file.getValueOfKey("hand_outer_diameter", 0.12); + hand_geom.depth_ = config_file.getValueOfKey("hand_depth", 0.06); + hand_geom.height_ = config_file.getValueOfKey("hand_height", 0.02); + hand_geom.init_bite_ = config_file.getValueOfKey("init_bite", 0.01); + + std::cout << "finger_width: " << hand_geom.finger_width_ << "\n"; + std::cout << "hand_outer_diameter: " << hand_geom.outer_diameter_ << "\n"; + std::cout << "hand_depth: " << hand_geom.depth_ << "\n"; + std::cout << "hand_height: " << hand_geom.height_ << "\n"; + std::cout << "init_bite: " << hand_geom.init_bite_ << "\n"; + + bool voxelize = config_file.getValueOfKey("voxelize", true); + bool remove_outliers = + config_file.getValueOfKey("remove_outliers", false); + std::string workspace_str = + config_file.getValueOfKeyAsString("workspace", ""); + std::string camera_pose_str = + config_file.getValueOfKeyAsString("camera_pose", ""); + std::vector workspace = stringToDouble(workspace_str); + std::vector camera_pose = stringToDouble(camera_pose_str); + std::cout << "voxelize: " << voxelize << "\n"; + std::cout << "remove_outliers: " << remove_outliers << "\n"; + std::cout << "workspace: " << workspace_str << "\n"; + std::cout << "camera_pose: " << camera_pose_str << "\n"; + + int num_samples = config_file.getValueOfKey("num_samples", 1000); + int num_threads = config_file.getValueOfKey("num_threads", 1); + double nn_radius = config_file.getValueOfKey("nn_radius", 0.01); + int num_orientations = config_file.getValueOfKey("num_orientations", 8); + std::vector hand_axes = + config_file.getValueOfKeyAsStdVectorInt("hand_axes", "2"); + std::cout << "num_samples: " << num_samples << "\n"; + std::cout << "num_threads: " << num_threads << "\n"; + std::cout << "nn_radius: " << nn_radius << "\n"; + std::cout << "num_orientations: " << num_orientations << "\n"; + printf("hand_axes: "); + for (int i = 0; i < hand_axes.size(); ++i) { + printf("%d ", hand_axes[i]); + } + printf("\n"); + + bool plot_grasps = config_file.getValueOfKey("plot_grasps", true); + bool plot_all_grasps = + config_file.getValueOfKey("plot_all_grasps", true); + bool plot_normals = config_file.getValueOfKey("plot_normals", false); + std::cout << "plot_grasps: " << plot_grasps << "\n"; + std::cout << "plot_all_grasps: " << plot_all_grasps << "\n"; + std::cout << "plot_normals: " << plot_normals << "\n"; + + // Create object to generate grasp candidates. + candidate::CandidatesGenerator::Parameters generator_params; + generator_params.num_samples_ = num_samples; + generator_params.num_threads_ = num_threads; + generator_params.remove_statistical_outliers_ = remove_outliers; + generator_params.voxelize_ = voxelize; + generator_params.workspace_ = workspace; + candidate::HandSearch::Parameters hand_search_params; + hand_search_params.hand_geometry_ = hand_geom; + hand_search_params.nn_radius_frames_ = nn_radius; + hand_search_params.num_orientations_ = num_orientations; + hand_search_params.num_samples_ = num_samples; + hand_search_params.num_threads_ = num_threads; + hand_search_params.hand_axes_ = hand_axes; + candidate::CandidatesGenerator candidates_generator(generator_params, + hand_search_params); + + // Set the camera pose. + Eigen::Matrix3Xd view_points(3, 1); + view_points << camera_pose[3], camera_pose[6], camera_pose[9]; + + // Create object to load point cloud from file. + util::Cloud cloud_cam(argv[2], view_points); + if (cloud_cam.getCloudOriginal()->size() == 0) { + std::cout << "Input point cloud is empty or does not exist!\n"; + return (-1); + } + + // Load surface normals from file. + if (argc > 3) { + cloud_cam.setNormalsFromFile(argv[3]); + std::cout << "Loaded surface normals from file.\n"; + } + + // Preprocess the point cloud: voxelize, remove statistical outliers, + // workspace filter, compute normals, subsample. + candidates_generator.preprocessPointCloud(cloud_cam); + + // Generate a list of grasp candidates. + std::vector> candidates = + candidates_generator.generateGraspCandidates(cloud_cam); + + return 0; +} + +} // namespace generate_candidates +} // namespace apps +} // namespace gpd + +int main(int argc, char *argv[]) { + return gpd::apps::generate_candidates::DoMain(argc, argv); +} diff --git a/src/generate_data.cpp b/src/generate_data.cpp new file mode 100644 index 0000000..7436384 --- /dev/null +++ b/src/generate_data.cpp @@ -0,0 +1,71 @@ +#include + +#include +#include + +namespace gpd { +namespace apps { +namespace generate_data { + +int DoMain(int argc, char **argv) { + // Read arguments from command line. + if (argc < 2) { + std::cout << "Error: Not enough input arguments!\n\n"; + std::cout << "Usage: generate_data CONFIG_FILE\n\n"; + std::cout << "Generate data using parameters from CONFIG_FILE (*.cfg).\n\n"; + return (-1); + } + + // Seed the random number generator. + std::srand(std::time(0)); + + // Read path to config file. + std::string config_filename = argv[1]; + + // Create training data. + DataGenerator generator(config_filename); + + generator.generateData(); + + // std::vector idx; + // for (int i = 0; i < 120; i++) { + // idx.push_back(i * 3); + // } + // idx.push_back(1); + // idx.push_back(5); + // idx.push_back(15); + // idx.push_back(25); + // idx.push_back(31); + // Cloud cloud = generator.createMultiViewCloud("blue_clover_baby_toy", idx); + // int camera = 1; + // int reference_camera = 5; + // util::Cloud cloud = generator.createMultiViewCloud( + // "pop_tarts_strawberry", camera, idx, reference_camera); + // pcl::io::savePCDFileASCII("test_pcd.pcd", *cloud.getCloudOriginal()); + + // cv::Ptr h5io = cv::hdf::open("test.hdf5"); + // std::string IMAGE_DS_NAME = "images"; + // if (!h5io->hlexists(IMAGE_DS_NAME)) + // { + // int n_dims = 3; + // int dsdims[n_dims] = { 505000, 60, 60 }; + // int chunks[n_dims] = { 10000, 60, 60 }; + // cv::Mat images(n_dims, dsdims, CV_8UC(15), cv::Scalar(0.0)); + // + // printf("Creating dataset ...\n"); + // h5io->dscreate(n_dims, dsdims, CV_8UC(15), IMAGE_DS_NAME, 9, chunks); + // printf("Writing dataset ...\n"); + // h5io->dswrite(images, IMAGE_DS_NAME); + // } + // h5io->close(); + + return 0; +} + +} // namespace generate_data +} // namespace apps +} // namespace gpd + +int main(int argc, char *argv[]) { + return gpd::apps::generate_data::DoMain(argc, argv); +} diff --git a/src/gpd/caffe_classifier.cpp b/src/gpd/caffe_classifier.cpp deleted file mode 100644 index 5241aee..0000000 --- a/src/gpd/caffe_classifier.cpp +++ /dev/null @@ -1,81 +0,0 @@ -#include "../../include/gpd/caffe_classifier.h" - - -CaffeClassifier::CaffeClassifier(const std::string& model_file, const std::string& weights_file, Classifier::Device device) -{ - // Initialize Caffe. - switch (device) - { - case Classifier::Device::eGPU: - caffe::Caffe::set_mode(caffe::Caffe::GPU); - break; - default: - caffe::Caffe::set_mode(caffe::Caffe::CPU); - } - - // Load pretrained network. - net_.reset(new caffe::Net(model_file, caffe::TEST)); - net_->CopyTrainedLayersFrom(weights_file); - - input_layer_ = boost::static_pointer_cast < caffe::MemoryDataLayer < float > >(net_->layer_by_name("data")); -} - - -std::vector CaffeClassifier::classifyImages(const std::vector& image_list) -{ - int batch_size = input_layer_->batch_size(); - int num_iterations = (int) ceil(image_list.size() / (double) batch_size); - float loss = 0.0; - std::cout << "# images: " << image_list.size() << ", # iterations: " << num_iterations << ", batch size: " - << batch_size << "\n"; - - std::vector predictions; - - // Process the images in batches. - for (int i = 0; i < num_iterations; i++) - { - std::vector::const_iterator end_it; - std::vector sub_image_list; - - if (i < num_iterations - 1) - { - end_it = image_list.begin() + (i + 1) * batch_size; - sub_image_list.assign(image_list.begin() + i * batch_size, end_it); - } - // Fill the batch with empty images to match the required batch size. - else - { - end_it = image_list.end(); - sub_image_list.assign(image_list.begin() + i * batch_size, end_it); - std::cout << "Adding " << batch_size - sub_image_list.size() << " empty images to batch to match batch size.\n"; - - for (int t = sub_image_list.size(); t < batch_size; t++) - { - cv::Mat empty_mat(input_layer_->height(), input_layer_->width(), CV_8UC(input_layer_->channels()), cv::Scalar(0.0)); - sub_image_list.push_back(empty_mat); - } - } - - std::vector label_list; - label_list.resize(sub_image_list.size()); - - for (int j = 0; j < label_list.size(); j++) - { - label_list[j] = 0; - } - - // Classify the batch. - input_layer_->AddMatVector(sub_image_list, label_list); - std::vector*> results = net_->Forward(&loss); - std::vector out(results[0]->cpu_data(), results[0]->cpu_data() + results[0]->count()); -// std::cout << "#results: " << results.size() << ", " << results[0]->count() << "\n"; - - for (int l = 0; l < results[0]->count() / results[0]-> channels(); l++) - { - predictions.push_back(out[2 * l + 1] - out[2 * l]); -// std::cout << "positive score: " << out[2 * l + 1] << ", negative score: " << out[2 * l] << "\n"; - } - } - - return predictions; -} diff --git a/src/gpd/candidate/antipodal.cpp b/src/gpd/candidate/antipodal.cpp new file mode 100644 index 0000000..254462f --- /dev/null +++ b/src/gpd/candidate/antipodal.cpp @@ -0,0 +1,162 @@ +#include + +namespace gpd { +namespace candidate { + +const int Antipodal::NO_GRASP = 0; // normals point not toward any finger +const int Antipodal::HALF_GRASP = 1; // normals point towards one finger +const int Antipodal::FULL_GRASP = 2; // normals point towards both fingers + +int Antipodal::evaluateGrasp(const util::PointList &point_list, + double extremal_thresh, int lateral_axis, + int forward_axis, int vertical_axis) const { + int result = NO_GRASP; + + const Eigen::Matrix3Xd &pts = point_list.getPoints(); + const Eigen::Matrix3Xd &normals = point_list.getNormals(); + + // Select points that are extremal and have their surface normal within the + // friction cone of the closing direction. + Eigen::Vector3d l, r; + if (lateral_axis == 0) { + l << -1.0, 0.0, 0.0; + r << 1.0, 0.0, 0.0; + } else if (lateral_axis == 1) { + l << 0.0, -1.0, 0.0; + r << 0.0, 1.0, 0.0; + } + double cos_friction_coeff_ = cos(friction_coeff_ * M_PI / 180.0); + double min_x = pts.row(lateral_axis).minCoeff() + extremal_thresh; + double max_x = pts.row(lateral_axis).maxCoeff() - extremal_thresh; + std::vector left_idx_viable, right_idx_viable; + + for (int i = 0; i < pts.cols(); i++) { + bool is_within_left_close = + (l.transpose() * normals.col(i)) > cos_friction_coeff_; + bool is_within_right_close = + (r.transpose() * normals.col(i)) > cos_friction_coeff_; + bool is_left_extremal = pts(lateral_axis, i) < min_x; + bool is_right_extremal = pts(lateral_axis, i) > max_x; + + if (is_within_left_close && is_left_extremal) left_idx_viable.push_back(i); + if (is_within_right_close && is_right_extremal) + right_idx_viable.push_back(i); + } + + if (left_idx_viable.size() > 0 || right_idx_viable.size() > 0) + result = HALF_GRASP; + + if (left_idx_viable.size() > 0 && right_idx_viable.size() > 0) { + Eigen::Matrix3Xd left_pts_viable(3, left_idx_viable.size()), + right_pts_viable(3, right_idx_viable.size()); + for (int i = 0; i < left_idx_viable.size(); i++) { + left_pts_viable.col(i) = pts.col(left_idx_viable[i]); + } + for (int i = 0; i < right_idx_viable.size(); i++) { + right_pts_viable.col(i) = pts.col(right_idx_viable[i]); + } + + double top_viable_y = + std::min(left_pts_viable.row(forward_axis).maxCoeff(), + right_pts_viable.row(forward_axis).maxCoeff()); + double bottom_viable_y = + std::max(left_pts_viable.row(forward_axis).minCoeff(), + right_pts_viable.row(forward_axis).minCoeff()); + + double top_viable_z = + std::min(left_pts_viable.row(vertical_axis).maxCoeff(), + right_pts_viable.row(vertical_axis).maxCoeff()); + double bottom_viable_z = + std::max(left_pts_viable.row(vertical_axis).minCoeff(), + right_pts_viable.row(vertical_axis).minCoeff()); + + int num_viable_left = 0; + for (int i = 0; i < left_pts_viable.cols(); i++) { + double y = left_pts_viable(forward_axis, i); + double z = left_pts_viable(vertical_axis, i); + if (y >= bottom_viable_y && y <= top_viable_y && z >= bottom_viable_z && + z <= top_viable_z) + num_viable_left++; + } + int num_viable_right = 0; + for (int i = 0; i < right_pts_viable.cols(); i++) { + double y = right_pts_viable(forward_axis, i); + double z = right_pts_viable(vertical_axis, i); + if (y >= bottom_viable_y && y <= top_viable_y && z >= bottom_viable_z && + z <= top_viable_z) + num_viable_right++; + } + + if (num_viable_left >= min_viable_ && num_viable_right >= min_viable_) { + result = FULL_GRASP; + } + } + + return result; +} + +int Antipodal::evaluateGrasp(const Eigen::Matrix3Xd &normals, + double thresh_half, double thresh_full) const { + int num_thresh = 6; + int grasp = 0; + double cos_thresh = cos(thresh_half * M_PI / 180.0); + int numl = 0; + int numr = 0; + Eigen::Vector3d l, r; + l << -1, 0, 0; + r << 1, 0, 0; + bool is_half_grasp = false; + bool is_full_grasp = false; + + // check whether this is a half grasp + for (int i = 0; i < normals.cols(); i++) { + if (l.dot(normals.col(i)) > cos_thresh) { + numl++; + if (numl > num_thresh) { + is_half_grasp = true; + break; + } + } + + if (r.dot(normals.col(i)) > cos_thresh) { + numr++; + if (numr > num_thresh) { + is_half_grasp = true; + break; + } + } + } + + // check whether this is a full grasp + cos_thresh = cos(thresh_full * M_PI / 180.0); + numl = 0; + numr = 0; + + for (int i = 0; i < normals.cols(); i++) { + if (l.dot(normals.col(i)) > cos_thresh) { + numl++; + if (numl > num_thresh && numr > num_thresh) { + is_full_grasp = true; + break; + } + } + + if (r.dot(normals.col(i)) > cos_thresh) { + numr++; + if (numl > num_thresh && numr > num_thresh) { + is_full_grasp = true; + break; + } + } + } + + if (is_full_grasp) + return FULL_GRASP; + else if (is_half_grasp) + return HALF_GRASP; + + return NO_GRASP; +} + +} // namespace candidate +} // namespace gpd diff --git a/src/gpd/candidate/candidates_generator.cpp b/src/gpd/candidate/candidates_generator.cpp new file mode 100644 index 0000000..8d087f2 --- /dev/null +++ b/src/gpd/candidate/candidates_generator.cpp @@ -0,0 +1,77 @@ +#include + +namespace gpd { +namespace candidate { + +CandidatesGenerator::CandidatesGenerator( + const Parameters ¶ms, const HandSearch::Parameters &hand_search_params) + : params_(params) { + Eigen::initParallel(); + + hand_search_ = std::make_unique(hand_search_params); +} + +void CandidatesGenerator::preprocessPointCloud(util::Cloud &cloud) { + printf("Processing cloud with %zu points.\n", + cloud.getCloudOriginal()->size()); + + cloud.removeNans(); + + cloud.filterWorkspace(params_.workspace_); + + if (params_.voxelize_) { + cloud.voxelizeCloud(params_.voxel_size_); + } + + cloud.calculateNormals(params_.num_threads_, params_.normals_radius_); + + if (params_.refine_normals_k_ > 0) { + cloud.refineNormals(params_.refine_normals_k_); + } + + if (params_.sample_above_plane_) { + cloud.sampleAbovePlane(); + } + + cloud.subsample(params_.num_samples_); +} + +std::vector> CandidatesGenerator::generateGraspCandidates( + const util::Cloud &cloud_cam) { + // Find sets of grasp candidates. + std::vector> hand_set_list = + hand_search_->searchHands(cloud_cam); + printf("Evaluated %d hand sets with %d potential hand poses.\n", + (int)hand_set_list.size(), + (int)(hand_set_list.size() * hand_set_list[0]->getHands().size())); + + // Extract the grasp candidates. + std::vector> candidates; + for (int i = 0; i < hand_set_list.size(); i++) { + for (int j = 0; j < hand_set_list[i]->getHands().size(); j++) { + if (hand_set_list[i]->getIsValid()(j)) { + candidates.push_back(std::move(hand_set_list[i]->getHands()[j])); + } + } + } + std::cout << "Generated " << candidates.size() << " grasp candidates.\n"; + + return candidates; +} + +std::vector> +CandidatesGenerator::generateGraspCandidateSets(const util::Cloud &cloud_cam) { + // Find sets of grasp candidates. + std::vector> hand_set_list = + hand_search_->searchHands(cloud_cam); + + return hand_set_list; +} + +std::vector CandidatesGenerator::reevaluateHypotheses( + const util::Cloud &cloud, std::vector> &grasps) { + return hand_search_->reevaluateHypotheses(cloud, grasps); +} + +} // namespace candidate +} // namespace gpd diff --git a/src/gpd/candidate/finger_hand.cpp b/src/gpd/candidate/finger_hand.cpp new file mode 100644 index 0000000..88d3929 --- /dev/null +++ b/src/gpd/candidate/finger_hand.cpp @@ -0,0 +1,187 @@ +#include + +namespace gpd { +namespace candidate { + +FingerHand::FingerHand(double finger_width, double hand_outer_diameter, + double hand_depth, int num_placements) + : finger_width_(finger_width), + hand_depth_(hand_depth), + lateral_axis_(-1), + forward_axis_(-1) { + // Calculate the finger spacing. + Eigen::VectorXd fs_half; + fs_half.setLinSpaced(num_placements, 0.0, hand_outer_diameter - finger_width); + finger_spacing_.resize(2 * num_placements); + finger_spacing_ + << (fs_half.array() - hand_outer_diameter + finger_width_).matrix(), + fs_half; + + fingers_ = Eigen::Array::Constant( + 1, 2 * num_placements, false); + hand_ = + Eigen::Array::Constant(1, num_placements, false); +} + +void FingerHand::evaluateFingers(const Eigen::Matrix3Xd &points, double bite, + int idx) { + // Calculate top and bottom of the hand (top = fingertip, bottom = base). + top_ = bite; + bottom_ = bite - hand_depth_; + + center_ = 0.0; + + fingers_.setConstant(false); + + // Crop points at bite. + std::vector cropped_indices; + for (int i = 0; i < points.cols(); i++) { + if (points(forward_axis_, i) < bite) { + // Check that the hand would be able to extend by onto the object + // without causing the back of the hand to + // collide with . + if (points(forward_axis_, i) < bottom_) { + return; + } + + cropped_indices.push_back(i); + } + } + + // Check that there is at least one point in between the fingers. + if (cropped_indices.size() == 0) { + return; + } + + // Identify free gaps (finger placements that do not collide with the point + // cloud). + if (idx == -1) { + for (int i = 0; i < fingers_.size(); i++) { + if (isGapFree(points, cropped_indices, i)) { + fingers_(i) = true; + } + } + } else { + if (isGapFree(points, cropped_indices, idx)) { + fingers_(idx) = true; + } + + if (isGapFree(points, cropped_indices, fingers_.size() / 2 + idx)) { + fingers_(fingers_.size() / 2 + idx) = true; + } + } +} + +void FingerHand::evaluateHand() { + const int n = fingers_.size() / 2; + + for (int i = 0; i < n; i++) { + hand_(i) = (fingers_(i) == true && fingers_(n + i) == true); + } +} + +void FingerHand::evaluateHand(int idx) { + const int n = fingers_.size() / 2; + hand_.setConstant(false); + hand_(idx) = (fingers_(idx) == true && fingers_(n + idx) == true); +} + +int FingerHand::chooseMiddleHand() { + std::vector hand_idx; + + for (int i = 0; i < hand_.cols(); i++) { + if (hand_(i) == true) { + hand_idx.push_back(i); + } + } + + if (hand_idx.size() == 0) { + return -1; + } + + int idx = hand_idx[ceil(hand_idx.size() / 2.0) - 1]; + + return idx; +} + +int FingerHand::deepenHand(const Eigen::Matrix3Xd &points, double min_depth, + double max_depth) { + // Choose middle hand. + int hand_eroded_idx = chooseMiddleHand(); // middle index + int opposite_idx = + fingers_.size() / 2 + hand_eroded_idx; // opposite finger index + + // Attempt to deepen hand (move as far onto the object as possible without + // collision). + const double DEEPEN_STEP_SIZE = 0.005; + FingerHand new_hand = *this; + FingerHand last_new_hand = new_hand; + + for (double depth = min_depth + DEEPEN_STEP_SIZE; depth <= max_depth; + depth += DEEPEN_STEP_SIZE) { + // Check if the new hand placement is feasible + new_hand.evaluateFingers(points, depth, hand_eroded_idx); + if (!new_hand.fingers_(hand_eroded_idx) || + !new_hand.fingers_(opposite_idx)) { + break; + } + + hand_(hand_eroded_idx) = true; + last_new_hand = new_hand; + } + + // Recover the deepest hand. + *this = last_new_hand; + hand_.setConstant(false); + hand_(hand_eroded_idx) = true; + + return hand_eroded_idx; +} + +std::vector FingerHand::computePointsInClosingRegion( + const Eigen::Matrix3Xd &points, int idx) { + // Find feasible finger placement. + if (idx == -1) { + for (int i = 0; i < hand_.cols(); i++) { + if (hand_(i) == true) { + idx = i; + break; + } + } + } + + // Calculate the lateral parameters of the hand closing region for this finger + // placement. + left_ = finger_spacing_(idx) + finger_width_; + right_ = finger_spacing_(hand_.cols() + idx); + center_ = 0.5 * (left_ + right_); + surface_ = points.row(lateral_axis_).minCoeff(); + + // Find points inside the hand closing region defined by , , + // and . + std::vector indices; + for (int i = 0; i < points.cols(); i++) { + if (points(forward_axis_, i) > bottom_ && points(forward_axis_, i) < top_ && + points(lateral_axis_, i) > left_ && points(lateral_axis_, i) < right_) { + indices.push_back(i); + } + } + + return indices; +} + +bool FingerHand::isGapFree(const Eigen::Matrix3Xd &points, + const std::vector &indices, int idx) { + for (int i = 0; i < indices.size(); i++) { + const double &x = points(lateral_axis_, indices[i]); + + if (x > finger_spacing_(idx) && x < finger_spacing_(idx) + finger_width_) { + return false; + } + } + + return true; +} + +} // namespace candidate +} // namespace gpd diff --git a/src/gpd/candidate/frame_estimator.cpp b/src/gpd/candidate/frame_estimator.cpp new file mode 100644 index 0000000..82ca777 --- /dev/null +++ b/src/gpd/candidate/frame_estimator.cpp @@ -0,0 +1,98 @@ +#include + +namespace gpd { +namespace candidate { + +std::vector FrameEstimator::calculateLocalFrames( + const util::Cloud &cloud_cam, const std::vector &indices, + double radius, const pcl::KdTreeFLANN &kdtree) const { + double t1 = omp_get_wtime(); + std::vector> frames; + frames.resize(indices.size()); + +#ifdef _OPENMP // parallelization using OpenMP +#pragma omp parallel for num_threads(num_threads_) +#endif + for (int i = 0; i < indices.size(); i++) { + const pcl::PointXYZRGBA &sample = + cloud_cam.getCloudProcessed()->points[indices[i]]; + frames[i] = + calculateFrame(cloud_cam.getNormals(), + sample.getVector3fMap().cast(), radius, kdtree); + } + + std::vector frames_out; + for (int i = 0; i < frames.size(); i++) { + if (frames[i]) { + frames_out.push_back(*frames[i].get()); + } + } + + double t2 = omp_get_wtime(); + printf("Estimated %zu frames in %3.4fs.\n", frames_out.size(), t2 - t1); + + return frames_out; +} + +std::vector FrameEstimator::calculateLocalFrames( + const util::Cloud &cloud_cam, const Eigen::Matrix3Xd &samples, + double radius, const pcl::KdTreeFLANN &kdtree) const { + double t1 = omp_get_wtime(); + std::vector> frames; + frames.resize(samples.cols()); + +#ifdef _OPENMP // parallelization using OpenMP +#pragma omp parallel for num_threads(num_threads_) +#endif + for (int i = 0; i < samples.cols(); i++) { + frames[i] = + calculateFrame(cloud_cam.getNormals(), samples.col(i), radius, kdtree); + } + + // Only keep frames that are not null. + std::vector frames_out; + for (int i = 0; i < frames.size(); i++) { + if (frames[i]) { + frames_out.push_back(*frames[i].get()); + } + } + + double t2 = omp_get_wtime(); + printf("Estimated %zu frames in %3.4fs.\n", frames_out.size(), t2 - t1); + + return frames_out; +} + +std::unique_ptr FrameEstimator::calculateFrame( + const Eigen::Matrix3Xd &normals, const Eigen::Vector3d &sample, + double radius, const pcl::KdTreeFLANN &kdtree) const { + std::unique_ptr frame = nullptr; + std::vector nn_indices; + std::vector nn_dists; + pcl::PointXYZRGBA sample_pcl = eigenVectorToPcl(sample); + + if (kdtree.radiusSearch(sample_pcl, radius, nn_indices, nn_dists) > 0) { + Eigen::Matrix3Xd nn_normals(3, nn_indices.size()); + + for (int i = 0; i < nn_normals.cols(); i++) { + nn_normals.col(i) = normals.col(nn_indices[i]); + } + + frame = std::make_unique(sample); + frame->findAverageNormalAxis(nn_normals); + } + + return frame; +} + +pcl::PointXYZRGBA FrameEstimator::eigenVectorToPcl( + const Eigen::Vector3d &v) const { + pcl::PointXYZRGBA p; + p.x = v(0); + p.y = v(1); + p.z = v(2); + return p; +} + +} // namespace candidate +} // namespace gpd diff --git a/src/gpd/candidate/hand.cpp b/src/gpd/candidate/hand.cpp new file mode 100644 index 0000000..d979b56 --- /dev/null +++ b/src/gpd/candidate/hand.cpp @@ -0,0 +1,89 @@ +#include + +namespace gpd { +namespace candidate { + +Hand::Hand() : grasp_width_(0.0), label_(0.0, false, false) {} + +Hand::Hand(const Eigen::Vector3d &sample, const Eigen::Matrix3d &frame, + const FingerHand &finger_hand, double grasp_width) + : sample_(sample), grasp_width_(grasp_width), label_(0.0, false, false) { + orientation_ = frame; + + construct(finger_hand); +} + +Hand::Hand(const Eigen::Vector3d &sample, const Eigen::Matrix3d &frame, + const FingerHand &finger_hand) + : sample_(sample), grasp_width_(0.0), label_(0.0, false, false) { + orientation_ = frame; + + construct(finger_hand); +} + +void Hand::construct(const FingerHand &finger_hand) { + closing_box_.top_ = finger_hand.getTop(); + closing_box_.bottom_ = finger_hand.getBottom(); + closing_box_.center_ = finger_hand.getCenter(); + + calculateGraspPositions(finger_hand); + + // Determine the index of the finger placement that resulted in this grasp. + const Eigen::Array &indices = finger_hand.getHand(); + for (int i = 0; i < indices.size(); i++) { + if (indices[i] == true) { + finger_placement_index_ = i; + break; + } + } +} + +void Hand::calculateGraspPositions(const FingerHand &finger_hand) { + Eigen::Vector3d pos_bottom; + pos_bottom << getBottom(), finger_hand.getCenter(), 0.0; + position_ = getFrame() * pos_bottom + sample_; +} + +void Hand::writeHandsToFile(const std::string &filename, + const std::vector &hands) const { + std::ofstream myfile; + myfile.open(filename.c_str()); + + for (int i = 0; i < hands.size(); i++) { + std::cout << "Hand " << i << std::endl; + print(); + + myfile << vectorToString(hands[i].getPosition()) + << vectorToString(hands[i].getAxis()) + << vectorToString(hands[i].getApproach()) + << vectorToString(hands[i].getBinormal()) + << std::to_string(hands[i].getGraspWidth()) << "\n"; + } + + myfile.close(); +} + +void Hand::print() const { + std::cout << "position: " << getPosition().transpose() << std::endl; + std::cout << "approach: " << getApproach().transpose() << std::endl; + std::cout << "binormal: " << getBinormal().transpose() << std::endl; + std::cout << "axis: " << getAxis().transpose() << std::endl; + std::cout << "score: " << getScore() << std::endl; + std::cout << "full-antipodal: " << isFullAntipodal() << std::endl; + std::cout << "half-antipodal: " << isHalfAntipodal() << std::endl; + std::cout << "closing box:\n"; + std::cout << " bottom: " << getBottom() << std::endl; + std::cout << " top: " << getTop() << std::endl; + std::cout << " center: " << getTop() << std::endl; +} + +std::string Hand::vectorToString(const Eigen::VectorXd &v) const { + std::string s = ""; + for (int i = 0; i < v.rows(); i++) { + s += std::to_string(v(i)) + ","; + } + return s; +} + +} // namespace candidate +} // namespace gpd diff --git a/src/gpd/candidate/hand_geometry.cpp b/src/gpd/candidate/hand_geometry.cpp new file mode 100644 index 0000000..00c8373 --- /dev/null +++ b/src/gpd/candidate/hand_geometry.cpp @@ -0,0 +1,46 @@ +#include + +namespace gpd { +namespace candidate { + +HandGeometry::HandGeometry() + : finger_width_(0.0), + outer_diameter_(0.0), + depth_(0.0), + height_(0.0), + init_bite_(0.0) {} + +HandGeometry::HandGeometry(double finger_width, double outer_diameter, + double hand_depth, double hand_height, + double init_bite) + : finger_width_(finger_width), + outer_diameter_(outer_diameter), + depth_(hand_depth), + height_(hand_height), + init_bite_(init_bite) {} + +HandGeometry::HandGeometry(const std::string &filepath) { + util::ConfigFile config_file(filepath); + config_file.ExtractKeys(); + finger_width_ = config_file.getValueOfKey("finger_width", 0.01); + outer_diameter_ = + config_file.getValueOfKey("hand_outer_diameter", 0.12); + depth_ = config_file.getValueOfKey("hand_depth", 0.06); + height_ = config_file.getValueOfKey("hand_height", 0.02); + init_bite_ = config_file.getValueOfKey("init_bite", 0.01); +} + +std::ostream &operator<<(std::ostream &stream, + const HandGeometry &hand_geometry) { + stream << "============ HAND GEOMETRY ======================\n"; + stream << "finger_width: " << hand_geometry.finger_width_ << "\n"; + stream << "hand_outer_diameter: " << hand_geometry.outer_diameter_ << "\n"; + stream << "hand_depth: " << hand_geometry.depth_ << "\n"; + stream << "hand_height: " << hand_geometry.height_ << "\n"; + stream << "init_bite: " << hand_geometry.init_bite_ << "\n"; + stream << "=================================================\n"; + return stream; +} + +} // namespace candidate +} // namespace gpd diff --git a/src/gpd/candidate/hand_search.cpp b/src/gpd/candidate/hand_search.cpp new file mode 100644 index 0000000..b848636 --- /dev/null +++ b/src/gpd/candidate/hand_search.cpp @@ -0,0 +1,231 @@ +#include + +namespace gpd { +namespace candidate { + +const int HandSearch::ROTATION_AXIS_NORMAL = 0; +const int HandSearch::ROTATION_AXIS_BINORMAL = 1; +const int HandSearch::ROTATION_AXIS_CURVATURE_AXIS = 2; + +HandSearch::HandSearch(Parameters params) + : params_(params), plots_local_axes_(false) { + // Calculate radius for nearest neighbor search. + const HandGeometry &hand_geom = params_.hand_geometry_; + Eigen::Vector3d hand_dims; + hand_dims << hand_geom.outer_diameter_ - hand_geom.finger_width_, + hand_geom.depth_, hand_geom.height_ / 2.0; + nn_radius_ = hand_dims.maxCoeff(); + antipodal_ = + std::make_unique(params.friction_coeff_, params.min_viable_); + plot_ = std::make_unique(params_.hand_axes_.size(), + params_.num_orientations_); +} + +std::vector> HandSearch::searchHands( + const util::Cloud &cloud_cam) const { + double t0_total = omp_get_wtime(); + + // Create KdTree for neighborhood search. + const PointCloudRGB::Ptr &cloud = cloud_cam.getCloudProcessed(); + pcl::KdTreeFLANN kdtree; + kdtree.setInputCloud(cloud); + + // 1. Estimate local reference frames. + std::cout << "Estimating local reference frames ...\n"; + std::vector frames; + FrameEstimator frame_estimator(params_.num_threads_); + if (cloud_cam.getSamples().cols() > 0) { // use samples + frames = frame_estimator.calculateLocalFrames( + cloud_cam, cloud_cam.getSamples(), params_.nn_radius_frames_, kdtree); + } else if (cloud_cam.getSampleIndices().size() > 0) { // use indices + frames = frame_estimator.calculateLocalFrames( + cloud_cam, cloud_cam.getSampleIndices(), params_.nn_radius_frames_, + kdtree); + } else { + std::cout << "Error: No samples or no indices!\n"; + std::vector> hand_set_list(0); + // hand_set_list.resize(0); + return hand_set_list; + } + + if (plots_local_axes_) { + plot_->plotLocalAxes(frames, cloud_cam.getCloudOriginal()); + } + + // 2. Evaluate possible hand placements. + std::cout << "Finding hand poses ...\n"; + std::vector> hand_set_list = + evalHands(cloud_cam, frames, kdtree); + + const double t2 = omp_get_wtime(); + std::cout << "====> HAND SEARCH TIME: " << t2 - t0_total << std::endl; + + return hand_set_list; +} + +std::vector HandSearch::reevaluateHypotheses( + const util::Cloud &cloud_cam, + std::vector> &grasps, + bool plot_samples) const { + // Create KdTree for neighborhood search. + const Eigen::MatrixXi &camera_source = cloud_cam.getCameraSource(); + const Eigen::Matrix3Xd &cloud_normals = cloud_cam.getNormals(); + const PointCloudRGB::Ptr &cloud = cloud_cam.getCloudProcessed(); + pcl::KdTreeFLANN kdtree; + kdtree.setInputCloud(cloud); + + if (plot_samples) { + Eigen::Matrix3Xd samples(3, grasps.size()); + for (int i = 0; i < grasps.size(); i++) { + samples.col(i) = grasps[i]->getSample(); + } + + plot_->plotSamples(samples, cloud); + } + + std::vector nn_indices; + std::vector nn_dists; + Eigen::Matrix3Xd points = + cloud->getMatrixXfMap().block(0, 0, 3, cloud->size()).cast(); + util::PointList point_list(points, cloud_normals, camera_source, + cloud_cam.getViewPoints()); + util::PointList nn_points; + std::vector labels(grasps.size()); + +#ifdef _OPENMP +#pragma omp parallel for private(nn_indices, nn_dists, nn_points) \ + num_threads(params_.num_threads_) +#endif + for (int i = 0; i < grasps.size(); i++) { + labels[i] = 0; + grasps[i]->setHalfAntipodal(false); + grasps[i]->setFullAntipodal(false); + const Eigen::Vector3d &sample = grasps[i]->getSample(); + pcl::PointXYZRGBA sample_pcl = eigenVectorToPcl(sample); + + if (kdtree.radiusSearch(sample_pcl, nn_radius_, nn_indices, nn_dists) > 0) { + nn_points = point_list.slice(nn_indices); + util::PointList nn_points_frame; + FingerHand finger_hand(params_.hand_geometry_.finger_width_, + params_.hand_geometry_.outer_diameter_, + params_.hand_geometry_.depth_, + params_.num_finger_placements_); + + // Set the lateral and forward axes of the robot hand frame (closing + // direction and grasp approach direction). + finger_hand.setForwardAxis(0); + finger_hand.setLateralAxis(1); + + // Check for collisions and if the hand contains at least one point. + if (reevaluateHypothesis(nn_points, *grasps[i], finger_hand, + nn_points_frame)) { + int label = labelHypothesis(nn_points_frame, finger_hand); + if (label == Antipodal::FULL_GRASP) { + labels[i] = 1; + grasps[i]->setFullAntipodal(true); + } else if (label == Antipodal::HALF_GRASP) { + grasps[i]->setHalfAntipodal(true); + } + } + } + } + + return labels; +} + +pcl::PointXYZRGBA HandSearch::eigenVectorToPcl(const Eigen::Vector3d &v) const { + pcl::PointXYZRGBA p; + p.x = v(0); + p.y = v(1); + p.z = v(2); + return p; +} + +std::vector> HandSearch::evalHands( + const util::Cloud &cloud_cam, + const std::vector &frames, + const pcl::KdTreeFLANN &kdtree) const { + double t1 = omp_get_wtime(); + + // possible angles used for hand orientations + const Eigen::VectorXd angles_space = Eigen::VectorXd::LinSpaced( + params_.num_orientations_ + 1, -1.0 * M_PI / 2.0, M_PI / 2.0); + + // necessary b/c assignment in Eigen does not change vector size + const Eigen::VectorXd angles = angles_space.head(params_.num_orientations_); + + std::vector nn_indices; + std::vector nn_dists; + const PointCloudRGB::Ptr &cloud = cloud_cam.getCloudProcessed(); + const Eigen::Matrix3Xd points = + cloud->getMatrixXfMap().block(0, 0, 3, cloud->size()).cast(); + std::vector> hand_set_list(frames.size()); + const util::PointList point_list(points, cloud_cam.getNormals(), + cloud_cam.getCameraSource(), + cloud_cam.getViewPoints()); + util::PointList nn_points; + +#ifdef _OPENMP // parallelization using OpenMP +#pragma omp parallel for private(nn_indices, nn_dists, nn_points) \ + num_threads(params_.num_threads_) +#endif + for (std::size_t i = 0; i < frames.size(); i++) { + pcl::PointXYZRGBA sample = eigenVectorToPcl(frames[i].getSample()); + hand_set_list[i] = std::make_unique( + params_.hand_geometry_, angles, params_.hand_axes_, + params_.num_finger_placements_, params_.deepen_hand_, *antipodal_); + + if (kdtree.radiusSearch(sample, nn_radius_, nn_indices, nn_dists) > 0) { + nn_points = point_list.slice(nn_indices); + hand_set_list[i]->evalHandSet(nn_points, frames[i]); + } + } + + printf("Found %d hand sets in %3.2fs\n", (int)hand_set_list.size(), + omp_get_wtime() - t1); + + return hand_set_list; +} + +bool HandSearch::reevaluateHypothesis( + const util::PointList &point_list, const candidate::Hand &hand, + FingerHand &finger_hand, util::PointList &point_list_cropped) const { + // Transform points into hand frame and crop them on . + util::PointList point_list_frame = point_list.transformToHandFrame( + hand.getSample(), hand.getFrame().transpose()); + point_list_cropped = + point_list_frame.cropByHandHeight(params_.hand_geometry_.height_); + + // Check that the finger placement is possible. + finger_hand.evaluateFingers(point_list_cropped.getPoints(), hand.getTop(), + hand.getFingerPlacementIndex()); + finger_hand.evaluateHand(hand.getFingerPlacementIndex()); + + if (finger_hand.getHand().any()) { + return true; + } + + return false; +} + +int HandSearch::labelHypothesis(const util::PointList &point_list, + FingerHand &finger_hand) const { + std::vector indices_learning = + finger_hand.computePointsInClosingRegion(point_list.getPoints()); + if (indices_learning.size() == 0) { + return Antipodal::NO_GRASP; + } + + // extract data for classification + util::PointList point_list_learning = point_list.slice(indices_learning); + + // evaluate if the grasp is antipodal + int antipodal_result = antipodal_->evaluateGrasp( + point_list_learning, 0.003, finger_hand.getLateralAxis(), + finger_hand.getForwardAxis(), 2); + + return antipodal_result; +} + +} // namespace candidate +} // namespace gpd diff --git a/src/gpd/candidate/hand_set.cpp b/src/gpd/candidate/hand_set.cpp new file mode 100644 index 0000000..17a272a --- /dev/null +++ b/src/gpd/candidate/hand_set.cpp @@ -0,0 +1,286 @@ +#include + +#include + +namespace gpd { +namespace candidate { + +const Eigen::Vector3d HandSet::AXES[3] = {Eigen::Vector3d::UnitX(), + Eigen::Vector3d::UnitY(), + Eigen::Vector3d::UnitZ()}; + +const bool HandSet::MEASURE_TIME = false; + +int HandSet::seed_ = 0; + +HandSet::HandSet(const HandGeometry &hand_geometry, + const Eigen::VectorXd &angles, + const std::vector &hand_axes, int num_finger_placements, + bool deepen_hand, Antipodal &antipodal) + : hand_geometry_(hand_geometry), + angles_(angles), + hand_axes_(hand_axes), + num_finger_placements_(num_finger_placements), + deepen_hand_(deepen_hand), + antipodal_(antipodal) { + sample_.setZero(); + hands_.resize(0); + is_valid_.resize(0); +} + +void HandSet::evalHandSet(const util::PointList &point_list, + const LocalFrame &local_frame) { + hands_.resize(hand_axes_.size() * angles_.size()); + is_valid_ = Eigen::Array::Constant( + 1, angles_.size() * hand_axes_.size(), false); + + // Local reference frame + sample_ = local_frame.getSample(); + frame_ << local_frame.getNormal(), local_frame.getBinormal(), + local_frame.getCurvatureAxis(); + + // Iterate over rotation axes. + for (int i = 0; i < hand_axes_.size(); i++) { + int start = i * angles_.size(); + evalHands(point_list, local_frame, hand_axes_[i], start); + } +} + +void HandSet::evalHands(const util::PointList &point_list, + const LocalFrame &local_frame, int axis, int start) { + // Rotate about binormal by 180 degrees to reverses direction of normal. + const Eigen::Matrix3d ROT_BINORMAL = + Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitY()).toRotationMatrix(); + + // This object is used to evaluate the finger placement. + FingerHand finger_hand(hand_geometry_.finger_width_, + hand_geometry_.outer_diameter_, hand_geometry_.depth_, + num_finger_placements_); + + // Set the forward and lateral axis of the robot hand frame (closing direction + // and grasp approach direction). + finger_hand.setForwardAxis(0); + finger_hand.setLateralAxis(1); + + // Evaluate grasp at each hand orientation. + for (int i = 0; i < angles_.rows(); i++) { + // Rotation about by radians. + Eigen::Matrix3d rot = + Eigen::AngleAxisd(angles_(i), AXES[axis]).toRotationMatrix(); + + // Rotate points into this hand orientation. + Eigen::Matrix3d frame_rot; + frame_rot.noalias() = frame_ * ROT_BINORMAL * rot; + util::PointList point_list_frame = point_list.transformToHandFrame( + local_frame.getSample(), frame_rot.transpose()); + + // Crop points on hand height. + util::PointList point_list_cropped = + point_list_frame.cropByHandHeight(hand_geometry_.height_); + + // Evaluate finger placements for this orientation. + finger_hand.evaluateFingers(point_list_cropped.getPoints(), + hand_geometry_.init_bite_); + + // Check that there is at least one feasible 2-finger placement. + finger_hand.evaluateHand(); + + // Create the grasp candidate. + hands_[start + i] = std::make_unique(local_frame.getSample(), + frame_rot, finger_hand, 0.0); + + // Check that there is at least one feasible 2-finger placement. + if (finger_hand.getHand().any()) { + int finger_idx; + if (deepen_hand_) { + // Try to move the hand as deep as possible onto the object. + finger_idx = finger_hand.deepenHand(point_list_cropped.getPoints(), + hand_geometry_.init_bite_, + hand_geometry_.depth_); + } else { + finger_idx = finger_hand.chooseMiddleHand(); + } + // Calculate points in the closing region of the hand. + std::vector indices_closing = + finger_hand.computePointsInClosingRegion( + point_list_cropped.getPoints(), finger_idx); + if (indices_closing.size() == 0) { + continue; + } + + is_valid_[start + i] = true; + modifyCandidate(*hands_[start + i], point_list_cropped, indices_closing, + finger_hand); + } + } +} + +Eigen::Matrix3Xd HandSet::calculateShadow(const util::PointList &point_list, + double shadow_length) const { + // Set voxel size for points that fill occluded region. + const double voxel_grid_size = 0.003; + // Calculate number of points along each shadow vector. + double num_shadow_points = floor(shadow_length / voxel_grid_size); + + const int num_cams = point_list.getCamSource().rows(); + + Eigen::Matrix3Xd shadow; + + // Calculate the set of cameras which see the points. + Eigen::VectorXi camera_set = point_list.getCamSource().rowwise().sum(); + + // Calculate the center point of the point neighborhood. + Eigen::Vector3d center = point_list.getPoints().rowwise().sum(); + center /= (double)point_list.size(); + + // Stores the list of all bins of the voxelized, occluded points. + std::vector shadows; + shadows.resize(num_cams, Vector3iSet(num_shadow_points * 10000)); + + for (int i = 0; i < num_cams; i++) { + if (camera_set(i) >= 1) { + double t0_if = omp_get_wtime(); + + // Calculate the unit vector that points from the camera position to the + // center of the point neighborhood. + Eigen::Vector3d shadow_vec = center - point_list.getViewPoints().col(i); + + // Scale that vector by the shadow length. + shadow_vec = shadow_length * shadow_vec / shadow_vec.norm(); + + // Calculate occluded points for this camera. + calculateShadowForCamera(point_list.getPoints(), shadow_vec, + num_shadow_points, voxel_grid_size, shadows[i]); + } + } + + // Only one camera view point. + if (num_cams == 1) { + // Convert voxels back to points. + shadow = shadowVoxelsToPoints( + std::vector(shadows[0].begin(), shadows[0].end()), + voxel_grid_size); + return shadow; + } + + // Multiple camera view points: find the intersection of all sets of occluded + // points. + double t0_intersection = omp_get_wtime(); + Vector3iSet bins_all = shadows[0]; + + for (int i = 1; i < num_cams; i++) { + // Check that there are points seen by this camera. + if (camera_set(i) >= 1) { + bins_all = intersection(bins_all, shadows[i]); + } + } + if (MEASURE_TIME) { + printf("intersection runtime: %.3fs\n", omp_get_wtime() - t0_intersection); + } + + // Convert voxels back to points. + std::vector voxels(bins_all.begin(), bins_all.end()); + shadow = shadowVoxelsToPoints(voxels, voxel_grid_size); + return shadow; +} + +Eigen::Matrix3Xd HandSet::shadowVoxelsToPoints( + const std::vector &voxels, double voxel_grid_size) const { + // Convert voxels back to points. + double t0_voxels = omp_get_wtime(); + std::random_device rd{}; + std::mt19937 gen{rd()}; + std::normal_distribution distr{0.0, 1.0}; + Eigen::Matrix3Xd shadow(3, voxels.size()); + + for (int i = 0; i < voxels.size(); i++) { + shadow.col(i) = + voxels[i].cast() * voxel_grid_size + + Eigen::Vector3d::Ones() * distr(gen) * voxel_grid_size * 0.3; + } + if (MEASURE_TIME) { + printf("voxels-to-points runtime: %.3fs\n", omp_get_wtime() - t0_voxels); + } + + return shadow; +} + +void HandSet::calculateShadowForCamera(const Eigen::Matrix3Xd &points, + const Eigen::Vector3d &shadow_vec, + int num_shadow_points, + double voxel_grid_size, + Vector3iSet &shadow_set) const { + double t0_set = omp_get_wtime(); + const int n = points.cols() * num_shadow_points; + const double voxel_grid_size_mult = 1.0 / voxel_grid_size; + const double max = 1.0 / 32767.0; + + for (int i = 0; i < n; i++) { + const int pt_idx = i / num_shadow_points; + shadow_set.insert( + ((points.col(pt_idx) + ((double)fastrand() * max) * shadow_vec) * + voxel_grid_size_mult) + .cast()); + } + + if (MEASURE_TIME) { + printf( + "Shadow (1 camera) calculation. Runtime: %.3f, #points: %d, " + "num_shadow_points: %d, #shadow: %d, max #shadow: %d\n", + omp_get_wtime() - t0_set, (int)points.cols(), num_shadow_points, + (int)shadow_set.size(), n); + } +} + +void HandSet::modifyCandidate(Hand &hand, const util::PointList &point_list, + const std::vector &indices, + const FingerHand &finger_hand) const { + // Modify the grasp. + hand.construct(finger_hand); + + // Extract points in hand closing region. + util::PointList point_list_closing = point_list.slice(indices); + + // Calculate grasp width (hand opening width). + double width = point_list_closing.getPoints().row(1).maxCoeff() - + point_list_closing.getPoints().row(1).minCoeff(); + hand.setGraspWidth(width); + + // Evaluate if the grasp is antipodal. + labelHypothesis(point_list_closing, finger_hand, hand); +} + +void HandSet::labelHypothesis(const util::PointList &point_list, + const FingerHand &finger_hand, Hand &hand) const { + int label = + antipodal_.evaluateGrasp(point_list, 0.003, finger_hand.getLateralAxis(), + finger_hand.getForwardAxis(), 2); + hand.setHalfAntipodal(label == Antipodal::HALF_GRASP || + label == Antipodal::FULL_GRASP); + hand.setFullAntipodal(label == Antipodal::FULL_GRASP); +} + +inline int HandSet::fastrand() const { + seed_ = (214013 * seed_ + 2531011); + return (seed_ >> 16) & 0x7FFF; +} + +Vector3iSet HandSet::intersection(const Vector3iSet &set1, + const Vector3iSet &set2) const { + if (set2.size() < set1.size()) { + return intersection(set2, set1); + } + + Vector3iSet set_out(set1.size()); + + for (Vector3iSet::const_iterator it = set1.begin(); it != set1.end(); it++) { + if (set2.find(*it) != set2.end()) { + set_out.insert(*it); + } + } + + return set_out; +} + +} // namespace candidate +} // namespace gpd diff --git a/src/gpd/candidate/local_frame.cpp b/src/gpd/candidate/local_frame.cpp new file mode 100644 index 0000000..5a8d27b --- /dev/null +++ b/src/gpd/candidate/local_frame.cpp @@ -0,0 +1,44 @@ +#include + +namespace gpd { +namespace candidate { + +void LocalFrame::print() { + std::cout << "sample: " << sample_.transpose() << std::endl; + std::cout << "curvature_axis: " << curvature_axis_.transpose() << std::endl; + std::cout << "normal: " << normal_.transpose() << std::endl; + std::cout << "binormal: " << binormal_.transpose() << std::endl; + std::cout << "-----------\n"; +} + +void LocalFrame::findAverageNormalAxis(const Eigen::MatrixXd &normals) { + // 1. Calculate "curvature axis" (corresponds to minor principal curvature + // axis). + Eigen::Matrix3d M = normals * normals.transpose(); + Eigen::SelfAdjointEigenSolver eigen_solver( + M); // M is adjoint (M is equal to its transpose) + Eigen::Vector3d eigen_values = eigen_solver.eigenvalues().real(); + Eigen::Matrix3d eigen_vectors = eigen_solver.eigenvectors().real(); + int min_index; + eigen_values.minCoeff(&min_index); + curvature_axis_ = eigen_vectors.col(min_index); + + // 2. Calculate surface normal. + int max_index; + eigen_values.maxCoeff(&max_index); + normal_ = eigen_vectors.col(max_index); + + // 3. Ensure that the new normal is pointing in the same direction as the + // existing normals. + Eigen::Vector3d avg_normal = normals.rowwise().sum(); + avg_normal /= avg_normal.norm(); + if (avg_normal.transpose() * normal_ < 0) { + normal_ *= -1.0; + } + + // 4. Create binormal (corresponds to major principal curvature axis). + binormal_ = curvature_axis_.cross(normal_); +} + +} // namespace candidate +} // namespace gpd diff --git a/src/gpd/clustering.cpp b/src/gpd/clustering.cpp index ba030fb..15a6bad 100644 --- a/src/gpd/clustering.cpp +++ b/src/gpd/clustering.cpp @@ -1,97 +1,107 @@ -#include "../../include/gpd/clustering.h" +#include +namespace gpd { -std::vector Clustering::findClusters(const std::vector& hand_list, bool remove_inliers) -{ +std::vector> Clustering::findClusters( + const std::vector> &hand_list, + bool remove_inliers) { // const double AXIS_ALIGN_ANGLE_THRESH = 15.0 * M_PI/180.0; - const double AXIS_ALIGN_ANGLE_THRESH = 12.0 * M_PI/180.0; + const double AXIS_ALIGN_ANGLE_THRESH = 12.0 * M_PI / 180.0; const double AXIS_ALIGN_DIST_THRESH = 0.005; // const double MAX_DIST_THRESH = 0.07; const double MAX_DIST_THRESH = 0.05; // const int max_inliers = 50; - std::vector hands_out; + std::vector> hands_out; std::vector has_used; - if (remove_inliers) - { + if (remove_inliers) { has_used.resize(hand_list.size()); - for (int i = 0; i < hand_list.size(); i++) - { + for (int i = 0; i < hand_list.size(); i++) { has_used[i] = false; } } std::vector inliers; - for (int i = 0; i < hand_list.size(); i++) - { + for (int i = 0; i < hand_list.size(); i++) { int num_inliers = 0; Eigen::Vector3d position_delta = Eigen::Vector3d::Zero(); - Eigen::Matrix3d axis_outer_prod = hand_list[i].getAxis() * hand_list[i].getAxis().transpose(); + Eigen::Matrix3d axis_outer_prod = + hand_list[i]->getAxis() * hand_list[i]->getAxis().transpose(); inliers.resize(0); double mean = 0.0; double standard_deviation = 0.0; - std::vector scores(0); - for (int j = 0; j < hand_list.size(); j++) - { - if (i == j || (remove_inliers && has_used[j])) - continue; + for (int j = 0; j < hand_list.size(); j++) { + if (i == j || (remove_inliers && has_used[j])) continue; // Which hands have an axis within of this one? - double axis_aligned = hand_list[i].getAxis().transpose() * hand_list[j].getAxis(); - bool axis_aligned_binary = fabs(axis_aligned) > cos(AXIS_ALIGN_ANGLE_THRESH); + double axis_aligned = + hand_list[i]->getAxis().transpose() * hand_list[j]->getAxis(); + bool axis_aligned_binary = + fabs(axis_aligned) > cos(AXIS_ALIGN_ANGLE_THRESH); // Which hands are within of this one? - Eigen::Vector3d delta_pos = hand_list[i].getGraspBottom() - hand_list[j].getGraspBottom(); + Eigen::Vector3d delta_pos = + hand_list[i]->getPosition() - hand_list[j]->getPosition(); double delta_pos_mag = delta_pos.norm(); bool delta_pos_mag_binary = delta_pos_mag <= MAX_DIST_THRESH; - // Which hands are within of this one when projected onto the plane orthognal to this + // Which hands are within of this one when + // projected onto the plane orthognal to this // one's axis? - Eigen::Matrix3d axis_orth_proj = Eigen::Matrix3d::Identity() - axis_outer_prod; + Eigen::Matrix3d axis_orth_proj = + Eigen::Matrix3d::Identity() - axis_outer_prod; Eigen::Vector3d delta_pos_proj = axis_orth_proj * delta_pos; double delta_pos_proj_mag = delta_pos_proj.norm(); - bool delta_pos_proj_mag_binary = delta_pos_proj_mag <= AXIS_ALIGN_DIST_THRESH; + bool delta_pos_proj_mag_binary = + delta_pos_proj_mag <= AXIS_ALIGN_DIST_THRESH; - bool inlier_binary = axis_aligned_binary && delta_pos_mag_binary && delta_pos_proj_mag_binary; - if (inlier_binary) - { + bool inlier_binary = axis_aligned_binary && delta_pos_mag_binary && + delta_pos_proj_mag_binary; + if (inlier_binary) { inliers.push_back(i); - scores.push_back(hand_list[j].getScore()); num_inliers++; - position_delta += hand_list[j].getGraspBottom(); - mean += hand_list[j].getScore(); - standard_deviation += hand_list[j].getScore() * hand_list[j].getScore(); - if (remove_inliers) - { + position_delta += hand_list[j]->getPosition(); + double old_mean = mean; + mean += (hand_list[j]->getScore() - mean) / + static_cast(num_inliers); + standard_deviation += (hand_list[j]->getScore() - mean) * + (hand_list[j]->getScore() - old_mean); + // mean += hand_list[j]->getScore(); + // standard_deviation += + // hand_list[j]->getScore() * hand_list[j]->getScore(); + if (remove_inliers) { has_used[j] = true; } } } - if (num_inliers >= min_inliers_) - { - position_delta = position_delta / (double) num_inliers - hand_list[i].getGraspBottom(); - mean = std::max(mean / (double) num_inliers, (double) num_inliers); - standard_deviation = standard_deviation == 0.0 ? 0.0 : sqrt(standard_deviation/(double) num_inliers - mean*mean); - std::nth_element(scores.begin(), scores.begin() + scores.size()/2, scores.end()); - double median = scores[scores.size()/2]; - double conf_lb = mean - 2.576*standard_deviation/sqrt((double)num_inliers); - double conf_ub = mean + 2.576*standard_deviation/sqrt((double)num_inliers); -// std::cout << "grasp " << i << ", num_inliers: " << num_inliers << ", ||pos_delta||: " << position_delta.norm() -// << ", mean: " << mean << ", std: " << standard_deviation << ", median: " << median << ", conf: " << conf_lb -// << ", " << conf_ub << "\n"; - Grasp hand = hand_list[i]; - hand.setGraspSurface(hand.getGraspSurface() + position_delta); - hand.setGraspBottom(hand.getGraspBottom() + position_delta); - hand.setGraspTop(hand.getGraspTop() + position_delta); - // hand.setScore(avg_score); - hand.setScore(conf_lb); - hand.setFullAntipodal(hand_list[i].isFullAntipodal()); - hands_out.push_back(hand); + if (num_inliers >= min_inliers_) { + double dNumInliers = static_cast(num_inliers); + position_delta = + position_delta / dNumInliers - hand_list[i]->getPosition(); + standard_deviation /= dNumInliers; + if (standard_deviation != 0) { + standard_deviation = sqrt(standard_deviation); + } + double sqrt_num_inliers = sqrt((double)num_inliers); + double conf_lb = mean - 2.576 * standard_deviation / sqrt_num_inliers; + double conf_ub = mean + 2.576 * standard_deviation / sqrt_num_inliers; + printf("grasp %d, inliers: %d, ||position_delta||: %3.4f, ", i, + num_inliers, position_delta.norm()); + printf("mean: %3.4f, STD: %3.4f, conf_int: (%3.4f, %3.4f)\n", mean, + standard_deviation, conf_lb, conf_ub); + std::unique_ptr hand = + std::make_unique(*hand_list[i]); + hand->setPosition(hand->getPosition() + position_delta); + hand->setScore(conf_lb); + hand->setFullAntipodal(hand_list[i]->isFullAntipodal()); + hands_out.push_back(std::move(hand)); } } return hands_out; } + +} // namespace gpd diff --git a/src/gpd/data_generator.cpp b/src/gpd/data_generator.cpp index ab70a94..ac43193 100644 --- a/src/gpd/data_generator.cpp +++ b/src/gpd/data_generator.cpp @@ -1,150 +1,380 @@ -#include "../../include/gpd/data_generator.h" +#include + +#include + +namespace gpd { + +const std::string DataGenerator::IMAGES_DS_NAME = "images"; +const std::string DataGenerator::LABELS_DS_NAME = "labels"; + +DataGenerator::DataGenerator(const std::string &config_filename) { + detector_ = std::make_unique(config_filename); + + // Read parameters from configuration file. + util::ConfigFile config_file(config_filename); + config_file.ExtractKeys(); + data_root_ = config_file.getValueOfKeyAsString("data_root", ""); + objects_file_location_ = + config_file.getValueOfKeyAsString("objects_file_location", ""); + output_root_ = config_file.getValueOfKeyAsString("output_root", ""); + chunk_size_ = config_file.getValueOfKey("chunk_size", 1000); + max_in_memory_ = config_file.getValueOfKey("max_in_memory", 100000); + num_views_per_object_ = + config_file.getValueOfKey("num_views_per_object", 1); + min_grasps_per_view_ = + config_file.getValueOfKey("min_grasps_per_view", 100); + max_grasps_per_view_ = + config_file.getValueOfKey("max_grasps_per_view", 500); + test_views_ = + config_file.getValueOfKeyAsStdVectorInt("test_views", "3 7 11 15 19"); + num_threads_ = config_file.getValueOfKey("num_threads", 1); + num_samples_ = config_file.getValueOfKey("num_samples", 500); + remove_nans_ = config_file.getValueOfKey("remove_nans", true); + voxel_size_views_ = + config_file.getValueOfKey("voxel_size_views", 0.003); + normals_radius_ = config_file.getValueOfKey("normals_radius", 0.03); + reverse_mesh_normals_ = + config_file.getValueOfKey("reverse_mesh_normals", true); + reverse_view_normals_ = + config_file.getValueOfKey("reverse_view_normals", true); + + printf("============ DATA GENERATION =================\n"); + std::cout << "data_root: " << data_root_ << "\n"; + std::cout << "objects_file_location: " << objects_file_location_ << "\n"; + std::cout << "output_root: " << output_root_ << "\n"; + std::cout << "num_views_per_object: " << num_views_per_object_ << "\n"; + std::cout << "min_grasps_per_view: " << min_grasps_per_view_ << "\n"; + std::cout << "max_grasps_per_view: " << max_grasps_per_view_ << "\n"; + std::cout << "test_views: "; + for (int i = 0; i < test_views_.size(); i++) { + std::cout << test_views_[i] << " "; + } + std::cout << "\n"; + printf("==============================================\n"); + + printf("============ CLOUD PREPROCESSING =============\n"); + printf("remove_nans: %d\n", remove_nans_); + printf("voxel_size_views: %.3f\n", voxel_size_views_); + printf("normals_radius_: %.3f\n", normals_radius_); + printf("reverse_mesh_normals: %d\n", reverse_mesh_normals_); + printf("reverse_view_normals: %d\n", reverse_view_normals_); + printf("==============================================\n"); + + printf("============ CANDIDATE GENERATION ============\n"); + printf("num_samples: %d\n", num_samples_); + printf("num_threads: %d\n", num_threads_); + printf("==============================================\n"); + + Eigen::VectorXi cam_sources = Eigen::VectorXi::LinSpaced((360 / 3), 0, 360); + all_cam_sources_.resize(cam_sources.size()); + Eigen::VectorXi::Map(&all_cam_sources_[0], cam_sources.rows()) = cam_sources; +} +void DataGenerator::generateData() { + double total_time = 0.0; -DataGenerator::DataGenerator(ros::NodeHandle& node) -{ - candidates_generator_ = createCandidatesGenerator(node); - learning_ = createLearning(node); -} + const candidate::HandGeometry &hand_geom = + detector_->getHandSearchParameters().hand_geometry_; + std::vector objects = loadObjectNames(objects_file_location_); -DataGenerator::~DataGenerator() -{ - delete candidates_generator_; - delete learning_; -} + int store_step = 1; + bool plot_grasps = false; + int num_objects = objects.size(); + // debugging + // num_objects = 1; + // num_views_per_object_ = 20; -CandidatesGenerator* DataGenerator::createCandidatesGenerator(ros::NodeHandle& node) -{ - // Create objects to store parameters - CandidatesGenerator::Parameters generator_params; - HandSearch::Parameters hand_search_params; - - // Read hand geometry parameters - node.param("finger_width", hand_search_params.finger_width_, 0.01); - node.param("hand_outer_diameter", hand_search_params.hand_outer_diameter_, 0.09); - node.param("hand_depth", hand_search_params.hand_depth_, 0.06); - node.param("hand_height", hand_search_params.hand_height_, 0.02); - node.param("init_bite", hand_search_params.init_bite_, 0.015); - - // Read local hand search parameters - node.param("nn_radius", hand_search_params.nn_radius_frames_, 0.01); - node.param("num_orientations", hand_search_params.num_orientations_, 8); - node.param("num_samples", hand_search_params.num_samples_, 500); - node.param("num_threads", hand_search_params.num_threads_, 1); - node.param("rotation_axis", hand_search_params.rotation_axis_, 2); - - // Read general parameters - generator_params.num_samples_ = hand_search_params.num_samples_; - generator_params.num_threads_ = hand_search_params.num_threads_; - node.param("plot_candidates", generator_params.plot_grasps_, false); - - // Read preprocessing parameters - node.param("remove_outliers", generator_params.remove_statistical_outliers_, true); - node.param("voxelize", generator_params.voxelize_, true); - node.getParam("workspace", generator_params.workspace_); - - // Read plotting parameters. - generator_params.plot_grasps_ = false; - node.param("plot_normals", generator_params.plot_normals_, false); - - // Create object to generate grasp candidates. - return new CandidatesGenerator(generator_params, hand_search_params); -} + std::vector positives_list, negatives_list; + std::vector train_data, test_data; + int num_train_views = num_views_per_object_ - test_views_.size(); + const int n_train = store_step * num_train_views * max_grasps_per_view_; + const int n_test = store_step * test_views_.size() * max_grasps_per_view_; + train_data.reserve(n_train); + test_data.reserve(n_test); + std::string train_file_path = output_root_ + "train.h5"; + std::string test_file_path = output_root_ + "test.h5"; + createDatasetsHDF5(train_file_path, objects.size() * n_train); + createDatasetsHDF5(test_file_path, objects.size() * n_test); + int train_offset = 0; + int test_offset = 0; + + util::Plot plotter(1, 8); + + for (int i = 0; i < num_objects; i++) { + printf("===> Generating images for object %d/%d: %s\n", i + 1, num_objects, + objects[i].c_str()); + double t0 = omp_get_wtime(); + // Load mesh for ground truth. + std::string prefix = data_root_ + objects[i]; + util::Cloud mesh = loadMesh(prefix + "_gt.pcd", prefix + "_gt_normals.csv"); + mesh.calculateNormalsOMP(num_threads_, normals_radius_); + if (reverse_mesh_normals_) { + mesh.setNormals(mesh.getNormals() * (-1.0)); + } -Learning* DataGenerator::createLearning(ros::NodeHandle& node) -{ - // Read grasp image parameters. - Learning::ImageParameters image_params; - node.param("image_outer_diameter", image_params.outer_diameter_, 0.09); - node.param("image_depth", image_params.depth_, 0.06); - node.param("image_height", image_params.height_, 0.02); - node.param("image_size", image_params.size_, 60); - node.param("image_num_channels", image_params.num_channels_, 15); - - // Read learning parameters. - bool remove_plane; - int num_orientations, num_threads; - node.param("remove_plane_before_image_calculation", remove_plane, false); - node.param("num_orientations", num_orientations, 8); - node.param("num_threads", num_threads, 1); - - // Create object to create grasp images from grasp candidates (used for classification). - return new Learning(image_params, num_threads, num_orientations, false, remove_plane); -} + for (int j = 0; j < num_views_per_object_; j++) { + printf("===> Processing view %d/%d\n", j + 1, num_views_per_object_); + std::vector positives_view(0); + std::vector negatives_view(0); + std::vector> labeled_grasps_view(0); + std::vector> images_view(0); -CloudCamera DataGenerator::loadCloudCameraFromFile(ros::NodeHandle& node) -{ - // Set the position from which the camera sees the point cloud. - std::vector camera_position; - node.getParam("camera_position", camera_position); - Eigen::Matrix3Xd view_points(3,1); - view_points << camera_position[0], camera_position[1], camera_position[2]; + // 1. Load point cloud. + Eigen::Matrix3Xd view_points(3, 1); + view_points << 0.0, 0.0, 0.0; // TODO: Load camera position. + util::Cloud cloud(prefix + "_" + std::to_string(j + 1) + ".pcd", + view_points); + if (remove_nans_) { + cloud.removeNans(); + } + cloud.voxelizeCloud(voxel_size_views_); + cloud.calculateNormalsOMP(num_threads_, normals_radius_); + if (reverse_view_normals_) { + cloud.setNormals(cloud.getNormals() * (-1.0)); + } + + while (positives_view.size() < min_grasps_per_view_) { + cloud.subsampleUniformly(num_samples_); + + // 2. Find grasps in point cloud. + std::vector> grasps; + std::vector> images; + bool has_grasps = detector_->createGraspImages(cloud, grasps, images); + + if (plot_grasps) { + // Plot plotter; + // plotter.plotNormals(cloud.getCloudOriginal(), + // cloud.getNormals()); + // plotter.plotFingers(grasps, cloud.getCloudOriginal(), + // "Grasps on view"); + // plotter.plotFingers3D(candidates, + // cloud_cam.getCloudOriginal(), "Grasps on view", + // hand_geom.outer_diameter_, + // hand_geom.finger_width_, + // hand_geom.depth_, + // hand_geom.height_); + } + + // 3. Evaluate grasps against ground truth (mesh). + printf("Eval GT ...\n"); + std::vector labels = detector_->evalGroundTruth(mesh, grasps); + + // 4. Split grasps into positives and negatives. + std::vector positives; + std::vector negatives; + splitInstances(labels, positives, negatives); + + if (positives_view.size() > 0 && positives.size() > 0) { + for (int k = 0; k < positives.size(); k++) { + positives[k] += images_view.size(); + } + } + if (negatives_view.size() > 0 && negatives.size() > 0) { + for (int k = 0; k < negatives.size(); k++) { + negatives[k] += images_view.size(); + } + } + + images_view.insert(images_view.end(), + std::make_move_iterator(images.begin()), + std::make_move_iterator(images.end())); + labeled_grasps_view.insert(labeled_grasps_view.end(), + std::make_move_iterator(grasps.begin()), + std::make_move_iterator(grasps.end())); + + positives_view.insert(positives_view.end(), positives.begin(), + positives.end()); + negatives_view.insert(negatives_view.end(), negatives.begin(), + negatives.end()); + } + printf("positives, negatives found for this view: %zu, %zu\n", + positives_view.size(), negatives_view.size()); - // Load the point cloud from the file. - std::string filename; - node.param("cloud_file_name", filename, std::string("")); + // 5. Balance the number of positives and negatives. + balanceInstances(max_grasps_per_view_, positives_view, negatives_view, + positives_list, negatives_list); + printf("#positives: %d, #negatives: %d\n", (int)positives_list.size(), + (int)negatives_list.size()); - return CloudCamera(filename, view_points); + // 6. Assign instances to training or test data. + if (std::find(test_views_.begin(), test_views_.end(), j) != + test_views_.end()) { + addInstances(labeled_grasps_view, images_view, positives_list, + negatives_list, test_data); + std::cout << "test view, # test data: " << test_data.size() << "\n"; + } else { + addInstances(labeled_grasps_view, images_view, positives_list, + negatives_list, train_data); + std::cout << "train view, # train data: " << train_data.size() << "\n"; + } + printf("------------------------------------\n"); + } + + if ((i + 1) % store_step == 0) { + // Shuffle the data. + std::random_shuffle(train_data.begin(), train_data.end()); + std::random_shuffle(test_data.begin(), test_data.end()); + train_offset = insertIntoHDF5(train_file_path, train_data, train_offset); + test_offset = insertIntoHDF5(test_file_path, test_data, test_offset); + printf("train_offset: %d, test_offset: %d\n", train_offset, test_offset); + train_data.clear(); + test_data.clear(); + train_data.reserve(store_step * num_views_per_object_ * 1000); + test_data.reserve(store_step * num_views_per_object_ * 1000); + } + + double ti = omp_get_wtime() - t0; + total_time += ti; + double num_objects_left = static_cast(num_objects - i); + double avg_time = total_time / (i + 1); + printf("Number of objects left: %3.4f\n", num_objects_left); + printf("Time for this object: %4.2fs. Total time: %3.2fs.\n", ti, + total_time); + printf("Average time per object: %4.2fs.\n", avg_time); + printf( + "Estimated time remaining (based on time for this object): %4.4fh or " + "%4.4fs.\n", + ti * num_objects_left * (1.0 / 3600.0), ti * num_objects_left); + printf( + "Estimated time remaining (based on average time per object): %4.4fh " + "or %4.4fs.\n", + avg_time * num_objects_left * (1.0 / 3600.0), + avg_time * num_objects_left); + printf("======================================\n\n"); + } + + // Store remaining data. + if (train_data.size() > 0) { + printf("Storing remaining instances ...\n"); + + // Shuffle the data. + std::random_shuffle(train_data.begin(), train_data.end()); + std::random_shuffle(test_data.begin(), test_data.end()); + train_offset = insertIntoHDF5(train_file_path, train_data, train_offset); + test_offset = insertIntoHDF5(test_file_path, test_data, test_offset); + printf("train_offset: %d, test_offset: %d\n", train_offset, test_offset); + } + + printf("Generated %d training and test %d instances\n", train_offset, + test_offset); + std::string train_file_path_resized = output_root_ + "train2.h5"; + std::string test_file_path_resized = output_root_ + "test2.h5"; + printf("Resizing HDF5 training set to remove empty elements ...\n"); + reshapeHDF5(train_file_path, train_file_path_resized, "images", train_offset, + chunk_size_, max_in_memory_); + reshapeHDF5(train_file_path, train_file_path_resized, "labels", train_offset, + chunk_size_, max_in_memory_); + printf("Resizing HDF5 test set to remove empty elements ...\n"); + reshapeHDF5(test_file_path, test_file_path_resized, "images", test_offset, + chunk_size_, max_in_memory_); + reshapeHDF5(test_file_path, test_file_path_resized, "labels", test_offset, + chunk_size_, max_in_memory_); + printf("Wrote data to training and test databases\n"); } +void DataGenerator::createDatasetsHDF5(const std::string &filepath, + int num_data) { + printf("Opening HDF5 file at: %s\n", filepath.c_str()); + cv::Ptr h5io = cv::hdf::open(filepath); + + int n_dims_labels = 2; + int dsdims_labels[n_dims_labels] = {num_data, 1}; + int chunks_labels[n_dims_labels] = {chunk_size_, dsdims_labels[1]}; + printf("Creating dataset <%s>: %d x %d\n", LABELS_DS_NAME.c_str(), + dsdims_labels[0], dsdims_labels[1]); + h5io->dscreate(n_dims_labels, dsdims_labels, CV_8UC1, LABELS_DS_NAME, 4, + chunks_labels); + + const descriptor::ImageGeometry &image_geom = detector_->getImageGeometry(); + int n_dims_images = 4; + int dsdims_images[n_dims_images] = { + num_data, image_geom.size_, image_geom.size_, image_geom.num_channels_}; + int chunks_images[n_dims_images] = {chunk_size_, dsdims_images[1], + dsdims_images[2], dsdims_images[3]}; + printf("Creating dataset <%s>: %d x %d x %d x %d\n", IMAGES_DS_NAME.c_str(), + dsdims_images[0], dsdims_images[1], dsdims_images[2], + dsdims_images[3]); + h5io->dscreate(n_dims_images, dsdims_images, CV_8UC1, IMAGES_DS_NAME, + n_dims_images, chunks_images); + h5io->close(); +} -CloudCamera DataGenerator::loadMesh(const std::string& mesh_file_path, const std::string& normals_file_path) -{ +void DataGenerator::reshapeHDF5(const std::string &in, const std::string &out, + const std::string &dataset, int num, + int chunk_size, int max_in_memory) { + const int COMPRESSION_LEVEL = 9; + + cv::Ptr h5io_in = cv::hdf::open(in); + std::vector dims = h5io_in->dsgetsize(dataset); + std::vector offsets(dims.size()); + for (int i = 0; i < dims.size(); i++) { + offsets[i] = 0; + } + printf("Resizing dataset %s of length %d to %d\n", dataset.c_str(), dims[0], + num); + + std::vector chunks(dims.begin(), dims.end()); + chunks[0] = chunk_size; + dims[0] = num; + int type = h5io_in->dsgettype(dataset); + cv::Ptr h5io_out = cv::hdf::open(out); + h5io_out->dscreate(dims.size(), &dims[0], type, dataset, COMPRESSION_LEVEL, + &chunks[0]); + + int steps = num / max_in_memory + 1; + dims[0] = max_in_memory; + for (int i = 0; i < steps; i++) { + cv::Mat mat; + dims[0] = (i == steps - 1) ? num - offsets[0] : max_in_memory; + h5io_in->dsread(mat, dataset, offsets, dims); + printf("Processing block %d/%d: %d to %d\n", i + 1, steps, offsets[0], + offsets[0] + dims[0]); + if (!mat.isContinuous()) { + std::cout << "Error: matrix is not continuous! Consider resizing using " + "python.\n"; + break; + } + h5io_out->dsinsert(mat, dataset, offsets); + offsets[0] += max_in_memory; + } + + h5io_in->close(); + h5io_out->close(); +} + +util::Cloud DataGenerator::loadMesh(const std::string &mesh_file_path, + const std::string &normals_file_path) { // Load mesh for ground truth. std::cout << " mesh_file_path: " << mesh_file_path << '\n'; std::cout << " normals_file_path: " << normals_file_path << '\n'; // Set the position from which the camera sees the point cloud. - Eigen::Matrix3Xd view_points(3,1); + Eigen::Matrix3Xd view_points(3, 1); view_points << 0.0, 0.0, 0.0; - CloudCamera mesh_cloud_cam(mesh_file_path, view_points); + // Load the point cloud. + util::Cloud mesh_cloud_cam(mesh_file_path, view_points); // Load surface normals for the mesh. mesh_cloud_cam.setNormalsFromFile(normals_file_path); - std::cout << "Loaded mesh with " << mesh_cloud_cam.getCloudProcessed()->size() << " points.\n"; + printf("Loaded mesh with %d points.\n", + (int)mesh_cloud_cam.getCloudProcessed()->size()); return mesh_cloud_cam; } - -std::vector DataGenerator::loadPointCloudFiles(const std::string& cloud_folder) -{ - boost::filesystem::path path(cloud_folder); - boost::filesystem::directory_iterator it(path); - std::vector files; - - while (it != boost::filesystem::directory_iterator()) - { - const std::string& filepath = (*it).path().string(); - - if (filepath.find("mesh") == std::string::npos && filepath.find(".pcd") != std::string::npos) - { - files.push_back((*it).path()); - } - - it++; - } - - std::sort(files.begin(), files.end()); - - return files; -} - - -std::vector DataGenerator::loadObjectNames(const std::string& objects_file_location) -{ +std::vector DataGenerator::loadObjectNames( + const std::string &objects_file_location) { std::ifstream in; in.open(objects_file_location.c_str()); std::string line; std::vector objects; - while(std::getline(in, line)) - { + while (std::getline(in, line)) { std::stringstream lineStream(line); std::string object; std::getline(lineStream, object, '\n'); @@ -155,227 +385,319 @@ std::vector DataGenerator::loadObjectNames(const std::string& objec return objects; } - -bool DataGenerator::createGraspImages(CloudCamera& cloud_cam, std::vector& grasps_out, - std::vector& images_out) -{ - // Preprocess the point cloud. - candidates_generator_->preprocessPointCloud(cloud_cam); - - // Reverse direction of surface normals. - cloud_cam.setNormals(cloud_cam.getNormals().array() * (-1.0)); - - // Generate grasp candidates. - std::vector candidates = candidates_generator_->generateGraspCandidateSets(cloud_cam); - - if (candidates.size() == 0) - { - ROS_ERROR("No grasp candidates found!"); - return false; +void DataGenerator::splitInstances(const std::vector &labels, + std::vector &positives, + std::vector &negatives) { + positives.resize(0); + negatives.resize(0); + + for (int i = 0; i < labels.size(); i++) { + if (labels[i] == 1) { + positives.push_back(i); + } else { + negatives.push_back(i); + } } - // Create the grasp images. - std::vector image_list = learning_->createImages(cloud_cam, candidates); - grasps_out.resize(0); - images_out.resize(0); - learning_->extractGraspsAndImages(candidates, image_list, grasps_out, images_out); - return true; + printf("#grasps: %zu, #positives: %zu, #negatives: %zu\n", labels.size(), + positives.size(), negatives.size()); } - -void DataGenerator::balanceInstances(int max_grasps_per_view, const std::vector& positives_in, - const std::vector& negatives_in, std::vector& positives_out, std::vector& negatives_out) -{ - int end = 0; +void DataGenerator::balanceInstances(int max_grasps_per_view, + const std::vector &positives_in, + const std::vector &negatives_in, + std::vector &positives_out, + std::vector &negatives_out) { + size_t end; positives_out.resize(0); negatives_out.resize(0); - // Number of positives is less than or equal to number of negatives - if (positives_in.size() > 0 && positives_in.size() <= negatives_in.size()) - { - end = std::min((int) positives_in.size(), max_grasps_per_view); - positives_out.insert(positives_out.end(), positives_in.begin(), positives_in.begin() + end); - negatives_out.insert(negatives_out.end(), negatives_in.begin(), negatives_in.begin() + end); - } - else - { - end = std::min((int) negatives_in.size(), max_grasps_per_view); - negatives_out.insert(negatives_out.end(), negatives_in.begin(), negatives_in.begin() + end); - - if (positives_in.size() > negatives_in.size()) - { - positives_out.insert(positives_out.end(), positives_in.begin(), positives_in.begin() + end); - } + if (negatives_in.size() > positives_in.size()) { + end = + std::min(positives_in.size(), (size_t)floor(0.5 * max_grasps_per_view)); + positives_out.insert(positives_out.end(), positives_in.begin(), + positives_in.begin() + end); + negatives_out.insert(negatives_out.end(), negatives_in.begin(), + negatives_in.begin() + end); + } else { + end = + std::min(negatives_in.size(), (size_t)floor(0.5 * max_grasps_per_view)); + negatives_out.insert(negatives_out.end(), negatives_in.begin(), + negatives_in.begin() + end); + positives_out.insert(positives_out.end(), positives_in.begin(), + positives_in.begin() + end); } } - -void DataGenerator::addInstances(const std::vector& grasps, const std::vector& images, - const std::vector& positives, const std::vector& negatives, std::vector& dataset) -{ - for (int k = 0; k < positives.size(); k++) - { +void DataGenerator::addInstances( + const std::vector> &grasps, + std::vector> &images, + const std::vector &positives, const std::vector &negatives, + std::vector &dataset) { + for (int k = 0; k < positives.size(); k++) { int idx = positives[k]; - dataset.push_back(Instance(images[idx], grasps[idx].isFullAntipodal())); + if (!images[idx]) { + printf(" => idx: %d is nullptr!\n", idx); + char c; + std::cin >> c; + } + dataset.push_back( + Instance(std::move(images[idx]), grasps[idx]->isFullAntipodal())); } - for (int k = 0; k < negatives.size(); k++) - { + for (int k = 0; k < negatives.size(); k++) { int idx = negatives[k]; - dataset.push_back(Instance(images[idx], grasps[idx].isFullAntipodal())); + if (!images[idx]) { + printf(" => idx: %d is nullptr!\n", idx); + char c; + std::cin >> c; + } + dataset.push_back( + Instance(std::move(images[idx]), grasps[idx]->isFullAntipodal())); } } +int DataGenerator::insertIntoHDF5(const std::string &file_path, + const std::vector &dataset, + int offset) { + if (dataset.empty()) { + printf("Error: Dataset is empty!\n"); + return offset; + } + + printf("Storing %d items in HDF5: %s ... \n", (int)dataset.size(), + file_path.c_str()); -void DataGenerator::storeLMDB(const std::vector& dataset, const std::string& file_location) -{ - // Create new database - boost::scoped_ptr db(caffe::db::GetDB("lmdb")); - db->Open(file_location, caffe::db::NEW); - boost::scoped_ptr txn(db->NewTransaction()); - - // Storing to db - caffe::Datum datum; - int count = 0; - int data_size = 0; - bool data_size_initialized = false; - - for (int i = 0; i < dataset.size(); i++) - { - caffe::CVMatToDatum(dataset[i].image_, &datum); - datum.set_label(dataset[i].label_); - - std::string key_str = caffe::format_int(i, 8); - printf("%s, size: %d x %d x %d, label: %d, %d\n", key_str.c_str(), datum.channels(), datum.height(), datum.width(), datum.label(), dataset[i].label_); -// std::cout << key_str << ", " << datum.channels() << "x " datum.label() << ", " << dataset[i].label_ << "\n"; - - // Put in db - std::string out; - CHECK(datum.SerializeToString(&out)); - txn->Put(key_str, out); - - if (count % 1000 == 0) - { - // Commit db - txn->Commit(); - txn.reset(db->NewTransaction()); - LOG(INFO) << "Processed " << count << " files."; - } + cv::Ptr h5io = cv::hdf::open(file_path); + printf(" Opened HDF5 file\n"); + + const int num = static_cast(dataset.size()); + const int rows = dataset[0].image_->rows; + const int cols = dataset[0].image_->cols; + const int channels = dataset[0].image_->channels(); + + cv::Mat labels(dataset.size(), 1, CV_8UC1, cv::Scalar(0.0)); - count++; + for (int i = 0; i < dataset.size(); i++) { + labels.at(i) = (uchar)dataset[i].label_; } - // write the last batch - if (count % 1000 != 0) - { - txn->Commit(); - LOG(INFO) << "Processed " << count << " files."; + const int dims_images = 4; + int dsdims_images[dims_images] = {num, rows, cols, channels}; + cv::Mat images(dims_images, dsdims_images, CV_8UC1, cv::Scalar(0.0)); + const int dims_image = 3; + int dsdims_image[dims_image] = {rows, cols, channels}; + + for (int i = 0; i < dataset.size(); i++) { + if (!dataset[i].image_) { + printf("FATAL ERROR! %d is nullptr\n", i); + char c; + std::cin >> c; + } + copyMatrix(*dataset[i].image_, images, i, dsdims_image); } -} + printf(" Inserting into images dataset ...\n"); + const int dims_offset_images = 4; + int offsets_images[dims_offset_images] = {offset, 0, 0, 0}; + h5io->dsinsert(images, IMAGES_DS_NAME, offsets_images); -void DataGenerator::createTrainingData(ros::NodeHandle& node) -{ - int store_step = 1; - int max_grasps_per_view = 1000; - Eigen::VectorXi test_views(5); - test_views << 2, 5, 8, 13, 16; - - std::string data_root, objects_file_location, output_root; - node.param("data_root", data_root, std::string("")); - node.param("objects_file", objects_file_location, std::string("")); - node.param("output_root", output_root, std::string("")); - bool plot_grasps; - node.param("plot_grasps", plot_grasps, false); - int num_views; - node.param("num_views", num_views, 20); - - std::vector objects = loadObjectNames(objects_file_location); - std::vector positives_list, negatives_list; - std::vector train_data, test_data; - train_data.reserve(store_step * num_views * 100); - test_data.reserve(store_step * num_views * 100); + printf(" Inserting into labels dataset ...\n"); + const int dims_offset_labels = 2; + int offsets_labels[dims_offset_labels] = {offset, 0}; + h5io->dsinsert(labels, LABELS_DS_NAME, offsets_labels); - for (int i = 0; i < objects.size(); i++) - { - printf("===> (%d) Generating images for object: %s\n", i, objects[i].c_str()); + h5io->close(); - // Load mesh for ground truth. - std::string prefix = data_root + objects[i]; - CloudCamera mesh_cloud_cam = loadMesh(prefix + "_gt.pcd", prefix + "_gt_normals.csv"); + return offset + static_cast(images.size[0]); +} - for (int j = 0; j < num_views; j++) - { - printf("===> Processing view %d\n", j + 1); +void DataGenerator::storeHDF5(const std::vector &dataset, + const std::string &file_location) { + const std::string IMAGE_DS_NAME = "images"; + const std::string LABELS_DS_NAME = "labels"; - // 1. Load point cloud. - Eigen::Matrix3Xd view_points(3,1); - view_points << 0.0, 0.0, 0.0; // TODO: Load camera position. - CloudCamera cloud_cam(prefix + "_" + boost::lexical_cast(j + 1) + ".pcd", view_points); - - // 2. Find grasps in point cloud. - std::vector grasps; - std::vector images; - bool has_grasps = createGraspImages(cloud_cam, grasps, images); - - if (plot_grasps) - { - Plot plotter; - plotter.plotNormals(cloud_cam.getCloudOriginal(), cloud_cam.getNormals()); - plotter.plotFingers(grasps, cloud_cam.getCloudOriginal(), "Grasps on view"); - } + if (dataset.empty()) { + printf("Error: Dataset is empty!\n"); + return; + } - // 3. Evaluate grasps against ground truth (mesh). - std::vector labeled_grasps = candidates_generator_->reevaluateHypotheses(mesh_cloud_cam, grasps); - - // 4. Split grasps into positives and negatives. - std::vector positives; - std::vector negatives; - for (int k = 0; k < labeled_grasps.size(); k++) - { - if (labeled_grasps[k].isFullAntipodal()) - positives.push_back(k); - else - negatives.push_back(k); - } - printf("#grasps: %d, #positives: %d, #negatives: %d\n", (int) labeled_grasps.size(), (int) positives.size(), - (int) negatives.size()); + printf("Storing data as HDF5 at: %s\n", file_location.c_str()); + + int n_dims = 3; + int dsdims_images[n_dims] = {static_cast(dataset.size()), + dataset[0].image_->rows, + dataset[0].image_->cols}; + cv::Mat images(n_dims, dsdims_images, CV_8UC(dataset[0].image_->channels()), + cv::Scalar(0.0)); + cv::Mat labels(dataset.size(), 1, CV_8UC1, cv::Scalar(0.0)); + printf(" Dataset dimensions: %d x %d x %d x%d\n", dsdims_images[0], + dsdims_images[1], dsdims_images[2], images.channels()); + int dims_image[n_dims] = {dataset[0].image_->rows, dataset[0].image_->cols, + dataset[0].image_->channels()}; + + for (int i = 0; i < dataset.size(); i++) { + // ranges don't seem to work + // std::vector ranges; + // ranges.push_back(cv::Range(i,i+1)); + // ranges.push_back(cv::Range(0,60)); + // ranges.push_back(cv::Range(0,60)); + // data(&ranges[0]) = dataset[i].image_.clone(); + // dataset[0].image_->copyTo(data(&ranges[0])); + copyMatrix(*dataset[i].image_, images, i, dims_image); + labels.at(i) = (uchar)dataset[i].label_; + // printf("dataset.label: %d, labels(i): %d\n", dataset[i].label_, + // labels.at(i)); + } - // 5. Balance the number of positives and negatives. - balanceInstances(max_grasps_per_view, positives, negatives, positives_list, negatives_list); - printf("#positives: %d, #negatives: %d\n", (int) positives_list.size(), (int) negatives_list.size()); + cv::Ptr h5io = cv::hdf::open(file_location); + if (!h5io->hlexists(IMAGE_DS_NAME)) { + int n_dims_labels = 2; + int dsdims_labels[n_dims_labels] = {static_cast(dataset.size()), 1}; + printf("Creating dataset ...\n"); + h5io->dscreate(n_dims_labels, dsdims_labels, CV_8UC1, LABELS_DS_NAME, 9); + printf("Writing dataset ...\n"); + h5io->dswrite(labels, LABELS_DS_NAME); + printf("Wrote dataset of length %d to HDF5.\n", (int)dataset.size()); + + printf("Creating dataset ...\n"); + int chunks[n_dims] = {chunk_size_, dsdims_images[1], dsdims_images[2]}; + h5io->dscreate(n_dims, dsdims_images, CV_8UC(dataset[0].image_->channels()), + IMAGE_DS_NAME, 9, chunks); + printf("Writing dataset ...\n"); + h5io->dswrite(images, IMAGE_DS_NAME); + } + h5io->close(); +} - // 6. Assign instances to training or test data. - bool is_test_view = false; - - for (int k = 0; k < test_views.rows(); k++) - { - if (j == test_views(k)) - { - is_test_view = true; - break; +void DataGenerator::printMatrix(const cv::Mat &mat) { + for (int i = 0; i < 1; i++) { + for (int j = 0; j < 60; j++) { + for (int k = 0; k < 60; k++) { + for (int l = 0; l < 15; l++) { + int idx[4] = {i, j, k, l}; + if (mat.at(idx) > 0) + printf("%d,%d,%d,%d: %d\n", i, j, k, l, (int)mat.at(idx)); + // std::cout << i << ", " << j << ", " << k << ", l" << ": " + // << (int) mat.at(idx) << " \n"; } } + } + } +} - if (is_test_view) - { - addInstances(labeled_grasps, images, positives_list, negatives_list, test_data); - std::cout << "test view, # test data: " << test_data.size() << "\n"; +void DataGenerator::printMatrix15(const cv::Mat &mat) { + for (int j = 0; j < 60; j++) { + for (int k = 0; k < 60; k++) { + for (int l = 0; l < 15; l++) { + int idx[3] = {j, k, l}; + if (mat.at(idx) > 0) + printf("%d,%d,%d: %d\n", j, k, l, (int)mat.at(idx)); } - else - { - addInstances(labeled_grasps, images, positives_list, negatives_list, train_data); - std::cout << "train view, # train data: " << train_data.size() << "\n"; + } + } +} + +// src: multi-channels image; dst: multi-dimensional matrix +void DataGenerator::copyMatrix(const cv::Mat &src, cv::Mat &dst, int idx_in, + int *dims_img) { + const int rows = dims_img[0]; + const int cols = dims_img[1]; + const int channels = dims_img[2]; + for (int j = 0; j < rows; j++) { + for (int k = 0; k < cols; k++) { + for (int l = 0; l < channels; l++) { + int idx_dst[4] = {idx_in, j, k, l}; + dst.at(idx_dst) = src.ptr(j)[k * channels + l]; } } } +} + +util::Cloud DataGenerator::createMultiViewCloud(const std::string &object, + int camera, + const std::vector angles, + int reference_camera) const { + const std::string prefix = data_root_ + object; + const std::string np_str = "NP" + std::to_string(camera) + "_"; + const std::string cloud_prefix = prefix + "/clouds/" + np_str; + const std::string pose_prefix = prefix + "/poses/" + np_str; + + PointCloudRGB::Ptr concatenated_cloud(new PointCloudRGB); + Eigen::Matrix3Xf camera_positions(3, angles.size()); + std::vector sizes; + sizes.resize(angles.size()); + util::Cloud tmp; + + for (int i = 0; i < angles.size(); i++) { + std::string cloud_filename = + cloud_prefix + std::to_string(angles[i]) + ".pcd"; + printf("cloud_filename: %s\n", cloud_filename.c_str()); + PointCloudRGB::Ptr cloud = tmp.loadPointCloudFromFile(cloud_filename); + + Eigen::Matrix4f T = + calculateTransform(object, camera, angles[i], reference_camera); + pcl::transformPointCloud(*cloud, *cloud, T); + + pcl::io::savePCDFileASCII( + object + np_str + std::to_string(angles[i]) + ".pcd", *cloud); + + *concatenated_cloud += *cloud; + camera_positions.col(i) = T.col(3).head(3); + sizes[i] = cloud->size(); + } + + Eigen::MatrixXi camera_sources = + Eigen::MatrixXi::Zero(angles.size(), concatenated_cloud->size()); + int start = 0; + for (int i = 0; i < angles.size(); i++) { + camera_sources.block(i, start, 1, sizes[i]).setConstant(1); + start += sizes[i]; + } + + util::Cloud multi_view_cloud(concatenated_cloud, camera_sources, + camera_positions.cast()); - // Shuffle the data. - std::random_shuffle(train_data.begin(), train_data.end()); - std::random_shuffle(test_data.begin(), test_data.end()); + pcl::io::savePCDFileASCII(object + np_str + "_merged.pcd", + *multi_view_cloud.getCloudOriginal()); - // Store the grasp images and their labels in LMDBs. - storeLMDB(train_data, output_root + "train_lmdb"); - storeLMDB(test_data, output_root + "test_lmdb"); - std::cout << "Wrote data to training and test LMDBs\n"; + return multi_view_cloud; } + +Eigen::Matrix4f DataGenerator::calculateTransform(const std::string &object, + int camera, int angle, + int reference_camera) const { + std::string pose_filename = data_root_ + object + "/poses/NP" + + std::to_string(reference_camera) + "_" + + std::to_string(angle) + "_pose.h5"; + std::string table_from_ref_key = "H_table_from_reference_camera"; + printf("pose_filename: %s\n", pose_filename.c_str()); + Eigen::Matrix4f T_table_from_ref = + readPoseFromHDF5(pose_filename, table_from_ref_key); + + std::string calibration_filename = data_root_ + object + "/calibration.h5"; + std::string cam_from_ref_key = "H_NP" + std::to_string(camera) + "_from_NP" + + std::to_string(reference_camera); + printf("calibration_filename: %s\n", calibration_filename.c_str()); + printf("cam_from_ref_key: %s\n", cam_from_ref_key.c_str()); + Eigen::Matrix4f T_cam_from_ref = + readPoseFromHDF5(calibration_filename, cam_from_ref_key); + + Eigen::Matrix4f T = T_table_from_ref * T_cam_from_ref.inverse(); + + return T; +} + +Eigen::Matrix4f DataGenerator::readPoseFromHDF5( + const std::string &hdf5_filename, const std::string &dsname) const { + cv::Ptr h5io = cv::hdf::open(hdf5_filename); + cv::Mat mat_cv(4, 4, CV_32FC1); + h5io->dsread(mat_cv, dsname); + Eigen::Matrix4f mat_eigen; + cv::cv2eigen(mat_cv, mat_eigen); + h5io->close(); + + return mat_eigen; +} + +} // namespace gpd diff --git a/src/gpd/descriptor/image_12_channels_strategy.cpp b/src/gpd/descriptor/image_12_channels_strategy.cpp new file mode 100644 index 0000000..7a9e27f --- /dev/null +++ b/src/gpd/descriptor/image_12_channels_strategy.cpp @@ -0,0 +1,122 @@ +#include + +namespace gpd { +namespace descriptor { + +std::vector> Image12ChannelsStrategy::createImages( + const candidate::HandSet &hand_set, + const util::PointList &nn_points) const { + const std::vector> &hands = + hand_set.getHands(); + std::vector> images(hands.size()); + + for (int i = 0; i < hands.size(); i++) { + if (hand_set.getIsValid()(i)) { + images[i] = std::make_unique( + image_params_.size_, image_params_.size_, + CV_8UC(image_params_.num_channels_), cv::Scalar(0.0)); + createImage(nn_points, *hands[i], *images[i]); + } + } + + return images; +} + +void Image12ChannelsStrategy::createImage(const util::PointList &point_list, + const candidate::Hand &hand, + cv::Mat &image) const { + // 1. Transform points and normals in neighborhood into the unit image. + Matrix3XdPair points_normals = transformToUnitImage(point_list, hand); + + // 2. Create grasp image. + image = calculateImage(points_normals.first, points_normals.second); +} + +cv::Mat Image12ChannelsStrategy::calculateImage( + const Eigen::Matrix3Xd &points, const Eigen::Matrix3Xd &normals) const { + double t = omp_get_wtime(); + const int kNumProjections = 3; + + std::vector channels(image_params_.num_channels_); + + Eigen::Matrix3Xd points_proj = points; + int swap_indices[3][2] = {{-1, -1}, {0, 2}, {1, 2}}; + // int swap_indices[3][2] = {{-1, -1}, {0, 2}, {0, 1}}; + + for (size_t i = 0; i < kNumProjections; i++) { + if (i > 0) { + points_proj.row(swap_indices[i][0]) + .swap(points_proj.row(swap_indices[i][1])); + } + + std::vector channels_i = calculateChannels(points_proj, normals); + for (size_t j = 0; j < channels_i.size(); j++) { + channels[i * 4 + j] = channels_i[j]; + } + } + + cv::Mat image; + cv::merge(channels, image); + + t = omp_get_wtime() - t; + // printf("runtime: %3.10f\n", t); + + if (is_plotting_) { + showImage(image); + } + + return image; +} + +std::vector Image12ChannelsStrategy::calculateChannels( + const Eigen::Matrix3Xd &points, const Eigen::Matrix3Xd &normals) const { + std::vector channels(4); + + Eigen::VectorXi cell_indices = findCellIndices(points); + cv::Mat normals_image = createNormalsImage(normals, cell_indices); + std::vector normals_image_channels; + cv::split(normals_image, normals_image_channels); + for (size_t i = 0; i < normals_image.channels(); i++) { + channels[i] = normals_image_channels[i]; + } + + channels[3] = createDepthImage(points, cell_indices); + + return channels; +} + +void Image12ChannelsStrategy::showImage(const cv::Mat &image) const { + int border = 5; + int n = 3; // number of images in each row + int m = 2; // number of images in each column + int image_size = image_params_.size_; + int height = n * (image_size + border) + border; + int width = m * (image_size + border) + border; + cv::Mat image_out(height, width, CV_8UC3, cv::Scalar(0.5)); + std::vector channels; + cv::split(image, channels); + + for (int i = 0; i < n; i++) { + cv::Mat normals_rgb, depth_rgb; + std::vector normals_channels(3); + for (int j = 0; j < normals_channels.size(); j++) { + normals_channels[j] = channels[i * 4 + j]; + } + cv::merge(normals_channels, normals_rgb); + // OpenCV requires images to be in BGR or grayscale to be displayed. + cvtColor(normals_rgb, normals_rgb, cv::COLOR_RGB2BGR); + cvtColor(channels[i * 4 + 3], depth_rgb, cv::COLOR_GRAY2RGB); + normals_rgb.copyTo(image_out(cv::Rect( + border, border + i * (border + image_size), image_size, image_size))); + depth_rgb.copyTo(image_out(cv::Rect(2 * border + image_size, + border + i * (border + image_size), + image_size, image_size))); + } + + cv::namedWindow("Grasp Image (12 channels)", cv::WINDOW_NORMAL); + cv::imshow("Grasp Image (12 channels)", image_out); + cv::waitKey(0); +} + +} // namespace descriptor +} // namespace gpd diff --git a/src/gpd/descriptor/image_15_channels_strategy.cpp b/src/gpd/descriptor/image_15_channels_strategy.cpp new file mode 100644 index 0000000..38bbe03 --- /dev/null +++ b/src/gpd/descriptor/image_15_channels_strategy.cpp @@ -0,0 +1,144 @@ +#include + +namespace gpd { +namespace descriptor { + +std::vector> Image15ChannelsStrategy::createImages( + const candidate::HandSet &hand_set, + const util::PointList &nn_points) const { + const std::vector> &hands = + hand_set.getHands(); + std::vector> images(hands.size()); + + Eigen::Matrix3Xd shadow = hand_set.calculateShadow(nn_points, shadow_length_); + + for (int i = 0; i < hands.size(); i++) { + if (hand_set.getIsValid()(i)) { + images[i] = std::make_unique( + image_params_.size_, image_params_.size_, + CV_8UC(image_params_.num_channels_), cv::Scalar(0.0)); + createImage(nn_points, *hands[i], shadow, *images[i]); + } + } + + return images; +} + +void Image15ChannelsStrategy::createImage(const util::PointList &point_list, + const candidate::Hand &hand, + const Eigen::Matrix3Xd &shadow, + cv::Mat &image) const { + // 1. Transform points and normals in neighborhood into the unit image. + Matrix3XdPair points_normals = transformToUnitImage(point_list, hand); + + // 2. Transform occluded points into hand frame. + Eigen::Matrix3Xd shadow_frame = + shadow - hand.getSample().replicate(1, shadow.cols()); + shadow_frame = hand.getFrame().transpose() * shadow_frame; + std::vector indices = findPointsInUnitImage(hand, shadow_frame); + Eigen::Matrix3Xd cropped_shadow_points = + transformPointsToUnitImage(hand, shadow_frame, indices); + + // 3. Create grasp image. + image = calculateImage(points_normals.first, points_normals.second, + cropped_shadow_points); +} + +cv::Mat Image15ChannelsStrategy::calculateImage( + const Eigen::Matrix3Xd &points, const Eigen::Matrix3Xd &normals, + const Eigen::Matrix3Xd &shadow) const { + double t = omp_get_wtime(); + const int kNumProjections = 3; + + std::vector channels(image_params_.num_channels_); + + Eigen::Matrix3Xd points_proj = points; + Eigen::Matrix3Xd shadow_proj = shadow; + int swap_indices[3][2] = {{-1, -1}, {0, 2}, {1, 2}}; + + for (size_t i = 0; i < kNumProjections; i++) { + if (i > 0) { + points_proj.row(swap_indices[i][0]) + .swap(points_proj.row(swap_indices[i][1])); + shadow_proj.row(swap_indices[i][0]) + .swap(shadow_proj.row(swap_indices[i][1])); + } + std::vector channels_i = + calculateChannels(points_proj, normals, shadow_proj); + for (size_t j = 0; j < channels_i.size(); j++) { + channels[i * 5 + j] = channels_i[j]; + } + } + + cv::Mat image; + cv::merge(channels, image); + + t = omp_get_wtime() - t; + // printf("runtime: %3.10f\n", t); + + if (is_plotting_) { + showImage(image); + } + + return image; +} + +std::vector Image15ChannelsStrategy::calculateChannels( + const Eigen::Matrix3Xd &points, const Eigen::Matrix3Xd &normals, + const Eigen::Matrix3Xd &shadow) const { + std::vector channels(5); + + Eigen::VectorXi cell_indices = findCellIndices(points); + cv::Mat normals_image = createNormalsImage(normals, cell_indices); + std::vector normals_image_channels; + cv::split(normals_image, normals_image_channels); + for (size_t i = 0; i < normals_image.channels(); i++) { + channels[i] = normals_image_channels[i]; + } + + channels[3] = createDepthImage(points, cell_indices); + + cell_indices = findCellIndices(shadow); + channels[4] = createShadowImage(shadow, cell_indices); + + return channels; +} + +void Image15ChannelsStrategy::showImage(const cv::Mat &image) const { + int border = 5; + int n = 3; + int image_size = image_params_.size_; + int total_size = n * (image_size + border) + border; + cv::Mat image_out(total_size, total_size, CV_8UC3, cv::Scalar(0.5)); + std::vector channels; + cv::split(image, channels); + + for (int i = 0; i < n; i++) { + // OpenCV requires images to be in BGR or grayscale to be displayed. + cv::Mat normals_rgb, depth_rgb, shadow_rgb; + std::vector normals_channels(3); + for (int j = 0; j < normals_channels.size(); j++) { + normals_channels[j] = channels[i * 5 + j]; + } + cv::merge(normals_channels, normals_rgb); + // OpenCV requires images to be in BGR or grayscale to be displayed. + cvtColor(normals_rgb, normals_rgb, cv::COLOR_RGB2BGR); + cvtColor(channels[i * 5 + 3], depth_rgb, cv::COLOR_GRAY2RGB); + cvtColor(channels[i * 5 + 4], shadow_rgb, cv::COLOR_GRAY2RGB); + normals_rgb.copyTo(image_out(cv::Rect( + border, border + i * (border + image_size), image_size, image_size))); + depth_rgb.copyTo(image_out(cv::Rect(2 * border + image_size, + border + i * (border + image_size), + image_size, image_size))); + shadow_rgb.copyTo(image_out(cv::Rect(3 * border + 2 * image_size, + border + i * (border + image_size), + image_size, image_size))); + } + + cv::namedWindow("Grasp Image (15 channels)", cv::WINDOW_NORMAL); + cv::imshow("Grasp Image (15 channels)", image_out); + cv::waitKey(0); +} + +} // namespace descriptor +} // namespace gpd diff --git a/src/gpd/descriptor/image_1_channels_strategy.cpp b/src/gpd/descriptor/image_1_channels_strategy.cpp new file mode 100644 index 0000000..956f71c --- /dev/null +++ b/src/gpd/descriptor/image_1_channels_strategy.cpp @@ -0,0 +1,51 @@ +#include + +namespace gpd { +namespace descriptor { + +std::vector> Image1ChannelsStrategy::createImages( + const candidate::HandSet &hand_set, + const util::PointList &nn_points) const { + const std::vector> &hands = + hand_set.getHands(); + std::vector> images(hands.size()); + + for (int i = 0; i < hands.size(); i++) { + if (hand_set.getIsValid()(i)) { + images[i] = std::make_unique( + image_params_.size_, image_params_.size_, + CV_8UC(image_params_.num_channels_), cv::Scalar(0.0)); + createImage(nn_points, *hands[i], *images[i]); + } + } + + return images; +} + +void Image1ChannelsStrategy::createImage(const util::PointList &point_list, + const candidate::Hand &hand, + cv::Mat &image) const { + // 1. Transform points and normals in neighborhood into the unit image. + const Eigen::Matrix3Xd points = point_list.getPoints(); + const Eigen::Matrix3d rot = hand.getFrame().transpose(); + Eigen::Matrix3Xd points_frame = + rot * (points - hand.getSample().replicate(1, points.cols())); + std::vector indices = findPointsInUnitImage(hand, points_frame); + points_frame = transformPointsToUnitImage(hand, points_frame, indices); + + // 2. Calculate grasp image. + Eigen::VectorXi cell_indices = findCellIndices(points_frame); + image = createDepthImage(points_frame, cell_indices); + + if (is_plotting_) { + std::string title = "Grasp Image (1 channel)"; + cv::namedWindow(title, cv::WINDOW_NORMAL); + cv::Mat image_rgb; + cvtColor(image, image_rgb, cv::COLOR_GRAY2RGB); + cv::imshow(title, image_rgb); + cv::waitKey(0); + } +} + +} // namespace descriptor +} // namespace gpd diff --git a/src/gpd/descriptor/image_3_channels_strategy.cpp b/src/gpd/descriptor/image_3_channels_strategy.cpp new file mode 100644 index 0000000..a5d01d8 --- /dev/null +++ b/src/gpd/descriptor/image_3_channels_strategy.cpp @@ -0,0 +1,44 @@ +#include + +namespace gpd { +namespace descriptor { + +std::vector> Image3ChannelsStrategy::createImages( + const candidate::HandSet &hand_set, + const util::PointList &nn_points) const { + const std::vector> &hands = + hand_set.getHands(); + std::vector> images(hands.size()); + + for (int i = 0; i < hands.size(); i++) { + if (hand_set.getIsValid()(i)) { + images[i] = std::make_unique( + image_params_.size_, image_params_.size_, + CV_8UC(image_params_.num_channels_), cv::Scalar(0.0)); + createImage(nn_points, *hands[i], *images[i]); + } + } + + return images; +} + +void Image3ChannelsStrategy::createImage(const util::PointList &point_list, + const candidate::Hand &hand, + cv::Mat &image) const { + // 1. Transform points and normals in neighborhood into the unit image. + Matrix3XdPair points_normals = transformToUnitImage(point_list, hand); + + // 2. Calculate grasp image. + Eigen::VectorXi cell_indices = findCellIndices(points_normals.first); + image = createNormalsImage(points_normals.second, cell_indices); + + if (is_plotting_) { + std::string title = "Grasp Image (3 channels)"; + cv::namedWindow(title, cv::WINDOW_NORMAL); + cv::imshow(title, image); + cv::waitKey(0); + } +} + +} // namespace descriptor +} // namespace gpd diff --git a/src/gpd/descriptor/image_generator.cpp b/src/gpd/descriptor/image_generator.cpp new file mode 100644 index 0000000..bc99b56 --- /dev/null +++ b/src/gpd/descriptor/image_generator.cpp @@ -0,0 +1,132 @@ +#include + +namespace gpd { +namespace descriptor { + +ImageGenerator::ImageGenerator(const descriptor::ImageGeometry &image_geometry, + int num_threads, int num_orientations, + bool is_plotting, bool remove_plane) + : image_params_(image_geometry), + num_threads_(num_threads), + num_orientations_(num_orientations), + remove_plane_(remove_plane) { + image_strategy_ = descriptor::ImageStrategy::makeImageStrategy( + image_geometry, num_threads_, num_orientations_, is_plotting); +} + +void ImageGenerator::createImages( + const util::Cloud &cloud_cam, + const std::vector> &hand_set_list, + std::vector> &images_out, + std::vector> &hands_out) const { + double t0 = omp_get_wtime(); + + Eigen::Matrix3Xd points = + cloud_cam.getCloudProcessed()->getMatrixXfMap().cast().block( + 0, 0, 3, cloud_cam.getCloudProcessed()->points.size()); + util::PointList point_list(points, cloud_cam.getNormals(), + cloud_cam.getCameraSource(), + cloud_cam.getViewPoints()); + + // Segment the support/table plane to speed up shadow computation. + if (remove_plane_) { + removePlane(cloud_cam, point_list); + } + + // Prepare kd-tree for neighborhood searches in the point cloud. + pcl::KdTreeFLANN kdtree; + kdtree.setInputCloud(cloud_cam.getCloudProcessed()); + std::vector nn_indices; + std::vector nn_dists; + + // Set the radius for the neighborhood search to the largest image dimension. + Eigen::Vector3d image_dims; + image_dims << image_params_.depth_, image_params_.height_ / 2.0, + image_params_.outer_diameter_; + double radius = image_dims.maxCoeff(); + + // 1. Find points within image dimensions. + std::vector nn_points_list; + nn_points_list.resize(hand_set_list.size()); + + double t_slice = omp_get_wtime(); + +#ifdef _OPENMP // parallelization using OpenMP +#pragma omp parallel for private(nn_indices, nn_dists) num_threads(num_threads_) +#endif + for (int i = 0; i < hand_set_list.size(); i++) { + pcl::PointXYZRGBA sample_pcl; + sample_pcl.getVector3fMap() = hand_set_list[i]->getSample().cast(); + + if (kdtree.radiusSearch(sample_pcl, radius, nn_indices, nn_dists) > 0) { + nn_points_list[i] = point_list.slice(nn_indices); + } + } + printf("neighborhoods search time: %3.4f\n", omp_get_wtime() - t_slice); + + createImageList(hand_set_list, nn_points_list, images_out, hands_out); + printf("Created %zu images in %3.4fs\n", images_out.size(), + omp_get_wtime() - t0); +} + +void ImageGenerator::createImageList( + const std::vector> &hand_set_list, + const std::vector &nn_points_list, + std::vector> &images_out, + std::vector> &hands_out) const { + double t0_images = omp_get_wtime(); + + int m = hand_set_list[0]->getHands().size(); + int n = hand_set_list.size() * m; + std::vector>> images_list(n); + +#ifdef _OPENMP // parallelization using OpenMP +#pragma omp parallel for num_threads(num_threads_) +#endif + for (int i = 0; i < hand_set_list.size(); i++) { + images_list[i] = + image_strategy_->createImages(*hand_set_list[i], nn_points_list[i]); + } + + for (int i = 0; i < hand_set_list.size(); i++) { + for (int j = 0; j < hand_set_list[i]->getHands().size(); j++) { + if (hand_set_list[i]->getIsValid()(j)) { + images_out.push_back(std::move(images_list[i][j])); + hands_out.push_back(std::move(hand_set_list[i]->getHands()[j])); + } + } + } +} + +void ImageGenerator::removePlane(const util::Cloud &cloud_cam, + util::PointList &point_list) const { + pcl::SACSegmentation seg; + pcl::PointIndices::Ptr inliers(new pcl::PointIndices); + pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients); + seg.setInputCloud(cloud_cam.getCloudProcessed()); + seg.setOptimizeCoefficients(true); + seg.setModelType(pcl::SACMODEL_PLANE); + seg.setMethodType(pcl::SAC_RANSAC); + seg.setDistanceThreshold(0.01); + seg.segment(*inliers, *coefficients); + if (inliers->indices.size() > 0) { + pcl::ExtractIndices extract; + extract.setInputCloud(cloud_cam.getCloudProcessed()); + extract.setIndices(inliers); + extract.setNegative(true); + std::vector indices; + extract.filter(indices); + if (indices.size() > 0) { + PointCloudRGBA::Ptr cloud(new PointCloudRGBA); + extract.filter(*cloud); + point_list = point_list.slice(indices); + printf("Removed plane from point cloud. %zu points remaining.\n", + cloud->size()); + } else { + printf("Plane fit failed. Using entire point cloud ...\n"); + } + } +} + +} // namespace descriptor +} // namespace gpd diff --git a/src/gpd/descriptor/image_geometry.cpp b/src/gpd/descriptor/image_geometry.cpp new file mode 100644 index 0000000..337e946 --- /dev/null +++ b/src/gpd/descriptor/image_geometry.cpp @@ -0,0 +1,45 @@ +#include + +namespace gpd { +namespace descriptor { + +ImageGeometry::ImageGeometry() + : outer_diameter_(0.0), + depth_(0.0), + height_(0.0), + size_(0), + num_channels_(0) {} + +ImageGeometry::ImageGeometry(double outer_diameter, double depth, double height, + int size, int num_channels) + : outer_diameter_(outer_diameter), + depth_(depth), + height_(height), + size_(size), + num_channels_(num_channels) {} + +ImageGeometry::ImageGeometry(const std::string &filepath) { + util::ConfigFile config_file(filepath); + config_file.ExtractKeys(); + outer_diameter_ = config_file.getValueOfKey("volume_width", 0.10); + depth_ = config_file.getValueOfKey("volume_depth", 0.06); + height_ = config_file.getValueOfKey("volume_height", 0.02); + size_ = config_file.getValueOfKey("image_size", 60); + num_channels_ = config_file.getValueOfKey("image_num_channels", 15); +} + +std::ostream &operator<<(std::ostream &stream, + const ImageGeometry &image_geometry) { + stream << "============ GRASP IMAGE GEOMETRY ===============\n"; + stream << "volume width: " << image_geometry.outer_diameter_ << "\n"; + stream << "volume depth: " << image_geometry.depth_ << "\n"; + stream << "volume height: " << image_geometry.height_ << "\n"; + stream << "image_size: " << image_geometry.size_ << "\n"; + stream << "image_num_channels: " << image_geometry.num_channels_ << "\n"; + stream << "=================================================\n"; + + return stream; +} + +} // namespace descriptor +} // namespace gpd diff --git a/src/gpd/descriptor/image_strategy.cpp b/src/gpd/descriptor/image_strategy.cpp new file mode 100644 index 0000000..fd8cb5b --- /dev/null +++ b/src/gpd/descriptor/image_strategy.cpp @@ -0,0 +1,246 @@ +#include + +#include +#include +#include +#include + +namespace gpd { +namespace descriptor { + +std::unique_ptr ImageStrategy::makeImageStrategy( + const ImageGeometry &image_params, int num_threads, int num_orientations, + bool is_plotting) { + std::unique_ptr strategy; + if (image_params.num_channels_ == 1) { + strategy = std::make_unique( + image_params, num_threads, num_orientations, is_plotting); + } else if (image_params.num_channels_ == 3) { + strategy = std::make_unique( + image_params, num_threads, num_orientations, is_plotting); + } else if (image_params.num_channels_ == 12) { + strategy = std::make_unique( + image_params, num_threads, num_orientations, is_plotting); + } else if (image_params.num_channels_ == 15) { + strategy = std::make_unique( + image_params, num_threads, num_orientations, is_plotting); + } + + return strategy; +} + +Matrix3XdPair ImageStrategy::transformToUnitImage( + const util::PointList &point_list, const candidate::Hand &hand) const { + // 1. Transform points and normals in neighborhood into the hand frame. + const Eigen::Matrix3Xd rotation = hand.getFrame().transpose(); + const Eigen::Vector3d &sample = hand.getSample(); + Matrix3XdPair points_normals( + rotation * + (point_list.getPoints() - sample.replicate(1, point_list.size())), + rotation * point_list.getNormals()); + + // 2. Find points in unit image. + const std::vector indices = + findPointsInUnitImage(hand, points_normals.first); + points_normals.first = + transformPointsToUnitImage(hand, points_normals.first, indices); + points_normals.second = + util::EigenUtils::sliceMatrix(points_normals.second, indices); + + return points_normals; +} + +std::vector ImageStrategy::findPointsInUnitImage( + const candidate::Hand &hand, const Eigen::Matrix3Xd &points) const { + std::vector indices; + const double half_outer_diameter = image_params_.outer_diameter_ / 2.0; + + for (int i = 0; i < points.cols(); i++) { + if ((points(0, i) > hand.getBottom()) && + (points(0, i) < hand.getBottom() + image_params_.depth_) && + (points(1, i) > hand.getCenter() - half_outer_diameter) && + (points(1, i) < hand.getCenter() + half_outer_diameter) && + (points(2, i) > -1.0 * image_params_.height_) && + (points(2, i) < image_params_.height_)) { + indices.push_back(i); + } + } + + return indices; +} + +Eigen::Matrix3Xd ImageStrategy::transformPointsToUnitImage( + const candidate::Hand &hand, const Eigen::Matrix3Xd &points, + const std::vector &indices) const { + Eigen::Matrix3Xd points_out(3, indices.size()); + const double half_outer_diameter = image_params_.outer_diameter_ / 2.0; + const double double_height = 2.0 * image_params_.height_; + + for (int i = 0; i < indices.size(); i++) { + points_out(0, i) = + (points(0, indices[i]) - hand.getBottom()) / image_params_.depth_; + points_out(1, i) = + (points(1, indices[i]) - (hand.getCenter() - half_outer_diameter)) / + image_params_.outer_diameter_; + points_out(2, i) = + (points(2, indices[i]) + image_params_.height_) / double_height; + } + + return points_out; +} + +Eigen::VectorXi ImageStrategy::findCellIndices( + const Eigen::Matrix3Xd &points) const { + double cellsize = 1.0 / (double)image_params_.size_; + const Eigen::VectorXi vertical_cells = + (floorVector(points.row(0) / cellsize)).cwiseMin(image_params_.size_ - 1); + const Eigen::VectorXi horizontal_cells = + (floorVector(points.row(1) / cellsize)).cwiseMin(image_params_.size_ - 1); + Eigen::VectorXi cell_indices = + horizontal_cells + vertical_cells * image_params_.size_; + return cell_indices; +} + +cv::Mat ImageStrategy::createBinaryImage( + const Eigen::VectorXi &cell_indices) const { + cv::Mat image(image_params_.size_, image_params_.size_, CV_8UC1, + cv::Scalar(0)); + + for (int i = 0; i < cell_indices.rows(); i++) { + const int &idx = cell_indices[i]; + int row = image.rows - 1 - idx / image.cols; + int col = idx % image.cols; + image.at(row, col) = 255; + } + + // Dilate the image to fill in holes. + cv::Mat dilation_element = + cv::getStructuringElement(cv::MORPH_RECT, cv::Size(3, 3)); + dilate(image, image, dilation_element); + + return image; +} + +cv::Mat ImageStrategy::createNormalsImage( + const Eigen::Matrix3Xd &normals, + const Eigen::VectorXi &cell_indices) const { + cv::Mat image(image_params_.size_, image_params_.size_, CV_32FC3, + cv::Scalar(0.0)); + + for (int i = 0; i < cell_indices.rows(); i++) { + const int &idx = cell_indices[i]; + int row = image_params_.size_ - 1 - idx / image_params_.size_; + int col = idx % image_params_.size_; + cv::Vec3f &v = image.at(row, col); + const Eigen::Vector3d &n = normals.col(i); + if (v(0) == 0 && v(1) == 0 && v(2) == 0) { + v = cv::Vec3f(fabs(n(0)), fabs(n(1)), fabs(n(2))); + } else { + v += (cv::Vec3f(fabs(n(0)), fabs(n(1)), fabs(n(2))) - v) * + (1.0 / sqrt(v(0) * v(0) + v(1) * v(1) + v(2) * v(2))); + } + } + + // Dilate the image to fill in holes. + cv::Mat dilation_element = + cv::getStructuringElement(cv::MORPH_RECT, cv::Size(3, 3)); + dilate(image, image, dilation_element); + + // Normalize the image to the range [0,1]. + cv::normalize(image, image, 0.0, 1.0, cv::NORM_MINMAX, CV_32FC3); + + // Convert float image to uchar image, required by Caffe. + image.convertTo(image, CV_8U, 255.0); + + return image; +} + +cv::Mat ImageStrategy::createDepthImage( + const Eigen::Matrix3Xd &points, const Eigen::VectorXi &cell_indices) const { + cv::Mat image(image_params_.size_, image_params_.size_, CV_32FC1, + cv::Scalar(0.0)); + float avgs[image_params_.size_ * image_params_.size_] = {0}; + float counts[image_params_.size_ * image_params_.size_] = {0}; + + // double t0 = omp_get_wtime(); + for (int i = 0; i < cell_indices.rows(); i++) { + const int &idx = cell_indices[i]; + int row = image.rows - 1 - idx / image.cols; + int col = idx % image.cols; + counts[idx] += 1.0; + avgs[idx] += (points(2, i) - avgs[idx]) * (1.0 / counts[idx]); + float &v = image.at(row, col); + v = 1.0 - avgs[idx]; + } + // printf("average calc runtime: %3.8f\n", omp_get_wtime() - t0); + // t0 = omp_get_wtime(); + + // Dilate the image to fill in holes. + cv::Mat dilation_element = + cv::getStructuringElement(cv::MORPH_RECT, cv::Size(3, 3)); + dilate(image, image, dilation_element); + + // Normalize the image to the range [0,1]. + cv::normalize(image, image, 0.0, 1.0, cv::NORM_MINMAX, CV_32FC1); + + // Convert float image to uchar image, required by Caffe. + image.convertTo(image, CV_8U, 255.0); + // printf("image ops runtime: %3.8f\n", omp_get_wtime() - t0); + + return image; +} + +cv::Mat ImageStrategy::createShadowImage( + const Eigen::Matrix3Xd &points, const Eigen::VectorXi &cell_indices) const { + // Calculate average depth image. + cv::Mat image(image_params_.size_, image_params_.size_, CV_32FC1, + cv::Scalar(0.0)); + cv::Mat nonzero(image_params_.size_, image_params_.size_, CV_8UC1, + cv::Scalar(0)); + float counts[image_params_.size_ * image_params_.size_] = {0}; + + for (int i = 0; i < cell_indices.rows(); i++) { + const int &idx = cell_indices[i]; + int row = image.rows - 1 - idx / image.cols; + int col = idx % image.cols; + counts[idx] += 1.0; + image.at(row, col) += + (points(2, i) - image.at(row, col)) * (1.0 / counts[idx]); + nonzero.at(row, col) = 1; + } + + // Reverse depth so that closest points have largest value. + double min, max; + cv::Point min_loc, max_loc; + cv::minMaxLoc(image, &min, &max, &min_loc, &max_loc, nonzero); + cv::Mat max_img(image_params_.size_, image_params_.size_, CV_32FC1, + cv::Scalar(0.0)); + max_img.setTo(max, nonzero); + image = max_img - image; + + // Dilate the image to fill in holes. + cv::Mat dilation_element = + cv::getStructuringElement(cv::MORPH_RECT, cv::Size(3, 3)); + dilate(image, image, dilation_element); + + // Normalize the image to the range [0,1]. + cv::normalize(image, image, 0.0, 1.0, cv::NORM_MINMAX, CV_32FC1); + + // Convert float image to uchar image, required by Caffe. + image.convertTo(image, CV_8U, 255.0); + + return image; +} + +Eigen::VectorXi ImageStrategy::floorVector(const Eigen::VectorXd &a) const { + Eigen::VectorXi b(a.size()); + + for (int i = 0; i < b.size(); i++) { + b(i) = floor((double)a(i)); + } + + return b; +} + +} // namespace descriptor +} // namespace gpd diff --git a/src/gpd/grasp_detector.cpp b/src/gpd/grasp_detector.cpp index ee5b2b8..764da65 100644 --- a/src/gpd/grasp_detector.cpp +++ b/src/gpd/grasp_detector.cpp @@ -1,474 +1,572 @@ -#include "../../include/gpd/grasp_detector.h" +#include +namespace gpd { -GraspDetector::GraspDetector(ros::NodeHandle& node) -{ +GraspDetector::GraspDetector(const std::string &config_filename) { Eigen::initParallel(); - // Create objects to store parameters. - CandidatesGenerator::Parameters generator_params; - HandSearch::Parameters hand_search_params; + // Read parameters from configuration file. + util::ConfigFile config_file(config_filename); + config_file.ExtractKeys(); // Read hand geometry parameters. - node.param("finger_width", hand_search_params.finger_width_, 0.01); - node.param("hand_outer_diameter", hand_search_params.hand_outer_diameter_, 0.09); - node.param("hand_depth", hand_search_params.hand_depth_, 0.06); - node.param("hand_height", hand_search_params.hand_height_, 0.02); - node.param("init_bite", hand_search_params.init_bite_, 0.015); - outer_diameter_ = hand_search_params.hand_outer_diameter_; - - // Read local hand search parameters. - node.param("nn_radius", hand_search_params.nn_radius_frames_, 0.01); - node.param("num_orientations", hand_search_params.num_orientations_, 8); - node.param("num_samples", hand_search_params.num_samples_, 500); - node.param("num_threads", hand_search_params.num_threads_, 1); - node.param("rotation_axis", hand_search_params.rotation_axis_, 2); // cannot be changed + std::string hand_geometry_filename = + config_file.getValueOfKeyAsString("hand_geometry_filename", ""); + if (hand_geometry_filename == "0") { + hand_geometry_filename = config_filename; + } + candidate::HandGeometry hand_geom(hand_geometry_filename); + std::cout << hand_geom; // Read plotting parameters. - node.param("plot_samples", plot_samples_, false); - node.param("plot_normals", plot_normals_, false); - generator_params.plot_normals_ = plot_normals_; - node.param("plot_filtered_grasps", plot_filtered_grasps_, false); - node.param("plot_valid_grasps", plot_valid_grasps_, false); - node.param("plot_clusters", plot_clusters_, false); - node.param("plot_selected_grasps", plot_selected_grasps_, false); - - // Read general parameters. - generator_params.num_samples_ = hand_search_params.num_samples_; - generator_params.num_threads_ = hand_search_params.num_threads_; - node.param("plot_candidates", generator_params.plot_grasps_, false); - - // Read preprocessing parameters. - node.param("remove_outliers", generator_params.remove_statistical_outliers_, true); - node.param("voxelize", generator_params.voxelize_, true); - node.getParam("workspace", generator_params.workspace_); - node.getParam("workspace_grasps", workspace_); + plot_normals_ = config_file.getValueOfKey("plot_normals", false); + plot_samples_ = config_file.getValueOfKey("plot_samples", true); + plot_candidates_ = config_file.getValueOfKey("plot_candidates", false); + plot_filtered_candidates_ = + config_file.getValueOfKey("plot_filtered_candidates", false); + plot_valid_grasps_ = + config_file.getValueOfKey("plot_valid_grasps", false); + plot_clustered_grasps_ = + config_file.getValueOfKey("plot_clustered_grasps", false); + plot_selected_grasps_ = + config_file.getValueOfKey("plot_selected_grasps", false); + printf("============ PLOTTING ========================\n"); + printf("plot_normals: %s\n", plot_normals_ ? "true" : "false"); + printf("plot_samples %s\n", plot_samples_ ? "true" : "false"); + printf("plot_candidates: %s\n", plot_candidates_ ? "true" : "false"); + printf("plot_filtered_candidates: %s\n", + plot_filtered_candidates_ ? "true" : "false"); + printf("plot_valid_grasps: %s\n", plot_valid_grasps_ ? "true" : "false"); + printf("plot_clustered_grasps: %s\n", + plot_clustered_grasps_ ? "true" : "false"); + printf("plot_selected_grasps: %s\n", + plot_selected_grasps_ ? "true" : "false"); + printf("==============================================\n"); // Create object to generate grasp candidates. - candidates_generator_ = new CandidatesGenerator(generator_params, hand_search_params); + candidate::CandidatesGenerator::Parameters generator_params; + generator_params.num_samples_ = + config_file.getValueOfKey("num_samples", 1000); + generator_params.num_threads_ = + config_file.getValueOfKey("num_threads", 1); + generator_params.remove_statistical_outliers_ = + config_file.getValueOfKey("remove_outliers", false); + generator_params.sample_above_plane_ = + config_file.getValueOfKey("sample_above_plane", false); + generator_params.voxelize_ = + config_file.getValueOfKey("voxelize", true); + generator_params.voxel_size_ = + config_file.getValueOfKey("voxel_size", 0.003); + generator_params.normals_radius_ = + config_file.getValueOfKey("normals_radius", 0.03); + generator_params.refine_normals_k_ = + config_file.getValueOfKey("refine_normals_k", 0); + generator_params.workspace_ = + config_file.getValueOfKeyAsStdVectorDouble("workspace", "-1 1 -1 1 -1 1"); + + candidate::HandSearch::Parameters hand_search_params; + hand_search_params.hand_geometry_ = hand_geom; + hand_search_params.nn_radius_frames_ = + config_file.getValueOfKey("nn_radius", 0.01); + hand_search_params.num_samples_ = + config_file.getValueOfKey("num_samples", 1000); + hand_search_params.num_threads_ = + config_file.getValueOfKey("num_threads", 1); + hand_search_params.num_orientations_ = + config_file.getValueOfKey("num_orientations", 8); + hand_search_params.num_finger_placements_ = + config_file.getValueOfKey("num_finger_placements", 10); + hand_search_params.deepen_hand_ = + config_file.getValueOfKey("deepen_hand", true); + hand_search_params.hand_axes_ = + config_file.getValueOfKeyAsStdVectorInt("hand_axes", "2"); + hand_search_params.friction_coeff_ = + config_file.getValueOfKey("friction_coeff", 20.0); + hand_search_params.min_viable_ = + config_file.getValueOfKey("min_viable", 6); + candidates_generator_ = std::make_unique( + generator_params, hand_search_params); + + printf("============ CLOUD PREPROCESSING =============\n"); + printf("voxelize: %s\n", generator_params.voxelize_ ? "true" : "false"); + printf("voxel_size: %.3f\n", generator_params.voxel_size_); + printf("remove_outliers: %s\n", + generator_params.remove_statistical_outliers_ ? "true" : "false"); + printStdVector(generator_params.workspace_, "workspace"); + printf("sample_above_plane: %s\n", + generator_params.sample_above_plane_ ? "true" : "false"); + printf("normals_radius: %.3f\n", generator_params.normals_radius_); + printf("refine_normals_k: %d\n", generator_params.refine_normals_k_); + printf("==============================================\n"); + + printf("============ CANDIDATE GENERATION ============\n"); + printf("num_samples: %d\n", hand_search_params.num_samples_); + printf("num_threads: %d\n", hand_search_params.num_threads_); + printf("nn_radius: %3.2f\n", hand_search_params.nn_radius_frames_); + printStdVector(hand_search_params.hand_axes_, "hand axes"); + printf("num_orientations: %d\n", hand_search_params.num_orientations_); + printf("num_finger_placements: %d\n", + hand_search_params.num_finger_placements_); + printf("deepen_hand: %s\n", + hand_search_params.deepen_hand_ ? "true" : "false"); + printf("friction_coeff: %3.2f\n", hand_search_params.friction_coeff_); + printf("min_viable: %d\n", hand_search_params.min_viable_); + printf("==============================================\n"); + + // TODO: Set the camera position. + // Eigen::Matrix3Xd view_points(3,1); + // view_points << camera_position[0], camera_position[1], camera_position[2]; + + // Read grasp image parameters. + std::string image_geometry_filename = + config_file.getValueOfKeyAsString("image_geometry_filename", ""); + if (image_geometry_filename == "0") { + image_geometry_filename = config_filename; + } + descriptor::ImageGeometry image_geom(image_geometry_filename); + std::cout << image_geom; // Read classification parameters and create classifier. - std::string model_file, weights_file; - int device; - node.param("model_file", model_file, std::string("")); - node.param("trained_file", weights_file, std::string("")); - node.param("min_score_diff", min_score_diff_, 500.0); - node.param("create_image_batches", create_image_batches_, true); - node.param("device", device, 0); - classifier_ = Classifier::create(model_file, weights_file, static_cast(device)); + std::string model_file = config_file.getValueOfKeyAsString("model_file", ""); + std::string weights_file = + config_file.getValueOfKeyAsString("weights_file", ""); + if (!model_file.empty() || !weights_file.empty()) { + int device = config_file.getValueOfKey("device", 0); + int batch_size = config_file.getValueOfKey("batch_size", 1); + classifier_ = net::Classifier::create( + model_file, weights_file, static_cast(device), + batch_size); + min_score_ = config_file.getValueOfKey("min_score", 0); + printf("============ CLASSIFIER ======================\n"); + printf("model_file: %s\n", model_file.c_str()); + printf("weights_file: %s\n", weights_file.c_str()); + printf("batch_size: %d\n", batch_size); + printf("==============================================\n"); + } - // Read grasp image parameters. - node.param("image_outer_diameter", image_params_.outer_diameter_, hand_search_params.hand_outer_diameter_); - node.param("image_depth", image_params_.depth_, hand_search_params.hand_depth_); - node.param("image_height", image_params_.height_, hand_search_params.hand_height_); - node.param("image_size", image_params_.size_, 60); - node.param("image_num_channels", image_params_.num_channels_, 15); - - // Read learning parameters. - bool remove_plane; - node.param("remove_plane_before_image_calculation", remove_plane, false); - - // Create object to create grasp images from grasp candidates (used for classification) - learning_ = new Learning(image_params_, hand_search_params.num_threads_, hand_search_params.num_orientations_, false, remove_plane); - - // Read grasp filtering parameters - node.param("filter_grasps", filter_grasps_, false); - node.param("filter_half_antipodal", filter_half_antipodal_, false); - std::vector gripper_width_range(2); - gripper_width_range[0] = 0.03; - gripper_width_range[1] = 0.07; - node.getParam("gripper_width_range", gripper_width_range); - min_aperture_ = gripper_width_range[0]; - max_aperture_ = gripper_width_range[1]; - - // Read clustering parameters - int min_inliers; - node.param("min_inliers", min_inliers, 0); - clustering_ = new Clustering(min_inliers); + // Read additional grasp image creation parameters. + bool remove_plane = config_file.getValueOfKey( + "remove_plane_before_image_calculation", false); + + // Create object to create grasp images from grasp candidates (used for + // classification). + image_generator_ = std::make_unique( + image_geom, hand_search_params.num_threads_, + hand_search_params.num_orientations_, false, remove_plane); + + // Read grasp filtering parameters based on robot workspace and gripper width. + workspace_grasps_ = config_file.getValueOfKeyAsStdVectorDouble( + "workspace_grasps", "-1 1 -1 1 -1 1"); + min_aperture_ = config_file.getValueOfKey("min_aperture", 0.0); + max_aperture_ = config_file.getValueOfKey("max_aperture", 0.085); + printf("============ CANDIDATE FILTERING =============\n"); + printStdVector(workspace_grasps_, "candidate_workspace"); + printf("min_aperture: %3.4f\n", min_aperture_); + printf("max_aperture: %3.4f\n", max_aperture_); + printf("==============================================\n"); + + // Read grasp filtering parameters based on approach direction. + filter_approach_direction_ = + config_file.getValueOfKey("filter_approach_direction", false); + std::vector approach = + config_file.getValueOfKeyAsStdVectorDouble("direction", "1 0 0"); + direction_ << approach[0], approach[1], approach[2]; + thresh_rad_ = config_file.getValueOfKey("thresh_rad", 2.3); + + // Read clustering parameters. + int min_inliers = config_file.getValueOfKey("min_inliers", 1); + clustering_ = std::make_unique(min_inliers); cluster_grasps_ = min_inliers > 0 ? true : false; + printf("============ CLUSTERING ======================\n"); + printf("min_inliers: %d\n", min_inliers); + printf("==============================================\n\n"); + + // Read grasp selection parameters. + num_selected_ = config_file.getValueOfKey("num_selected", 100); - // Read grasp selection parameters - node.param("num_selected", num_selected_, 100); + // Create plotter. + plotter_ = std::make_unique(hand_search_params.hand_axes_.size(), + hand_search_params.num_orientations_); } +std::vector> GraspDetector::detectGrasps( + const util::Cloud &cloud) { + double t0_total = omp_get_wtime(); + std::vector> hands_out; -std::vector GraspDetector::detectGrasps(const CloudCamera& cloud_cam) -{ - std::vector selected_grasps(0); + const candidate::HandGeometry &hand_geom = + candidates_generator_->getHandSearchParams().hand_geometry_; // Check if the point cloud is empty. - if (cloud_cam.getCloudOriginal()->size() == 0) - { - ROS_INFO("Point cloud is empty!"); - return selected_grasps; + if (cloud.getCloudOriginal()->size() == 0) { + printf("ERROR: Point cloud is empty!"); + hands_out.resize(0); + return hands_out; } - Plot plotter; - // Plot samples/indices. - if (plot_samples_) - { - if (cloud_cam.getSamples().cols() > 0) - { - plotter.plotSamples(cloud_cam.getSamples(), cloud_cam.getCloudProcessed()); - } - else if (cloud_cam.getSampleIndices().size() > 0) - { - plotter.plotSamples(cloud_cam.getSampleIndices(), cloud_cam.getCloudProcessed()); + if (plot_samples_) { + if (cloud.getSamples().cols() > 0) { + plotter_->plotSamples(cloud.getSamples(), cloud.getCloudProcessed()); + } else if (cloud.getSampleIndices().size() > 0) { + plotter_->plotSamples(cloud.getSampleIndices(), + cloud.getCloudProcessed()); } } - if (plot_normals_) - { + if (plot_normals_) { std::cout << "Plotting normals for different camera sources\n"; - plotter.plotNormals(cloud_cam); + plotter_->plotNormals(cloud); } // 1. Generate grasp candidates. - std::vector candidates = generateGraspCandidates(cloud_cam); - ROS_INFO_STREAM("Generated " << candidates.size() << " grasp candidate sets."); - if (candidates.size() == 0) - { - return selected_grasps; + double t0_candidates = omp_get_wtime(); + std::vector> hand_set_list = + candidates_generator_->generateGraspCandidateSets(cloud); + printf("Generated %zu hand sets.\n", hand_set_list.size()); + if (hand_set_list.size() == 0) { + return hands_out; } - - // 2.1 Prune grasp candidates based on min. and max. robot hand aperture and fingers below table surface. - if (filter_grasps_) - { - candidates = filterGraspsWorkspace(candidates, workspace_); - - if (plot_filtered_grasps_) - { - const HandSearch::Parameters& params = candidates_generator_->getHandSearchParams(); - plotter.plotFingers3D(candidates, cloud_cam.getCloudOriginal(), "Valid Grasps", params.hand_outer_diameter_, - params.finger_width_, params.hand_depth_, params.hand_height_); - } + double t_candidates = omp_get_wtime() - t0_candidates; + if (plot_candidates_) { + plotter_->plotFingers3D(hand_set_list, cloud.getCloudOriginal(), + "Grasp candidates", hand_geom); } - // 2.2 Filter half grasps. - if (filter_half_antipodal_) - { - candidates = filterHalfAntipodal(candidates); - - if (plot_filtered_grasps_) - { - const HandSearch::Parameters& params = candidates_generator_->getHandSearchParams(); - plotter.plotFingers3D(candidates, cloud_cam.getCloudOriginal(), "Valid Grasps", params.hand_outer_diameter_, - params.finger_width_, params.hand_depth_, params.hand_height_); - } + // 2. Filter the candidates. + double t0_filter = omp_get_wtime(); + std::vector> hand_set_list_filtered = + filterGraspsWorkspace(hand_set_list, workspace_grasps_); + if (hand_set_list_filtered.size() == 0) { + return hands_out; } - - // 3. Classify each grasp candidate. (Note: switch from a list of hypothesis sets to a list of grasp hypotheses) - std::vector valid_grasps = classifyGraspCandidates(cloud_cam, candidates); - ROS_INFO_STREAM("Predicted " << valid_grasps.size() << " valid grasps."); - - if (valid_grasps.size() <= 2) - { - std::cout << "Not enough valid grasps predicted! Using all grasps from previous step.\n"; - valid_grasps = extractHypotheses(candidates); + if (plot_filtered_candidates_) { + plotter_->plotFingers3D(hand_set_list_filtered, cloud.getCloudOriginal(), + "Filtered Grasps (Aperture, Workspace)", hand_geom); } - - // 4. Cluster the grasps. - std::vector clustered_grasps; - - if (cluster_grasps_) - { - clustered_grasps = findClusters(valid_grasps); - ROS_INFO_STREAM("Found " << clustered_grasps.size() << " clusters."); - if (clustered_grasps.size() <= 3) - { - std::cout << "Not enough clusters found! Using all grasps from previous step.\n"; - clustered_grasps = valid_grasps; - } - - if (plot_clusters_) - { - const HandSearch::Parameters& params = candidates_generator_->getHandSearchParams(); - plotter.plotFingers3D(clustered_grasps, cloud_cam.getCloudOriginal(), "Valid Grasps", params.hand_outer_diameter_, - params.finger_width_, params.hand_depth_, params.hand_height_); + if (filter_approach_direction_) { + hand_set_list_filtered = + filterGraspsDirection(hand_set_list_filtered, direction_, thresh_rad_); + if (plot_filtered_candidates_) { + plotter_->plotFingers3D(hand_set_list_filtered, cloud.getCloudOriginal(), + "Filtered Grasps (Approach)", hand_geom); } } - else - { - clustered_grasps = valid_grasps; + double t_filter = omp_get_wtime() - t0_filter; + if (hand_set_list_filtered.size() == 0) { + return hands_out; } - // 5. Select highest-scoring grasps. - if (clustered_grasps.size() > num_selected_) - { - std::cout << "Partial Sorting the grasps based on their score ... \n"; - std::partial_sort(clustered_grasps.begin(), clustered_grasps.begin() + num_selected_, clustered_grasps.end(), - isScoreGreater); - selected_grasps.assign(clustered_grasps.begin(), clustered_grasps.begin() + num_selected_); - } - else - { - std::cout << "Sorting the grasps based on their score ... \n"; - std::sort(clustered_grasps.begin(), clustered_grasps.end(), isScoreGreater); - selected_grasps = clustered_grasps; + // 3. Create grasp descriptors (images). + double t0_images = omp_get_wtime(); + std::vector> hands; + std::vector> images; + image_generator_->createImages(cloud, hand_set_list_filtered, images, hands); + double t_images = omp_get_wtime() - t0_images; + + // 4. Classify the grasp candidates. + double t0_classify = omp_get_wtime(); + std::vector scores = classifier_->classifyImages(images); + for (int i = 0; i < hands.size(); i++) { + hands[i]->setScore(scores[i]); } + double t_classify = omp_get_wtime() - t0_classify; - for (int i = 0; i < selected_grasps.size(); i++) - { - std::cout << "Grasp " << i << ": " << selected_grasps[i].getScore() << "\n"; + // 5. Select the highest scoring grasps. + hands = selectGrasps(hands); + if (plot_valid_grasps_) { + plotter_->plotFingers3D(hands, cloud.getCloudOriginal(), "Valid Grasps", + hand_geom); } - ROS_INFO_STREAM("Selected the " << selected_grasps.size() << " highest scoring grasps."); + // 6. Cluster the grasps. + double t0_cluster = omp_get_wtime(); + std::vector> clusters; + if (cluster_grasps_) { + clusters = clustering_->findClusters(hands); + printf("Found %d clusters.\n", (int)clusters.size()); + if (clusters.size() <= 3) { + printf( + "Not enough clusters found! Adding all grasps from previous step."); + for (int i = 0; i < hands.size(); i++) { + clusters.push_back(std::move(hands[i])); + } + } + if (plot_clustered_grasps_) { + plotter_->plotFingers3D(clusters, cloud.getCloudOriginal(), + "Clustered Grasps", hand_geom); + } + } else { + clusters = std::move(hands); + } + double t_cluster = omp_get_wtime() - t0_cluster; - if (plot_selected_grasps_) - { - const HandSearch::Parameters& params = candidates_generator_->getHandSearchParams(); - plotter.plotFingers3D(selected_grasps, cloud_cam.getCloudOriginal(), "Valid Grasps", params.hand_outer_diameter_, - params.finger_width_, params.hand_depth_, params.hand_height_); + // 7. Sort grasps by their score. + std::sort(clusters.begin(), clusters.end(), isScoreGreater); + printf("======== Selected grasps ========\n"); + for (int i = 0; i < clusters.size(); i++) { + std::cout << "Grasp " << i << ": " << clusters[i]->getScore() << "\n"; + } + printf("Selected the %d best grasps.\n", (int)clusters.size()); + double t_total = omp_get_wtime() - t0_total; + + printf("======== RUNTIMES ========\n"); + printf(" 1. Candidate generation: %3.4fs\n", t_candidates); + printf(" 2. Descriptor extraction: %3.4fs\n", t_images); + printf(" 3. Classification: %3.4fs\n", t_classify); + // printf(" Filtering: %3.4fs\n", t_filter); + // printf(" Clustering: %3.4fs\n", t_cluster); + printf("==========\n"); + printf(" TOTAL: %3.4fs\n", t_total); + + if (plot_selected_grasps_) { + plotter_->plotFingers3D(clusters, cloud.getCloudOriginal(), + "Selected Grasps", hand_geom, false); } - return selected_grasps; + return clusters; } - -std::vector GraspDetector::generateGraspCandidates(const CloudCamera& cloud_cam) -{ - return candidates_generator_->generateGraspCandidateSets(cloud_cam); +void GraspDetector::preprocessPointCloud(util::Cloud &cloud) { + candidates_generator_->preprocessPointCloud(cloud); } +std::vector> +GraspDetector::filterGraspsWorkspace( + std::vector> &hand_set_list, + const std::vector &workspace) const { + int remaining = 0; + std::vector> hand_set_list_out; + printf("Filtering grasps outside of workspace ...\n"); -void GraspDetector::preprocessPointCloud(CloudCamera& cloud_cam) -{ - candidates_generator_->preprocessPointCloud(cloud_cam); -} + const candidate::HandGeometry &hand_geometry = + candidates_generator_->getHandSearchParams().hand_geometry_; + for (int i = 0; i < hand_set_list.size(); i++) { + const std::vector> &hands = + hand_set_list[i]->getHands(); + Eigen::Array is_valid = + hand_set_list[i]->getIsValid(); -std::vector GraspDetector::classifyGraspCandidates(const CloudCamera& cloud_cam, - std::vector& candidates) -{ - // Create a grasp image for each grasp candidate. - double t0 = omp_get_wtime(); - std::cout << "Creating grasp images for classifier input ...\n"; - std::vector scores; - std::vector grasp_list; - int num_orientations = candidates[0].getHypotheses().size(); - - // Create images in batches if required (less memory usage). - if (create_image_batches_) - { - int batch_size = classifier_->getBatchSize(); - int num_iterations = (int) ceil(candidates.size() * num_orientations / (double) batch_size); - int step_size = (int) floor(batch_size / (double) num_orientations); - std::cout << " num_iterations: " << num_iterations << ", step_size: " << step_size << "\n"; - - // Process the grasp candidates in batches. - for (int i = 0; i < num_iterations; i++) - { - std::cout << i << "\n"; - std::vector::iterator start = candidates.begin() + i * step_size; - std::vector::iterator stop; - if (i < num_iterations - 1) - { - stop = candidates.begin() + i * step_size + step_size; + for (int j = 0; j < hands.size(); j++) { + if (!is_valid(j)) { + continue; } - else - { - stop = candidates.end(); + double half_width = 0.5 * hand_geometry.outer_diameter_; + Eigen::Vector3d left_bottom = + hands[j]->getPosition() + half_width * hands[j]->getBinormal(); + Eigen::Vector3d right_bottom = + hands[j]->getPosition() - half_width * hands[j]->getBinormal(); + Eigen::Vector3d left_top = + left_bottom + hand_geometry.depth_ * hands[j]->getApproach(); + Eigen::Vector3d right_top = + left_bottom + hand_geometry.depth_ * hands[j]->getApproach(); + Eigen::Vector3d approach = + hands[j]->getPosition() - 0.05 * hands[j]->getApproach(); + Eigen::VectorXd x(5), y(5), z(5); + x << left_bottom(0), right_bottom(0), left_top(0), right_top(0), + approach(0); + y << left_bottom(1), right_bottom(1), left_top(1), right_top(1), + approach(1); + z << left_bottom(2), right_bottom(2), left_top(2), right_top(2), + approach(2); + + // Ensure the object fits into the hand and avoid grasps outside the + // workspace. + if (hands[j]->getGraspWidth() >= min_aperture_ && + hands[j]->getGraspWidth() <= max_aperture_ && + x.minCoeff() >= workspace[0] && x.maxCoeff() <= workspace[1] && + y.minCoeff() >= workspace[2] && y.maxCoeff() <= workspace[3] && + z.minCoeff() >= workspace[4] && z.maxCoeff() <= workspace[5]) { + is_valid(j) = true; + remaining++; + } else { + is_valid(j) = false; } - - std::vector hand_set_sublist(start, stop); - std::vector image_list = learning_->createImages(cloud_cam, hand_set_sublist); - - std::vector valid_grasps; - std::vector valid_images; - extractGraspsAndImages(candidates, image_list, valid_grasps, valid_images); - - std::vector scores_sublist = classifier_->classifyImages(valid_images); - scores.insert(scores.end(), scores_sublist.begin(), scores_sublist.end()); - grasp_list.insert(grasp_list.end(), valid_grasps.begin(), valid_grasps.end()); } - } - else - { - // Create the grasp images. - std::vector image_list = learning_->createImages(cloud_cam, candidates); - std::cout << " Image creation time: " << omp_get_wtime() - t0 << std::endl; - - std::vector valid_grasps; - std::vector valid_images; - extractGraspsAndImages(candidates, image_list, valid_grasps, valid_images); - - // Classify the grasp images. - double t0_prediction = omp_get_wtime(); - scores = classifier_->classifyImages(valid_images); - grasp_list.assign(valid_grasps.begin(), valid_grasps.end()); - std::cout << " Prediction time: " << omp_get_wtime() - t0 << std::endl; - } - // Select grasps with a score of at least . - std::vector valid_grasps; - - for (int i = 0; i < grasp_list.size(); i++) - { - if (scores[i] >= min_score_diff_) - { - std::cout << "grasp #" << i << ", score: " << scores[i] << "\n"; - valid_grasps.push_back(grasp_list[i]); - valid_grasps[valid_grasps.size() - 1].setScore(scores[i]); - valid_grasps[valid_grasps.size() - 1].setFullAntipodal(true); + if (is_valid.any()) { + hand_set_list_out.push_back(std::move(hand_set_list[i])); + hand_set_list_out[hand_set_list_out.size() - 1]->setIsValid(is_valid); } } - std::cout << "Found " << valid_grasps.size() << " grasps with a score >= " << min_score_diff_ << "\n"; - std::cout << "Total classification time: " << omp_get_wtime() - t0 << std::endl; + printf("Number of grasp candidates within workspace and gripper width: %d\n", + remaining); - if (plot_valid_grasps_) - { - Plot plotter; - const HandSearch::Parameters& params = candidates_generator_->getHandSearchParams(); - plotter.plotFingers3D(valid_grasps, cloud_cam.getCloudOriginal(), "Valid Grasps", params.hand_outer_diameter_, - params.finger_width_, params.hand_depth_, params.hand_height_); - } + return hand_set_list_out; +} - return valid_grasps; +std::vector> +GraspDetector::generateGraspCandidates(const util::Cloud &cloud) { + return candidates_generator_->generateGraspCandidateSets(cloud); } +std::vector> GraspDetector::selectGrasps( + std::vector> &hands) const { + printf("Selecting the %d highest scoring grasps ...\n", num_selected_); -std::vector GraspDetector::filterGraspsWorkspace(const std::vector& hand_set_list, - const std::vector& workspace) -{ - int remaining = 0; - std::vector hand_set_list_out; - - for (int i = 0; i < hand_set_list.size(); i++) - { - const std::vector& hands = hand_set_list[i].getHypotheses(); - Eigen::Array is_valid = hand_set_list[i].getIsValid(); - - for (int j = 0; j < hands.size(); j++) - { - if (is_valid(j)) - { - double half_width = 0.5 * outer_diameter_; - Eigen::Vector3d left_bottom = hands[j].getGraspBottom() + half_width * hands[j].getBinormal(); - Eigen::Vector3d right_bottom = hands[j].getGraspBottom() - half_width * hands[j].getBinormal(); - Eigen::Vector3d left_top = hands[j].getGraspTop() + half_width * hands[j].getBinormal(); - Eigen::Vector3d right_top = hands[j].getGraspTop() - half_width * hands[j].getBinormal(); - Eigen::Vector3d approach = hands[j].getGraspBottom() - 0.05 * hands[j].getApproach(); - Eigen::VectorXd x(5), y(5), z(5); - x << left_bottom(0), right_bottom(0), left_top(0), right_top(0), approach(0); - y << left_bottom(1), right_bottom(1), left_top(1), right_top(1), approach(1); - z << left_bottom(2), right_bottom(2), left_top(2), right_top(2), approach(2); - double aperture = hands[j].getGraspWidth(); - - if (aperture >= min_aperture_ && aperture <= max_aperture_ // make sure the object fits into the hand - && x.minCoeff() >= workspace[0] && x.maxCoeff() <= workspace[1] // avoid grasping outside the x-workspace - && y.minCoeff() >= workspace[2] && y.maxCoeff() <= workspace[3] // avoid grasping outside the y-workspace - && z.minCoeff() >= workspace[4] && z.maxCoeff() <= workspace[5]) // avoid grasping outside the z-workspace - { - is_valid(j) = true; - remaining++; - } - else - { - is_valid(j) = false; - } - } - } + int middle = std::min((int)hands.size(), num_selected_); + std::partial_sort(hands.begin(), hands.begin() + middle, hands.end(), + isScoreGreater); + std::vector> hands_out; - if (is_valid.any()) - { - hand_set_list_out.push_back(hand_set_list[i]); - hand_set_list_out[hand_set_list_out.size() - 1].setIsValid(is_valid); - } + for (int i = 0; i < middle; i++) { + hands_out.push_back(std::move(hands[i])); + printf(" grasp #%d, score: %3.4f\n", i, hands_out[i]->getScore()); } - ROS_INFO_STREAM("# grasps within workspace and gripper width: " << remaining); - - return hand_set_list_out; + return hands_out; } - -std::vector GraspDetector::filterHalfAntipodal(const std::vector& hand_set_list) -{ +std::vector> +GraspDetector::filterGraspsDirection( + std::vector> &hand_set_list, + const Eigen::Vector3d &direction, const double thresh_rad) { + std::vector> hand_set_list_out; int remaining = 0; - std::vector hand_set_list_out; - - for (int i = 0; i < hand_set_list.size(); i++) - { - const std::vector& hands = hand_set_list[i].getHypotheses(); - Eigen::Array is_valid = hand_set_list[i].getIsValid(); - - for (int j = 0; j < hands.size(); j++) - { - if (is_valid(j)) - { - if (!hands[j].isHalfAntipodal() || hands[j].isFullAntipodal()) - { - is_valid(j) = true; - remaining++; - } - else - { + + for (int i = 0; i < hand_set_list.size(); i++) { + const std::vector> &hands = + hand_set_list[i]->getHands(); + Eigen::Array is_valid = + hand_set_list[i]->getIsValid(); + + for (int j = 0; j < hands.size(); j++) { + if (is_valid(j)) { + double angle = acos(direction.transpose() * hands[j]->getApproach()); + if (angle > thresh_rad) { is_valid(j) = false; + } else { + remaining++; } } } - if (is_valid.any()) - { - hand_set_list_out.push_back(hand_set_list[i]); - hand_set_list_out[hand_set_list_out.size() - 1].setIsValid(is_valid); + if (is_valid.any()) { + hand_set_list_out.push_back(std::move(hand_set_list[i])); + hand_set_list_out[hand_set_list_out.size() - 1]->setIsValid(is_valid); } } - ROS_INFO_STREAM("# grasps that are not half-antipodal: " << remaining); + printf("Number of grasp candidates with correct approach direction: %d\n", + remaining); return hand_set_list_out; } +bool GraspDetector::createGraspImages( + util::Cloud &cloud, + std::vector> &hands_out, + std::vector> &images_out) { + // Check if the point cloud is empty. + if (cloud.getCloudOriginal()->size() == 0) { + printf("ERROR: Point cloud is empty!"); + hands_out.resize(0); + images_out.resize(0); + return false; + } -std::vector GraspDetector::extractHypotheses(const std::vector& hand_set_list) -{ - std::vector hands_out; - hands_out.resize(0); + // Plot samples/indices. + if (plot_samples_) { + if (cloud.getSamples().cols() > 0) { + plotter_->plotSamples(cloud.getSamples(), cloud.getCloudProcessed()); + } else if (cloud.getSampleIndices().size() > 0) { + plotter_->plotSamples(cloud.getSampleIndices(), + cloud.getCloudProcessed()); + } + } - for (int i = 0; i < hand_set_list.size(); i++) - { - const std::vector& hands = hand_set_list[i].getHypotheses(); + if (plot_normals_) { + std::cout << "Plotting normals for different camera sources\n"; + plotter_->plotNormals(cloud); + } - for (int j = 0; j < hands.size(); j++) - { - if (hand_set_list[i].getIsValid()(j)) - { - hands_out.push_back(hands[j]); - } - } + // 1. Generate grasp candidates. + std::vector> hand_set_list = + candidates_generator_->generateGraspCandidateSets(cloud); + printf("Generated %zu hand sets.\n", hand_set_list.size()); + if (hand_set_list.size() == 0) { + hands_out.resize(0); + images_out.resize(0); + return false; } - return hands_out; -} + const candidate::HandGeometry &hand_geom = + candidates_generator_->getHandSearchParams().hand_geometry_; + // 2. Filter the candidates. + std::vector> hand_set_list_filtered = + filterGraspsWorkspace(hand_set_list, workspace_grasps_); + if (plot_filtered_candidates_) { + plotter_->plotFingers3D(hand_set_list_filtered, cloud.getCloudOriginal(), + "Filtered Grasps (Aperture, Workspace)", hand_geom); + } + if (filter_approach_direction_) { + hand_set_list_filtered = + filterGraspsDirection(hand_set_list_filtered, direction_, thresh_rad_); + if (plot_filtered_candidates_) { + plotter_->plotFingers3D(hand_set_list_filtered, cloud.getCloudOriginal(), + "Filtered Grasps (Approach)", hand_geom); + } + } -void GraspDetector::extractGraspsAndImages(const std::vector& hand_set_list, - const std::vector& images, std::vector& grasps_out, std::vector& images_out) -{ - grasps_out.resize(0); - images_out.resize(0); - int num_orientations = hand_set_list[0].getHypotheses().size(); + // 3. Create grasp descriptors (images). + std::vector> hands; + std::vector> images; + image_generator_->createImages(cloud, hand_set_list_filtered, images_out, + hands_out); - for (int i = 0; i < hand_set_list.size(); i++) - { - const std::vector& hands = hand_set_list[i].getHypotheses(); + return true; +} - for (int j = 0; j < hands.size(); j++) - { - if (hand_set_list[i].getIsValid()(j)) - { - grasps_out.push_back(hands[j]); - images_out.push_back(images[i * num_orientations + j]); - } +std::vector GraspDetector::evalGroundTruth( + const util::Cloud &cloud_gt, + std::vector> &hands) { + return candidates_generator_->reevaluateHypotheses(cloud_gt, hands); +} + +std::vector> +GraspDetector::pruneGraspCandidates( + const util::Cloud &cloud, + const std::vector> &hand_set_list, + double min_score) { + // 1. Create grasp descriptors (images). + std::vector> hands; + std::vector> images; + image_generator_->createImages(cloud, hand_set_list, images, hands); + + // 2. Classify the grasp candidates. + std::vector scores = classifier_->classifyImages(images); + std::vector> hands_out; + + // 3. Only keep grasps with a score larger than . + for (int i = 0; i < hands.size(); i++) { + if (scores[i] > min_score) { + hands[i]->setScore(scores[i]); + hands_out.push_back(std::move(hands[i])); } } + + return hands_out; } +void GraspDetector::printStdVector(const std::vector &v, + const std::string &name) const { + printf("%s: ", name.c_str()); + for (int i = 0; i < v.size(); i++) { + printf("%d ", v[i]); + } + printf("\n"); +} -std::vector GraspDetector::findClusters(const std::vector& grasps) -{ - return clustering_->findClusters(grasps); +void GraspDetector::printStdVector(const std::vector &v, + const std::string &name) const { + printf("%s: ", name.c_str()); + for (int i = 0; i < v.size(); i++) { + printf("%3.2f ", v[i]); + } + printf("\n"); } + +} // namespace gpd diff --git a/src/gpd/grasp_image.cpp b/src/gpd/grasp_image.cpp deleted file mode 100644 index b842690..0000000 --- a/src/gpd/grasp_image.cpp +++ /dev/null @@ -1,173 +0,0 @@ -#include "../../include/gpd/grasp_image.h" - - -int GraspImage::image_size_; - - -GraspImage::GraspImage(int image_size, int num_channels, bool is_plotting) - : num_channels_(num_channels), is_plotting_(is_plotting) -{ - GraspImage::image_size_ = image_size; -} - - -Eigen::VectorXi GraspImage::findCellIndices(const Eigen::Matrix3Xd& points) -{ - double cellsize = 1.0 / (double) image_size_; - Eigen::VectorXi vertical_cells = (floorVector(points.row(0) / cellsize)).cwiseMin(image_size_ - 1); - Eigen::VectorXi horizontal_cells = (floorVector(points.row(1) / cellsize)).cwiseMin(image_size_ - 1); - Eigen::VectorXi cell_indices = horizontal_cells + vertical_cells * image_size_; - return cell_indices; -} - - -cv::Mat GraspImage::createBinaryImage(const Eigen::VectorXi& cell_indices) -{ - cv::Mat image(image_size_, image_size_, CV_8UC1, cv::Scalar(0)); - - // Calculate average depth image. - for (int i = 0; i < cell_indices.rows(); i++) - { - const int& idx = cell_indices[i]; - int row = image.rows - 1 - idx / image.cols; - int col = idx % image.cols; - image.at(row, col) = 255; - } - - // Dilate the image to fill in holes. - cv::Mat dilation_element = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(3,3)); - dilate(image, image, dilation_element); - - return image; -} - - -cv::Mat GraspImage::createNormalsImage(const Eigen::Matrix3Xd& normals, const Eigen::VectorXi& cell_indices) -{ - // For each cell, calculate the average surface normal of the points that fall into that cell. - cv::Mat image(image_size_, image_size_, CV_32FC3, cv::Scalar(0.0)); - - for (int i = 0; i < cell_indices.rows(); i++) - { - const int& idx = cell_indices[i]; - int row = image_size_ - 1 - idx / image_size_; - int col = idx % image_size_; - cv::Vec3f& v = image.at(row, col); - const Eigen::Vector3d& n = normals.col(i); - if (v(0) == 0 && v(1) == 0 && v(2) == 0) - { - v = cv::Vec3f(fabs(n(0)), fabs(n(1)), fabs(n(2))); - } - else - { - v += (cv::Vec3f(fabs(n(0)), fabs(n(1)), fabs(n(2))) - v) * (1.0/sqrt(v(0)*v(0) + v(1)*v(1) + v(2)*v(2))); - } - } - - // Dilate the image to fill in holes. - cv::Mat dilation_element = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(3,3)); - dilate(image, image, dilation_element); - - // Normalize the image to the range [0,1]. - cv::normalize(image, image, 0.0, 1.0, cv::NORM_MINMAX, CV_32FC3); - - // Convert float image to uchar image, required by Caffe. - image.convertTo(image, CV_8U, 255.0); - - return image; -} - - -cv::Mat GraspImage::createDepthImage(const Eigen::Matrix3Xd& points, const Eigen::VectorXi& cell_indices) -{ - cv::Mat image(image_size_, image_size_, CV_32FC1, cv::Scalar(0.0)); - float avgs[image_size_ * image_size_]; // average of cell - float counts[image_size_ * image_size_]; // count of cell - - for (int i = 0; i < image_size_ * image_size_; i++) - { - avgs[i] = 0.0; - counts[i] = 0; - } - - // Calculate average depth image. - for (int i = 0; i < cell_indices.rows(); i++) - { - const int& idx = cell_indices[i]; - int row = image.rows - 1 - idx / image.cols; - int col = idx % image.cols; - counts[idx] += 1.0; - avgs[idx] += (points(2,i) - avgs[idx]) * (1.0/counts[idx]); - float& v = image.at(row, col); - v = 1.0 - avgs[idx]; - } - - // Dilate the image to fill in holes. - cv::Mat dilation_element = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(3,3)); - dilate(image, image, dilation_element); - - // Normalize the image to the range [0,1]. - cv::normalize(image, image, 0.0, 1.0, cv::NORM_MINMAX, CV_32FC1); - - // Convert float image to uchar image, required by Caffe. - image.convertTo(image, CV_8U, 255.0); - - return image; -} - - -cv::Mat GraspImage::createShadowImage(const Eigen::Matrix3Xd& points, const Eigen::VectorXi& cell_indices) -{ - // Calculate average depth image. - cv::Mat image(image_size_, image_size_, CV_32FC1, cv::Scalar(0.0)); - cv::Mat nonzero(image_size_, image_size_, CV_8UC1, cv::Scalar(0)); - float counts[image_size_ * image_size_]; - - for (int i = 0; i < image_size_ * image_size_; i++) - { - counts[i] = 0; - } - - for (int i = 0; i < cell_indices.rows(); i++) - { - const int& idx = cell_indices[i]; - int row = image.rows - 1 - idx / image.cols; - int col = idx % image.cols; - counts[idx] += 1.0; - image.at(row, col) += (points(2,i) - image.at(row, col)) * (1.0/counts[idx]); - nonzero.at(row, col) = 1; - } - - // Reverse depth so that closest points have largest value. - double min, max; - cv::Point min_loc, max_loc; - cv::minMaxLoc(image, &min, &max, &min_loc, &max_loc, nonzero); - cv::Mat max_img(image_size_, image_size_, CV_32FC1, cv::Scalar(0.0)); - max_img.setTo(max, nonzero); - image = max_img - image; - - // Dilate the image to fill in holes. - cv::Mat dilation_element = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(3,3)); - dilate(image, image, dilation_element); - - // Normalize the image to the range [0,1]. - cv::normalize(image, image, 0.0, 1.0, cv::NORM_MINMAX, CV_32FC1); - - // Convert float image to uchar image, required by Caffe. - image.convertTo(image, CV_8U, 255.0); - - return image; -} - - -Eigen::VectorXi GraspImage::floorVector(const Eigen::VectorXd& a) -{ - Eigen::VectorXi b(a.size()); - - for (int i = 0; i < b.size(); i++) - { - b(i) = floor((double) a(i)); - } - - return b; -} diff --git a/src/gpd/grasp_image_15_channels.cpp b/src/gpd/grasp_image_15_channels.cpp deleted file mode 100644 index 8a955a1..0000000 --- a/src/gpd/grasp_image_15_channels.cpp +++ /dev/null @@ -1,145 +0,0 @@ -#include "../../include/gpd/grasp_image_15_channels.h" - - -const int GraspImage15Channels::NUM_CHANNELS = 15; - - -GraspImage15Channels::GraspImage15Channels(int image_size, bool is_plotting, Eigen::Matrix3Xd* points, - Eigen::Matrix3Xd* normals, Eigen::Matrix3Xd* shadow) - : GraspImage(image_size, NUM_CHANNELS, is_plotting), points_(points), normals_(normals), shadow_(shadow) -{ - -} - - -GraspImage15Channels::~GraspImage15Channels() -{ - -} - - -cv::Mat GraspImage15Channels::calculateImage() -{ - // 1. Create the images for the first projection. - Eigen::Vector3i order; - order << 0, 1, 2; - double t0 = omp_get_wtime(); - Projection projection1 = calculateProjection(*points_, *normals_, *shadow_); -// std::cout << " time for view 1: " << omp_get_wtime() - t0 << "\n"; - - Eigen::Matrix3Xd points = *points_; - Eigen::Matrix3Xd shadow = *shadow_; - - // 2. Create the images for the second view. - order << 2, 1, 0; - t0 = omp_get_wtime(); - points.row(0).swap(points.row(2)); - shadow.row(0).swap(shadow.row(2)); - Projection projection2 = calculateProjection(points, *normals_, shadow); -// std::cout << " time for view 2: " << omp_get_wtime() - t0 << "\n"; - - // 3. Create the images for the third view. - order << 2, 0, 1; - t0 = omp_get_wtime(); - points.row(1).swap(points.row(2)); - shadow.row(1).swap(shadow.row(2)); - Projection projection3 = calculateProjection(points, *normals_, shadow); -// std::cout << " time for view 3: " << omp_get_wtime() - t0 << "\n"; - - // 4. Concatenate the images from the three projections into a single image. - t0 = omp_get_wtime(); - cv::Mat img = concatenateProjections(projection1, projection2, projection3); -// std::cout << " time for concatenating views: " << omp_get_wtime() - t0 << "\n"; - - // Visualize the final image. - if (is_plotting_) - { - std::vector projections; - projections.push_back(projection1); - projections.push_back(projection2); - projections.push_back(projection3); - showImage(projections); - } - - return img; -} - - -Projection GraspImage15Channels::calculateProjection(const Eigen::Matrix3Xd& points, const Eigen::Matrix3Xd& normals, - const Eigen::Matrix3Xd& shadow) -{ - double t0 = omp_get_wtime(); - - Projection projection; - - Eigen::VectorXi cell_indices = findCellIndices(points); - // std::cout << " time for finding cell indices: " << omp_get_wtime() - t0 << "s\n"; - - t0 = omp_get_wtime(); - projection.normals_image_ = createNormalsImage(normals, cell_indices); - // std::cout << " time for normals image: " << omp_get_wtime() - t0 << "s\n"; - - t0 = omp_get_wtime(); - projection.depth_image_ = createDepthImage(points, cell_indices); - // std::cout << " time for depth image: " << omp_get_wtime() - t0 << "s\n"; - - t0 = omp_get_wtime(); - cell_indices = findCellIndices(shadow); - projection.shadow_image_ = createShadowImage(shadow, cell_indices); - // std::cout << " time for shadow image: " << omp_get_wtime() - t0 << "s\n"; - - return projection; -} - - -cv::Mat GraspImage15Channels::concatenateProjections(const Projection& projection1, const Projection& projection2, - const Projection& projection3) const -{ - cv::Mat image(GraspImage::image_size_, GraspImage::image_size_, CV_8UC(num_channels_)); - - // Not sure if this is optimal but it avoids loops for reducing code length. - std::vector projection1_list, projection2_list, projection3_list; - cv::split(projection1.normals_image_, projection1_list); - cv::split(projection2.normals_image_, projection2_list); - cv::split(projection3.normals_image_, projection3_list); - projection1_list.push_back(projection1.depth_image_); - projection1_list.push_back(projection1.shadow_image_); - projection1_list.push_back(projection2_list[0]); - projection1_list.push_back(projection2_list[1]); - projection1_list.push_back(projection2_list[2]); - projection1_list.push_back(projection2.depth_image_); - projection1_list.push_back(projection2.shadow_image_); - projection1_list.push_back(projection3_list[0]); - projection1_list.push_back(projection3_list[1]); - projection1_list.push_back(projection3_list[2]); - projection1_list.push_back(projection3.depth_image_); - projection1_list.push_back(projection3.shadow_image_); - cv::merge(projection1_list, image); - - return image; -} - - -void GraspImage15Channels::showImage(const std::vector& projections) const -{ - int border = 5; - int n = 3; - int total_size = n * (GraspImage::image_size_ + border) + border; - cv::Mat image_out(total_size, total_size, CV_8UC3, cv::Scalar(0.5)); - - for (int i = 0; i < n; i++) - { - // OpenCV requires images to be in BGR or grayscale to be displayed. - cv::Mat normals_rgb, depth_rgb, shadow_rgb; - cvtColor(projections[i].normals_image_, normals_rgb, CV_RGB2BGR); - cvtColor(projections[i].depth_image_, depth_rgb, CV_GRAY2RGB); - cvtColor(projections[i].shadow_image_, shadow_rgb, CV_GRAY2RGB); - normals_rgb.copyTo(image_out(cv::Rect(border, border + i * (border + image_size_), image_size_, image_size_))); - depth_rgb.copyTo(image_out(cv::Rect(2 * border + image_size_, border + i * (border + image_size_), image_size_, image_size_))); - shadow_rgb.copyTo(image_out(cv::Rect(3 * border + 2 * image_size_, border + i * (border + image_size_), image_size_, image_size_))); - } - - cv::namedWindow("Grasp Image (15 channels)", cv::WINDOW_NORMAL); - cv::imshow("Grasp Image (15 channels)", image_out); - cv::waitKey(0); -} diff --git a/src/gpd/learning.cpp b/src/gpd/learning.cpp deleted file mode 100644 index d053e0c..0000000 --- a/src/gpd/learning.cpp +++ /dev/null @@ -1,352 +0,0 @@ -#include "../../include/gpd/learning.h" - - -void Learning::extractGraspsAndImages(const std::vector& hand_set_list, const std::vector& images, - std::vector& grasps_out, std::vector& images_out) -{ - grasps_out.resize(0); - images_out.resize(0); - int num_orientations = hand_set_list[0].getHypotheses().size(); - - for (int i = 0; i < hand_set_list.size(); i++) - { - const std::vector& hands = hand_set_list[i].getHypotheses(); - - for (int j = 0; j < hands.size(); j++) - { - if (hand_set_list[i].getIsValid()(j)) - { - grasps_out.push_back(hands[j]); - images_out.push_back(images[i * num_orientations + j]); - } - } - } -} - - -std::vector Learning::createImages(const CloudCamera& cloud_cam, - const std::vector& hand_set_list) const -{ - double t0_total = omp_get_wtime(); - - PointCloudRGBA::Ptr cloud(new PointCloudRGBA); - *cloud = *cloud_cam.getCloudProcessed(); - Eigen::Matrix3Xd points = cloud->getMatrixXfMap().cast(); - PointList point_list(points, cloud_cam.getNormals(), cloud_cam.getCameraSource(), cloud_cam.getViewPoints()); - - if (remove_plane_) - { - // Segment the plane to speed up shadow computation. - pcl::SACSegmentation seg; - pcl::PointIndices::Ptr inliers(new pcl::PointIndices); - pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients); - seg.setInputCloud(cloud_cam.getCloudProcessed()); - seg.setOptimizeCoefficients(true); - seg.setModelType(pcl::SACMODEL_PLANE); - seg.setMethodType(pcl::SAC_RANSAC); -// seg.setMaxIterations(100); - seg.setDistanceThreshold(0.01); - seg.segment(*inliers, *coefficients); - if (inliers->indices.size () > 0) - { - pcl::ExtractIndices extract; - extract.setInputCloud(cloud_cam.getCloudProcessed()); - extract.setIndices(inliers); - extract.setNegative(true); - std::vector indices; - extract.filter(indices); -// pcl::visualization::CloudViewer viewer("Filtered Cloud"); -// viewer.showCloud(cloud); -// while (!viewer.wasStopped()) { } - if (indices.size() > 0) - { - extract.filter(*cloud); - point_list = point_list.slice(indices); - printf("Removed plane from point cloud. Remaining points in cloud: %d.\n", (int) cloud->size()); - std::cout << point_list.getPoints().col(0).transpose() << "\n" << cloud->at(0).getVector3fMap().transpose() << "\n"; - } - else - { - printf("No points remaining after plane is removed! Using entire point cloud ...\n"); - } - } - } - - // Prepare kd-tree for neighborhood searches in the point cloud. - pcl::KdTreeFLANN kdtree; - kdtree.setInputCloud(cloud); - std::vector nn_indices; - std::vector nn_dists; - - // Set the radius for the neighborhood search to the largest image dimension. - Eigen::Vector3d image_dims; - image_dims << image_params_.depth_, image_params_.height_ / 2.0, image_params_.outer_diameter_; - double radius = image_dims.maxCoeff(); - - // 1. Find points within image dimensions. - bool is_valid[hand_set_list.size()]; - std::vector nn_points_list; - nn_points_list.resize(hand_set_list.size()); - -#ifdef _OPENMP // parallelization using OpenMP -#pragma omp parallel for private(nn_indices, nn_dists) num_threads(num_threads_) -#endif - for (int i = 0; i < hand_set_list.size(); i++) - { - pcl::PointXYZRGBA sample_pcl; - sample_pcl.getVector3fMap() = hand_set_list[i].getSample().cast(); - - if (kdtree.radiusSearch(sample_pcl, radius, nn_indices, nn_dists) > 0) - { - nn_points_list[i] = point_list.slice(nn_indices); - is_valid[i] = true; - } - else - { - is_valid[i] = false; - } - } - - std::cout << "time for computing " << nn_points_list.size() << " point neighborhoods with " << num_threads_ << " threads: " << omp_get_wtime() - t0_total << "s\n"; - - if (image_params_.num_channels_ == 1 || image_params_.num_channels_ == 3) // 3 channels image (only surface normals) - { - return createImages1or3Channels(hand_set_list, nn_points_list, is_valid, image_dims); - } - else if (image_params_.num_channels_ == 15) // 15 channels image - { - return createImages15Channels(hand_set_list, nn_points_list, is_valid, image_dims); - } - - std::vector empty; - empty.resize(0); - return empty; -} - - -std::vector Learning::createImages1or3Channels(const std::vector& hand_set_list, - const std::vector& nn_points_list, const bool* is_valid, const Eigen::Vector3d& image_dims) const -{ - double t0_images = omp_get_wtime(); - std::vector images(hand_set_list.size() * num_orientations_, cv::Mat(60, 60, CV_8UC(image_params_.num_channels_), cv::Scalar(0.0))); - -#ifdef _OPENMP // parallelization using OpenMP -#pragma omp parallel for num_threads(num_threads_) -#endif - for (int i = 0; i < hand_set_list.size(); i++) - { - if (is_valid[i]) - { - const std::vector& hands = hand_set_list[i].getHypotheses(); - - for (int j = 0; j < hands.size(); j++) - { - if (hand_set_list[i].getIsValid()(j)) - { - const int idx = i * num_orientations_ + j; - images[idx] = createImage1or3Channels(nn_points_list[i], hands[j]); - } - } - } - } - - return images; -} - - -cv::Mat Learning::createImage1or3Channels(const PointList& point_list, const Grasp& hand) const -{ - // 1. Transform points and normals in neighborhood into the unit image. - Matrix3XdPair points_normals = transformToUnitImage(point_list, hand); - - // 2. Calculate grasp image. - Eigen::VectorXi cell_indices = GraspImage::findCellIndices(points_normals.first); - GraspImage::setImageSize(image_params_.size_); - cv::Mat image; - - if (image_params_.num_channels_ == 1) - { - image = GraspImage::createBinaryImage(cell_indices); - } - else if (image_params_.num_channels_ == 3) - { - image = GraspImage::createNormalsImage(points_normals.second, cell_indices); - } - - if (is_plotting_) - { - std::string title = "Grasp Image (" + boost::lexical_cast(image_params_.num_channels_); - cv::namedWindow(title, cv::WINDOW_NORMAL); - cv::imshow(title, image); - cv::waitKey(0); - } - - return image; -} - - -std::vector Learning::createImages15Channels(const std::vector& hand_set_list, - const std::vector& nn_points_list, const bool* is_valid, const Eigen::Vector3d& image_dims) const -{ - // 1. Calculate shadow points for each point neighborhood. - double t0_shadows = omp_get_wtime(); - - // Calculate shadow length (= max length of shadow required to fill image window). - double shadow_length = image_dims.maxCoeff(); - - std::vector images(hand_set_list.size() * num_orientations_, cv::Mat(60, 60, CV_8UC(15), cv::Scalar(0.0))); - int num_images = 0; - -#ifdef _OPENMP // parallelization using OpenMP -#pragma omp parallel for num_threads(num_threads_) -#endif - for (int i = 0; i < nn_points_list.size(); i++) - { - if (is_valid[i]) - { - Eigen::Matrix3Xd shadow = hand_set_list[i].calculateShadow4(nn_points_list[i], shadow_length); - - const std::vector& hands = hand_set_list[i].getHypotheses(); - - for (int j = 0; j < hands.size(); j++) - { - if (hand_set_list[i].getIsValid()(j)) - { - const int idx = i * num_orientations_ + j; - images[idx] = createImage15Channels(nn_points_list[i], shadow, hands[j]); - num_images++; - } - } - } - } - - std::cout << "time to calculate " << num_images << " images with " << num_threads_ << " threads: " - << omp_get_wtime() - t0_shadows << "s\n"; - return images; - -//#ifdef _OPENMP // parallelization using OpenMP -//#pragma omp parallel for num_threads(num_threads_) -//#endif -// for (int i = 0; i < nn_points_list.size(); i++) -// { -// if (is_valid[i]) -// { -// shadows[i] = hand_set_list[i].calculateShadow4(nn_points_list[i], shadow_length); -// } -// } -// -// std::cout << "time to calculate " << nn_points_list.size() << " shadows with " << num_threads_ << " threads: " << omp_get_wtime() - t0_shadows << "s\n"; -// -// // 2. Calculate the grasp images. -// double t0_images = omp_get_wtime(); -// std::vector images(hand_set_list.size() * num_orientations_, cv::Mat(60, 60, CV_8UC(15), cv::Scalar(0.0))); -// int num_images = 0; -// -//#ifdef _OPENMP // parallelization using OpenMP -//#pragma omp parallel for num_threads(num_threads_) -//#endif -// for (int i = 0; i < hand_set_list.size(); i++) -// { -// if (is_valid[i]) -// { -// const std::vector& hands = hand_set_list[i].getHypotheses(); -// -// for (int j = 0; j < hands.size(); j++) -// { -// if (hand_set_list[i].getIsValid()(j)) -// { -// const int idx = i * num_orientations_ + j; -// images[idx] = createImage15Channels(nn_points_list[i], shadows[i], hands[j]); -// num_images++; -// } -// } -// } -// } -// -// std::cout << "time to calculate " << num_images << " images with " << num_threads_ << " threads: " << omp_get_wtime() - t0_images << "s\n"; -//// std::cout << "==> total time to create images: " << omp_get_wtime() - t0_total << "s\n"; -// -// return images; -} - - -cv::Mat Learning::createImage15Channels(const PointList& point_list, const Eigen::Matrix3Xd& shadow, const Grasp& hand) - const -{ - // 1. Transform points and normals in neighborhood into the unit image. - Matrix3XdPair points_normals = transformToUnitImage(point_list, hand); - - // 2. Transform occluded points into hand frame. - Eigen::Matrix3Xd shadow_frame = shadow - hand.getSample().replicate(1, shadow.cols()); - shadow_frame = hand.getFrame().transpose() * shadow_frame; - std::vector indices = findPointsInUnitImage(hand, shadow_frame); - Eigen::Matrix3Xd cropped_shadow_points = transformPointsToUnitImage(hand, shadow_frame, indices); - - // 3. Create grasp image. - GraspImage15Channels grasp_image(image_params_.size_, is_plotting_, &points_normals.first, &points_normals.second, - &cropped_shadow_points); - cv::Mat image = grasp_image.calculateImage(); - return image; -} - - -Matrix3XdPair Learning::transformToUnitImage(const PointList& point_list, const Grasp& hand) const -{ - // 1. Transform points and normals in neighborhood into the hand frame. - Matrix3XdPair points_normals = transformToHandFrame(point_list, hand.getSample(), hand.getFrame().transpose()); - - // 2. Find points in unit image. - std::vector indices = findPointsInUnitImage(hand, points_normals.first); - points_normals.first = transformPointsToUnitImage(hand, points_normals.first, indices); - points_normals.second = EigenUtils::sliceMatrix(points_normals.second, indices); - - return points_normals; -} - - -std::vector Learning::findPointsInUnitImage(const Grasp& hand, const Eigen::Matrix3Xd& points) const -{ - std::vector indices; - const double half_outer_diameter = image_params_.outer_diameter_ / 2.0; - - for (int i = 0; i < points.cols(); i++) - { - if ( (points(0,i) > hand.getBottom()) && (points(0,i) < hand.getBottom() + image_params_.depth_) - && (points(1,i) > hand.getCenter() - half_outer_diameter) - && (points(1,i) < hand.getCenter() + half_outer_diameter) - && (points(2,i) > -1.0 * image_params_.height_) && (points(2,i) < image_params_.height_)) - { - indices.push_back(i); - } - } - - return indices; -} - - -Eigen::Matrix3Xd Learning::transformPointsToUnitImage(const Grasp& hand, const Eigen::Matrix3Xd& points, - const std::vector& indices) const -{ - Eigen::Matrix3Xd points_out(3, indices.size()); - const double half_outer_diameter = image_params_.outer_diameter_ / 2.0; - const double double_height = 2.0 * image_params_.height_; - - for (int i = 0; i < indices.size(); i++) - { - points_out(0,i) = (points(0,indices[i]) - hand.getBottom()) / image_params_.depth_; - points_out(1,i) = (points(1,indices[i]) - (hand.getCenter() - half_outer_diameter)) / image_params_.outer_diameter_; - points_out(2,i) = (points(2,indices[i]) + image_params_.height_) / double_height; - } - - return points_out; -} - - -Matrix3XdPair Learning::transformToHandFrame(const PointList& point_list, const Eigen::Vector3d& centroid, - const Eigen::Matrix3d& rotation) const -{ - Matrix3XdPair pair; - pair.first = rotation * (point_list.getPoints() - centroid.replicate(1, point_list.getPoints().cols())); - pair.second = rotation * point_list.getNormals(); - return pair; -} diff --git a/src/gpd/net/caffe_classifier.cpp b/src/gpd/net/caffe_classifier.cpp new file mode 100644 index 0000000..9dfb977 --- /dev/null +++ b/src/gpd/net/caffe_classifier.cpp @@ -0,0 +1,79 @@ +#include + +namespace gpd { +namespace net { + +CaffeClassifier::CaffeClassifier(const std::string &model_file, + const std::string &weights_file, + Classifier::Device device, int batch_size) { + // Initialize Caffe. + switch (device) { + case Classifier::Device::eGPU: + caffe::Caffe::set_mode(caffe::Caffe::GPU); + break; + default: + caffe::Caffe::set_mode(caffe::Caffe::CPU); + } + + // Load pretrained network. + net_.reset(new caffe::Net(model_file, caffe::TEST)); + net_->CopyTrainedLayersFrom(weights_file); + + input_layer_ = boost::static_pointer_cast>( + net_->layer_by_name("data")); + input_layer_->set_batch_size(batch_size); +} + +std::vector CaffeClassifier::classifyImages( + const std::vector> &image_list) { + int batch_size = input_layer_->batch_size(); + int num_iterations = (int)ceil(image_list.size() / (double)batch_size); + float loss = 0.0; + std::cout << "# images: " << image_list.size() + << ", # iterations: " << num_iterations + << ", batch size: " << batch_size << "\n"; + + std::vector predictions; + + // Process the images in batches. + for (int i = 0; i < num_iterations; i++) { + std::vector sub_image_list; + + if (i < num_iterations - 1) { + for (int j = 0; j < batch_size; j++) { + sub_image_list.push_back(*image_list[i * batch_size + j]); + } + } else { + for (int j = 0; j < image_list.size() - i * batch_size; j++) { + sub_image_list.push_back(*image_list[i * batch_size + j]); + } + for (int j = sub_image_list.size(); j < batch_size; j++) { + cv::Mat empty_mat(input_layer_->height(), input_layer_->width(), + CV_8UC(input_layer_->channels()), cv::Scalar(0.0)); + sub_image_list.push_back(empty_mat); + } + } + + std::vector label_list; + label_list.resize(sub_image_list.size()); + + for (int j = 0; j < label_list.size(); j++) { + label_list[j] = 0; + } + + // Classify the batch. + input_layer_->AddMatVector(sub_image_list, label_list); + std::vector *> results = net_->Forward(&loss); + std::vector out(results[0]->cpu_data(), + results[0]->cpu_data() + results[0]->count()); + + for (int l = 0; l < results[0]->count() / results[0]->channels(); l++) { + predictions.push_back(out[2 * l + 1] - out[2 * l]); + } + } + + return predictions; +} + +} // namespace net +} // namespace gpd diff --git a/src/gpd/net/classifier.cpp b/src/gpd/net/classifier.cpp new file mode 100644 index 0000000..4ceb855 --- /dev/null +++ b/src/gpd/net/classifier.cpp @@ -0,0 +1,65 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018 Intel Corporation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * 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. + */ + +#include +#if defined(USE_OPENVINO) +#include +#elif defined(USE_CAFFE) +#include +#elif defined(USE_OPENCV) +#include +#else +#include +#endif + +namespace gpd { +namespace net { + +std::shared_ptr Classifier::create(const std::string &model_file, + const std::string &weights_file, + Classifier::Device device, + int batch_size) { +#if defined(USE_OPENVINO) + return std::make_shared(model_file, weights_file, device, + batch_size); +#elif defined(USE_CAFFE) + return std::make_shared(model_file, weights_file, device, + batch_size); +#elif defined(USE_OPENCV) + return std::make_shared(model_file, weights_file, device); +#else + return std::make_shared(model_file, weights_file, device, + batch_size); +#endif +} + +} // namespace net +} // namespace gpd diff --git a/src/gpd/net/conv_layer.cpp b/src/gpd/net/conv_layer.cpp new file mode 100644 index 0000000..920cc1e --- /dev/null +++ b/src/gpd/net/conv_layer.cpp @@ -0,0 +1,101 @@ +#include + +namespace gpd { +namespace net { + +ConvLayer::ConvLayer(int width, int height, int depth, int num_filters, + int spatial_extent, int stride, int padding) + : w1(width), + h1(height), + d1(depth), + d2(num_filters), + f(spatial_extent), + s(stride) { + w2 = (w1 - spatial_extent + 2 * padding) / stride + 1; + h2 = (h1 - spatial_extent + 2 * padding) / stride + 1; + + // matrix X_col has size: spatial_extent*spatial_extent*d1 x w2*h2 + X_col_r = spatial_extent * spatial_extent * d1; + X_col_c = w2 * h2; + + // matrix W_row has size: num_filters x patial_extent*spatial_extent*d1 + W_row_r = num_filters; + W_row_c = spatial_extent * spatial_extent * d1; +} + +Eigen::MatrixXf ConvLayer::forward(const std::vector &x) const { + // Convert input image to matrix where each column is an image patch. + std::vector x_col_vec; + x_col_vec.resize(X_col_r * X_col_c); + imageToColumns(&x[0], d1, h1, w1, d2, f, f, s, s, &x_col_vec[0]); + Eigen::MatrixXf X_col = + ConvLayer::RowMajorMatrixMap(x_col_vec.data(), X_col_r, X_col_c); + + // Convert weights vector to matrix where each row is a kernel. + Eigen::MatrixXf W_row = + ConvLayer::RowMajorMatrixMap(weights_.data(), W_row_r, W_row_c); + + // Convert biases array to Eigen vector. + Eigen::VectorXf b = + Eigen::Map(biases_.data(), biases_.size()); + + // Calculate the convolution by calculating the dot product of W_row and + // X_col. + Eigen::MatrixXf H = forward(W_row, b, X_col); + return H; +} + +Eigen::MatrixXf ConvLayer::forward(const Eigen::MatrixXf &W, + const Eigen::VectorXf &b, + const Eigen::MatrixXf &X) const { + Eigen::MatrixXf B = b.replicate(1, X.cols()); + + // Calculate the forward pass. + Eigen::MatrixXf H = W * X + B; // np.dot(W_row, X_col) + return H; +} + +bool ConvLayer::is_a_ge_zero_and_a_lt_b(int a, int b) const { + return a >= 0 && a < b; +} + +void ConvLayer::imageToColumns(const float *data_im, const int channels, + const int height, const int width, + const int num_kernels, const int kernel_h, + const int kernel_w, const int stride_h, + const int stride_w, float *data_col) const { + const int output_h = (height - kernel_h) / stride_h + 1; + const int output_w = (width - kernel_w) / stride_w + 1; + const int channel_size = height * width; + + for (int channel = channels; channel--; data_im += channel_size) { + for (int kernel_row = 0; kernel_row < kernel_h; kernel_row++) { + for (int kernel_col = 0; kernel_col < kernel_w; kernel_col++) { + int input_row = kernel_row; + + for (int output_rows = output_h; output_rows; output_rows--) { + if (!is_a_ge_zero_and_a_lt_b(input_row, height)) { + for (int output_cols = output_w; output_cols; output_cols--) { + *(data_col++) = 0; + } + } else { + int input_col = kernel_col; + + for (int output_col = output_w; output_col; output_col--) { + if (is_a_ge_zero_and_a_lt_b(input_col, width)) { + *(data_col++) = data_im[input_row * width + input_col]; + } else { + *(data_col++) = 0; + } + input_col += stride_w; + } + } + input_row += stride_h; + } + } + } + } +} + +} // namespace net +} // namespace gpd diff --git a/src/gpd/net/dense_layer.cpp b/src/gpd/net/dense_layer.cpp new file mode 100644 index 0000000..c184c16 --- /dev/null +++ b/src/gpd/net/dense_layer.cpp @@ -0,0 +1,18 @@ +#include + +namespace gpd { +namespace net { + +Eigen::MatrixXf DenseLayer::forward(const std::vector &x) const { + Eigen::Map W(weights_.data(), num_units_, x.size()); + Eigen::Map b(biases_.data(), biases_.size()); + Eigen::Map X(x.data(), x.size()); + + // Calculate the forward pass. + Eigen::MatrixXf H = W * X + b; // np.dot(W_row, X_col) + + return H; +} + +} // namespace net +} // namespace gpd diff --git a/src/gpd/net/eigen_classifier.cpp b/src/gpd/net/eigen_classifier.cpp new file mode 100644 index 0000000..0b94ee1 --- /dev/null +++ b/src/gpd/net/eigen_classifier.cpp @@ -0,0 +1,207 @@ +#include + +namespace gpd { +namespace net { + +EigenClassifier::EigenClassifier(const std::string &model_file, + const std::string &weights_file, + Classifier::Device device, int batch_size) + : num_threads_(4) { + double start = omp_get_wtime(); + + const int image_size = 60; + const int num_channels = 15; + const int num_filters = 20; + const int spatial_extent = 5; + const int stride = 1; + const int padding = 0; + + // Construct the network. + conv1_ = + std::make_unique(image_size, image_size, num_channels, + num_filters, spatial_extent, stride, padding); + conv2_ = std::make_unique(28, 28, 20, 50, 5, 1, 0); + dense1_ = std::make_unique(500); + dense2_ = std::make_unique(2); + + // Set weights and biases. + const std::string ¶ms_dir = weights_file; + std::vector w_vec = + readBinaryFileIntoVector(params_dir + "conv1_weights.bin"); + std::vector b_vec = + readBinaryFileIntoVector(params_dir + "conv1_biases.bin"); + conv1_->setWeightsAndBiases(w_vec, b_vec); + + w_vec = readBinaryFileIntoVector(params_dir + "conv2_weights.bin"); + b_vec = readBinaryFileIntoVector(params_dir + "conv2_biases.bin"); + conv2_->setWeightsAndBiases(w_vec, b_vec); + + std::vector w_dense1 = + readBinaryFileIntoVector(params_dir + "ip1_weights.bin"); + std::vector b_dense1 = + readBinaryFileIntoVector(params_dir + "ip1_biases.bin"); + dense1_->setWeightsAndBiases(w_dense1, b_dense1); + + std::vector w_dense2 = + readBinaryFileIntoVector(params_dir + "ip2_weights.bin"); + std::vector b_dense2 = + readBinaryFileIntoVector(params_dir + "ip2_biases.bin"); + + dense2_->setWeightsAndBiases(w_dense2, b_dense2); + + x_conv2_.resize(20 * 28 * 28); + x_dense1_.resize(50 * 12 * 12); + x_dense2_.resize(500); + + std::cout << "NET SETUP runtime: " << omp_get_wtime() - start << std::endl; +} + +std::vector EigenClassifier::classifyImages( + const std::vector> &image_list) { + std::vector predictions; + predictions.resize(image_list.size()); + +#ifdef _OPENMP // parallelization using OpenMP +#pragma omp parallel for num_threads(num_threads_) +#endif + for (int i = 0; i < image_list.size(); i++) { + if (image_list[i]->isContinuous()) { + std::vector x = imageToArray(*image_list[i]); + + std::vector predictions_i = forward(x); + // std::cout << i << " -- positive score: " << yi[1] << ", negative + // score: " << yi[0] << "\n"; + predictions[i] = predictions_i[1] - predictions_i[0]; + } + } + + return predictions; +} + +std::vector EigenClassifier::forward(const std::vector &x) { + // double start = omp_get_wtime(); + + // 1st conv layer + Eigen::MatrixXf H1 = conv1_->forward(x); + + // 1st max pool layer + Eigen::MatrixXf P1 = poolForward(H1, 2, 2); + + // 2nd conv layer + double conv2_start = omp_get_wtime(); + Eigen::Matrix P1r(P1); + // std::vector x_conv2; + x_conv2_.assign(P1r.data(), P1r.data() + P1r.size()); + Eigen::MatrixXf H2 = conv2_->forward(x_conv2_); + // std::cout << "CONV2 runtime: " << omp_get_wtime() - conv2_start << + // std::endl; + + // 2nd max pool layer + Eigen::MatrixXf P2 = poolForward(H2, 2, 2); + + // Flatten the output of the 2nd max pool layer. + Eigen::Map f1(P2.data(), P2.size()); + + // 1st inner product layer + // double dense1_start = omp_get_wtime(); + Eigen::VectorXf::Map(&x_dense1_[0], f1.size()) = f1; + Eigen::MatrixXf H3 = dense1_->forward(x_dense1_); + // std::cout << "DENSE1 runtime: " << omp_get_wtime() - dense1_start << + // std::endl; + + // RELU layer + H3 = H3.cwiseMax(0); + + // 2nd inner product layer (output layer) + Eigen::Map f2(H3.data(), H3.size()); + Eigen::VectorXf::Map(&x_dense2_[0], f2.size()) = f2; + Eigen::MatrixXf Y = dense2_->forward(x_dense2_); + + // std::cout << "FORWARD PASS runtime: " << omp_get_wtime() - start << + // std::endl; + // int n = Eigen::nbThreads(); + // std::cout << "# CPU threads: " << n << std::endl; + + std::vector y; + y.assign(Y.data(), Y.data() + Y.size()); + return y; +} + +std::vector EigenClassifier::imageToArray(const cv::Mat &img) const { + std::vector x; + x.resize(img.channels() * img.rows * img.cols); + int k = 0; + + for (int channel = 0; channel < img.channels(); channel++) { + for (int row = 0; row < img.rows; row++) { + const uchar *ptr = img.ptr(row); + + for (int col = 0; col < img.cols; col++) { + const uchar *uc_pixel = ptr; + x[k] = uc_pixel[channel]; + ptr += 15; + k++; + } + } + } + + return x; +} + +Eigen::MatrixXf EigenClassifier::poolForward(const Eigen::MatrixXf &X, + int filter_size, + int stride) const { + int depth = X.rows(); + int width_in = sqrt(X.cols()); + int width_out = (width_in - filter_size) / stride + 1; + + int block_size = filter_size * filter_size; + + Eigen::MatrixXf M(depth, width_out * width_out); + + for (int i = 0; i < X.rows(); i++) { + int row_in = 0; + int col_in = 0; + + for (int j = 0; j < width_out * width_out; j++) { + int start = row_in * width_in + col_in; + + Eigen::VectorXf block(filter_size * filter_size); + block << X(i, start), X(i, start + 1), X(i, start + width_in), + X(i, start + width_in + 1); + M(i, j) = block.maxCoeff(); + + col_in += stride; + if (col_in == width_in) { + row_in += stride; + col_in = 0; + } + } + } + + return M; +} + +std::vector EigenClassifier::readBinaryFileIntoVector( + const std::string &location) { + std::vector vals; + + std::ifstream file(location.c_str(), std::ios::binary | std::ios::in); + if (!file.is_open()) { + std::cout << "ERROR: Cannot open file: " << location << "!\n"; + vals.resize(0); + return vals; + } + + float x; + while (file.read(reinterpret_cast(&x), sizeof(float))) { + vals.push_back(x); + } + + file.close(); + + return vals; +} + +} // namespace net +} // namespace gpd diff --git a/src/gpd/net/layer.cpp b/src/gpd/net/layer.cpp new file mode 100644 index 0000000..68a7e17 --- /dev/null +++ b/src/gpd/net/layer.cpp @@ -0,0 +1 @@ +#include diff --git a/src/gpd/net/openvino_classifier.cpp b/src/gpd/net/openvino_classifier.cpp new file mode 100644 index 0000000..fba0266 --- /dev/null +++ b/src/gpd/net/openvino_classifier.cpp @@ -0,0 +1,103 @@ +#include + +namespace gpd { +namespace net { + +using namespace InferenceEngine; + +std::map + OpenVinoClassifier::device_map_ = { + {Classifier::Device::eCPU, TargetDevice::eCPU}, + {Classifier::Device::eGPU, TargetDevice::eGPU}, + {Classifier::Device::eVPU, TargetDevice::eMYRIAD}, + {Classifier::Device::eFPGA, TargetDevice::eFPGA}}; + +OpenVinoClassifier::OpenVinoClassifier(const std::string &model_file, + const std::string &weights_file, + Classifier::Device device, + int batch_size) { + InferenceEngine::PluginDispatcher dispatcher({"../../../lib/intel64", ""}); + plugin_ = InferencePlugin(dispatcher.getSuitablePlugin(device_map_[device])); + + CNNNetReader network_reader; + network_reader.ReadNetwork(model_file); + network_reader.ReadWeights(weights_file); + network_ = network_reader.getNetwork(); + network_.setBatchSize(batch_size); + + InputsDataMap input = network_.getInputsInfo(); + input.begin()->second->setPrecision(Precision::FP32); + + OutputsDataMap output = network_.getOutputsInfo(); + output.begin()->second->setPrecision(Precision::FP32); + + infer_request_ = plugin_.LoadNetwork(network_, {}).CreateInferRequest(); + auto output_name = output.begin()->first; + output_blob_ = infer_request_.GetBlob(output_name); +} + +std::vector OpenVinoClassifier::classifyImages( + const std::vector> &image_list) { + std::vector predictions(0); + InputsDataMap input_info = network_.getInputsInfo(); + + for (const auto &item : input_info) { + Blob::Ptr input = infer_request_.GetBlob(item.first); + SizeVector dims = input->getTensorDesc().getDims(); + size_t channels = dims[1]; + size_t rows = dims[2]; + size_t cols = dims[3]; + size_t image_size = rows * cols; + auto data = + input->buffer().as::value_type *>(); + int num_iter = (int)ceil(image_list.size() / (double)getBatchSize()); + + for (size_t i = 0; i < num_iter; i++) { + int n = std::min(getBatchSize(), + (int)(image_list.size() - i * getBatchSize())); + for (size_t b = 0; b < n; b++) { + int image_id = i * getBatchSize() + b; + for (int r = 0; r < rows; r++) { + for (int c = 0; c < cols; c++) { + for (int ch = 0; ch < channels; ch++) { + int src_idx[3] = {r, c, ch}; + int dst_idx = + b * image_size * channels + ch * image_size + r * cols + c; + data[dst_idx] = image_list[image_id]->at(src_idx); + } + } + } + if (n < getBatchSize()) { + for (int b = n; b < getBatchSize(); b++) { + for (int r = 0; r < rows; r++) { + for (int c = 0; c < cols; c++) { + for (int ch = 0; ch < channels; ch++) { + int dst_idx = b * image_size * channels + ch * image_size + + r * cols + c; + data[dst_idx] = 0; + } + } + } + } + } + } + infer_request_.Infer(); + + auto output_data = + output_blob_->buffer() + .as::value_type *>(); + const int resultsCnt = output_blob_->size() / getBatchSize(); + + for (int j = 0; j < n; j++) { + predictions.push_back(output_data[2 * j + 1] - output_data[2 * j]); + } + } + } + + return predictions; +} + +int OpenVinoClassifier::getBatchSize() const { return network_.getBatchSize(); } + +} // namespace net +} // namespace gpd diff --git a/src/gpd/openvino_classifier.cpp b/src/gpd/openvino_classifier.cpp deleted file mode 100644 index 2001d60..0000000 --- a/src/gpd/openvino_classifier.cpp +++ /dev/null @@ -1,123 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2018 Intel Corporation - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * 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. - */ - -#include "gpd/openvino_classifier.h" - -using namespace InferenceEngine; - -std::map OpenVINOClassifier::device_map_ = { - {Classifier::Device::eCPU, TargetDevice::eCPU}, - {Classifier::Device::eGPU, TargetDevice::eGPU}, - {Classifier::Device::eVPU, TargetDevice::eMYRIAD}, - {Classifier::Device::eFPGA, TargetDevice::eFPGA}}; - -OpenVINOClassifier::OpenVINOClassifier(Classifier::Device device) -{ - // ---------------------Load MKLDNN Plugin for Inference Engine------------------------------------------------------- - InferenceEngine::PluginDispatcher dispatcher({"../../../lib/intel64", ""}); - plugin_ = InferencePlugin(dispatcher.getSuitablePlugin(device_map_[device])); - // --------------------Load IR Generated by ModelOptimizer (.xml and .bin files)-------------------------------------- - CNNNetReader network_reader; - switch (device) - { - case Classifier::Device::eCPU: - network_reader.ReadNetwork(std::string(MODELS_DIR) + "/fp32/single_view_15_channels.xml"); - network_reader.ReadWeights(std::string(MODELS_DIR) + "/fp32/single_view_15_channels.bin"); - break; - case Classifier::Device::eVPU: - network_reader.ReadNetwork(std::string(MODELS_DIR) + "/fp16/single_view_15_channels.xml"); - network_reader.ReadWeights(std::string(MODELS_DIR) + "/fp16/single_view_15_channels.bin"); - break; - case Classifier::Device::eGPU: - std::cout << "GPU device to be supported!!\n"; - // fall through; - case Classifier::Device::eFPGA: - std::cout << "FPGA device to be supported!!\n"; - // fall through; - default: - throw std::exception(); - } - network_ = network_reader.getNetwork(); - network_.setBatchSize(1); - - // -----------------------------Prepare input blobs------------------------------------------------------------------- - auto input_info = network_.getInputsInfo().begin()->second; - auto input_name = network_.getInputsInfo().begin()->first; - input_info->setPrecision(Precision::FP32); - - // ---------------------------Prepare output blobs-------------------------------------------------------------------- - auto output_info = network_.getOutputsInfo().begin()->second; - auto output_name = network_.getOutputsInfo().begin()->first; - output_info->setPrecision(Precision::FP32); - - // -------------------------Loading model to the plugin--------------------------------------------------------------- - std::cout << "network output: "<< output_name << ", input: " << input_name << "\n"; - infer_request_ = plugin_.LoadNetwork(network_, {}).CreateInferRequest(); - input_blob_ = infer_request_.GetBlob(input_name); - output_blob_ = infer_request_.GetBlob(output_name); - - // -------------------------Preparing the input blob buffer----------------------------------------------------------- - auto input_data = input_blob_->buffer().as::value_type*>(); - batch_image_list_.clear(); - for (int i = 0; i < input_blob_->dims()[2]; ++i) { - cv::Mat img(input_blob_->dims()[0], input_blob_->dims()[1], CV_32FC1, input_data); - batch_image_list_.push_back(img); - input_data += input_blob_->dims()[0] * input_blob_->dims()[1]; - } -} - -std::vector OpenVINOClassifier::classifyImages(const std::vector& image_list) -{ - std::vector predictions; - std::cout << "# images: " << image_list.size() << "\n"; - - for (int i = 0; i < image_list.size(); i++) - { - cv::Mat img; - image_list[i].convertTo(img, CV_32FC(input_blob_->dims()[2])); - cv::split(img, batch_image_list_); - - // ---------------------------Running the inference request synchronously------------------------------------------- - infer_request_.Infer(); - - // ---------------------------Postprocess output blobs-------------------------------------------------------------- - auto output_data = output_blob_->buffer().as::value_type*>(); - // std::cout << "positive score: " << output_data[1] << ", negative score: " << output_data[0] << "\n"; - predictions.push_back(output_data[1] - output_data[0]); - } - - return predictions; -} - -int OpenVINOClassifier::getBatchSize() const -{ - return network_.getBatchSize(); -} diff --git a/src/gpd/sequential_importance_sampling.cpp b/src/gpd/sequential_importance_sampling.cpp index d640eaa..83eabf5 100644 --- a/src/gpd/sequential_importance_sampling.cpp +++ b/src/gpd/sequential_importance_sampling.cpp @@ -1,100 +1,104 @@ -#include "../../include/gpd/sequential_importance_sampling.h" +#include +#include + +namespace gpd { // methods for sampling from a set of Gaussians const int SUM_OF_GAUSSIANS = 0; const int MAX_OF_GAUSSIANS = 1; -// standard parameters -const int SequentialImportanceSampling::NUM_ITERATIONS = 5; -const int SequentialImportanceSampling::NUM_SAMPLES = 50; -const int SequentialImportanceSampling::NUM_INIT_SAMPLES = 50; -const double SequentialImportanceSampling::PROB_RAND_SAMPLES = 0.3; -const double SequentialImportanceSampling::RADIUS = 0.02; -const bool SequentialImportanceSampling::VISUALIZE_STEPS = false; -const bool SequentialImportanceSampling::VISUALIZE_RESULTS = true; -const int SequentialImportanceSampling::SAMPLING_METHOD = SUM_OF_GAUSSIANS; - - -SequentialImportanceSampling::SequentialImportanceSampling(ros::NodeHandle& node) -{ - node.param("num_init_samples", num_init_samples_, NUM_INIT_SAMPLES); - node.param("num_iterations", num_iterations_, NUM_ITERATIONS); - node.param("num_samples_per_iteration", num_samples_, NUM_SAMPLES); - node.param("prob_rand_samples", prob_rand_samples_, PROB_RAND_SAMPLES); - node.param("std", radius_, RADIUS); - node.param("sampling_method", sampling_method_, SAMPLING_METHOD); - node.param("visualize_rounds", visualize_rounds_, false); - node.param("visualize_steps", visualize_steps_, VISUALIZE_STEPS); - node.param("visualize_results", visualize_results_, VISUALIZE_RESULTS); - node.param("filter_grasps", filter_grasps_, false); - node.param("num_threads", num_threads_, 1); - - node.getParam("workspace", workspace_); - node.getParam("workspace_grasps", workspace_grasps_); - - grasp_detector_ = new GraspDetector(node); +SequentialImportanceSampling::SequentialImportanceSampling( + const std::string &config_filename) { + // Read parameters from configuration file. + util::ConfigFile config_file(config_filename); + config_file.ExtractKeys(); + + num_init_samples_ = config_file.getValueOfKey("num_init_samples", 50); + num_iterations_ = config_file.getValueOfKey("num_iterations", 5); + num_samples_ = + config_file.getValueOfKey("num_samples_per_iteration", 50); + prob_rand_samples_ = + config_file.getValueOfKey("prob_rand_samples", 0.3); + radius_ = config_file.getValueOfKey("standard_deviation", 0.02); + sampling_method_ = + config_file.getValueOfKey("sampling_method", SUM_OF_GAUSSIANS); + min_score_ = config_file.getValueOfKey("min_score", 0); + + num_threads_ = config_file.getValueOfKey("num_threads", 1); + + workspace_ = + config_file.getValueOfKeyAsStdVectorDouble("workspace", "-1 1 -1 1 -1 1"); + workspace_grasps_ = config_file.getValueOfKeyAsStdVectorDouble( + "workspace_grasps", "-1 1 -1 1 -1 1"); + + filter_approach_direction_ = + config_file.getValueOfKey("filter_approach_direction", false); + std::vector approach = + config_file.getValueOfKeyAsStdVectorDouble("direction", "1 0 0"); + direction_ << approach[0], approach[1], approach[2]; + thresh_rad_ = config_file.getValueOfKey("thresh_rad", 2.3); + + visualize_rounds_ = + config_file.getValueOfKey("visualize_rounds", false); + visualize_steps_ = config_file.getValueOfKey("visualize_steps", false); + visualize_results_ = + config_file.getValueOfKey("visualize_results", false); + + grasp_detector_ = std::make_unique(config_filename); + + int min_inliers = config_file.getValueOfKey("min_inliers", 1); + clustering_ = std::make_unique(min_inliers); } - -std::vector SequentialImportanceSampling::detectGrasps(const CloudCamera& cloud_cam_in) -{ - // Check if the point cloud is empty. - if (cloud_cam_in.getCloudOriginal()->size() == 0) - { - ROS_INFO("Point cloud is empty!"); - std::vector grasps(0); +std::vector> +SequentialImportanceSampling::detectGrasps(util::Cloud &cloud) { + if (cloud.getCloudOriginal()->size() == 0) { + printf("Error: Point cloud is empty!"); + std::vector> grasps(0); return grasps; } double t0 = omp_get_wtime(); - CloudCamera cloud_cam = cloud_cam_in; - Plot plotter; - const PointCloudRGB::Ptr& cloud = cloud_cam.getCloudProcessed(); + + const candidate::HandGeometry &hand_geom = + grasp_detector_->getHandSearchParameters().hand_geometry_; + + util::Plot plotter( + grasp_detector_->getHandSearchParameters().hand_axes_.size(), + grasp_detector_->getHandSearchParameters().num_orientations_); // 1. Find initial grasp hypotheses. - cloud_cam.subsampleUniformly(num_init_samples_); - std::vector hand_set_list = grasp_detector_->generateGraspCandidates(cloud_cam); - std::cout << "Initially detected " << hand_set_list.size() << " grasp hypotheses" << std::endl; - if (hand_set_list.size() == 0) - { - std::vector empty_grasps(0); - return empty_grasps; + cloud.subsample(num_init_samples_); + if (visualize_steps_) { + plotter.plotSamples(cloud.getSampleIndices(), cloud.getCloudProcessed()); + } + std::vector> hand_set_list = + grasp_detector_->generateGraspCandidates(cloud); + printf("Initially detected grasp candidates: %zu\n", hand_set_list.size()); + if (hand_set_list.size() == 0) { + std::vector> grasps(0); + return grasps; } - // Filter grasps outside of workspace and robot hand dimensions. - std::vector filtered_candidates; - if (filter_grasps_) - { - grasp_detector_->filterGraspsWorkspace(hand_set_list, workspace_grasps_); - ROS_INFO_STREAM("Number of grasps within workspace: " << filtered_candidates.size()); + hand_set_list = + grasp_detector_->filterGraspsWorkspace(hand_set_list, workspace_grasps_); + printf("Grasps within workspace: %zu", hand_set_list.size()); + + if (filter_approach_direction_) { + hand_set_list = grasp_detector_->filterGraspsDirection( + hand_set_list, direction_, thresh_rad_); + if (visualize_rounds_) { + plotter.plotFingers3D(hand_set_list, cloud.getCloudOriginal(), + "Filtered Grasps (Approach)", hand_geom); + } } - if (visualize_rounds_) - { - plotter.plotFingers(hand_set_list, cloud_cam.getCloudOriginal(), "Initial Grasps"); + if (visualize_rounds_) { + plotter.plotFingers3D(hand_set_list, cloud.getCloudOriginal(), + "Initial Grasps", hand_geom); } - // Classify the grasps. -// classified_grasps = grasp_detector_->classifyGraspCandidates(cloud_cam, hands); -// std::cout << "Predicted " << filtered_candidates.size() << " valid grasps.\n"; -// -// if (visualize_steps_) -// { -// plotter.plotFingers(classified_grasps, cloud_cam.getCloudOriginal(), "Initial Grasps"); -// } -// -// if (classified_grasps.size() == 0) -// { -// return classified_grasps; -// } - -// if (classified_grasps.size() < 5) -// { -// classified_grasps = hands; -// } - - // 2. Create random generator for normal distribution. int num_rand_samples = prob_rand_samples_ * num_samples_; int num_gauss_samples = num_samples_ - num_rand_samples; double sigma = radius_; @@ -102,204 +106,167 @@ std::vector SequentialImportanceSampling::detectGrasps(const CloudCamera& diag_sigma.diagonal() << sigma, sigma, sigma; Eigen::Matrix3d inv_sigma = diag_sigma.inverse(); double term = 1.0 / sqrt(pow(2.0 * M_PI, 3.0) * pow(sigma, 3.0)); - boost::mt19937 *rng = new boost::mt19937(); - rng->seed(time(NULL)); - boost::normal_distribution<> distribution(0.0, 1.0); - boost::variate_generator > generator(*rng, distribution); Eigen::Matrix3Xd samples(3, num_samples_); -// std::vector hands_new = hands; - // 3. Find grasp hypotheses using importance sampling. - for (int i = 0; i < num_iterations_; i++) - { + // 2. Find grasp hypotheses using importance sampling. + for (int i = 0; i < num_iterations_; i++) { std::cout << i << " " << num_gauss_samples << std::endl; - // 3.1 Draw samples close to affordances (importance sampling). - if (this->sampling_method_ == SUM_OF_GAUSSIANS) - { - drawSamplesFromSumOfGaussians(hand_set_list, generator, sigma, num_gauss_samples, samples); - } - else if (this->sampling_method_ == MAX_OF_GAUSSIANS) // max of Gaussians - { - drawSamplesFromMaxOfGaussians(hand_set_list, generator, sigma, num_gauss_samples, samples, term); - } - else - { -// drawWeightedSamples(hand_set_list, generator, sigma, num_gauss_samples, samples); + // 2.1 Draw samples close to existing affordances. + if (this->sampling_method_ == SUM_OF_GAUSSIANS) { + drawSamplesFromSumOfGaussians(hand_set_list, sigma, num_gauss_samples, + samples); + } else if (this->sampling_method_ == MAX_OF_GAUSSIANS) { + drawSamplesFromMaxOfGaussians(hand_set_list, sigma, num_gauss_samples, + samples, term); } - // 3.2 Draw random samples. - for (int j = num_samples_ - num_rand_samples; j < num_samples_; j++) - { - int r = std::rand() % cloud->points.size(); -// while (!pcl::isFinite((*cloud)[r]) -// || !this->affordances.isPointInWorkspace(cloud->points[r].x, cloud->points[r].y, cloud->points[r].z)) -// r = std::rand() % cloud->points.size(); - samples.col(j) = cloud->points[r].getVector3fMap().cast(); - } + // 2.2 Draw random samples. + drawUniformSamples(cloud, num_rand_samples, num_samples_ - num_rand_samples, + samples); - // 3.3 Evaluate grasp hypotheses at . - cloud_cam.setSamples(samples); - std::vector hand_set_list_new = grasp_detector_->generateGraspCandidates(cloud_cam); + // 2.3 Evaluate grasp hypotheses at . + cloud.setSamples(samples); + std::vector> hand_set_list_new = + grasp_detector_->generateGraspCandidates(cloud); - if (filter_grasps_) - { - grasp_detector_->filterGraspsWorkspace(hand_set_list_new, workspace_grasps_); - ROS_INFO_STREAM("Number of grasps within gripper width range and workspace: " << hand_set_list_new.size()); - } + hand_set_list_new = grasp_detector_->filterGraspsWorkspace( + hand_set_list_new, workspace_grasps_); - hand_set_list.insert(hand_set_list.end(), hand_set_list_new.begin(), hand_set_list_new.end()); + if (filter_approach_direction_) { + hand_set_list_new = grasp_detector_->filterGraspsDirection( + hand_set_list_new, direction_, thresh_rad_); + if (visualize_steps_) { + plotter.plotFingers3D(hand_set_list_new, cloud.getCloudOriginal(), + "Filtered Grasps (Approach)", hand_geom); + } + } - if (visualize_rounds_) - { - plotter.plotSamples(samples, cloud); - plotter.plotFingers(hand_set_list_new, cloud_cam.getCloudProcessed(), "New Grasps"); + if (visualize_rounds_) { + plotter.plotSamples(samples, cloud.getCloudProcessed()); + plotter.plotFingers3D(hand_set_list_new, cloud.getCloudOriginal(), + "New Grasps", hand_geom); } - std::cout << "Added: " << hand_set_list_new.size() << ", total: " << hand_set_list.size() - << " grasp candidate sets in round " << i << std::endl; + hand_set_list.insert(hand_set_list.end(), + std::make_move_iterator(hand_set_list_new.begin()), + std::make_move_iterator(hand_set_list_new.end())); + + printf("Added %zu grasp candidates in round %d. Total: %zu.\n", + hand_set_list_new.size(), i, hand_set_list.size()); } - if (visualize_steps_) - { - plotter.plotFingers(hand_set_list, cloud_cam.getCloudOriginal(), "Grasp Candidates"); + + if (visualize_steps_) { + plotter.plotFingers3D(hand_set_list, cloud.getCloudOriginal(), + "Grasp Candidates", hand_geom); } - // Classify the grasps. - std::vector valid_grasps; - valid_grasps = grasp_detector_->classifyGraspCandidates(cloud_cam, hand_set_list); - std::cout << "Predicted " << valid_grasps.size() << " valid grasps.\n"; - if (visualize_steps_) - { - plotter.plotFingers(valid_grasps, cloud_cam.getCloudOriginal(), "Valid Grasps"); + // 3. Classify the grasps. + std::vector> valid_grasps; + valid_grasps = + grasp_detector_->pruneGraspCandidates(cloud, hand_set_list, min_score_); + printf("Valid grasps: %zu\n", valid_grasps.size()); + if (visualize_steps_) { + plotter.plotFingers3D(valid_grasps, cloud.getCloudOriginal(), + "Valid Grasps", hand_geom); } // 4. Cluster the grasps. - valid_grasps = grasp_detector_->findClusters(valid_grasps); - std::cout << "Final result: found " << valid_grasps.size() << " clusters.\n"; - std::cout << "Total runtime: " << omp_get_wtime() - t0 << " sec.\n"; - - if (visualize_results_ || visualize_steps_) - { - plotter.plotFingers(valid_grasps, cloud_cam.getCloudOriginal(), "Clusters"); + if (clustering_->getMinInliers() > 0) { + valid_grasps = clustering_->findClusters(valid_grasps); } + printf("Final result: found %zu grasps.\n", valid_grasps.size()); + printf("Total runtime: %3.4fs\n.\n", omp_get_wtime() - t0); - // Remove grasps that are very close in position and approach direction. -// std::set unique_grasps; -// for (int i = 0; i < classified_grasps.size(); ++i) -// { -// unique_grasps.insert(classified_grasps[i]); -// } -// std::cout << "Found " << unique_grasps.size() << " unique clusters.\n"; -// -// classified_grasps.resize(unique_grasps.size()); -// std::copy(unique_grasps.begin(), unique_grasps.end(), classified_grasps.begin()); + if (visualize_results_ || visualize_steps_) { + plotter.plotFingers3D(valid_grasps, cloud.getCloudOriginal(), "Clusters", + hand_geom); + } return valid_grasps; } - -void SequentialImportanceSampling::preprocessPointCloud(CloudCamera& cloud_cam) -{ - std::cout << "Processing cloud with: " << cloud_cam.getCloudOriginal()->size() << " points.\n"; - cloud_cam.filterWorkspace(workspace_); - std::cout << "After workspace filtering: " << cloud_cam.getCloudProcessed()->size() << " points left.\n"; - cloud_cam.voxelizeCloud(0.003); - std::cout << "After voxelizing: " << cloud_cam.getCloudProcessed()->size() << " points left.\n"; - cloud_cam.calculateNormals(num_threads_); -} - - -void SequentialImportanceSampling::drawSamplesFromSumOfGaussians(const std::vector& hands, - Gaussian& generator, double sigma, int num_gauss_samples, Eigen::Matrix3Xd& samples_out) -{ - for (std::size_t j = 0; j < num_gauss_samples; j++) - { - int idx = rand() % hands.size(); - samples_out(0, j) = hands[idx].getSample()(0) + generator() * sigma; - samples_out(1, j) = hands[idx].getSample()(1) + generator() * sigma; - samples_out(2, j) = hands[idx].getSample()(2) + generator() * sigma; +void SequentialImportanceSampling::drawSamplesFromSumOfGaussians( + const std::vector> &hand_sets, + double sigma, int num_gauss_samples, Eigen::Matrix3Xd &samples_out) { + static std::random_device rd{}; + static std::mt19937 gen{rd()}; + static std::normal_distribution distr{0.0, sigma}; + for (std::size_t j = 0; j < num_gauss_samples; j++) { + int idx = rand() % hand_sets.size(); + Eigen::Vector3d rand_vec; + rand_vec << distr(gen), distr(gen), distr(gen); + samples_out.col(j) = hand_sets[idx]->getSample() + rand_vec; } } - -void SequentialImportanceSampling::drawSamplesFromMaxOfGaussians(const std::vector& hands, - Gaussian& generator, double sigma, int num_gauss_samples, Eigen::Matrix3Xd& samples_out, double term) -{ +void SequentialImportanceSampling::drawSamplesFromMaxOfGaussians( + const std::vector> &hand_sets, + double sigma, int num_gauss_samples, Eigen::Matrix3Xd &samples_out, + double term) { int j = 0; - while (j < num_gauss_samples) // draw samples using rejection sampling - { - int idx = rand() % hands.size(); - Eigen::Vector3d x; - x(0) = hands[idx].getSample()(0) + generator() * sigma; - x(1) = hands[idx].getSample()(1) + generator() * sigma; - x(2) = hands[idx].getSample()(2) + generator() * sigma; + static std::random_device rd{}; + static std::mt19937 gen{rd()}; + static std::normal_distribution distr{0.0, sigma}; + + // Draw samples using rejection sampling. + while (j < num_gauss_samples) { + int idx = rand() % hand_sets.size(); + Eigen::Vector3d rand_vec; + rand_vec << distr(gen), distr(gen), distr(gen); + Eigen::Vector3d x = hand_sets[idx]->getSample() + rand_vec; double maxp = 0; - for (std::size_t k = 0; k < hands.size(); k++) - { - double p = (x - hands[k].getSample()).transpose() * (x - hands[k].getSample()); + for (std::size_t k = 0; k < hand_sets.size(); k++) { + double p = (x - hand_sets[k]->getSample()).transpose() * + (x - hand_sets[k]->getSample()); p = term * exp((-1.0 / (2.0 * sigma)) * p); - if (p > maxp) + if (p > maxp) { maxp = p; + } } - double p = (x - hands[idx].getSample()).transpose() * (x - hands[idx].getSample()); + double p = (x - hand_sets[idx]->getSample()).transpose() * + (x - hand_sets[idx]->getSample()); p = term * exp((-1.0 / (2.0 * sigma)) * p); - if (p >= maxp) - { + if (p >= maxp) { samples_out.col(j) = x; j++; } } } - -void SequentialImportanceSampling::drawWeightedSamples(const std::vector& hands, Gaussian& generator, - double sigma, int num_gauss_samples, Eigen::Matrix3Xd& samples_out) -{ - Eigen::VectorXd scores(hands.size()); - double sum = 0.0; - for (int i = 0; i < hands.size(); i++) - { - scores(i) = hands[i].getScore(); - sum += scores(i); - } - - for (int i = 0; i < hands.size(); i++) - { - scores(i) /= sum; - } - - boost::mt19937 *rng = new boost::mt19937(); - rng->seed(time(NULL)); - boost::uniform_real<> distribution(0.0, 1.0); - boost::variate_generator > uniform_generator(*rng, distribution); -// std::cout << "scores\n" << scores << std::endl; - - for (int i = 0; i < num_gauss_samples; i++) - { - double r = uniform_generator(); - double x = 0.0; - int idx = -1; - - for (int j = 0; j < scores.size(); j++) - { - x += scores(j); - if (r < x) - { - idx = j; - break; - } +void SequentialImportanceSampling::drawUniformSamples( + const util::Cloud &cloud, int num_samples, int start_idx, + Eigen::Matrix3Xd &samples) { + int i = 0; + while (i < num_samples) { + int idx; + Eigen::Vector3d sample; + if (cloud.getSampleIndices().size() > 0) { + int idx = cloud.getSampleIndices()[std::rand() % + cloud.getSampleIndices().size()]; + sample = cloud.getCloudProcessed() + ->points[idx] + .getVector3fMap() + .cast(); + } else if (cloud.getSamples().size() > 0) { + int idx = std::rand() % cloud.getSamples().size(); + sample = cloud.getSamples().col(idx); + } else { + int idx = std::rand() % cloud.getCloudProcessed()->points.size(); + sample = cloud.getCloudProcessed() + ->points[idx] + .getVector3fMap() + .cast(); } - - if (idx > -1) - { -// std::cout << "r: " << r << ", idx: " << idx << std::endl; - samples_out(0,i) = hands[idx].getGraspSurface()(0) + generator() * sigma; - samples_out(1,i) = hands[idx].getGraspSurface()(1) + generator() * sigma; - samples_out(2,i) = hands[idx].getGraspSurface()(2) + generator() * sigma; + if (sample(0) >= workspace_[0] && sample(0) <= workspace_[1] && + sample(1) >= workspace_[2] && sample(1) <= workspace_[3] && + sample(2) >= workspace_[4] && sample(2) <= workspace_[5]) { + samples.col(start_idx + i) = sample; + i++; } - else - std::cout << "Error: idx is " << idx << std::endl; } } + +} // namespace gpd diff --git a/src/gpd/util/cloud.cpp b/src/gpd/util/cloud.cpp new file mode 100644 index 0000000..2b8420a --- /dev/null +++ b/src/gpd/util/cloud.cpp @@ -0,0 +1,665 @@ +#include + +#include +#include +#include +#include + +namespace gpd { +namespace util { + +Cloud::Cloud() + : cloud_original_(new PointCloudRGB), cloud_processed_(new PointCloudRGB) { + view_points_.resize(3, 1); + view_points_ << 0.0, 0.0, 0.0; + sample_indices_.resize(0); + samples_.resize(3, 0); + normals_.resize(3, 0); +} + +Cloud::Cloud(const PointCloudRGB::Ptr &cloud, + const Eigen::MatrixXi &camera_source, + const Eigen::Matrix3Xd &view_points) + : cloud_processed_(new PointCloudRGB), + cloud_original_(new PointCloudRGB), + camera_source_(camera_source), + view_points_(view_points) { + sample_indices_.resize(0); + samples_.resize(3, 0); + normals_.resize(3, 0); + + pcl::copyPointCloud(*cloud, *cloud_original_); + *cloud_processed_ = *cloud_original_; +} + +Cloud::Cloud(const PointCloudPointNormal::Ptr &cloud, + const Eigen::MatrixXi &camera_source, + const Eigen::Matrix3Xd &view_points) + : cloud_processed_(new PointCloudRGB), + cloud_original_(new PointCloudRGB), + camera_source_(camera_source), + view_points_(view_points) { + sample_indices_.resize(0); + samples_.resize(3, 0); + normals_.resize(3, 0); + + pcl::copyPointCloud(*cloud, *cloud_original_); + *cloud_processed_ = *cloud_original_; +} + +Cloud::Cloud(const PointCloudPointNormal::Ptr &cloud, int size_left_cloud, + const Eigen::Matrix3Xd &view_points) + : cloud_processed_(new PointCloudRGB), + cloud_original_(new PointCloudRGB), + view_points_(view_points) { + sample_indices_.resize(0); + samples_.resize(3, 0); + + pcl::copyPointCloud(*cloud, *cloud_original_); + *cloud_processed_ = *cloud_original_; + + // set the camera source matrix: (i,j) = 1 if point j is seen by camera i + if (size_left_cloud == 0) // one camera + { + camera_source_ = Eigen::MatrixXi::Ones(1, cloud->size()); + } else // two cameras + { + int size_right_cloud = cloud->size() - size_left_cloud; + camera_source_ = Eigen::MatrixXi::Zero(2, cloud->size()); + camera_source_.block(0, 0, 1, size_left_cloud) = + Eigen::MatrixXi::Ones(1, size_left_cloud); + camera_source_.block(1, size_left_cloud, 1, size_right_cloud) = + Eigen::MatrixXi::Ones(1, size_right_cloud); + } + + normals_.resize(3, cloud->size()); + for (int i = 0; i < cloud->size(); i++) { + normals_.col(i) << cloud->points[i].normal_x, cloud->points[i].normal_y, + cloud->points[i].normal_z; + } +} + +Cloud::Cloud(const PointCloudRGB::Ptr &cloud, int size_left_cloud, + const Eigen::Matrix3Xd &view_points) + : cloud_processed_(cloud), + cloud_original_(cloud), + view_points_(view_points) { + sample_indices_.resize(0); + samples_.resize(3, 0); + normals_.resize(3, 0); + + // set the camera source matrix: (i,j) = 1 if point j is seen by camera i + if (size_left_cloud == 0) // one camera + { + camera_source_ = Eigen::MatrixXi::Ones(1, cloud->size()); + } else // two cameras + { + int size_right_cloud = cloud->size() - size_left_cloud; + camera_source_ = Eigen::MatrixXi::Zero(2, cloud->size()); + camera_source_.block(0, 0, 1, size_left_cloud) = + Eigen::MatrixXi::Ones(1, size_left_cloud); + camera_source_.block(1, size_left_cloud, 1, size_right_cloud) = + Eigen::MatrixXi::Ones(1, size_right_cloud); + } +} + +Cloud::Cloud(const std::string &filename, const Eigen::Matrix3Xd &view_points) + : cloud_processed_(new PointCloudRGB), + cloud_original_(new PointCloudRGB), + view_points_(view_points) { + sample_indices_.resize(0); + samples_.resize(3, 0); + normals_.resize(3, 0); + cloud_processed_ = loadPointCloudFromFile(filename); + cloud_original_ = cloud_processed_; + camera_source_ = Eigen::MatrixXi::Ones(1, cloud_processed_->size()); + std::cout << "Loaded point cloud with " << camera_source_.cols() + << " points \n"; +} + +Cloud::Cloud(const std::string &filename_left, + const std::string &filename_right, + const Eigen::Matrix3Xd &view_points) + : cloud_processed_(new PointCloudRGB), + cloud_original_(new PointCloudRGB), + view_points_(view_points) { + sample_indices_.resize(0); + samples_.resize(3, 0); + normals_.resize(3, 0); + + // load and combine the two point clouds + std::cout << "Loading point clouds ...\n"; + PointCloudRGB::Ptr cloud_left(new PointCloudRGB), + cloud_right(new PointCloudRGB); + cloud_left = loadPointCloudFromFile(filename_left); + cloud_right = loadPointCloudFromFile(filename_right); + + std::cout << "Concatenating point clouds ...\n"; + *cloud_processed_ = *cloud_left + *cloud_right; + cloud_original_ = cloud_processed_; + + std::cout << "Loaded left point cloud with " << cloud_left->size() + << " points \n"; + std::cout << "Loaded right point cloud with " << cloud_right->size() + << " points \n"; + + // set the camera source matrix: (i,j) = 1 if point j is seen by camera i + camera_source_ = Eigen::MatrixXi::Zero(2, cloud_processed_->size()); + camera_source_.block(0, 0, 1, cloud_left->size()) = + Eigen::MatrixXi::Ones(1, cloud_left->size()); + camera_source_.block(1, cloud_left->size(), 1, cloud_right->size()) = + Eigen::MatrixXi::Ones(1, cloud_right->size()); +} + +void Cloud::removeNans() { + pcl::PointIndices::Ptr inliers(new pcl::PointIndices()); + pcl::removeNaNFromPointCloud(*cloud_processed_, inliers->indices); + if (inliers->indices.size() < cloud_processed_->size()) { + pcl::ExtractIndices eifilter(true); + eifilter.setInputCloud(cloud_processed_); + eifilter.setIndices(inliers); + eifilter.filter(*cloud_processed_); + printf("Cloud after removing NANs: %zu\n", cloud_processed_->size()); + } +} + +void Cloud::removeStatisticalOutliers() { + pcl::StatisticalOutlierRemoval sor; + sor.setInputCloud(cloud_processed_); + sor.setMeanK(50); + sor.setStddevMulThresh(1.0); + sor.filter(*cloud_processed_); + printf("Cloud after removing statistical outliers: %zu\n", + cloud_processed_->size()); +} + +void Cloud::refineNormals(int k) { + std::vector> k_indices; + std::vector> k_sqr_distances; + pcl::search::KdTree search; + search.setInputCloud(cloud_processed_); + search.nearestKSearch(*cloud_processed_, std::vector(), k, k_indices, + k_sqr_distances); + + pcl::PointCloud pcl_normals; + pcl_normals.resize(normals_.cols()); + for (int i = 0; i < normals_.cols(); i++) { + pcl_normals.at(i).getNormalVector3fMap() = normals_.col(i).cast(); + } + + pcl::PointCloud normals_refined; + pcl::NormalRefinement nr(k_indices, k_sqr_distances); + nr.setInputCloud(pcl_normals.makeShared()); + nr.filter(normals_refined); + + Eigen::MatrixXf diff = + normals_refined.getMatrixXfMap() - pcl_normals.getMatrixXfMap(); + printf("Refining surface normals ...\n"); + printf(" mean: %.3f, max: %.3f, sum: %.3f\n", diff.mean(), diff.maxCoeff(), + diff.sum()); + + normals_ = normals_refined.getMatrixXfMap() + .block(0, 0, 3, pcl_normals.size()) + .cast(); +} + +void Cloud::filterWorkspace(const std::vector &workspace) { + // Filter indices into the point cloud. + if (sample_indices_.size() > 0) { + std::vector indices_to_keep; + + for (int i = 0; i < sample_indices_.size(); i++) { + const pcl::PointXYZRGBA &p = cloud_processed_->points[sample_indices_[i]]; + if (p.x > workspace[0] && p.x < workspace[1] && p.y > workspace[2] && + p.y < workspace[3] && p.z > workspace[4] && p.z < workspace[5]) { + indices_to_keep.push_back(i); + } + } + + sample_indices_ = indices_to_keep; + std::cout << sample_indices_.size() + << " sample indices left after workspace filtering \n"; + } + + // Filter (x,y,z)-samples. + if (samples_.cols() > 0) { + std::vector indices_to_keep; + + for (int i = 0; i < samples_.cols(); i++) { + if (samples_(0, i) > workspace[0] && samples_(0, i) < workspace[1] && + samples_(1, i) > workspace[2] && samples_(1, i) < workspace[3] && + samples_(2, i) > workspace[4] && samples_(2, i) < workspace[5]) { + indices_to_keep.push_back(i); + } + } + + samples_ = EigenUtils::sliceMatrix(samples_, indices_to_keep); + std::cout << samples_.cols() + << " samples left after workspace filtering \n"; + } + + // Filter the point cloud. + std::vector indices; + for (int i = 0; i < cloud_processed_->size(); i++) { + const pcl::PointXYZRGBA &p = cloud_processed_->points[i]; + if (p.x > workspace[0] && p.x < workspace[1] && p.y > workspace[2] && + p.y < workspace[3] && p.z > workspace[4] && p.z < workspace[5]) { + indices.push_back(i); + } + } + + Eigen::MatrixXi camera_source(camera_source_.rows(), indices.size()); + PointCloudRGB::Ptr cloud(new PointCloudRGB); + cloud->points.resize(indices.size()); + for (int i = 0; i < indices.size(); i++) { + camera_source.col(i) = camera_source_.col(indices[i]); + cloud->points[i] = cloud_processed_->points[indices[i]]; + } + if (normals_.cols() > 0) { + Eigen::Matrix3Xd normals(3, indices.size()); + for (int i = 0; i < indices.size(); i++) { + normals.col(i) = normals_.col(indices[i]); + } + normals_ = normals; + } + cloud_processed_ = cloud; + camera_source_ = camera_source; +} + +void Cloud::filterSamples(const std::vector &workspace) { + std::vector indices; + for (int i = 0; i < samples_.size(); i++) { + if (samples_(0, i) > workspace[0] && samples_(0, i) < workspace[1] && + samples_(1, i) > workspace[2] && samples_(1, i) < workspace[3] && + samples_(2, i) > workspace[4] && samples_(2, i) < workspace[5]) { + indices.push_back(i); + } + } + + Eigen::Matrix3Xd filtered_samples(3, indices.size()); + for (int i = 0; i < indices.size(); i++) { + filtered_samples.col(i) = samples_.col(i); + } + samples_ = filtered_samples; +} + +void Cloud::voxelizeCloud(float cell_size) { + // Find the cell that each point falls into. + pcl::PointXYZRGBA min_pt_pcl; + pcl::PointXYZRGBA max_pt_pcl; + pcl::getMinMax3D(*cloud_processed_, min_pt_pcl, max_pt_pcl); + const Eigen::Vector3f min_pt = min_pt_pcl.getVector3fMap(); + std::set bins; + Eigen::Matrix3Xd avg_normals = + Eigen::Matrix3Xd::Zero(3, cloud_processed_->size()); + Eigen::VectorXi counts = Eigen::VectorXi::Zero(cloud_processed_->size()); + + for (int i = 0; i < cloud_processed_->size(); i++) { + const Eigen::Vector3f pt = cloud_processed_->at(i).getVector3fMap(); + Eigen::Vector4i v4; + v4.head(3) = EigenUtils::floorVector((pt - min_pt) / cell_size); + v4(3) = i; + std::pair::iterator, + bool> + res = bins.insert(v4); + + if (normals_.cols() > 0) { + const int &idx = (*res.first)(3); + avg_normals.col(idx) += normals_.col(i); + counts(idx)++; + } + } + + // Calculate the point value and the average surface normal for each cell, and + // set the camera source for each point. + Eigen::Matrix3Xf voxels(3, bins.size()); + Eigen::Matrix3Xd normals(3, bins.size()); + Eigen::MatrixXi camera_source(camera_source_.rows(), bins.size()); + int i = 0; + std::set::iterator it; + + for (it = bins.begin(); it != bins.end(); it++) { + voxels.col(i) = min_pt + cell_size * (*it).head(3).cast(); + const int &idx = (*it)(3); + + for (int j = 0; j < camera_source_.rows(); j++) { + camera_source(j, i) = (camera_source_(j, idx) == 1) ? 1 : 0; + } + if (normals_.cols() > 0) { + normals.col(i) = avg_normals.col(idx) / (double)counts(idx); + } + i++; + } + + // Copy the voxels into the point cloud. + cloud_processed_->points.resize(voxels.cols()); + for (int i = 0; i < voxels.cols(); i++) { + cloud_processed_->points[i].getVector3fMap() = voxels.col(i); + } + + camera_source_ = camera_source; + + if (normals_.cols() > 0) { + normals_ = normals; + } + + printf("Voxelized cloud: %zu\n", cloud_processed_->size()); +} + +void Cloud::subsample(int num_samples) { + if (num_samples == 0) { + return; + } + + if (samples_.cols() > 0) { + subsampleSamples(num_samples); + } else if (sample_indices_.size() > 0) { + subsampleSampleIndices(num_samples); + } else { + subsampleUniformly(num_samples); + } +} + +void Cloud::subsampleUniformly(int num_samples) { + sample_indices_.resize(num_samples); + pcl::RandomSample random_sample; + random_sample.setInputCloud(cloud_processed_); + random_sample.setSample(num_samples); + random_sample.filter(sample_indices_); +} + +void Cloud::subsampleSamples(int num_samples) { + if (num_samples == 0 || num_samples >= samples_.cols()) { + return; + } else { + std::cout << "Using " << num_samples << " out of " << samples_.cols() + << " available samples.\n"; + std::vector seq(samples_.cols()); + for (int i = 0; i < seq.size(); i++) { + seq[i] = i; + } + std::random_shuffle(seq.begin(), seq.end()); + + Eigen::Matrix3Xd subsamples(3, num_samples); + for (int i = 0; i < num_samples; i++) { + subsamples.col(i) = samples_.col(seq[i]); + } + samples_ = subsamples; + + std::cout << "Subsampled " << samples_.cols() + << " samples at random uniformly.\n"; + } +} + +void Cloud::subsampleSampleIndices(int num_samples) { + if (sample_indices_.size() == 0 || num_samples >= sample_indices_.size()) { + return; + } + + std::vector indices(num_samples); + for (int i = 0; i < num_samples; i++) { + indices[i] = sample_indices_[rand() % sample_indices_.size()]; + } + sample_indices_ = indices; +} + +void Cloud::sampleAbovePlane() { + double t0 = omp_get_wtime(); + printf("Sampling above plane ...\n"); + std::vector indices(0); + pcl::SACSegmentation seg; + pcl::PointIndices::Ptr inliers(new pcl::PointIndices); + pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients); + seg.setInputCloud(cloud_processed_); + seg.setOptimizeCoefficients(true); + seg.setModelType(pcl::SACMODEL_PLANE); + seg.setMethodType(pcl::SAC_RANSAC); + seg.setDistanceThreshold(0.01); + seg.segment(*inliers, *coefficients); + if (inliers->indices.size() > 0) { + pcl::ExtractIndices extract; + extract.setInputCloud(cloud_processed_); + extract.setIndices(inliers); + extract.setNegative(true); + extract.filter(indices); + } + if (indices.size() > 0) { + sample_indices_ = indices; + printf(" Plane fit succeeded. %zu samples above plane.\n", + sample_indices_.size()); + } else { + printf(" Plane fit failed. Using entire point cloud ...\n"); + } + std::cout << " runtime (plane fit): " << omp_get_wtime() - t0 << "\n"; +} + +void Cloud::writeNormalsToFile(const std::string &filename, + const Eigen::Matrix3Xd &normals) { + std::ofstream myfile; + myfile.open(filename.c_str()); + + for (int i = 0; i < normals.cols(); i++) { + myfile << std::to_string(normals(0, i)) << "," + << std::to_string(normals(1, i)) << "," + << std::to_string(normals(2, i)) << "\n"; + } + + myfile.close(); +} + +void Cloud::calculateNormals(int num_threads, double radius) { + double t_gpu = omp_get_wtime(); + printf("Calculating surface normals ...\n"); + std::string mode; + +#if defined(USE_PCL_GPU) + calculateNormalsGPU(); + mode = "gpu"; +#else + if (cloud_processed_->isOrganized()) { + calculateNormalsOrganized(); + mode = "integral images"; + } else { + printf("num_threads: %d\n", num_threads); + calculateNormalsOMP(num_threads, radius); + mode = "OpenMP"; + } +#endif + + t_gpu = omp_get_wtime() - t_gpu; + printf("Calculated %zu surface normals in %3.4fs (mode: %s).\n", + normals_.cols(), t_gpu, mode.c_str()); + printf( + "Reversing direction of normals that do not point to at least one camera " + "...\n"); + reverseNormals(); +} + +void Cloud::calculateNormalsOrganized() { + if (!cloud_processed_->isOrganized()) { + std::cout << "Error: point cloud is not organized!\n"; + return; + } + + std::cout << "Using integral images for surface normals estimation ...\n"; + pcl::PointCloud::Ptr cloud_normals( + new pcl::PointCloud); + pcl::IntegralImageNormalEstimation ne; + ne.setInputCloud(cloud_processed_); + ne.setViewPoint(view_points_(0, 0), view_points_(1, 0), view_points_(2, 0)); + ne.setNormalEstimationMethod(ne.COVARIANCE_MATRIX); + ne.setNormalSmoothingSize(20.0f); + ne.compute(*cloud_normals); + normals_ = cloud_normals->getMatrixXfMap().cast(); +} + +void Cloud::calculateNormalsOMP(int num_threads, double radius) { + std::vector> indices = convertCameraSourceMatrixToLists(); + + // Calculate surface normals for each view point. + std::vector normals_list(view_points_.cols()); + pcl::NormalEstimationOMP estimator( + num_threads); + pcl::search::KdTree::Ptr tree_ptr( + new pcl::search::KdTree); + estimator.setInputCloud(cloud_processed_); + estimator.setSearchMethod(tree_ptr); + estimator.setRadiusSearch(radius); + pcl::IndicesPtr indices_ptr(new std::vector); + + for (int i = 0; i < view_points_.cols(); i++) { + PointCloudNormal::Ptr normals_cloud(new PointCloudNormal); + indices_ptr->assign(indices[i].begin(), indices[i].end()); + estimator.setIndices(indices_ptr); + estimator.setViewPoint(view_points_(0, i), view_points_(1, i), + view_points_(2, i)); + double t0 = omp_get_wtime(); + estimator.compute(*normals_cloud); + printf(" runtime(computeNormals): %3.4f\n", omp_get_wtime() - t0); + normals_list[i] = normals_cloud; + printf("camera: %d, #indices: %d, #normals: %d \n", i, + (int)indices[i].size(), (int)normals_list[i]->size()); + } + + // Assign the surface normals to the points. + normals_.resize(3, camera_source_.cols()); + + for (int i = 0; i < normals_list.size(); i++) { + for (int j = 0; j < normals_list[i]->size(); j++) { + const pcl::Normal &normal = normals_list[i]->at(j); + normals_.col(indices[i][j]) << normal.normal_x, normal.normal_y, + normal.normal_z; + } + } +} + +#if defined(USE_PCL_GPU) +void Cloud::calculateNormalsGPU() { + std::vector> indices = convertCameraSourceMatrixToLists(); + + PointCloudXYZ::Ptr cloud_xyz(new PointCloudXYZ); + pcl::copyPointCloud(*cloud_processed_, *cloud_xyz); + pcl::gpu::Feature::PointCloud cloud_device; + cloud_device.upload(cloud_xyz->points); + pcl::gpu::Feature::Normals normals_device; + pcl::gpu::NormalEstimation ne; + ne.setInputCloud(cloud_device); + // ne.setRadiusSearch(0.03, 1000); + ne.setRadiusSearch(0.03, 2000); + // ne.setRadiusSearch(0.03, 4000); + // ne.setRadiusSearch(0.03, 8000); + pcl::gpu::Feature::Indices indices_device; + std::vector downloaded; + normals_.resize(3, camera_source_.cols()); + + // Calculate surface normals for each view point. + for (int i = 0; i < view_points_.cols(); i++) { + const Eigen::Vector3d &view_point = view_points_.col(i); + indices_device.upload(indices[i]); + ne.setViewPoint(view_point(0), view_point(1), view_point(2)); + ne.setIndices(indices_device); + ne.compute(normals_device); + normals_device.download(downloaded); + + for (int j = 0; j < indices[i].size(); j++) { + normals_.col(indices[i][j]) = + downloaded[i].getVector3fMap().cast(); + } + } +} +#endif + +void Cloud::reverseNormals() { + double t1 = omp_get_wtime(); + int c = 0; + + for (int i = 0; i < normals_.cols(); i++) { + bool needs_reverse = true; + + for (int j = 0; j < view_points_.cols(); j++) { + if (camera_source_(j, i) == 1) // point is seen by this camera + { + Eigen::Vector3d cam_to_point = + cloud_processed_->at(i).getVector3fMap().cast() - + view_points_.col(j); + + if (normals_.col(i).dot(cam_to_point) < + 0) // normal points toward camera + { + needs_reverse = false; + break; + } + } + } + + if (needs_reverse) { + normals_.col(i) *= -1.0; + c++; + } + } + + std::cout << " reversed " << c << " normals\n"; + std::cout << " runtime (reverse normals): " << omp_get_wtime() - t1 << "\n"; +} + +std::vector> Cloud::convertCameraSourceMatrixToLists() { + std::vector> indices(view_points_.cols()); + + for (int i = 0; i < camera_source_.cols(); i++) { + for (int j = 0; j < view_points_.cols(); j++) { + if (camera_source_(j, i) == 1) // point is seen by this camera + { + indices[j].push_back(i); + break; // TODO: multiple cameras + } + } + } + + return indices; +} + +void Cloud::setNormalsFromFile(const std::string &filename) { + std::ifstream in; + in.open(filename.c_str()); + std::string line; + normals_.resize(3, cloud_original_->size()); + int i = 0; + + while (std::getline(in, line)) { + std::stringstream lineStream(line); + std::string cell; + int j = 0; + + while (std::getline(lineStream, cell, ',')) { + normals_(i, j) = std::stod(cell); + j++; + } + + i++; + } +} + +PointCloudRGB::Ptr Cloud::loadPointCloudFromFile( + const std::string &filename) const { + PointCloudRGB::Ptr cloud(new PointCloudRGB); + std::string extension = filename.substr(filename.size() - 3); + printf("extension: %s\n", extension.c_str()); + + if (extension == "pcd" && + pcl::io::loadPCDFile(filename, *cloud) == -1) { + printf("Couldn't read PCD file: %s\n", filename.c_str()); + cloud->points.resize(0); + } else if (extension == "ply" && + pcl::io::loadPLYFile(filename, *cloud) == -1) { + printf("Couldn't read PLY file: %s\n", filename.c_str()); + cloud->points.resize(0); + } + + return cloud; +} + +void Cloud::setSamples(const Eigen::Matrix3Xd &samples) { samples_ = samples; } + +} // namespace util +} // namespace gpd diff --git a/src/gpd/util/config_file.cpp b/src/gpd/util/config_file.cpp new file mode 100644 index 0000000..e1227b9 --- /dev/null +++ b/src/gpd/util/config_file.cpp @@ -0,0 +1,170 @@ +#include + +namespace gpd { +namespace util { + +void ConfigFile::removeComment(std::string &line) const { + if (line.find('#') != line.npos) { + line.erase(line.find('#')); + } +} + +bool ConfigFile::onlyWhitespace(const std::string &line) const { + return (line.find_first_not_of(' ') == line.npos); +} + +bool ConfigFile::validLine(const std::string &line) const { + std::string temp = line; + temp.erase(0, temp.find_first_not_of("\t ")); + if (temp[0] == '=') { + return false; + } + + for (size_t i = temp.find('=') + 1; i < temp.length(); i++) { + if (temp[i] != ' ') { + return true; + } + } + + return false; +} + +void ConfigFile::extractKey(std::string &key, size_t const &sepPos, + const std::string &line) const { + key = line.substr(0, sepPos); + if (key.find('\t') != line.npos || key.find(' ') != line.npos) { + key.erase(key.find_first_of("\t ")); + } +} + +void ConfigFile::extractValue(std::string &value, size_t const &sepPos, + const std::string &line) const { + value = line.substr(sepPos + 1); + value.erase(0, value.find_first_not_of("\t ")); + value.erase(value.find_last_not_of("\t ") + 1); +} + +void ConfigFile::extractContents(const std::string &line) { + std::string temp = line; + temp.erase(0, temp.find_first_not_of("\t ")); + size_t sepPos = temp.find('='); + + std::string key, value; + extractKey(key, sepPos, temp); + extractValue(value, sepPos, temp); + + if (!keyExists(key)) { + contents.insert(std::pair(key, value)); + } else { + std::cout << "CFG: Can only have unique key names!\n"; + } +} + +void ConfigFile::parseLine(const std::string &line, size_t const lineNo) { + if (line.find('=') == line.npos) { + std::cout << "CFG: Couldn't find separator on line: " + + T_to_string(lineNo) + "\n"; + } + + if (!validLine(line)) { + std::cout << "CFG: Bad format for line: " + T_to_string(lineNo) + "\n"; + } + + extractContents(line); +} + +bool ConfigFile::ExtractKeys() { + std::ifstream file; + file.open(fName.c_str()); + if (!file) { + std::cout << "Config file " + fName + " could not be found!\n"; + return false; + } + + std::string line; + size_t lineNo = 0; + while (std::getline(file, line)) { + lineNo++; + std::string temp = line; + + if (temp.empty()) { + continue; + } + + removeComment(temp); + if (onlyWhitespace(temp)) { + continue; + } + + parseLine(temp, lineNo); + } + + file.close(); + return true; +} + +ConfigFile::ConfigFile(const std::string &fName) { this->fName = fName; } + +bool ConfigFile::keyExists(const std::string &key) const { + return contents.find(key) != contents.end(); +} + +std::string ConfigFile::getValueOfKeyAsString(const std::string &key, + const std::string &defaultValue) { + if (!keyExists(key)) { + return defaultValue; + } + + return contents.find(key)->second; +} + +std::vector ConfigFile::getValueOfKeyAsStdVectorDouble( + const std::string &key, const std::string &defaultValue) { + std::string s = getValueOfKeyAsString(key, defaultValue); + + std::vector vec = stringToDouble(s); + + return vec; +} + +std::vector ConfigFile::getValueOfKeyAsStdVectorInt( + const std::string &key, const std::string &defaultValue) { + std::string s = getValueOfKeyAsString(key, defaultValue); + + std::vector vec = stringToInt(s); + + return vec; +} + +std::vector ConfigFile::stringToDouble(const std::string &str) { + std::vector values; + std::stringstream ss(str); + double v; + + while (ss >> v) { + values.push_back(v); + if (ss.peek() == ' ') { + ss.ignore(); + } + } + + return values; +} + +std::vector ConfigFile::stringToInt(const std::string &str) { + std::vector values; + std::stringstream ss(str); + double v; + + while (ss >> v) { + values.push_back(v); + if (ss.peek() == ' ') { + ss.ignore(); + } + } + + return values; +} + +} // namespace util +} // namespace gpd diff --git a/src/gpd/util/eigen_utils.cpp b/src/gpd/util/eigen_utils.cpp new file mode 100644 index 0000000..70dd641 --- /dev/null +++ b/src/gpd/util/eigen_utils.cpp @@ -0,0 +1,35 @@ +#include + +namespace gpd { +namespace util { + +Eigen::Matrix3Xd EigenUtils::sliceMatrix(const Eigen::Matrix3Xd &mat, + const std::vector &indices) { + Eigen::Matrix3Xd mat_out(3, indices.size()); + + for (int j = 0; j < indices.size(); j++) { + mat_out.col(j) = mat.col(indices[j]); + } + + return mat_out; +} + +Eigen::MatrixXi EigenUtils::sliceMatrix(const Eigen::MatrixXi &mat, + const std::vector &indices) { + Eigen::MatrixXi mat_out(mat.rows(), indices.size()); + + for (int j = 0; j < indices.size(); j++) { + mat_out.col(j) = mat.col(indices[j]); + } + + return mat_out; +} + +Eigen::Vector3i EigenUtils::floorVector(const Eigen::Vector3f &a) { + Eigen::Vector3i b; + b << floor(a(0)), floor(a(1)), floor(a(2)); + return b; +} + +} // namespace util +} // namespace gpd diff --git a/src/gpd/util/plot.cpp b/src/gpd/util/plot.cpp new file mode 100644 index 0000000..a472e13 --- /dev/null +++ b/src/gpd/util/plot.cpp @@ -0,0 +1,799 @@ +#include + +#include +#include + +namespace gpd { +namespace util { + +void Plot::plotHandGeometry(const candidate::Hand &hand, + const PointCloudRGBA::Ptr &cloud, + const candidate::HandGeometry &hand_geom, + const descriptor::ImageGeometry &image_geom) { + Eigen::Vector3d vol_rgb(0.0, 0.8, 0.0); + Eigen::Vector3d hand_rgb(0.0, 0.5, 0.5); + PCLVisualizer viewer = createViewer("Hand Geometry"); + plotHand3D(viewer, hand, hand_geom, 0, hand_rgb); + Eigen::Vector3d vol_pos = + hand.getPosition() + 0.5 * image_geom.depth_ * hand.getApproach(); + Eigen::Quaterniond vol_quat(hand.getFrame()); + plotCube(viewer, vol_pos, vol_quat, image_geom.depth_, + image_geom.outer_diameter_, 2.0 * image_geom.height_, "volume", + vol_rgb); + + Eigen::Vector3d dimensions(hand_geom.depth_, hand_geom.outer_diameter_, + 2.0 * hand_geom.height_); + Eigen::Matrix3d colors; + colors << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0; + Eigen::Vector3d center = hand.getPosition() - + hand_geom.height_ * hand.getAxis() - + 0.5 * hand_geom.outer_diameter_ * hand.getBinormal(); + std::vector labels; + labels.push_back("hand_depth"); + labels.push_back("hand_outer_diameter"); + labels.push_back("hand_height * 2"); + addDimensions(center, hand.getOrientation(), dimensions, colors, labels, + viewer); + + Eigen::Vector3d p = center + 2.0 * hand_geom.height_ * hand.getAxis(); + Eigen::Vector3d q = p + hand_geom.finger_width_ * hand.getBinormal(); + addDoubleArrow(p, q, "finger_width", Eigen::Vector3d(0.0, 1.0, 1.0), viewer); + + dimensions << image_geom.depth_, -image_geom.outer_diameter_, + 2.0 * image_geom.height_; + colors *= 0.6; + center = hand.getPosition() - image_geom.height_ * hand.getAxis() + + 0.5 * image_geom.outer_diameter_ * hand.getBinormal(); + labels.resize(0); + labels.push_back("volume_depth"); + labels.push_back("volume_width"); + labels.push_back("volume_height * 2"); + addDimensions(center, hand.getOrientation(), dimensions, colors, labels, + viewer); + + pcl::visualization::PointCloudColorHandlerRGBField rgb( + cloud); + viewer->addPointCloud(cloud, rgb, "cloud"); + viewer->setPointCloudRenderingProperties( + pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "cloud"); + + runViewer(viewer); +} + +void Plot::addDimensions(const Eigen::Vector3d ¢er, + const Eigen::Matrix3d &rot, + const Eigen::Vector3d &dimensions, + const Eigen::Matrix3d &colors, + const std::vector &labels, + PCLVisualizer &viewer) { + bool is_label_at_start[3] = {false, true, false}; + for (size_t i = 0; i < 3; i++) { + Eigen::Vector3d p = center; + Eigen::Vector3d q = p + dimensions(i) * rot.col(i); + addDoubleArrow(p, q, labels[i], colors.row(i), viewer, + is_label_at_start[i]); + } +} + +void Plot::addDoubleArrow(const Eigen::Vector3d &start, + const Eigen::Vector3d &end, const std::string &label, + const Eigen::Vector3d &rgb, PCLVisualizer &viewer, + bool is_label_at_start) { + pcl::PointXYZRGB p; + pcl::PointXYZRGB q; + p.getVector3fMap() = start.cast(); + q.getVector3fMap() = end.cast(); + viewer->addArrow(p, q, rgb[0], rgb[1], rgb[2], false, + label + std::to_string(0)); + viewer->addArrow(q, p, rgb[0], rgb[1], rgb[2], false, + label + std::to_string(1)); + if (is_label_at_start) { + viewer->addText3D(label, p, 0.008, rgb[0], rgb[1], rgb[2]); + } else { + viewer->addText3D(label, q, 0.008, rgb[0], rgb[1], rgb[2]); + } +} + +void Plot::plotVolumes3D( + const std::vector> &hand_set_list, + const PointCloudRGBA::Ptr &cloud, std::string str, double outer_diameter, + double finger_width, double hand_depth, double hand_height, + double volume_width, double volume_depth, double volume_height) { + Eigen::Vector3d vol_rgb(0.0, 0.8, 0.0); + Eigen::Vector3d hand_rgb(0.0, 0.5, 0.5); + + PCLVisualizer viewer = createViewer(str); + + for (int i = 0; i < hand_set_list.size(); i++) { + const std::vector> &hands = + hand_set_list[i]->getHands(); + + for (int j = 0; j < hands.size(); j++) { + if (!hand_set_list[i]->getIsValid()[j]) { + continue; + } + + int idx = i * hands.size() + j; + + // Plot the hand. + plotHand3D(viewer, *hands[j], outer_diameter, finger_width, hand_depth, + hand_height, idx, hand_rgb); + + // Plot the associated volume. + Eigen::Vector3d vol_pos = hands[j]->getPosition() + + 0.5 * volume_depth * hands[j]->getApproach(); + Eigen::Quaterniond vol_quat(hands[i]->getFrame()); + std::string num = std::to_string(idx); + plotCube(viewer, vol_pos, vol_quat, volume_depth, volume_width, + volume_height, "volume_" + num, vol_rgb); + } + } + + pcl::visualization::PointCloudColorHandlerRGBField rgb( + cloud); + viewer->addPointCloud(cloud, rgb, "cloud"); + viewer->setPointCloudRenderingProperties( + pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "cloud"); + + runViewer(viewer); +} + +void Plot::plotVolumes3D( + const std::vector> &hand_list, + const PointCloudRGBA::Ptr &cloud, std::string str, double outer_diameter, + double finger_width, double hand_depth, double hand_height, + double volume_width, double volume_depth, double volume_height) { + Eigen::Vector3d vol_rgb(0.0, 0.8, 0.0); + Eigen::Vector3d hand_rgb(0.0, 0.5, 0.5); + + PCLVisualizer viewer = createViewer(str); + + for (int i = 0; i < hand_list.size(); i++) { + // Plot the hand. + plotHand3D(viewer, *hand_list[i], outer_diameter, finger_width, hand_depth, + hand_height, i, hand_rgb); + + // Plot the associated volume. + Eigen::Vector3d vol_pos = hand_list[i]->getPosition() + + 0.5 * volume_depth * hand_list[i]->getApproach(); + Eigen::Quaterniond vol_quat(hand_list[i]->getFrame()); + std::string num = std::to_string(i); + plotCube(viewer, vol_pos, vol_quat, volume_depth, volume_width, + volume_height, "volume_" + num, vol_rgb); + } + + pcl::visualization::PointCloudColorHandlerRGBField rgb( + cloud); + viewer->addPointCloud(cloud, rgb, "cloud"); + viewer->setPointCloudRenderingProperties( + pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "cloud"); + + runViewer(viewer); +} + +void Plot::plotFingers3D( + const std::vector> &hand_set_list, + const PointCloudRGBA::Ptr &cloud, std::string str, + const candidate::HandGeometry &geometry, bool draw_all, bool draw_frame) { + const Eigen::Vector3d RGB[3] = {Eigen::Vector3d(0.5, 0, 0), + Eigen::Vector3d(0, 0.5, 0), + Eigen::Vector3d(0, 0, 0.5)}; + PCLVisualizer viewer = createViewer(str); + const int max_hands_per_set_ = num_axes_ * num_orientations_; + + for (int i = 0; i < hand_set_list.size(); i++) { + for (int j = 0; j < hand_set_list[i]->getHands().size(); j++) { + if (draw_all || hand_set_list[i]->getIsValid()(j)) { + // Choose color based on rotation axis. + Eigen::Vector3d rgb; + if (draw_all) { + int idx_color = j / num_orientations_; + rgb = RGB[idx_color]; + } else { + rgb << 0.0, 0.5, 0.5; + } + plotHand3D(viewer, *hand_set_list[i]->getHands()[j], geometry, + i * max_hands_per_set_ + j, rgb); + } + } + + if (draw_frame) { + plotFrame(viewer, hand_set_list[i]->getSample(), + hand_set_list[i]->getFrame(), std::to_string(i)); + } + } + + pcl::visualization::PointCloudColorHandlerRGBField rgb( + cloud); + viewer->addPointCloud(cloud, rgb, "cloud"); + viewer->setPointCloudRenderingProperties( + pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "cloud"); + + runViewer(viewer); +} + +void Plot::plotFingers3D( + const std::vector> &hand_list, + const PointCloudRGBA::Ptr &cloud, const std::string &str, + const candidate::HandGeometry &geometry, bool use_same_color) { + PCLVisualizer viewer = createViewer(str); + + double min = std::numeric_limits::max(); + double max = std::numeric_limits::min(); + for (int i = 0; i < hand_list.size(); i++) { + if (hand_list[i]->getScore() < min) { + min = hand_list[i]->getScore(); + } + if (hand_list[i]->getScore() > max) { + max = hand_list[i]->getScore(); + } + } + + Eigen::Vector3d hand_rgb; + if (use_same_color) { + hand_rgb << 0.0, 0.5, 0.5; + } + + for (int i = 0; i < hand_list.size(); i++) { + if (!use_same_color) { + double c = (hand_list[i]->getScore() - min) / (max - min); + hand_rgb = Eigen::Vector3d(1.0 - c, c, 0.0); + } + plotHand3D(viewer, *hand_list[i], geometry, i, hand_rgb); + } + + pcl::visualization::PointCloudColorHandlerRGBField rgb( + cloud); + viewer->addPointCloud(cloud, rgb, "cloud"); + viewer->setPointCloudRenderingProperties( + pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "cloud"); + + runViewer(viewer); +} + +void Plot::plotAntipodalHands( + const std::vector> &hand_list, + const PointCloudRGBA::Ptr &cloud, const std::string &str, + const candidate::HandGeometry &geometry) { + PCLVisualizer viewer = createViewer(str); + + Eigen::Vector3d antipodal_color; + Eigen::Vector3d non_antipodal_color; + antipodal_color << 0.0, 0.7, 0.0; + non_antipodal_color << 0.7, 0.0, 0.0; + + for (int i = 0; i < hand_list.size(); i++) { + Eigen::Vector3d color = + hand_list[i]->isFullAntipodal() ? antipodal_color : non_antipodal_color; + plotHand3D(viewer, *hand_list[i], geometry, i, color); + } + + pcl::visualization::PointCloudColorHandlerRGBField rgb( + cloud); + viewer->addPointCloud(cloud, rgb, "cloud"); + viewer->setPointCloudRenderingProperties( + pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "cloud"); + + runViewer(viewer); +} + +void Plot::plotValidHands( + const std::vector> &hand_list, + const PointCloudRGBA::Ptr &cloud, const PointCloudRGBA::Ptr &mesh, + const std::string &str, const candidate::HandGeometry &geometry) { + PCLVisualizer viewer = createViewer(str); + + Eigen::Vector3d antipodal_color; + Eigen::Vector3d non_antipodal_color; + antipodal_color << 0.0, 0.7, 0.0; + non_antipodal_color << 0.7, 0.0, 0.0; + + for (int i = 0; i < hand_list.size(); i++) { + Eigen::Vector3d color = + hand_list[i]->isFullAntipodal() ? antipodal_color : non_antipodal_color; + plotHand3D(viewer, *hand_list[i], geometry, i, color); + } + + viewer->addPointCloud(mesh, "mesh"); + viewer->setPointCloudRenderingProperties( + pcl::visualization::PCL_VISUALIZER_COLOR, 0.0, 0.0, 1.0, "mesh"); + viewer->setPointCloudRenderingProperties( + pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "mesh"); + + viewer->addPointCloud(cloud, "cloud"); + viewer->setPointCloudRenderingProperties( + pcl::visualization::PCL_VISUALIZER_COLOR, 1.0, 0.0, 0.0, "cloud"); + viewer->setPointCloudRenderingProperties( + pcl::visualization::PCL_VISUALIZER_OPACITY, 0.6, "cloud"); + viewer->setPointCloudRenderingProperties( + pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "cloud"); + + runViewer(viewer); +} + +void Plot::plotFingers3D(const std::vector &hand_set_list, + const PointCloudRGBA::Ptr &cloud, std::string str, + double outer_diameter, double finger_width, + double hand_depth, double hand_height, bool draw_all, + int num_axes, int num_orientations) { + const Eigen::Vector3d RGB[3] = {Eigen::Vector3d(0.5, 0, 0), + Eigen::Vector3d(0, 0.5, 0), + Eigen::Vector3d(0, 0, 0.5)}; + + PCLVisualizer viewer = createViewer(str); + + for (int i = 0; i < hand_set_list.size(); i++) { + for (int j = 0; j < hand_set_list[i].getHands().size(); j++) { + if (draw_all || hand_set_list[i].getIsValid()(j)) { + Eigen::Vector3d rgb; + if (draw_all) { + int idx_color = j / num_orientations; + rgb = RGB[idx_color]; + } else { + rgb << 0.0, 0.5, 0.5; + } + plotHand3D(viewer, *hand_set_list[i].getHands()[j], outer_diameter, + finger_width, hand_depth, hand_height, + i * (num_axes * num_orientations) + j, rgb); + } + } + } + + pcl::visualization::PointCloudColorHandlerRGBField rgb( + cloud); + viewer->addPointCloud(cloud, rgb, "cloud"); + viewer->setPointCloudRenderingProperties( + pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "cloud"); + + runViewer(viewer); +} + +void Plot::plotFingers3D(const std::vector &hand_list, + const PointCloudRGBA::Ptr &cloud, std::string str, + double outer_diameter, double finger_width, + double hand_depth, double hand_height, bool draw_all) { + PCLVisualizer viewer = createViewer(str); + Eigen::Vector3d hand_rgb(0.0, 0.5, 0.5); + + for (int i = 0; i < hand_list.size(); i++) { + plotHand3D(viewer, hand_list[i], outer_diameter, finger_width, hand_depth, + hand_height, i, hand_rgb); + } + + pcl::visualization::PointCloudColorHandlerRGBField rgb( + cloud); + viewer->addPointCloud(cloud, rgb, "cloud"); + viewer->setPointCloudRenderingProperties( + pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "cloud"); + + runViewer(viewer); +} + +void Plot::plotHand3D(PCLVisualizer &viewer, const candidate::Hand &hand, + const candidate::HandGeometry &geometry, int idx, + const Eigen::Vector3d &rgb) { + plotHand3D(viewer, hand, geometry.outer_diameter_, geometry.finger_width_, + geometry.depth_, geometry.height_, idx, rgb); +} + +void Plot::plotHand3D(PCLVisualizer &viewer, const candidate::Hand &hand, + double outer_diameter, double finger_width, + double hand_depth, double hand_height, int idx, + const Eigen::Vector3d &rgb) { + const double hw = 0.5 * outer_diameter; + const double base_depth = 0.02; + const double approach_depth = 0.07; + + Eigen::Vector3d left_bottom = + hand.getPosition() - (hw - 0.5 * finger_width) * hand.getBinormal(); + Eigen::Vector3d right_bottom = + hand.getPosition() + (hw - 0.5 * finger_width) * hand.getBinormal(); + Eigen::VectorXd left_center = + left_bottom + 0.5 * hand_depth * hand.getApproach(); + Eigen::VectorXd right_center = + right_bottom + 0.5 * hand_depth * hand.getApproach(); + Eigen::Vector3d base_center = left_bottom + + 0.5 * (right_bottom - left_bottom) - + 0.01 * hand.getApproach(); + Eigen::Vector3d approach_center = base_center - 0.04 * hand.getApproach(); + + const Eigen::Quaterniond quat(hand.getFrame()); + const std::string num = std::to_string(idx); + + plotCube(viewer, left_center, quat, hand_depth, finger_width, hand_height, + "left_finger_" + num, rgb); + plotCube(viewer, right_center, quat, hand_depth, finger_width, hand_height, + "right_finger_" + num, rgb); + plotCube(viewer, base_center, quat, base_depth, outer_diameter, hand_height, + "base_" + num, rgb); + plotCube(viewer, approach_center, quat, approach_depth, finger_width, + 0.5 * hand_height, "approach_" + num, rgb); +} + +void Plot::plotCube(PCLVisualizer &viewer, const Eigen::Vector3d &position, + const Eigen::Quaterniond &rotation, double width, + double height, double depth, const std::string &name, + const Eigen::Vector3d &rgb) { + viewer->addCube(position.cast(), rotation.cast(), width, height, + depth, name); + viewer->setShapeRenderingProperties( + pcl::visualization::PCL_VISUALIZER_REPRESENTATION, + pcl::visualization::PCL_VISUALIZER_REPRESENTATION_SURFACE, name); + viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, + rgb(0), rgb(1), rgb(2), name); + viewer->setShapeRenderingProperties( + pcl::visualization::PCL_VISUALIZER_OPACITY, 0.25, name); +} + +void Plot::plotFrame(PCLVisualizer &viewer, const Eigen::Vector3d &translation, + const Eigen::Matrix3d &rotation, const std::string &id, + double axis_length) { + const Eigen::Matrix3d pts = axis_length * rotation; + const std::string names[3] = {"normal_" + id, "binormal_" + id, + "curvature_" + id}; + const double colors[3][3] = { + {1.0, 0.0, 0.0}, {0.0, 1.0, 0.0}, {0.0, 0.0, 1.0}}; + pcl::PointXYZ p; + p.getVector3fMap() = translation.cast(); + for (int i = 0; i < 3; i++) { + pcl::PointXYZ q; + q.getVector3fMap() = (translation + pts.col(i)).cast(); + viewer->addLine(p, q, colors[i][0], colors[i][1], + colors[i][2], names[i]); + viewer->setShapeRenderingProperties( + pcl::visualization::PCL_VISUALIZER_LINE_WIDTH, 5, names[i]); + } +} + +void Plot::plotSamples(const std::vector &index_list, + const PointCloudRGBA::Ptr &cloud) { + PointCloudRGBA::Ptr samples_cloud(new PointCloudRGBA); + for (int i = 0; i < index_list.size(); i++) { + samples_cloud->points.push_back(cloud->points[index_list[i]]); + } + + plotSamples(samples_cloud, cloud); +} + +void Plot::plotSamples(const Eigen::Matrix3Xd &samples, + const PointCloudRGBA::Ptr &cloud) { + PointCloudRGBA::Ptr samples_cloud(new PointCloudRGBA); + for (int i = 0; i < samples.cols(); i++) { + pcl::PointXYZRGBA p; + p.x = samples.col(i)(0); + p.y = samples.col(i)(1); + p.z = samples.col(i)(2); + samples_cloud->points.push_back(p); + } + + plotSamples(samples_cloud, cloud); +} + +void Plot::plotSamples(const PointCloudRGBA::Ptr &samples_cloud, + const PointCloudRGBA::Ptr &cloud) { + PCLVisualizer viewer = createViewer("Samples"); + + // draw the point cloud + pcl::visualization::PointCloudColorHandlerRGBField rgb( + cloud); + viewer->addPointCloud(cloud, rgb, + "registered point cloud"); + viewer->setPointCloudRenderingProperties( + pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, + "registered point cloud"); + viewer->setPointCloudRenderingProperties( + pcl::visualization::PCL_VISUALIZER_COLOR, 0.0, 0.0, 0.0, + "registered point cloud"); + + // draw the samples + viewer->addPointCloud(samples_cloud, "samples cloud"); + viewer->setPointCloudRenderingProperties( + pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 4, "samples cloud"); + viewer->setPointCloudRenderingProperties( + pcl::visualization::PCL_VISUALIZER_COLOR, 1.0, 0.0, 1.0, "samples cloud"); + + runViewer(viewer); +} + +void Plot::plotNormals(const Cloud &cloud_cam, bool draw_camera_cone) { + const int num_clouds = cloud_cam.getViewPoints().cols(); + std::vector clouds; + clouds.resize(num_clouds); + + for (int i = 0; i < num_clouds; i++) { + PointCloudPointNormal::Ptr cloud(new PointCloudPointNormal); + clouds[i] = cloud; + } + + for (int i = 0; i < cloud_cam.getNormals().cols(); i++) { + pcl::PointNormal p; + p.x = cloud_cam.getCloudProcessed()->points[i].x; + p.y = cloud_cam.getCloudProcessed()->points[i].y; + p.z = cloud_cam.getCloudProcessed()->points[i].z; + p.normal_x = cloud_cam.getNormals()(0, i); + p.normal_y = cloud_cam.getNormals()(1, i); + p.normal_z = cloud_cam.getNormals()(2, i); + + for (int j = 0; j < cloud_cam.getCameraSource().rows(); j++) { + if (cloud_cam.getCameraSource()(j, i) == 1) { + clouds[j]->push_back(p); + } + } + } + + double colors[6][3] = {{1.0, 0.0, 0.0}, {0.0, 1.0, 0.0}, {0.0, 0.0, 1.0}, + {1.0, 1.0, 0.0}, {1.0, 0.0, 1.0}, {0.0, 1.0, 1.0}}; + double normal_colors[6][3] = {{0.5, 0.0, 0.0}, {0.0, 0.5, 0.0}, + {0.0, 0.0, 0.5}, {0.5, 0.5, 0.0}, + {0.5, 0.0, 0.5}, {0.0, 0.5, 0.5}}; + + if (num_clouds == 1) { + normal_colors[0][0] = 0.0; + normal_colors[0][2] = 1.0; + } + + PCLVisualizer viewer = createViewer("Normals"); + viewer->setBackgroundColor(1.0, 1.0, 1.0); + for (int i = 0; i < num_clouds; i++) { + std::string cloud_name = "cloud_" + std::to_string(i); + std::string normals_name = "normals_" + std::to_string(i); + int color_id = i % 6; + viewer->addPointCloud(clouds[i], cloud_name); + viewer->addPointCloudNormals(clouds[i], 1, 0.01, + normals_name); + viewer->setPointCloudRenderingProperties( + pcl::visualization::PCL_VISUALIZER_COLOR, colors[color_id][0], + colors[color_id][1], colors[color_id][2], cloud_name); + viewer->setPointCloudRenderingProperties( + pcl::visualization::PCL_VISUALIZER_COLOR, normal_colors[color_id][0], + normal_colors[color_id][1], normal_colors[color_id][2], normals_name); + + // draw camera position as a cube + if (draw_camera_cone) { + const Eigen::Vector3d &cam_pos = cloud_cam.getViewPoints().col(i); + Eigen::Vector4f centroid_4d; + pcl::compute3DCentroid(*clouds[i], centroid_4d); + Eigen::Vector3d centroid; + centroid << centroid_4d(0), centroid_4d(1), centroid_4d(2); + Eigen::Vector3d cone_dir = centroid - cam_pos; + cone_dir.normalize(); + pcl::ModelCoefficients coeffs; + coeffs.values.push_back(cam_pos(0)); + coeffs.values.push_back(cam_pos(1)); + coeffs.values.push_back(cam_pos(2)); + coeffs.values.push_back(cone_dir(0)); + coeffs.values.push_back(cone_dir(1)); + coeffs.values.push_back(cone_dir(2)); + coeffs.values.push_back(20.0); + std::string cone_name = "cam" + std::to_string(i); + viewer->addCone(coeffs, cone_name, 0); + viewer->setShapeRenderingProperties( + pcl::visualization::PCL_VISUALIZER_COLOR, normal_colors[color_id][0], + normal_colors[color_id][1], normal_colors[color_id][2], cone_name); + } + } + + runViewer(viewer); +} + +void Plot::plotNormals(const PointCloudRGBA::Ptr &cloud, + const PointCloudRGBA::Ptr &cloud_samples, + const Eigen::Matrix3Xd &normals) { + PointCloudPointNormal::Ptr normals_cloud(new PointCloudPointNormal); + for (int i = 0; i < normals.cols(); i++) { + pcl::PointNormal p; + p.x = cloud_samples->points[i].x; + p.y = cloud_samples->points[i].y; + p.z = cloud_samples->points[i].z; + p.normal_x = normals(0, i); + p.normal_y = normals(1, i); + p.normal_z = normals(2, i); + normals_cloud->points.push_back(p); + } + std::cout << "Drawing " << normals_cloud->size() << " normals\n"; + + double red[3] = {1.0, 0.0, 0.0}; + double blue[3] = {0.0, 0.0, 1.0}; + + PCLVisualizer viewer = createViewer("Normals"); + + // draw the point cloud + pcl::visualization::PointCloudColorHandlerRGBField rgb( + cloud); + viewer->addPointCloud(cloud, rgb, + "registered point cloud"); + + // draw the normals + addCloudNormalsToViewer(viewer, normals_cloud, 2, blue, red, + std::string("cloud"), std::string("normals")); + + runViewer(viewer); +} + +void Plot::plotNormals(const PointCloudRGBA::Ptr &cloud, + const Eigen::Matrix3Xd &normals) { + PointCloudPointNormal::Ptr normals_cloud(new PointCloudPointNormal); + for (int i = 0; i < normals.cols(); i++) { + pcl::PointNormal p; + p.x = cloud->points[i].x; + p.y = cloud->points[i].y; + p.z = cloud->points[i].z; + p.normal_x = normals(0, i); + p.normal_y = normals(1, i); + p.normal_z = normals(2, i); + normals_cloud->points.push_back(p); + } + std::cout << "Drawing " << normals_cloud->size() << " normals\n"; + + double red[3] = {1.0, 0.0, 0.0}; + double blue[3] = {0.0, 0.0, 1.0}; + + PCLVisualizer viewer = createViewer("Normals"); + + // draw the point cloud + pcl::visualization::PointCloudColorHandlerRGBField rgb( + cloud); + viewer->addPointCloud(cloud, rgb, + "registered point cloud"); + + // draw the normals + addCloudNormalsToViewer(viewer, normals_cloud, 2, blue, red, + std::string("cloud"), std::string("normals")); + + runViewer(viewer); +} + +void Plot::plotNormals(const Eigen::Matrix3Xd &pts, + const Eigen::Matrix3Xd &normals) { + PointCloudPointNormal::Ptr normals_cloud(new PointCloudPointNormal); + for (int i = 0; i < normals.cols(); i++) { + pcl::PointNormal p; + p.x = pts(0, i); + p.y = pts(1, i); + p.z = pts(2, i); + p.normal_x = normals(0, i); + p.normal_y = normals(1, i); + p.normal_z = normals(2, i); + normals_cloud->points.push_back(p); + } + std::cout << "Drawing " << normals_cloud->size() << " normals\n"; + + double red[3] = {1.0, 0.0, 0.0}; + double blue[3] = {0.0, 0.0, 1.0}; + + PCLVisualizer viewer = createViewer("Normals"); + addCloudNormalsToViewer(viewer, normals_cloud, 2, blue, red, + std::string("cloud"), std::string("normals")); + runViewer(viewer); +} + +void Plot::plotLocalAxes(const std::vector &frames, + const PointCloudRGBA::Ptr &cloud) { + PCLVisualizer viewer = createViewer("Local Frames"); + viewer->addPointCloud(cloud, "registered point cloud"); + viewer->setPointCloudRenderingProperties( + pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, + "registered point cloud"); + + for (int i = 0; i < frames.size(); i++) { + const candidate::LocalFrame &frame = frames[i]; + pcl::PointXYZ p, q, r; + p.getVector3fMap() = frame.getSample().cast(); + q.x = p.x + 0.02 * frame.getCurvatureAxis()(0); + q.y = p.y + 0.02 * frame.getCurvatureAxis()(1); + q.z = p.z + 0.02 * frame.getCurvatureAxis()(2); + r.x = p.x + 0.02 * frame.getNormal()(0); + r.y = p.y + 0.02 * frame.getNormal()(1); + r.z = p.z + 0.02 * frame.getNormal()(2); + const std::string str_id = std::to_string(i); + viewer->addLine(p, q, 0, 0, 255, "curvature_axis_" + str_id); + viewer->addLine(p, r, 255, 0, 0, "normal_axis_" + str_id); + } + + runViewer(viewer); +} + +void Plot::plotCameraSource(const Eigen::VectorXi &pts_cam_source_in, + const PointCloudRGBA::Ptr &cloud) { + PointCloudRGBA::Ptr left_cloud(new PointCloudRGBA); + PointCloudRGBA::Ptr right_cloud(new PointCloudRGBA); + + for (int i = 0; i < pts_cam_source_in.size(); i++) { + if (pts_cam_source_in(i) == 0) + left_cloud->points.push_back(cloud->points[i]); + else if (pts_cam_source_in(i) == 1) + right_cloud->points.push_back(cloud->points[i]); + } + + PCLVisualizer viewer = createViewer("Camera Sources"); + viewer->addPointCloud(left_cloud, "left point cloud"); + viewer->setPointCloudRenderingProperties( + pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "left point cloud"); + viewer->setPointCloudRenderingProperties( + pcl::visualization::PCL_VISUALIZER_COLOR, 1.0, 0.0, 0.0, + "left point cloud"); + viewer->addPointCloud(right_cloud, "right point cloud"); + viewer->setPointCloudRenderingProperties( + pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "right point cloud"); + viewer->setPointCloudRenderingProperties( + pcl::visualization::PCL_VISUALIZER_COLOR, 0.0, 1.0, 0.0, + "right point cloud"); + runViewer(viewer); +} + +void Plot::addCloudNormalsToViewer(PCLVisualizer &viewer, + const PointCloudPointNormal::Ptr &cloud, + double line_width, double *color_cloud, + double *color_normals, + const std::string &cloud_name, + const std::string &normals_name) { + viewer->addPointCloud(cloud, cloud_name); + viewer->setPointCloudRenderingProperties( + pcl::visualization::PCL_VISUALIZER_COLOR, color_cloud[0], color_cloud[1], + color_cloud[2], cloud_name); + viewer->setPointCloudRenderingProperties( + pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 6, cloud_name); + viewer->addPointCloudNormals(cloud, 1, 0.01, normals_name); + viewer->setPointCloudRenderingProperties( + pcl::visualization::PCL_VISUALIZER_LINE_WIDTH, line_width, normals_name); + viewer->setPointCloudRenderingProperties( + pcl::visualization::PCL_VISUALIZER_COLOR, color_normals[0], + color_normals[1], color_normals[2], normals_name); +} + +void Plot::runViewer(PCLVisualizer &viewer) { + while (!viewer->wasStopped()) { + viewer->spinOnce(100); + std::this_thread::sleep_for(std::chrono::duration(1)); + } + + viewer->close(); +} + +PCLVisualizer Plot::createViewer(std::string title) { + PCLVisualizer viewer(new pcl::visualization::PCLVisualizer(title)); + viewer->setPosition(0, 0); + viewer->setSize(640, 480); + viewer->setBackgroundColor(1.0, 1.0, 1.0); + viewer->registerKeyboardCallback(&Plot::keyboardEventOccurred, *this, + (void *)viewer.get()); + + return viewer; +} + +void Plot::keyboardEventOccurred(const pcl::visualization::KeyboardEvent &event, + void *viewer_void) { + pcl::visualization::PCLVisualizer *viewer = + static_cast(viewer_void); + if (event.getKeySym() == "a" && event.keyDown()) { + if (viewer->contains("ref")) { + viewer->removeCoordinateSystem("ref"); + } else { + viewer->addCoordinateSystem(0.1, "ref"); + } + } +} + +void Plot::plotCloud(const PointCloudRGBA::Ptr &cloud_rgb, + const std::string &title) { + PCLVisualizer viewer = createViewer(title); + pcl::visualization::PointCloudColorHandlerRGBField rgb( + cloud_rgb); + viewer->addPointCloud(cloud_rgb, rgb, + "registered point cloud"); + viewer->setPointCloudRenderingProperties( + pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, + "registered point cloud"); + runViewer(viewer); +} + +pcl::PointXYZRGBA Plot::eigenVector3dToPointXYZRGBA(const Eigen::Vector3d &v) { + pcl::PointXYZRGBA p; + p.x = v(0); + p.y = v(1); + p.z = v(2); + return p; +} + +} // namespace util +} // namespace gpd diff --git a/src/gpd/util/point_list.cpp b/src/gpd/util/point_list.cpp new file mode 100644 index 0000000..61cf321 --- /dev/null +++ b/src/gpd/util/point_list.cpp @@ -0,0 +1,58 @@ +#include + +namespace gpd { +namespace util { + +PointList::PointList(int size, int num_cams) { + points_.resize(3, size); + normals_.resize(3, size); + cam_source_.resize(num_cams, size); + view_points_.resize(3, num_cams); +} + +PointList PointList::slice(const std::vector &indices) const { + Eigen::Matrix3Xd points_out = EigenUtils::sliceMatrix(points_, indices); + Eigen::Matrix3Xd normals_out = EigenUtils::sliceMatrix(normals_, indices); + Eigen::MatrixXi cam_source_out = + EigenUtils::sliceMatrix(cam_source_, indices); + + return PointList(points_out, normals_out, cam_source_out, view_points_); +} + +PointList PointList::transformToHandFrame( + const Eigen::Vector3d ¢roid, const Eigen::Matrix3d &rotation) const { + // Eigen::Matrix3Xd points_centered = points_ - centroid.replicate(1, + // size()); + Eigen::Matrix3Xd points_centered = points_; + points_centered.colwise() -= centroid; + points_centered = rotation * points_centered; + Eigen::Matrix3Xd normals(3, points_centered.cols()); + normals = rotation * normals_; + + return PointList(points_centered, normals, cam_source_, view_points_); +} + +PointList PointList::rotatePointList(const Eigen::Matrix3d &rotation) const { + Eigen::Matrix3Xd points(3, points_.cols()); + Eigen::Matrix3Xd normals(3, points_.cols()); + points = rotation * points_; + normals = rotation * normals_; + + return PointList(points, normals, cam_source_, view_points_); +} + +PointList PointList::cropByHandHeight(double height, int dim) const { + std::vector indices(size()); + int k = 0; + for (int i = 0; i < size(); i++) { + if (points_(dim, i) > -1.0 * height && points_(dim, i) < height) { + indices[k] = i; + k++; + } + } + + return slice(indices); +} + +} // namespace util +} // namespace gpd diff --git a/src/label_grasps.cpp b/src/label_grasps.cpp new file mode 100644 index 0000000..be439f4 --- /dev/null +++ b/src/label_grasps.cpp @@ -0,0 +1,134 @@ +#include + +#include + +namespace gpd { +namespace apps { +namespace detect_grasps { + +bool checkFileExists(const std::string &file_name) { + std::ifstream file; + file.open(file_name.c_str()); + if (!file) { + std::cout << "File " + file_name + " could not be found!\n"; + return false; + } + file.close(); + return true; +} + +int DoMain(int argc, char *argv[]) { + // Read arguments from command line. + if (argc < 4) { + std::cout << "Error: Not enough input arguments!\n\n"; + std::cout << "Usage: label_grasps CONFIG_FILE PCD_FILE MESH_FILE\n\n"; + std::cout << "Find grasp poses for a point cloud, PCD_FILE (*.pcd), " + "using parameters from CONFIG_FILE (*.cfg), and check them " + "against a mesh, MESH_FILE (*.pcd).\n\n"; + return (-1); + } + + std::string config_filename = argv[1]; + std::string pcd_filename = argv[2]; + std::string mesh_filename = argv[3]; + if (!checkFileExists(config_filename)) { + printf("Error: CONFIG_FILE not found!\n"); + return (-1); + } + if (!checkFileExists(pcd_filename)) { + printf("Error: PCD_FILE not found!\n"); + return (-1); + } + if (!checkFileExists(mesh_filename)) { + printf("Error: MESH_FILE not found!\n"); + return (-1); + } + + // Read parameters from configuration file. + const double VOXEL_SIZE = 0.003; + util::ConfigFile config_file(config_filename); + config_file.ExtractKeys(); + std::vector workspace = + config_file.getValueOfKeyAsStdVectorDouble("workspace", "-1 1 -1 1 -1 1"); + int num_threads = config_file.getValueOfKey("num_threads", 1); + int num_samples = config_file.getValueOfKey("num_samples", 100); + bool sample_above_plane = + config_file.getValueOfKey("sample_above_plane", 1); + double normals_radius = + config_file.getValueOfKey("normals_radius", 0.03); + printf("num_threads: %d, num_samples: %d\n", num_threads, num_samples); + printf("sample_above_plane: %d\n", sample_above_plane); + printf("normals_radius: %.3f\n", normals_radius); + + // View point from which the camera sees the point cloud. + Eigen::Matrix3Xd view_points(3, 1); + view_points.setZero(); + + // Load point cloud from file. + util::Cloud cloud(pcd_filename, view_points); + if (cloud.getCloudOriginal()->size() == 0) { + std::cout << "Error: Input point cloud is empty or does not exist!\n"; + return (-1); + } + + // Load point cloud from file. + util::Cloud mesh(mesh_filename, view_points); + if (mesh.getCloudOriginal()->size() == 0) { + std::cout << "Error: Mesh point cloud is empty or does not exist!\n"; + return (-1); + } + + // Prepare the point cloud. + cloud.filterWorkspace(workspace); + cloud.voxelizeCloud(VOXEL_SIZE); + cloud.calculateNormals(num_threads, normals_radius); + cloud.setNormals(cloud.getNormals() * (-1.0)); // TODO: do not do this! + if (sample_above_plane) { + cloud.sampleAbovePlane(); + } + cloud.subsample(num_samples); + + // Prepare the mesh. + mesh.calculateNormals(num_threads, normals_radius); + mesh.setNormals(mesh.getNormals() * (-1.0)); + + // Detect grasp poses. + std::vector> hands; + std::vector> images; + GraspDetector detector(config_filename); + detector.createGraspImages(cloud, hands, images); + + std::vector labels = detector.evalGroundTruth(mesh, hands); + printf("labels: %zu\n", labels.size()); + + const candidate::HandSearch::Parameters ¶ms = + detector.getHandSearchParameters(); + util::Plot plot(params.hand_axes_.size(), params.num_orientations_); + plot.plotAntipodalHands(hands, cloud.getCloudProcessed(), "Labeled Hands", + params.hand_geometry_); + + std::vector> valid_hands; + for (size_t i = 0; i < hands.size(); i++) { + printf("(%zu) label: %d\n", i, labels[i]); + if (hands[i]->isFullAntipodal()) { + printf("(%zu) label: %d\n", i, labels[i]); + valid_hands.push_back(std::move(hands[i])); + } + } + plot.plotValidHands(valid_hands, cloud.getCloudProcessed(), + mesh.getCloudProcessed(), "Antipodal Hands", + params.hand_geometry_); + // plot.plotFingers3D(valid_hands, cloud.getCloudProcessed(), "Antipodal + // Hands", + // params.hand_geometry_); + + return 0; +} + +} // namespace detect_grasps +} // namespace apps +} // namespace gpd + +int main(int argc, char *argv[]) { + return gpd::apps::detect_grasps::DoMain(argc, argv); +} diff --git a/src/nodes/classify_candidates.cpp b/src/nodes/classify_candidates.cpp deleted file mode 100644 index cfbc085..0000000 --- a/src/nodes/classify_candidates.cpp +++ /dev/null @@ -1,93 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2016, Andreas ten Pas - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * 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. - */ - - -// ROS -#include - -// Grasp Candidates Generator -#include - -// Custom -#include "../../include/gpd/grasp_detector.h" -#include "../../include/gpd/sequential_importance_sampling.h" - - -int main(int argc, char* argv[]) -{ - // initialize ROS - ros::init(argc, argv, "classify_grasp_candidates"); - ros::NodeHandle node("~"); - - // Set the position from which the camera sees the point cloud. - Eigen::Matrix3Xd view_points(3,1); - view_points << 0.0, 0.0, 0.0; - std::vector camera_position; - node.getParam("camera_position", camera_position); - view_points << camera_position[0], camera_position[1], camera_position[2]; - - // Load point cloud from file. - std::string filename; - node.param("cloud_file_name", filename, std::string("")); - CloudCamera cloud_cam(filename, view_points); - if (cloud_cam.getCloudOriginal()->size() == 0) - { - std::cout << "Input point cloud is empty or does not exist!\n"; - return (-1); - } - - // Detect grasp poses. - bool use_importance_sampling; - node.param("use_importance_sampling", use_importance_sampling, false); - - if (use_importance_sampling) - { - SequentialImportanceSampling detector(node); - - // Preprocess the point cloud (voxelize, workspace, etc.). - detector.preprocessPointCloud(cloud_cam); - - // Detect grasps in the point cloud. - std::vector grasps = detector.detectGrasps(cloud_cam); - } - else - { - GraspDetector detector(node); - - // Preprocess the point cloud (voxelize, workspace, etc.). - detector.preprocessPointCloud(cloud_cam); - - // Detect grasps in the point cloud. - std::vector grasps = detector.detectGrasps(cloud_cam); - } - - return 0; -} diff --git a/src/nodes/create_grasp_images.cpp b/src/nodes/create_grasp_images.cpp deleted file mode 100644 index e974581..0000000 --- a/src/nodes/create_grasp_images.cpp +++ /dev/null @@ -1,162 +0,0 @@ -// OpenCV -#include - -// ROS -#include - -// Custom -#include -#include - -#include "../../include/gpd/learning.h" - - -CandidatesGenerator createCandidatesGenerator(ros::NodeHandle& node) -{ - // Create objects to store parameters - CandidatesGenerator::Parameters generator_params; - HandSearch::Parameters hand_search_params; - - // Read hand geometry parameters - node.param("finger_width", hand_search_params.finger_width_, 0.01); - node.param("hand_outer_diameter", hand_search_params.hand_outer_diameter_, 0.09); - node.param("hand_depth", hand_search_params.hand_depth_, 0.06); - node.param("hand_height", hand_search_params.hand_height_, 0.02); - node.param("init_bite", hand_search_params.init_bite_, 0.015); - - // Read local hand search parameters - node.param("nn_radius", hand_search_params.nn_radius_frames_, 0.01); - node.param("num_orientations", hand_search_params.num_orientations_, 8); - node.param("num_samples", hand_search_params.num_samples_, 500); - node.param("num_threads", hand_search_params.num_threads_, 1); - node.param("rotation_axis", hand_search_params.rotation_axis_, 2); - - // Read general parameters - generator_params.num_samples_ = hand_search_params.num_samples_; - generator_params.num_threads_ = hand_search_params.num_threads_; - node.param("plot_candidates", generator_params.plot_grasps_, false); - - // Read preprocessing parameters - node.param("remove_outliers", generator_params.remove_statistical_outliers_, true); - node.param("voxelize", generator_params.voxelize_, true); - node.getParam("workspace", generator_params.workspace_); - - // Create object to generate grasp candidates. - return CandidatesGenerator(generator_params, hand_search_params); -} - - -CloudCamera loadCloudCameraFromFile(ros::NodeHandle& node) -{ - // Set the position from which the camera sees the point cloud. - std::vector camera_position; - node.getParam("camera_position", camera_position); - Eigen::Matrix3Xd view_points(3,1); - view_points << camera_position[0], camera_position[1], camera_position[2]; - - // Load the point cloud from the file. - std::string filename; - node.param("cloud_file_name", filename, std::string("")); - - return CloudCamera(filename, view_points); -} - - -Learning createLearning(ros::NodeHandle& node) -{ - // Read grasp image parameters. - Learning::ImageParameters image_params; - node.param("image_outer_diameter", image_params.outer_diameter_, 0.09); - node.param("image_depth", image_params.depth_, 0.06); - node.param("image_height", image_params.height_, 0.02); - node.param("image_size", image_params.size_, 60); - node.param("image_num_channels", image_params.num_channels_, 15); - - // Read learning parameters. - bool remove_plane; - int num_orientations, num_threads; - node.param("remove_plane_before_image_calculation", remove_plane, false); - node.param("num_orientations", num_orientations, 8); - node.param("num_threads", num_threads, 1); - - // Create object to create grasp images from grasp candidates (used for classification). - return Learning(image_params, num_threads, num_orientations, false, remove_plane); -} - - -void extractGraspsAndImages(const std::vector& hand_set_list, const std::vector& images, - std::vector& grasps_out, std::vector& images_out) -{ - grasps_out.resize(0); - images_out.resize(0); - int num_orientations = hand_set_list[0].getHypotheses().size(); - - for (int i = 0; i < hand_set_list.size(); i++) - { - const std::vector& hands = hand_set_list[i].getHypotheses(); - - for (int j = 0; j < hands.size(); j++) - { - if (hand_set_list[i].getIsValid()(j)) - { - grasps_out.push_back(hands[j]); - images_out.push_back(images[i * num_orientations + j]); - } - } - } -} - - -int main(int argc, char** argv) -{ - // Seed the random number generator. - std::srand(std::time(0)); - - // Initialize ROS. - ros::init(argc, argv, "create_grasp_images"); - ros::NodeHandle node("~"); - - // Load point cloud from file. - CloudCamera cloud_cam = loadCloudCameraFromFile(node); - if (cloud_cam.getCloudOriginal()->size() == 0) - { - ROS_ERROR("Point cloud is empty!"); - return (-1); - } - - // Create generator for grasp candidates. - CandidatesGenerator candidates_generator = createCandidatesGenerator(node); - - // Preprocess the point cloud. - candidates_generator.preprocessPointCloud(cloud_cam); - - // Generate grasp candidates. - std::vector candidates = candidates_generator.generateGraspCandidateSets(cloud_cam); - - if (candidates.size() == 0) - { - ROS_ERROR("No grasp candidates found!"); - return (-1); - } - - // Create object to generate grasp images. - Learning learning = createLearning(node); - - // Create the grasp images. - std::vector image_list = learning.createImages(cloud_cam, candidates); - std::vector valid_grasps; - std::vector valid_images; - extractGraspsAndImages(candidates, image_list, valid_grasps, valid_images); - - // Store the grasp images. - std::string file_out = "/home/andreas/test.xml"; - cv::FileStorage file_storage(file_out, cv::FileStorage::WRITE); - for (int i = 0; i < valid_images.size(); i++) - { - file_storage << "image_" + boost::lexical_cast(i) << image_list[i]; - file_storage << "label_" + boost::lexical_cast(i) << valid_grasps[i].isFullAntipodal(); - } - file_storage.release(); - - return 0; -} diff --git a/src/nodes/create_training_data.cpp b/src/nodes/create_training_data.cpp deleted file mode 100644 index ba3090f..0000000 --- a/src/nodes/create_training_data.cpp +++ /dev/null @@ -1,19 +0,0 @@ -// Custom -#include "../../include/gpd/data_generator.h" - - -int main(int argc, char** argv) -{ - // Seed the random number generator. - std::srand(std::time(0)); - - // Initialize ROS. - ros::init(argc, argv, "create_training_data"); - ros::NodeHandle node("~"); - - // Create training data. - DataGenerator generator(node); - generator.createTrainingData(node); - - return 0; -} diff --git a/src/nodes/generate_candidates.cpp b/src/nodes/generate_candidates.cpp deleted file mode 100644 index dc7f67f..0000000 --- a/src/nodes/generate_candidates.cpp +++ /dev/null @@ -1,69 +0,0 @@ -// ROS -#include - -// Custom -#include -#include - - -int main(int argc, char* argv[]) -{ - // initialize ROS - ros::init(argc, argv, "generate_grasp_candidates"); - ros::NodeHandle node("~"); - - // Create objects to store parameters - CandidatesGenerator::Parameters generator_params; - HandSearch::Parameters hand_search_params; - - // Read hand geometry parameters - node.param("finger_width", hand_search_params.finger_width_, 0.01); - node.param("hand_outer_diameter", hand_search_params.hand_outer_diameter_, 0.09); - node.param("hand_depth", hand_search_params.hand_depth_, 0.06); - node.param("hand_height", hand_search_params.hand_height_, 0.02); - node.param("init_bite", hand_search_params.init_bite_, 0.015); - - // Read local hand search parameters - node.param("nn_radius", hand_search_params.nn_radius_frames_, 0.01); - node.param("num_orientations", hand_search_params.num_orientations_, 8); - node.param("num_samples", hand_search_params.num_samples_, 500); - node.param("num_threads", hand_search_params.num_threads_, 1); - node.param("rotation_axis", hand_search_params.rotation_axis_, 2); - - // Read general parameters - generator_params.num_samples_ = hand_search_params.num_samples_; - generator_params.num_threads_ = hand_search_params.num_threads_; - node.param("plot_candidates", generator_params.plot_grasps_, false); - - // Read preprocessing parameters - node.param("remove_outliers", generator_params.remove_statistical_outliers_, true); - node.param("voxelize", generator_params.voxelize_, true); - node.getParam("workspace", generator_params.workspace_); - std::vector camera_position; - node.getParam("camera_position", camera_position); - - // Set the position from which the camera sees the point cloud. - Eigen::Matrix3Xd view_points(3,1); - view_points << camera_position[0], camera_position[1], camera_position[2]; - - // Load point cloud from file - std::string filename; - node.param("cloud_file_name", filename, std::string("")); - CloudCamera cloud_cam(filename, view_points); - if (cloud_cam.getCloudOriginal()->size() == 0) - { - std::cout << "Input point cloud is empty or does not exist!\n"; - return (-1); - } - - // Create object to generate grasp candidates. - CandidatesGenerator candidates_generator(generator_params, hand_search_params); - - // Preprocess the point cloud: voxelization, removing statistical outliers, workspace filtering. - candidates_generator.preprocessPointCloud(cloud_cam); - - // Generate grasp candidates. - std::vector candidates = candidates_generator.generateGraspCandidates(cloud_cam); - - return 0; -} diff --git a/src/nodes/grasp_detection_node.cpp b/src/nodes/grasp_detection_node.cpp deleted file mode 100644 index e057bfc..0000000 --- a/src/nodes/grasp_detection_node.cpp +++ /dev/null @@ -1,513 +0,0 @@ -#include "../../../gpd/include/nodes/grasp_detection_node.h" - - -/** constants for input point cloud types */ -const int GraspDetectionNode::POINT_CLOUD_2 = 0; ///< sensor_msgs/PointCloud2 -const int GraspDetectionNode::CLOUD_INDEXED = 1; ///< cloud with indices -const int GraspDetectionNode::CLOUD_SAMPLES = 2; ///< cloud with (x,y,z) samples - - -GraspDetectionNode::GraspDetectionNode(ros::NodeHandle& node) : has_cloud_(false), has_normals_(false), - size_left_cloud_(0), has_samples_(true), frame_("") -{ - cloud_camera_ = NULL; - - nh_ = node; // Assign the NodeHandle to the private variable - - // set camera viewpoint to default origin - std::vector camera_position; - nh_.getParam("camera_position", camera_position); - view_point_ << camera_position[0], camera_position[1], camera_position[2]; - - // choose sampling method for grasp detection - nh_.param("use_importance_sampling", use_importance_sampling_, false); - - if (use_importance_sampling_) - { - importance_sampling_ = new SequentialImportanceSampling(nh_); - } - grasp_detector_ = new GraspDetector(nh_); - - // Read input cloud and sample ROS topics parameters. - int cloud_type; - nh_.param("cloud_type", cloud_type, POINT_CLOUD_2); - std::string cloud_topic; - nh_.param("cloud_topic", cloud_topic, std::string("/camera/depth_registered/points")); - std::string samples_topic; - nh_.param("samples_topic", samples_topic, std::string("")); - std::string rviz_topic; - nh_.param("rviz_topic", rviz_topic, std::string("")); - - if (!rviz_topic.empty()) - { - grasps_rviz_pub_ = nh_.advertise(rviz_topic, 1); - use_rviz_ = true; - } - else - { - use_rviz_ = false; - } - - // subscribe to input point cloud ROS topic - if (cloud_type == POINT_CLOUD_2) - cloud_sub_ = nh_.subscribe(cloud_topic, 1, &GraspDetectionNode::cloud_callback, this); - else if (cloud_type == CLOUD_INDEXED) - cloud_sub_ = nh_.subscribe(cloud_topic, 1, &GraspDetectionNode::cloud_indexed_callback, this); - else if (cloud_type == CLOUD_SAMPLES) - { - cloud_sub_ = nh_.subscribe(cloud_topic, 1, &GraspDetectionNode::cloud_samples_callback, this); - // grasp_detector_->setUseIncomingSamples(true); - has_samples_ = false; - } - - // subscribe to input samples ROS topic - if (!samples_topic.empty()) - { - samples_sub_ = nh_.subscribe(samples_topic, 1, &GraspDetectionNode::samples_callback, this); - has_samples_ = false; - } - - // uses ROS topics to publish grasp candidates, antipodal grasps, and grasps after clustering - grasps_pub_ = nh_.advertise("clustered_grasps", 10); - - // Advertise the SetParameters service - srv_set_params_ = nh_.advertiseService("/gpd/set_params", &GraspDetectionNode::set_params_callback, this); - - nh_.getParam("workspace", workspace_); -} - - -void GraspDetectionNode::run() -{ - ros::Rate rate(100); - ROS_INFO("Waiting for point cloud to arrive ..."); - - while (ros::ok()) - { - if (has_cloud_) - { - // detect grasps in point cloud - std::vector grasps = detectGraspPosesInTopic(); - - // visualize grasps in rviz - if (use_rviz_) - { - const HandSearch::Parameters& params = grasp_detector_->getHandSearchParameters(); - grasps_rviz_pub_.publish(convertToVisualGraspMsg(grasps, params.hand_outer_diameter_, params.hand_depth_, - params.finger_width_, params.hand_height_, frame_)); - } - - // reset the system - has_cloud_ = false; - has_samples_ = false; - has_normals_ = false; - ROS_INFO("Waiting for point cloud to arrive ..."); - } - - ros::spinOnce(); - rate.sleep(); - } -} - -bool GraspDetectionNode::set_params_callback(gpd::SetParameters::Request &req, gpd::SetParameters::Response &resp) -{ - // Delete the existing sampler and detectors - if (use_importance_sampling_) - { - delete importance_sampling_; - } - delete grasp_detector_; - - // Set the workspace from the request - if (req.set_workspace) - { - workspace_.clear(); - for (int i = 0; i < req.workspace.size(); i++){ - - workspace_.push_back(req.workspace[i]); - } - nh_.setParam("workspace", workspace_); - } - - // Set the workspace_grasps from the request - if (req.set_workspace_grasps) - { - nh_.setParam("filter_grasps", true); - std::vector workspace_grasps; - for (int i = 0; i < req.workspace_grasps.size(); i++) - { - workspace_grasps.push_back(req.workspace_grasps[i]); - } - nh_.setParam("workspace_grasps", workspace_grasps); - } - else - { - nh_.setParam("filter_grasps", false); - } - - if (req.set_camera_position) - { - view_point_ << req.camera_position[0], req.camera_position[1], req.camera_position[2]; - std::vector camera_position; - camera_position.push_back(view_point_.x()); - camera_position.push_back(view_point_.y()); - camera_position.push_back(view_point_.z()); - nh_.setParam("camera_position", camera_position); - } - - // Creating new sampler and detector so they load the new rosparams - if (use_importance_sampling_) - { - importance_sampling_ = new SequentialImportanceSampling(nh_); - } - grasp_detector_ = new GraspDetector(nh_); - - resp.success = true; - return true; -} - -std::vector GraspDetectionNode::detectGraspPosesInTopic() -{ - // detect grasp poses - std::vector grasps; - - if (use_importance_sampling_) - { - cloud_camera_->filterWorkspace(workspace_); - cloud_camera_->voxelizeCloud(0.003); - cloud_camera_->calculateNormals(4); - grasps = importance_sampling_->detectGrasps(*cloud_camera_); - } - else - { - // preprocess the point cloud - grasp_detector_->preprocessPointCloud(*cloud_camera_); - - // detect grasps in the point cloud - grasps = grasp_detector_->detectGrasps(*cloud_camera_); - } - - // Publish the selected grasps. - gpd::GraspConfigList selected_grasps_msg = createGraspListMsg(grasps); - grasps_pub_.publish(selected_grasps_msg); - ROS_INFO_STREAM("Published " << selected_grasps_msg.grasps.size() << " highest-scoring grasps."); - - return grasps; -} - - -std::vector GraspDetectionNode::getSamplesInBall(const PointCloudRGBA::Ptr& cloud, - const pcl::PointXYZRGBA& centroid, float radius) -{ - std::vector indices; - std::vector dists; - pcl::KdTreeFLANN kdtree; - kdtree.setInputCloud(cloud); - kdtree.radiusSearch(centroid, radius, indices, dists); - return indices; -} - - -void GraspDetectionNode::cloud_callback(const sensor_msgs::PointCloud2& msg) -{ - if (!has_cloud_) - { - delete cloud_camera_; - cloud_camera_ = NULL; - - Eigen::Matrix3Xd view_points(3,1); - view_points.col(0) = view_point_; - - if (msg.fields.size() == 6 && msg.fields[3].name == "normal_x" && msg.fields[4].name == "normal_y" - && msg.fields[5].name == "normal_z") - { - PointCloudPointNormal::Ptr cloud(new PointCloudPointNormal); - pcl::fromROSMsg(msg, *cloud); - cloud_camera_ = new CloudCamera(cloud, 0, view_points); - cloud_camera_header_ = msg.header; - ROS_INFO_STREAM("Received cloud with " << cloud_camera_->getCloudProcessed()->size() << " points and normals."); - } - else - { - PointCloudRGBA::Ptr cloud(new PointCloudRGBA); - pcl::fromROSMsg(msg, *cloud); - cloud_camera_ = new CloudCamera(cloud, 0, view_points); - cloud_camera_header_ = msg.header; - ROS_INFO_STREAM("Received cloud with " << cloud_camera_->getCloudProcessed()->size() << " points."); - } - - has_cloud_ = true; - frame_ = msg.header.frame_id; - } -} - - -void GraspDetectionNode::cloud_indexed_callback(const gpd::CloudIndexed& msg) -{ - if (!has_cloud_) - { - initCloudCamera(msg.cloud_sources); - - // Set the indices at which to sample grasp candidates. - std::vector indices(msg.indices.size()); - for (int i=0; i < indices.size(); i++) - { - indices[i] = msg.indices[i].data; - } - cloud_camera_->setSampleIndices(indices); - - has_cloud_ = true; - frame_ = msg.cloud_sources.cloud.header.frame_id; - - ROS_INFO_STREAM("Received cloud with " << cloud_camera_->getCloudProcessed()->size() << " points, and " - << msg.indices.size() << " samples"); - } -} - - -void GraspDetectionNode::cloud_samples_callback(const gpd::CloudSamples& msg) -{ - if (!has_cloud_) - { - initCloudCamera(msg.cloud_sources); - - // Set the samples at which to sample grasp candidates. - Eigen::Matrix3Xd samples(3, msg.samples.size()); - for (int i=0; i < msg.samples.size(); i++) - { - samples.col(i) << msg.samples[i].x, msg.samples[i].y, msg.samples[i].z; - } - cloud_camera_->setSamples(samples); - - has_cloud_ = true; - has_samples_ = true; - frame_ = msg.cloud_sources.cloud.header.frame_id; - - ROS_INFO_STREAM("Received cloud with " << cloud_camera_->getCloudProcessed()->size() << " points, and " - << cloud_camera_->getSamples().cols() << " samples"); - } -} - - -void GraspDetectionNode::samples_callback(const gpd::SamplesMsg& msg) -{ - if (!has_samples_) - { - Eigen::Matrix3Xd samples(3, msg.samples.size()); - - for (int i=0; i < msg.samples.size(); i++) - { - samples.col(i) << msg.samples[i].x, msg.samples[i].y, msg.samples[i].z; - } - - cloud_camera_->setSamples(samples); - has_samples_ = true; - - ROS_INFO_STREAM("Received grasp samples message with " << msg.samples.size() << " samples"); - } -} - - -void GraspDetectionNode::initCloudCamera(const gpd::CloudSources& msg) -{ - // clean up - delete cloud_camera_; - cloud_camera_ = NULL; - - // Set view points. - Eigen::Matrix3Xd view_points(3, msg.view_points.size()); - for (int i = 0; i < msg.view_points.size(); i++) - { - view_points.col(i) << msg.view_points[i].x, msg.view_points[i].y, msg.view_points[i].z; - } - - // Set point cloud. - if (msg.cloud.fields.size() == 6 && msg.cloud.fields[3].name == "normal_x" - && msg.cloud.fields[4].name == "normal_y" && msg.cloud.fields[5].name == "normal_z") - { - PointCloudPointNormal::Ptr cloud(new PointCloudPointNormal); - pcl::fromROSMsg(msg.cloud, *cloud); - - // TODO: multiple cameras can see the same point - Eigen::MatrixXi camera_source = Eigen::MatrixXi::Zero(view_points.cols(), cloud->size()); - for (int i = 0; i < msg.camera_source.size(); i++) - { - camera_source(msg.camera_source[i].data, i) = 1; - } - - cloud_camera_ = new CloudCamera(cloud, camera_source, view_points); - } - else - { - PointCloudRGBA::Ptr cloud(new PointCloudRGBA); - pcl::fromROSMsg(msg.cloud, *cloud); - - // TODO: multiple cameras can see the same point - Eigen::MatrixXi camera_source = Eigen::MatrixXi::Zero(view_points.cols(), cloud->size()); - for (int i = 0; i < msg.camera_source.size(); i++) - { - camera_source(msg.camera_source[i].data, i) = 1; - } - - cloud_camera_ = new CloudCamera(cloud, camera_source, view_points); - std::cout << "view_points:\n" << view_points << "\n"; - } -} - - -gpd::GraspConfigList GraspDetectionNode::createGraspListMsg(const std::vector& hands) -{ - gpd::GraspConfigList msg; - - for (int i = 0; i < hands.size(); i++) - msg.grasps.push_back(convertToGraspMsg(hands[i])); - - msg.header = cloud_camera_header_; - - return msg; -} - - -gpd::GraspConfig GraspDetectionNode::convertToGraspMsg(const Grasp& hand) -{ - gpd::GraspConfig msg; - tf::pointEigenToMsg(hand.getGraspBottom(), msg.bottom); - tf::pointEigenToMsg(hand.getGraspTop(), msg.top); - tf::pointEigenToMsg(hand.getGraspSurface(), msg.surface); - tf::vectorEigenToMsg(hand.getApproach(), msg.approach); - tf::vectorEigenToMsg(hand.getBinormal(), msg.binormal); - tf::vectorEigenToMsg(hand.getAxis(), msg.axis); - msg.width.data = hand.getGraspWidth(); - msg.score.data = hand.getScore(); - tf::pointEigenToMsg(hand.getSample(), msg.sample); - - return msg; -} - - -visualization_msgs::MarkerArray GraspDetectionNode::convertToVisualGraspMsg(const std::vector& hands, - double outer_diameter, double hand_depth, double finger_width, double hand_height, const std::string& frame_id) -{ - double width = outer_diameter; - double hw = 0.5 * width; - - visualization_msgs::MarkerArray marker_array; - visualization_msgs::Marker left_finger, right_finger, base, approach; - Eigen::Vector3d left_bottom, right_bottom, left_top, right_top, left_center, right_center, approach_center, - base_center; - - for (int i = 0; i < hands.size(); i++) - { - left_bottom = hands[i].getGraspBottom() - (hw - 0.5*finger_width) * hands[i].getBinormal(); - right_bottom = hands[i].getGraspBottom() + (hw - 0.5*finger_width) * hands[i].getBinormal(); - left_top = left_bottom + hand_depth * hands[i].getApproach(); - right_top = right_bottom + hand_depth * hands[i].getApproach(); - left_center = left_bottom + 0.5*(left_top - left_bottom); - right_center = right_bottom + 0.5*(right_top - right_bottom); - base_center = left_bottom + 0.5*(right_bottom - left_bottom) - 0.01*hands[i].getApproach(); - approach_center = base_center - 0.04*hands[i].getApproach(); - - base = createHandBaseMarker(left_bottom, right_bottom, hands[i].getFrame(), 0.02, hand_height, i, frame_id); - left_finger = createFingerMarker(left_center, hands[i].getFrame(), hand_depth, finger_width, hand_height, i*3, frame_id); - right_finger = createFingerMarker(right_center, hands[i].getFrame(), hand_depth, finger_width, hand_height, i*3+1, frame_id); - approach = createFingerMarker(approach_center, hands[i].getFrame(), 0.08, finger_width, hand_height, i*3+2, frame_id); - - marker_array.markers.push_back(left_finger); - marker_array.markers.push_back(right_finger); - marker_array.markers.push_back(approach); - marker_array.markers.push_back(base); - } - - return marker_array; -} - - -visualization_msgs::Marker GraspDetectionNode::createFingerMarker(const Eigen::Vector3d& center, - const Eigen::Matrix3d& frame, double length, double width, double height, int id, const std::string& frame_id) -{ - visualization_msgs::Marker marker; - marker.header.frame_id = frame_id; - marker.header.stamp = ros::Time(); - marker.ns = "finger"; - marker.id = id; - marker.type = visualization_msgs::Marker::CUBE; - marker.action = visualization_msgs::Marker::ADD; - marker.pose.position.x = center(0); - marker.pose.position.y = center(1); - marker.pose.position.z = center(2); - marker.lifetime = ros::Duration(10); - - // use orientation of hand frame - Eigen::Quaterniond quat(frame); - marker.pose.orientation.x = quat.x(); - marker.pose.orientation.y = quat.y(); - marker.pose.orientation.z = quat.z(); - marker.pose.orientation.w = quat.w(); - - // these scales are relative to the hand frame (unit: meters) - marker.scale.x = length; // forward direction - marker.scale.y = width; // hand closing direction - marker.scale.z = height; // hand vertical direction - - marker.color.a = 0.5; - marker.color.r = 0.0; - marker.color.g = 0.0; - marker.color.b = 0.5; - - return marker; -} - - -visualization_msgs::Marker GraspDetectionNode::createHandBaseMarker(const Eigen::Vector3d& start, - const Eigen::Vector3d& end, const Eigen::Matrix3d& frame, double length, double height, int id, - const std::string& frame_id) -{ - Eigen::Vector3d center = start + 0.5 * (end - start); - - visualization_msgs::Marker marker; - marker.header.frame_id = frame_id; - marker.header.stamp = ros::Time(); - marker.ns = "hand_base"; - marker.id = id; - marker.type = visualization_msgs::Marker::CUBE; - marker.action = visualization_msgs::Marker::ADD; - marker.pose.position.x = center(0); - marker.pose.position.y = center(1); - marker.pose.position.z = center(2); - marker.lifetime = ros::Duration(10); - - // use orientation of hand frame - Eigen::Quaterniond quat(frame); - marker.pose.orientation.x = quat.x(); - marker.pose.orientation.y = quat.y(); - marker.pose.orientation.z = quat.z(); - marker.pose.orientation.w = quat.w(); - - // these scales are relative to the hand frame (unit: meters) - marker.scale.x = length; // forward direction - marker.scale.y = (end - start).norm(); // hand closing direction - marker.scale.z = height; // hand vertical direction - - marker.color.a = 0.5; - marker.color.r = 0.0; - marker.color.g = 0.0; - marker.color.b = 1.0; - - return marker; -} - - -int main(int argc, char** argv) -{ - // seed the random number generator - std::srand(std::time(0)); - - // initialize ROS - ros::init(argc, argv, "detect_grasps"); - ros::NodeHandle node("~"); - - GraspDetectionNode grasp_detection(node); - grasp_detection.run(); - - return 0; -} diff --git a/src/tests/test_conv_layer.cpp b/src/tests/test_conv_layer.cpp new file mode 100644 index 0000000..a3a822a --- /dev/null +++ b/src/tests/test_conv_layer.cpp @@ -0,0 +1,46 @@ +#include + +#include + +namespace gpd { +namespace test { +namespace { + +int DoMain(int argc, char *argv[]) { + // Create example input, weights, bias. + Eigen::MatrixXf X(5, 5); + Eigen::MatrixXf W(3, 3); + Eigen::VectorXf b = Eigen::VectorXf::Zero(1); + X << 1, 1, 1, 0, 0, 0, 1, 1, 1, 0, 0, 0, 1, 1, 1, 0, 0, 1, 1, 0, 0, 1, 1, 0, + 0; + W << 1, 0, 1, 0, 1, 0, 1, 0, 1; + + std::vector w_vec; + Eigen::Matrix + W_rowmajor(W); + Eigen::Map w(W_rowmajor.data(), W_rowmajor.size()); + w_vec.assign(w.data(), w.data() + w.size()); + + std::vector b_vec; + b_vec.assign(b.data(), b.data() + b.size()); + + // Create a convolutional layer and execute a forward pass. + net::ConvLayer conv1(5, 5, 1, 1, 3, 1, 0); + conv1.setWeightsAndBiases(w_vec, b_vec); + Eigen::Matrix + X_row_major(X); + Eigen::Map v1(X_row_major.data(), X_row_major.size()); + std::vector vec1; + vec1.assign(v1.data(), v1.data() + v1.size()); + Eigen::MatrixXf Y = conv1.forward(vec1); + + std::cout << "Y: " << Y.rows() << " x " << Y.cols() << std::endl; + std::cout << Y << std::endl; + std::cout << std::endl; +} + +} // namespace +} // namespace test +} // namespace gpd + +int main(int argc, char *argv[]) { return gpd::test::DoMain(argc, argv); } diff --git a/src/tests/test_grasp_image.cpp b/src/tests/test_grasp_image.cpp new file mode 100644 index 0000000..0fbafec --- /dev/null +++ b/src/tests/test_grasp_image.cpp @@ -0,0 +1,177 @@ +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace gpd { +namespace test { +namespace { + +int DoMain(int argc, char *argv[]) { + if (argc < 4) { + std::cout << "ERROR: Not enough arguments given!\n"; + std::cout << "Usage: rosrun gpd test_grasp_image INPUT_FILE SAMPLE_INDEX " + "DRAW_GRASP_IMAGES [IMAGE_CHANNELS] [HAND_AXES]\n"; + return -1; + } + + // View point from which the camera sees the point cloud. + Eigen::Matrix3Xd view_points(3, 1); + view_points.setZero(); + + // Load point cloud from file + std::string filename = argv[1]; + util::Cloud cloud(filename, view_points); + if (cloud.getCloudOriginal()->size() == 0) { + std::cout << "Error: Input point cloud is empty or does not exist!\n"; + return (-1); + } + + // Read the sample index from the terminal. + int sample_idx = std::stoi(argv[2]); + if (sample_idx >= cloud.getCloudOriginal()->size()) { + std::cout << "Error: Sample index is larger than the number of points in " + "the cloud!\n"; + return -1; + } + std::vector sample_indices; + sample_indices.push_back(sample_idx); + cloud.setSampleIndices(sample_indices); + + // Create objects to store parameters. + candidate::CandidatesGenerator::Parameters generator_params; + candidate::HandSearch::Parameters hand_search_params; + + // Hand geometry parameters + candidate::HandGeometry hand_geom; + hand_geom.finger_width_ = 0.01; + hand_geom.outer_diameter_ = 0.12; + hand_geom.depth_ = 0.06; + hand_geom.height_ = 0.02; + hand_geom.init_bite_ = 0.01; + hand_search_params.hand_geometry_ = hand_geom; + + // Image parameters + descriptor::ImageGeometry image_geom; + image_geom.outer_diameter_ = 0.10; + image_geom.depth_ = 0.06; + image_geom.height_ = 0.02; + image_geom.size_ = 60; + image_geom.num_channels_ = 15; + if (argc >= 5) { + image_geom.num_channels_ = std::stoi(argv[4]); + } + + // Local hand search parameters + hand_search_params.num_samples_ = 1; + hand_search_params.num_threads_ = 1; + hand_search_params.nn_radius_frames_ = 0.01; + hand_search_params.num_orientations_ = 1; + hand_search_params.num_finger_placements_ = 10; + hand_search_params.deepen_hand_ = true; + hand_search_params.friction_coeff_ = 20.0; + hand_search_params.min_viable_ = 6; + + std::vector hand_axes; + if (argc >= 6) { + for (int i = 5; i < argc; i++) { + hand_axes.push_back(std::stoi(argv[i])); + } + } else { + hand_axes.push_back(2); + } + hand_search_params.hand_axes_ = hand_axes; + std::cout << "hand_axes: "; + for (int i = 0; i < hand_search_params.hand_axes_.size(); i++) { + std::cout << hand_search_params.hand_axes_[i] << " "; + } + std::cout << "\n"; + + // General parameters + generator_params.num_samples_ = hand_search_params.num_samples_; + generator_params.num_threads_ = hand_search_params.num_threads_; + + // Preprocessing parameters + generator_params.remove_statistical_outliers_ = false; + generator_params.voxelize_ = false; + generator_params.workspace_.resize(6); + generator_params.workspace_[0] = -1.0; + generator_params.workspace_[1] = 1.0; + generator_params.workspace_[2] = -1.0; + generator_params.workspace_[3] = 1.0; + generator_params.workspace_[4] = -1.0; + generator_params.workspace_[5] = 1.0; + + double normals_radius = 0.03; + + // Calculate surface normals. + cloud.calculateNormals(4, normals_radius); + cloud.setNormals(cloud.getNormals() * (-1.0)); + + // Plot the normals. + util::Plot plot(hand_search_params.hand_axes_.size(), + hand_search_params.num_orientations_); + plot.plotNormals(cloud.getCloudProcessed(), cloud.getNormals()); + + // Generate grasp candidates. + candidate::CandidatesGenerator candidates_generator(generator_params, + hand_search_params); + std::vector> hand_set_list = + candidates_generator.generateGraspCandidateSets(cloud); + if (hand_set_list.size() == 0) { + return -1; + } + + plot.plotFingers3D(hand_set_list, cloud.getCloudProcessed(), + "Grasp candidates", hand_geom); + + const candidate::Hand &hand = *hand_set_list[0]->getHands()[0]; + std::cout << "sample: " << hand.getSample().transpose() << std::endl; + std::cout << "grasp orientation:\n" << hand.getFrame() << std::endl; + std::cout << "grasp position: " << hand.getPosition().transpose() + << std::endl; + + // Create the image for this grasp candidate. + bool plot_images = true; + plot_images = (std::stoi(argv[3]) == 1); + printf("Creating grasp image ...\n"); + printf("plot images: %d\n", plot_images); + const int kNumThreads = 1; + descriptor::ImageGenerator learn(image_geom, kNumThreads, + hand_search_params.num_orientations_, + plot_images, false); + std::vector> images; + std::vector> hands; + learn.createImages(cloud, hand_set_list, images, hands); + + // Evaluate if the grasp candidates are antipodal. + std::cout << "Antipodal: "; + for (int i = 0; i < hands.size(); i++) { + std::cout << hands[i]->isFullAntipodal() << " "; + } + std::cout << "\n"; + + plot.plotVolumes3D( + hands, cloud.getCloudProcessed(), "Volumes", hand_geom.outer_diameter_, + hand_geom.finger_width_, hand_geom.depth_, hand_geom.height_, + image_geom.outer_diameter_, image_geom.depth_, image_geom.height_); + + plot.plotHandGeometry(hand, cloud.getCloudProcessed(), hand_geom, image_geom); + + return 0; +} + +} // namespace +} // namespace test +} // namespace gpd + +int main(int argc, char *argv[]) { return gpd::test::DoMain(argc, argv); } diff --git a/src/tests/test_hdf5.cpp b/src/tests/test_hdf5.cpp new file mode 100644 index 0000000..0453711 --- /dev/null +++ b/src/tests/test_hdf5.cpp @@ -0,0 +1,92 @@ +#include +#include +#include +using namespace cv; + +static void write_root_group_single_channel() { + std::cout << "write_root_group_single_channel\n"; + String filename = "root_group_single_channel.h5"; + String dataset_name = + "/single"; // Note that it is a child of the root group / + // prepare data + Mat data; + data = (cv::Mat_(2, 3) << 0, 1, 2, 3, 4, 5, 6); + Ptr h5io = hdf::open(filename); + // write data to the given dataset + // the dataset "/single" is created automatically, since it is a child of the + // root + h5io->dswrite(data, dataset_name); + Mat expected; + h5io->dsread(expected, dataset_name); + double diff = norm(data - expected); + CV_Assert(abs(diff) < 1e-10); + h5io->close(); +} + +static void write_single_channel() { + String filename = "single_channel.h5"; + String parent_name = "/data"; + String dataset_name = parent_name + "/single"; + // prepare data + Mat data; + data = (cv::Mat_(2, 3) << 0, 1, 2, 3, 4, 5); + Ptr h5io = hdf::open(filename); + // first we need to create the parent group + if (!h5io->hlexists(parent_name)) h5io->grcreate(parent_name); + // create the dataset if it not exists + if (!h5io->hlexists(dataset_name)) + h5io->dscreate(data.rows, data.cols, data.type(), dataset_name); + // the following is the same with the above function + // write_root_group_single_channel() + h5io->dswrite(data, dataset_name); + Mat expected; + h5io->dsread(expected, dataset_name); + + std::cout << "expected:\n" << expected << "\n"; + + double diff = norm(data - expected); + CV_Assert(abs(diff) < 1e-10); + h5io->close(); +} + +/* + * creating, reading and writing multiple-channel matrices + * are the same with single channel matrices + */ +static void write_multiple_channels() { + String filename = "two_channels.h5"; + String parent_name = "/data"; + String dataset_name = parent_name + "/two_channels"; + // prepare data + Mat data(2, 3, CV_32SC2); + for (size_t i = 0; i < data.total() * data.channels(); i++) + ((int *)data.data)[i] = (int)i; + Ptr h5io = hdf::open(filename); + // first we need to create the parent group + if (!h5io->hlexists(parent_name)) h5io->grcreate(parent_name); + // create the dataset if it not exists + if (!h5io->hlexists(dataset_name)) + h5io->dscreate(data.rows, data.cols, data.type(), dataset_name); + // the following is the same with the above function + // write_root_group_single_channel() + h5io->dswrite(data, dataset_name); + Mat expected; + h5io->dsread(expected, dataset_name); + double diff = norm(data - expected); + CV_Assert(abs(diff) < 1e-10); + h5io->close(); +} + +int main(int argc, char *argv[]) { + write_root_group_single_channel(); + write_single_channel(); + write_multiple_channels(); + + Ptr h5io = hdf::open(argv[1]); + Mat expected; + h5io->dsread(expected, "H_table_from_reference_camera"); + std::cout << "H_table_from_reference_camera:\n" << expected << "\n"; + h5io->close(); + + return 0; +} diff --git a/src/tests/test_lenet.cpp b/src/tests/test_lenet.cpp new file mode 100644 index 0000000..131cde1 --- /dev/null +++ b/src/tests/test_lenet.cpp @@ -0,0 +1,25 @@ +#include + +#include + +namespace gpd { +namespace test { +namespace { + +int DoMain(int argc, char *argv[]) { + // Test with input from file. + net::Lenet network( + 1, "/home/atenpas/catkin_ws/src/gpd_no_ros/lenet/3channels/params/", 60, + 15); + std::vector x = network.readFileLineByLineIntoVector( + "src/gpd/caffe/15channels/txt/rand_input.txt"); + network.forward(x); + + return 0; +} + +} // namespace +} // namespace test +} // namespace gpd + +int main(int argc, char *argv[]) { return gpd::test::DoMain(argc, argv); } diff --git a/src/tests/test_occlusion.cpp b/src/tests/test_occlusion.cpp deleted file mode 100644 index 669b3ed..0000000 --- a/src/tests/test_occlusion.cpp +++ /dev/null @@ -1,166 +0,0 @@ -#include -#include -#include -#include - -#include "caffe/caffe.hpp" -#include "caffe/util/io.hpp" -#include "caffe/blob.hpp" -#include "caffe/layers/memory_data_layer.hpp" -#include "caffe/layers/input_layer.hpp" - -#include "../../include/gpd/learning.h" - - -using namespace caffe; - - -int main(int argc, char* argv[]) -{ - // View point from which the camera sees the point cloud. - Eigen::Matrix3Xd view_points(3,1); - view_points.setZero(); - - // Load point cloud from file - // std::string filename = "/media/andreas/2a9b7d00-f8c3-4849-9ddc-283f5b7c206a/data/object_datasets/bb_onesource/pcd/vo5_tea_therapy_healthful_green_tea_smoothing_shampoo_1_bin.pcd"; - std::string filename = "/home/andreas/data/bigbird/3m_high_track_spray_adhesive/clouds/NP1_0.pcd"; - // std::string filename = "/home/baxter/data/bigbird/3m_high_tack_spray_adhesive/clouds/NP1_0.pcd"; - CloudCamera cloud_cam(filename, view_points); - if (cloud_cam.getCloudOriginal()->size() == 0) - { - std::cout << "Input point cloud is empty or does not exist!\n"; - return (-1); - } - - // Use a custom sample. - Eigen::Matrix3Xd samples(3,1); - samples << -0.0129, 0.0515, 0.7042; - cloud_cam.setSamples(samples); - - // Create objects to store parameters. - CandidatesGenerator::Parameters generator_params; - HandSearch::Parameters hand_search_params; - - // Hand geometry parameters - hand_search_params.finger_width_ = 0.01; - hand_search_params.hand_outer_diameter_ = 0.12; - hand_search_params.hand_depth_ = 0.06; - hand_search_params.hand_height_ = 0.02; - hand_search_params.init_bite_ = 0.015; - - // Local hand search parameters - hand_search_params.nn_radius_frames_ = 0.01; - hand_search_params.num_orientations_ = 8; - hand_search_params.num_samples_ = 1; - hand_search_params.num_threads_ = 1; - hand_search_params.rotation_axis_ = 2; - - // General parameters - generator_params.num_samples_ = hand_search_params.num_samples_; - generator_params.num_threads_ = hand_search_params.num_threads_; - generator_params.plot_grasps_ = true; - - // Preprocessing parameters - generator_params.remove_statistical_outliers_ = false; - generator_params.voxelize_ = false; - generator_params.workspace_.resize(6); - generator_params.workspace_[0] = -1.0; - generator_params.workspace_[1] = 1.0; - generator_params.workspace_[2] = -1.0; - generator_params.workspace_[3] = 1.0; - generator_params.workspace_[4] = -1.0; - generator_params.workspace_[5] = 1.0; - - // Image parameters - Learning::ImageParameters image_params; - image_params.depth_ = 0.06; - image_params.height_ = 0.02; - image_params.outer_diameter_ = 0.10; - image_params.size_ = 60; - image_params.num_channels_ = 15; - - // Calculate surface normals. - cloud_cam.calculateNormals(4); - - // Plot the normals. - Plot plot; - plot.plotNormals(cloud_cam.getCloudProcessed(), cloud_cam.getNormals()); - - // Manually construct a grasp candidate. - Eigen::Matrix3d frame; - frame << 0.9317, 0.3561, 0.0717, - 0.0968, -0.0533, -0.9939, - -0.3501, 0.9329, -0.0841; - Eigen::Vector3d sample = samples.col(0); - FingerHand fh(hand_search_params.finger_width_, hand_search_params.hand_outer_diameter_, hand_search_params.hand_depth_); - fh.setBottom(-0.0150); - fh.setTop(0.0450); - fh.setSurface(-0.0180); - fh.setLeft(-0.0511); - fh.setRight(0.0489); - fh.setCenter(-0.0011); - - Grasp hand(sample, frame, fh); - hand.print(); - - std::vector hands; - hands.push_back(hand); - - GraspSet hand_set; - hand_set.setHands(hands); - hand_set.setSample(sample); - - std::vector hand_set_list; - hand_set_list.push_back(hand_set); - - // Create the image for this grasp candidate. - Learning learn(image_params, 1, hand_search_params.num_orientations_, true, false); - std::vector images = learn.createImages(cloud_cam, hand_set_list); - std::cout << "------------\n" << images[0].rows << " x " << images[0].cols << " x " << images[0].channels() << "\n"; - - // Predict if the grasp candidate is a good grasp or not. - Caffe::set_mode(Caffe::GPU); - std::string root = "/home/andreas/catkin_ws/src/gpd/caffe/"; - // std::string root = "/home/baxter/baxter_ws/src/gpd/caffe/"; - std::string model_file = root + "15channels/lenet_15_channels.prototxt"; - std::string weights_file = root + "15channels/two_views_15_channels_90_deg.caffemodel"; - std::string labels_file = root + "labels.txt"; - - caffe::Net net(model_file, caffe::TEST); - net.CopyTrainedLayersFrom(weights_file); - - float loss = 0.0; - std::vector label_list; - label_list.push_back(0); - - boost::shared_ptr > memory_data_layer; - memory_data_layer = boost::static_pointer_cast < caffe::MemoryDataLayer < float > >(net.layer_by_name("data")); - - std::cout << "#images: " << images.size() << "\n"; - - for (int i = images.size(); i < memory_data_layer->batch_size(); i++) - { - images.push_back(cv::Mat(60, 60, CV_8UC(15), cv::Scalar(0))); - label_list.push_back(0); - } - - memory_data_layer->AddMatVector(images, label_list); - - std::vector*> results = net.Forward(&loss); - std::cout << "loss: " << loss << "\n"; - - boost::shared_ptr > output_layer; - output_layer = net.blob_by_name("ip2"); - std::cout << "output_layer->channels(): " << output_layer->channels() << "\n"; - - const float* begin = results[0]->cpu_data(); - const float* end = begin + results[0]->count(); - std::vector out(begin, end); - - for (int l = 0; l < results[0]->count() / results[0]-> channels(); l++) - { - std::cout << "Score (positive): " << out[2 * l + 1] << ", score (negative): " << out[2 * l] << "\n"; - } - - return 0; -} diff --git a/srv/SetParameters.srv b/srv/SetParameters.srv deleted file mode 100644 index eeb527f..0000000 --- a/srv/SetParameters.srv +++ /dev/null @@ -1,11 +0,0 @@ -bool set_workspace -float32[6] workspace - -bool set_workspace_grasps -float32[6] workspace_grasps - -bool set_camera_position -float32[3] camera_position ---- - -bool success diff --git a/tutorials/hand_frame.png b/tutorials/hand_frame.png deleted file mode 100644 index 7eb5b72..0000000 Binary files a/tutorials/hand_frame.png and /dev/null differ diff --git a/tutorials/openni2.rviz b/tutorials/openni2.rviz deleted file mode 100644 index 4866760..0000000 --- a/tutorials/openni2.rviz +++ /dev/null @@ -1,165 +0,0 @@ -Panels: - - Class: rviz/Displays - Help Height: 81 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Status1 - - /PointCloud21 - - /TF1/Frames1 - - /Grasps1 - Splitter Ratio: 0.548077 - Tree Height: 815 - - Class: rviz/Selection - Name: Selection - - Class: rviz/Tool Properties - Expanded: - - /2D Pose Estimate1 - - /2D Nav Goal1 - - /Publish Point1 - Name: Tool Properties - Splitter Ratio: 0.588679 - - Class: rviz/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: rviz/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: PointCloud2 -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz/Grid - Color: 160; 160; 164 - Enabled: false - Line Style: - Line Width: 0.03 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 10 - Reference Frame: - Value: false - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 170; 0 - Color Transformer: RGB8 - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: PointCloud2 - Position Transformer: XYZ - Queue Size: 1 - Selectable: true - Size (Pixels): 3 - Size (m): 0.01 - Style: Points - Topic: /camera/depth_registered/points - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - - Class: rviz/TF - Enabled: false - Frame Timeout: 15 - Frames: - All Enabled: true - Marker Scale: 0.7 - Name: TF - Show Arrows: true - Show Axes: true - Show Names: true - Tree: - {} - Update Interval: 0 - Value: false - - Class: rviz/MarkerArray - Enabled: true - Marker Topic: /detect_grasps/grasps_rviz - Name: Grasps - Namespaces: - finger: true - hand_base: true - Queue Size: 100 - Value: true - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Fixed Frame: camera_link - Frame Rate: 30 - Name: root - Tools: - - Class: rviz/Interact - Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Topic: /initialpose - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint - Single click: true - Topic: /clicked_point - Value: true - Views: - Current: - Class: rviz/Orbit - Distance: 0.438661 - Enable Stereo Rendering: - Stereo Eye Separation: 0.06 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: 0.443235 - Y: 0.0711695 - Z: 0.205321 - Name: Current View - Near Clip Distance: 0.01 - Pitch: 0.355202 - Target Frame: - Value: Orbit (rviz) - Yaw: 2.47235 - Saved: ~ -Window Geometry: - Displays: - collapsed: false - Height: 1000 - Hide Left Dock: false - Hide Right Dock: true - QMainWindow State: 000000ff00000000fd000000040000000000000142000003bcfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005300fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000013000003bc000000b600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000359fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000035000003590000009900fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000077e0000003efc0100000002fb0000000800540069006d006500000000000000077e0000023000fffffffb0000000800540069006d006501000000000000045000000000000000000000062e000003bc00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730000000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: true - Width: 1910 - X: 0 - Y: 26 diff --git a/tutorials/rviz_grasps_openni2.png b/tutorials/rviz_grasps_openni2.png deleted file mode 100644 index ebef393..0000000 Binary files a/tutorials/rviz_grasps_openni2.png and /dev/null differ diff --git a/tutorials/rviz_grasps_tutorial2.png b/tutorials/rviz_grasps_tutorial2.png deleted file mode 100644 index 07f99b7..0000000 Binary files a/tutorials/rviz_grasps_tutorial2.png and /dev/null differ diff --git a/tutorials/tutorial2.rviz b/tutorials/tutorial2.rviz deleted file mode 100644 index cd1a8d8..0000000 --- a/tutorials/tutorial2.rviz +++ /dev/null @@ -1,150 +0,0 @@ -Panels: - - Class: rviz/Displays - Help Height: 81 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /PointCloud21 - - /PointCloud21/Autocompute Value Bounds1 - - /Grasps1 - Splitter Ratio: 0.548077 - Tree Height: 820 - - Class: rviz/Selection - Name: Selection - - Class: rviz/Tool Properties - Expanded: - - /2D Pose Estimate1 - - /2D Nav Goal1 - - /Publish Point1 - Name: Tool Properties - Splitter Ratio: 0.588679 - - Class: rviz/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: rviz/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: PointCloud2 -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz/Grid - Color: 160; 160; 164 - Enabled: true - Line Style: - Line Width: 0.03 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 10 - Reference Frame: - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 0.99121 - Min Value: 0.69345 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 85; 170; 0 - Color Transformer: AxisColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: PointCloud2 - Position Transformer: XYZ - Queue Size: 1 - Selectable: true - Size (Pixels): 3 - Size (m): 0.01 - Style: Points - Topic: /cloud_pcd - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - - Class: rviz/MarkerArray - Enabled: true - Marker Topic: /detect_grasps/grasps_rviz - Name: Grasps - Namespaces: - finger: true - hand_base: true - Queue Size: 100 - Value: true - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Fixed Frame: base_link - Frame Rate: 30 - Name: root - Tools: - - Class: rviz/Interact - Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Topic: /initialpose - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint - Single click: true - Topic: /clicked_point - Value: true - Views: - Current: - Class: rviz/Orbit - Distance: 0.147261 - Enable Stereo Rendering: - Stereo Eye Separation: 0.06 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: 0.073817 - Y: 0.0678573 - Z: 0.207995 - Name: Current View - Near Clip Distance: 0.01 - Pitch: -1.5698 - Target Frame: - Value: Orbit (rviz) - Yaw: 4.73735 - Saved: ~ -Window Geometry: - Displays: - collapsed: false - Height: 1005 - Hide Left Dock: false - Hide Right Dock: true - QMainWindow State: 000000ff00000000fd000000040000000000000142000003c1fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005300fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000013000003c1000000b600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000359fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000035000003590000009900fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000077e0000003efc0100000002fb0000000800540069006d006500000000000000077e0000023000fffffffb0000000800540069006d0065010000000000000450000000000000000000000638000003c100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730000000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: true - Width: 1920 - X: -5 - Y: 26 diff --git a/tutorials/tutorial_1_grasps_camera.md b/tutorials/tutorial_1_grasps_camera.md deleted file mode 100644 index 4815e10..0000000 --- a/tutorials/tutorial_1_grasps_camera.md +++ /dev/null @@ -1,97 +0,0 @@ -# Tutorial 1 - Detect Grasps With an RGBD Camera - - -In this tutorial, we will detect grasp poses using an RGBD camera such as the *Microsoft Kinect* or *Asus Xtion Pro*. - - -## 1. Setup - -Connect an RGBD camera to your robot/computer. - -All of the following commands need to be run in separate terminals/tabs. First, start the driver for your camera. -``` -roslaunch openni2_launch openni2.launch -``` - -Next, start rviz. -``` -rosrun rviz rviz -``` -In *rviz*, load the config file *gpd/tutorials/openni2.rviz*. - - -## 2. Detect Grasps - -Launch the ROS node that detects grasps. -``` -roslaunch gpd tutorial1.launch -``` -This will produce grasps on the ROS topic */detect_grasps/grasps*. An example is shown below. - -![rviz screenshot](./rviz_grasps_openni2.png "Grasps visualized in rviz") - - -## 3. Use the Grasps in Python - -To use the grasps, start up your favorite Python editor and enter the following code. - -```python -import rospy -from gpd.msg import GraspConfigList - -# global variable to store grasps -grasps = [] - - -# Callback function to receive grasps. -def callback(msg): - global grasps - grasps = msg.grasps - - -# ==================== MAIN ==================== -# Create a ROS node. -rospy.init_node('get_grasps') - -# Subscribe to the ROS topic that contains the grasps. -sub = rospy.Subscriber('/detect_grasps/clustered_grasps', GraspConfigList, callback) - -# Wait for grasps to arrive. -rate = rospy.Rate(1) - -while not rospy.is_shutdown(): - print '.' - if len(grasps) > 0: - rospy.loginfo('Received %d grasps.', len(grasps)) - break - rate.sleep() -``` - -This code creates a ROS node that waits for grasps. Once grasps arrive, the node prints the number of grasps -receveived and exits. You can start from this code to use our grasp detection algorithm on your robot! - - -## 2. Troubleshoot - -### No Grasps Found - -If the algorithm does not find grasps, it's usually because the workspace (`workspace`) is too large or that the number -of samples (`num_samples`) drawn from the point cloud is too small. You can change the corresponding parameters in the -launch file (*launch/openni2_15_channels.launch*). - - -## 3. Switching to PCL visualization - -If you prefer PCL's cloud visualization, open the file *launch/openni2_15_channels.launch*, set the parameter -`rviz_topic` to an empty string, *""*, and set any of the parameters starting with `plot_` to *True*. Those parameters allow to plot -different steps in the algorithm. For example, the following setting plots the detected grasps using pcl. -``` - - - - - - - - -``` diff --git a/tutorials/tutorial_2_grasp_select.md b/tutorials/tutorial_2_grasp_select.md deleted file mode 100644 index acced17..0000000 --- a/tutorials/tutorial_2_grasp_select.md +++ /dev/null @@ -1,150 +0,0 @@ -# Tutorial 2 - Detect Grasps on a Single Object - - -In this tutorial, we will detect grasp poses for a single object on a table and select a grasp for the robot to execute. - - -## 1. Remove the Plane - -To find grasps on a single object, we will fit a plane to the point cloud and use the nonplanar indices to sample grasp -candidates. - -We do this first with a PCD file. One way to load a PCD file in Python is to publish the file on a ROS -topic. -``` -rosrun pcl_ros pcd_to_pointcloud src/gpd/tutorials/table_mug.pcd -``` - -Next, we create a ROS node in Python that gets the point cloud from the topic (*/cloud_pcd*). -```python -import rospy -from sensor_msgs.msg import PointCloud2 -from sensor_msgs import point_cloud2 - -cloud = [] # global variable to store the point cloud - -def cloudCallback(msg): - global cloud - if len(cloud) == 0: - for p in point_cloud2.read_points(msg): - cloud.append([p[0], p[1], p[2]]) - - -# Create a ROS node. -rospy.init_node('select_grasp') - -# Subscribe to the ROS topic that contains the grasps. -cloud_sub = rospy.Subscriber('/cloud_pcd', PointCloud2, cloudCallback) - -# Wait for point cloud to arrive. -while len(cloud) == 0: - rospy.sleep(0.01) -``` - -To find the nonplanar indices, we first do a -[least squares fit](https://www.geometrictools.com/Documentation/LeastSquaresFitting.pdf) of the points to a plane, and -then we select the points that are at least a minimum distance away from that fitted plane. -```python -import numpy as np -from scipy.linalg import lstsq - -cloud = np.asarray(cloud) -A = np.c_[cloud[:,0], cloud[:,1], np.ones(cloud.shape[0])] -b = cloud[:,2] -C, _, _, _ = lstsq(A, b) -a, b, c, d = C[0], C[1], -1., C[2] # coefficients of the form: a*x + b*y + c*z + d = 0. -dist = ((a*cloud[:,0] + b*cloud[:,1] + d) - cloud[:,2])**2 -err = dist.sum() -idx = np.where(dist > 0.01) -``` - -Finally, we publish the point cloud and the indices at which we want to sample grasp candidates. -```python -# Publish point cloud and nonplanar indices. -from gpd.msg import CloudIndexed -from std_msgs.msg import Header, Int64 -from geometry_msgs.msg import Point - -pub = rospy.Publisher('cloud_indexed', CloudIndexed, queue_size=1) - -msg = CloudIndexed() -header = Header() -header.frame_id = "/base_link" -header.stamp = rospy.Time.now() -msg.cloud_sources.cloud = point_cloud2.create_cloud_xyz32(header, cloud.tolist()) -msg.cloud_sources.view_points.append(Point(0,0,0)) -for i in xrange(cloud.shape[0]): - msg.cloud_sources.camera_source.append(Int64(0)) -for i in idx[0]: - msg.indices.append(Int64(i)) -s = raw_input('Hit [ENTER] to publish') -pub.publish(msg) -rospy.sleep(2) -print 'Published cloud with', len(msg.indices), 'indices' -``` - -You can find the complete code in *scripts/select_grasp.py*. To test the code, start roscore, rviz, and the grasp -detection node (in separate terminals). -``` -roscore -rosrun rviz rviz -roslaunch gpd tutorial2.launch -``` - -In *rviz*, load the config file *gpd/tutorials/tutorial2.rviz*. - -Then, run the *select_grasp.py* node. -``` -python src/gpd/scripts/select_grasp.py -``` - -Send a PCD file to the the *select_grasp* node. -``` -rosrun pcl_ros pcd_to_pointcloud src/gpd/tutorials/table_mug.pcd -``` - -Hit the Enter key when asked. It takes a while to calculate the surface normals (about a minute on a -*3Ghz Intel i7 CPU*). Now you should see grasps in rviz as shown below. - -![rviz screenshot](./rviz_grasps_tutorial2.png "Grasps visualized in rviz") - - -## 3. Select a Grasp in Python - -To select a grasp for the robot to execute, we use a simple heuristic: we just pick the grasp with the largest -classification score. - -```python -# Select a grasp for the robot to execute. -from gpd.msg import GraspConfigList - -grasps = [] # global variable to store grasps - -def callback(msg): - global grasps - grasps = msg.grasps - -# Subscribe to the ROS topic that contains the grasps. -grasps_sub = rospy.Subscriber('/detect_grasps/clustered_grasps', GraspConfigList, callback) - -# Wait for grasps to arrive. -rate = rospy.Rate(1) - -while not rospy.is_shutdown(): - if len(grasps) > 0: - rospy.loginfo('Received %d grasps.', len(grasps)) - break - -grasp = grasps[0] # grasps are sorted in descending order by score -print 'Selected grasp with score:', grasp.score -``` - -After selecting a grasp, you can get the information about where to place the robot hand -from the *GraspConfig* message. Here is a visualization of the orientation. - -![hand orientation](./hand_frame.png "Hand orientation") - -To use an actual RGBD camera on your robot instead of a PCD file, you can start by connecting the *select_grasp* node -to *openni2* as in the first tutorial. - - diff --git a/tutorials/tutorial_openvino.md b/tutorials/tutorial_openvino.md deleted file mode 100644 index 6d880d3..0000000 --- a/tutorials/tutorial_openvino.md +++ /dev/null @@ -1,39 +0,0 @@ -# Tutorial Enable OpenVINO - -In this tutorial, we introduce how to enable OpenVINO option for grasp detection. - -## 1. Install OpenVINO toolkit -OpenVINO supports multiple host OS. We verified with Linux. -[Installing the OpenVINO Toolkit for Linux](https://software.intel.com/en-us/articles/OpenVINO-Install-Linux) - -## 2. Verify OpenVINO installation -Try to run the demo scripts. -[Verify the Demo Scripts](https://software.intel.com/en-us/articles/OpenVINO-Install-Linux#inpage-nav-3-5) -Then try to run OpenVINO inference example applications. -[Build the Samples](https://software.intel.com/en-us/articles/OpenVINO-InferEngine#inpage-nav-6), -[Running the Samples](https://software.intel.com/en-us/articles/OpenVINO-InferEngine#inpage-nav-7) - -## 3. Build GPD with OpenVINO -Setup OpenVINO environment variables, replacing with the specific location. -``` -source /bin/setupvars.sh -``` -Once OpenVINO installed, build GPD with option "USE_OPENVINO" ([OFF]|ON) -``` -catkin_make -DCMAKE_BUILD_TYPE=Release -DUSE_OPENVINO=ON --pkg gpd -``` - -## 4. Launch GPD with OpenVINO -The launch process is similar to [Detect Grasps With an RGBD camera](tutorials/tutorial_1_grasps_camera.md), -just with an additional param "device" ([0:CPU]|1:GPU|2:VPU|3:FPGA) to specify the target device to execute the -grasp detection. -``` -# launch the openni camera for pointcloud2 -roslaunch openni2_launch openni2.launch -# start rviz -rosrun rviz rviz -# setup OpenVINO environment variables, replacing with the specific location -source /bin/setupvars.sh -# launch the grasp detection -roslaunch gpd tutorial1.launch device:=0 -```