read required parameters from config file

CMakeLists.txt

@@ -278,6 +278,9 @@ set_target_properties(${PROJECT_NAME}_cem_detect_grasps
 set_target_properties(${PROJECT_NAME}_detect_grasps
   PROPERTIES OUTPUT_NAME detect_grasps PREFIX "")
 
+set_target_properties(${PROJECT_NAME}_generate_candidates
+  PROPERTIES OUTPUT_NAME generate_candidates PREFIX "")
+
 if(BUILD_DATA_GENERATION STREQUAL "ON")
   set_target_properties(${PROJECT_NAME}_generate_data
     PROPERTIES OUTPUT_NAME generate_data PREFIX "")

cfg/eigen_params.cfg

@@ -75,7 +75,7 @@ num_selected = 5
 #   plot_selected_grasps: plot the selected grasps (final output)
 plot_normals = 1
 plot_samples = 0
-plot_candidates = 0
+plot_candidates = 1
 plot_filtered_candidates = 0
 plot_valid_grasps = 0
 plot_clustered_grasps = 0

src/generate_candidates.cpp

@@ -7,6 +7,7 @@
 #include <gpd/candidate/candidates_generator.h>
 #include <gpd/candidate/hand_search.h>
 #include <gpd/util/config_file.h>
+#include <gpd/util/plot.h>
 
 namespace gpd {
 namespace apps {
@@ -43,6 +44,7 @@ int DoMain(int argc, char *argv[]) {
 
   // Read parameters from configuration file.
   util::ConfigFile config_file(argv[1]);
+  config_file.ExtractKeys();
 
   candidate::HandGeometry hand_geom;
   hand_geom.finger_width_ =
@@ -59,55 +61,64 @@ int DoMain(int argc, char *argv[]) {
   std::cout << "hand_height: " << hand_geom.height_ << "\n";
   std::cout << "init_bite: " << hand_geom.init_bite_ << "\n";
 
+  bool sample_above_plane =
+      config_file.getValueOfKey<bool>("sample_above_plane", false);
   bool voxelize = config_file.getValueOfKey<bool>("voxelize", true);
+  double voxel_size =  config_file.getValueOfKey<double>("voxel_size", 0.003);
   bool remove_outliers =
       config_file.getValueOfKey<bool>("remove_outliers", false);
+  double normals_radius =
+      config_file.getValueOfKey<double>("normals_radius", 0.03);
   std::string workspace_str =
       config_file.getValueOfKeyAsString("workspace", "");
-  std::string camera_pose_str =
-      config_file.getValueOfKeyAsString("camera_pose", "");
+  std::string camera_position_str =
+      config_file.getValueOfKeyAsString("camera_position", "");
+
   std::vector<double> workspace = stringToDouble(workspace_str);
-  std::vector<double> camera_pose = stringToDouble(camera_pose_str);
+  std::vector<double> camera_position = stringToDouble(camera_position_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";
+  std::cout << "camera_position: " << camera_position_str << "\n";
 
   int num_samples = config_file.getValueOfKey<int>("num_samples", 1000);
   int num_threads = config_file.getValueOfKey<int>("num_threads", 1);
   double nn_radius = config_file.getValueOfKey<double>("nn_radius", 0.01);
   int num_orientations = config_file.getValueOfKey<int>("num_orientations", 8);
+  int num_finger_placements =
+      config_file.getValueOfKey<int>("num_finger_placements", 10);
   std::vector<int> 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";
+  std::cout << "num_finger_placements: " << num_finger_placements << "\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<bool>("plot_grasps", true);
-  bool plot_all_grasps =
-      config_file.getValueOfKey<bool>("plot_all_grasps", true);
-  bool plot_normals = config_file.getValueOfKey<bool>("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";
+  bool plot_candidates =
+      config_file.getValueOfKey<bool>("plot_candidates", true);
+  std::cout << "plot_candidates: " << plot_candidates << "\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.sample_above_plane_ = sample_above_plane;
   generator_params.voxelize_ = voxelize;
+  generator_params.voxel_size_ = voxel_size;
+  generator_params.normals_radius_ = normals_radius;
   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_finger_placements_ = num_finger_placements;
   hand_search_params.num_samples_ = num_samples;
   hand_search_params.num_threads_ = num_threads;
   hand_search_params.hand_axes_ = hand_axes;
@@ -116,28 +127,34 @@ int DoMain(int argc, char *argv[]) {
 
   // Set the camera pose.
   Eigen::Matrix3Xd view_points(3, 1);
-  view_points << camera_pose[3], camera_pose[6], camera_pose[9];
+  view_points << camera_position[0], camera_position[1], camera_position[2];
 
   // Create object to load point cloud from file.
-  util::Cloud cloud_cam(argv[2], view_points);
-  if (cloud_cam.getCloudOriginal()->size() == 0) {
+  util::Cloud cloud(argv[2], view_points);
+  if (cloud.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]);
+    cloud.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);
+  candidates_generator.preprocessPointCloud(cloud);
 
   // Generate a list of grasp candidates.
   std::vector<std::unique_ptr<candidate::Hand>> candidates =
-      candidates_generator.generateGraspCandidates(cloud_cam);
+      candidates_generator.generateGraspCandidates(cloud);
+
+  if (plot_candidates) {
+    util::Plot plotter(hand_axes.size(), num_orientations);
+    plotter.plotFingers3D(candidates, cloud.getCloudOriginal(),
+        "Grasp candidates", hand_geom);
+  }
 
   return 0;
 }