fix importance sampling parameters

include/gpd/sequential_importance_sampling.h

@@ -85,6 +85,12 @@ class SequentialImportanceSampling
    */
   std::vector<Grasp> 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);
+
   /**
    * \brief Compare if two grasps are equal based on their position.
    * \param h1 the first grasp
@@ -145,12 +151,19 @@ class SequentialImportanceSampling
   int num_init_samples_; ///< number of initial samples
   double prob_rand_samples_; ///< probability of random samples
   double radius_; ///< radius
-  bool visualize_steps_; ///< if all iterations are visualized
-  bool visualize_results_; ///< if the results are visualized
   int sampling_method_; ///< what sampling method is used (sum, max, weighted)
 
+  // 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
+
+  // grasp filtering parameters
   bool filter_grasps_; ///< if grasps are filtered based on the robot's workspace
   std::vector<double> workspace_; ///< the robot's workspace
+  std::vector<double> workspace_grasps_; ///< the robot's workspace
+
+  int num_threads_; ///< number of CPU threads used in grasp detection
 
   // standard parameters
   static const int NUM_ITERATIONS;

launch/importance_sampling_file_15_channels.launch

@@ -6,56 +6,41 @@
   </include>
 
   <!-- Load classifier parameters -->  
-  <include file="$(find gpd)/launch/caffe/classifier_15channels.launch">
+  <include file="$(find gpd)/launch/caffe/ur5_classifier_15channels.launch">
     <arg name="node" value="classify_grasp_candidates" />
   </include>
 
 	<node name="classify_grasp_candidates" pkg="gpd" type="classify_candidates" output="screen">
 
     <!-- Where the point cloud is stored -->    
-<!--
-    <param name="cloud_file_name" value="/home/andreas/data/bigbird/3m_high_track_spray_adhesive/clouds/NP1_0.pcd" />
--->
-    <param name="cloud_file_name" value="/home/andreas/data/baxter_clutter/scene0.pcd" />
-<!--
-    <param name="cloud_file_name" value="/home/baxter/data/bigbird/3m_high_tack_spray_adhesive/clouds/NP1_0.pcd" />
--->
-<!--
-    <param name="cloud_file_name" value="/media/andreas/2a9b7d00-f8c3-4849-9ddc-283f5b7c206a/data/object_datasets/bb_onesource/pcd/vo5_tea_therapy_healthful_green_tea_smoothing_shampoo_gt.pcd" />
--->
+<!-- <param name="cloud_file_name" value="$(find gpd)/tutorials/table_mug.pcd" /> -->
+<!-- <param name="cloud_file_name" value="/media/andreas/storage/data/bigbird_pcds/white_rain_sensations_apple_blossom_hydrating_body_wash_gt.pcd" /> -->
+    <param name="cloud_file_name" value="/media/andreas/storage/data/baxter_clutter/scene0.pcd" />
 
     <!-- If sequential importance sampling is used -->
     <param name="use_importance_sampling" value="true" />
 
     <!-- Parameters for sequential importance sampling -->
     <param name="num_init_samples" value="50" />
     <param name="num_iterations" value="10" />    
-    <param name="num_samples_per_iteration" value="40" />
+    <param name="num_samples_per_iteration" value="20" />
     <param name="prob_rand_samples" value="0.2" />
     <param name="std" value="0.01" />
     <param name="sampling_method" value="1" /> <!-- 0: Sum of Gaussians, 1: Max of Gaussians -->
-    <param name="visualize_steps" value="true" />
-    <param name="visualize_results" value="true" />
-
-    <!-- Plotting parameters -->
-    <param name="plot_normals" value="false" />
-    <param name="plot_samples" value="false" />    
-    <param name="plot_candidates" value="false" />
-    <param name="plot_filtered_grasps" value="false" />
-    <param name="plot_valid_grasps" value="false" />
-    <param name="plot_clusters" value="false" />
-    <param name="plot_selected_grasps" value="false" />
 
+    <param name="visualize_rounds" value="false" />
+    <param name="visualize_steps" value="false" />
+    <param name="visualize_results" value="true" />
+
     <!-- Preprocessing of point cloud -->
     <param name="voxelize" value="true"/> <!-- Voxelize the point cloud -->
     <param name="remove_outliers" value="false"/> <!-- Remove statistical outliers from the point cloud -->
-    <rosparam param="workspace"> [-1, 1, -1, 1, -1, 1] </rosparam> <!-- (x,y,z)-dimensions of the workspace -->
+<!-- <rosparam param="workspace"> [-1, 1, -1, 1, -1, 1] </rosparam> --> <!-- (x,y,z)-dimensions of the workspace --> 
+    <rosparam param="workspace"> [0.5, 0.9, -0.4, 0.25, -0.28, 1] </rosparam> <!-- (x,y,z)-dimensions of the workspace -->
     <rosparam param="camera_position"> [0, 0, 0] </rosparam> <!-- The position of the camera -->
 
     <!-- General parameters -->
-    <param name="num_samples" value="100" /> <!-- The number of samples to draw from the point cloud -->
     <param name="num_threads" value="4" /> <!-- The number of CPU threads to be used -->
-    <param name="gaussian_table_size" value="10000000" />    
 
     <!-- Parameters for local grasp candidate search -->
     <param name="nn_radius" value="0.01" /> <!-- Radius for the point neighborhood search -->
@@ -65,17 +50,17 @@
     <param name="filter_grasps" value="false" /> <!-- on workspace and robot gripper width -->
     <param name="min_aperture" value="0.3" /> <!-- The minimum gripper width -->
     <param name="max_aperture" value="0.7" /> <!-- The maximum gripper width -->
-    <rosparam param="workspace_grasps"> [0.6, 1.0, -0.33, 0.13, -0.3, 1.0] </rosparam> <!-- (x,y,z)-dimensions of the workspace, should be smaller than <workspace> -->
+    <rosparam param="workspace_grasps"> [0.57, 0.87, -0.38, 0.23, -0.26, 1] </rosparam> <!-- (x,y,z)-dimensions of the workspace, should be smaller than <workspace> -->
     <param name="filter_half_antipodal" value="true"/> <!-- on half antipodal -->
 
     <!-- Grasp image creation -->
     <param name="create_image_batches" value="false" /> <!-- creates grasp images in batches (less memory usage) -->
 
     <!-- Clustering of grasps -->
-    <param name="min_inliers" value="1" /> <!-- min inliers in each cluster; set to 0 to avoid clustering -->
+    <param name="min_inliers" value="0" /> <!-- min inliers in each cluster; set to 0 to avoid clustering -->
 
     <!-- Grasp selection -->
-    <param name="min_score_diff" value="0" /> <!-- Threshold to sort out grasps with low scores -->
+    <param name="min_score_diff" value="50" /> <!-- Threshold to sort out grasps with low scores -->
     <param name="num_selected" value="100" /> <!-- The number of selected grasps (sorted by score) -->
 
 	</node>

launch/tutorial0.launch

@@ -12,8 +12,7 @@
 
 	<node name="classify_grasp_candidates" pkg="gpd" type="classify_candidates" output="screen">
 
-    <!-- Where the point cloud is stored -->    
-
+    <!-- Where the point cloud is stored -->
     <param name="cloud_file_name" value="$(find gpd)/tutorials/krylon.pcd" />
 
     <!-- Plotting parameters -->

src/gpd/clustering.cpp

@@ -88,6 +88,7 @@ std::vector<Grasp> Clustering::findClusters(const std::vector<Grasp>& hand_list,
       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);
     }
   }

src/gpd/grasp_detector.cpp

@@ -347,9 +347,9 @@ std::vector<GraspSet> GraspDetector::filterGraspsWorkspace(const std::vector<Gra
         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
+          && 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++;

src/gpd/sequential_importance_sampling.cpp

@@ -24,18 +24,29 @@ SequentialImportanceSampling::SequentialImportanceSampling(ros::NodeHandle& node
   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_grasps", workspace_);
+  node.getParam("workspace", workspace_);
+  node.getParam("workspace_grasps", workspace_grasps_);
 
   grasp_detector_ = new GraspDetector(node);
 }
 
 
 std::vector<Grasp> 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<Grasp> grasps(0);
+    return grasps;
+  }
+
   double t0 = omp_get_wtime();
   CloudCamera cloud_cam = cloud_cam_in;
   Plot plotter;
@@ -55,11 +66,11 @@ std::vector<Grasp> SequentialImportanceSampling::detectGrasps(const CloudCamera&
   std::vector<GraspSet> filtered_candidates;
   if (filter_grasps_)
   {
-    grasp_detector_->filterGraspsWorkspace(hand_set_list, workspace_);
-    ROS_INFO_STREAM("Number of grasps within gripper width range and workspace: " << filtered_candidates.size());
+    grasp_detector_->filterGraspsWorkspace(hand_set_list, workspace_grasps_);
+    ROS_INFO_STREAM("Number of grasps within workspace: " << filtered_candidates.size());
   }
 
-  if (visualize_steps_)
+  if (visualize_rounds_)
   {
     plotter.plotFingers(hand_set_list, cloud_cam.getCloudOriginal(), "Initial Grasps");
   }
@@ -133,13 +144,13 @@ std::vector<Grasp> SequentialImportanceSampling::detectGrasps(const CloudCamera&
 
     if (filter_grasps_)
     {
-      grasp_detector_->filterGraspsWorkspace(hand_set_list_new, workspace_);
+      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.insert(hand_set_list.end(), hand_set_list_new.begin(), hand_set_list_new.end());
 
-    if (visualize_steps_)
+    if (visualize_rounds_)
     {
       plotter.plotSamples(samples, cloud);
       plotter.plotFingers(hand_set_list_new, cloud_cam.getCloudProcessed(), "New Grasps");
@@ -148,6 +159,10 @@ std::vector<Grasp> SequentialImportanceSampling::detectGrasps(const CloudCamera&
     std::cout << "Added: " << hand_set_list_new.size() << ", total: " << hand_set_list.size()
       << " grasp candidate sets in round " << i << std::endl;
   }
+  if (visualize_steps_)
+  {
+    plotter.plotFingers(hand_set_list, cloud_cam.getCloudOriginal(), "Grasp Candidates");
+  }
 
   // Classify the grasps.
   std::vector<Grasp> valid_grasps;
@@ -160,8 +175,10 @@ std::vector<Grasp> SequentialImportanceSampling::detectGrasps(const CloudCamera&
 
   // 4. Cluster the grasps.
   valid_grasps = grasp_detector_->findClusters(valid_grasps);
-  std::cout << "Final result: found " << valid_grasps.size() << " clusters in " << omp_get_wtime() - t0 << " sec.\n";
-  if (visualize_results_)
+  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");
   }
@@ -181,6 +198,17 @@ std::vector<Grasp> SequentialImportanceSampling::detectGrasps(const CloudCamera&
 }
 
 
+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<GraspSet>& hands,
   Gaussian& generator, double sigma, int num_gauss_samples, Eigen::Matrix3Xd& samples_out)
 {

src/nodes/classify_candidates.cpp

@@ -70,11 +70,12 @@ int main(int argc, char* argv[])
 
   if (use_importance_sampling)
   {
-//    cloud_camera.filterWorkspace(workspace_);
-//    cloud_cam.voxelizeCloud(0.003);
-    cloud_cam.calculateNormals(4);
-
     SequentialImportanceSampling detector(node);
+
+    // Preprocess the point cloud (voxelize, workspace, etc.).
+    detector.preprocessPointCloud(cloud_cam);
+
+    // Detect grasps in the point cloud.
     std::vector<Grasp> grasps = detector.detectGrasps(cloud_cam);
   }
   else