Improve linemod2d clustering

recognition/include/pcl/recognition/linemod.h

@@ -369,10 +369,17 @@ namespace pcl
                                          std::vector<std::vector<LINEMODDetection>>& grouped_detections,
                                          const size_t grouping_threshold) const;
 
+      /** \brief Remove duplicated candidates (with same templateID) in a local neighborhood, in non-maximum-suppression style.
+        * \param[in, out] detections list of candidate detections.
+        * \param[in] translation_clustering_threshold threshold in terms of translation to select neighborhood.
+        * \param[in] rotation_clustering_threshold (not used!) threshold in terms of rotation (meaning 3d rotation, namely different template) to select neighborbood.
+        * \param[in] translation_clustering_threshold_2D_in_narrow_direction_of_long_and_narrow_template a (usually much smaller than translation_clustering_threshold) threshold of translation in narrow direction of template. 
+        */
       void
       removeOverlappingDetections (std::vector<LINEMODDetection> & detections,
                                    size_t translation_clustering_threshold,
-                                   float rotation_clustering_threshold) const;
+                                   float rotation_clustering_threshold,
+                                   size_t translation_clustering_threshold_2D_in_narrow_direction_of_long_and_narrow_template = 1) const;
 
       void
       sortDetections (std::vector<LINEMODDetection> & detections) const;

recognition/include/pcl/recognition/sparse_quantized_multi_mod_template.h

@@ -109,14 +109,17 @@ namespace pcl
   struct SparseQuantizedMultiModTemplate
   {
     /** \brief Constructor. */
-    SparseQuantizedMultiModTemplate () : features (), rx (0.f), ry (0.f), rz (0.f), region () {}
+    SparseQuantizedMultiModTemplate () : features (), rx (0.f), ry (0.f), rz (0.f), narrowDirectionOfLongAndNarrowTemplate{0.f, 0.f}, region () {}
 
     /** \brief The storage for the multi-modality features. */
     std::vector<QuantizedMultiModFeature> features;
 
     /** \brief The rotations assigned to the template. */
     float rx, ry, rz;
 
+    /** \brief for super long and narrow template*/
+    float narrowDirectionOfLongAndNarrowTemplate[2];
+
     /** \brief The region assigned to the template. */
     RegionXY region;
 

recognition/include/pcl/recognition/surface_normal_modality.h

@@ -1416,10 +1416,11 @@ pcl::SurfaceNormalModality<PointInT>::quantizeSurfaceNormals ()
 
       int bin_index = static_cast<int> (angle*8.0f/360.0f+1);
       // when angle is 359.999f, bin_index can overflow to 9
+
       if (bin_index >= 9){
         bin_index = 8;
       }
-
+      
       //quantized_surface_normals_.data[row_index*width+col_index] = 0x1 << bin_index;
       quantized_surface_normals_ (col_index, row_index) = static_cast<unsigned char> (bin_index);
     }

recognition/src/linemod.cpp

@@ -176,146 +176,69 @@ void
 pcl::LINEMOD::removeOverlappingDetections (
     std::vector<LINEMODDetection> & detections,
     size_t translation_clustering_threshold,
-    float rotation_clustering_threshold) const
+    float rotation_clustering_threshold,
+    size_t translation_clustering_threshold_2D_in_narrow_direction_of_long_and_narrow_template) const
 {
   // check if clustering is disabled
   if (translation_clustering_threshold == 0 && rotation_clustering_threshold == 0.f) {
+    PCL_ERROR ("clustering is disabled, as translation_clustering_threshold = %d and rotation_clustering_threshold = %g\n", translation_clustering_threshold, rotation_clustering_threshold);
     return;
   }
-
   if (translation_clustering_threshold == 0) {
     translation_clustering_threshold = 1;
   }
-  if (rotation_clustering_threshold == 0.f) {
-    rotation_clustering_threshold = std::numeric_limits<float>::epsilon();
-  }
+  // if (rotation_clustering_threshold == 0.f) {
+  //   rotation_clustering_threshold = std::numeric_limits<float>::epsilon();
+  // }
 
-  typedef std::tuple<int, int, int> TemplatesClusteringKey;
-  std::map<TemplatesClusteringKey, std::vector<size_t>> templateClusters;
   const size_t n_templates = templates_.size ();
-  for (size_t template_index = 0; template_index < n_templates; ++template_index)
-  {
-    const pcl::SparseQuantizedMultiModTemplate& template_ = templates_[template_index];
-    const TemplatesClusteringKey key = {
-      static_cast<int>(template_.rx / rotation_clustering_threshold),
-      static_cast<int>(template_.ry / rotation_clustering_threshold),
-      static_cast<int>(template_.rz / rotation_clustering_threshold),
-    };
-
-    templateClusters[key].push_back(template_index);
-  }
-
-  std::vector<size_t> clusteredTemplates(n_templates, std::numeric_limits<size_t>::max()); // index is template_index
-  if (templateClusters.size() <= 1) {
-    PCL_ERROR ("[removeOverlappingDetections] All %u templates got grouped into %u cluster(s). Either rotation threshold=%.4f is too large, or template rotations are not set properly.\n"
-               "Making each template in its own cluster for now...\n", n_templates, templateClusters.size(), rotation_clustering_threshold);
-    for (size_t template_index = 0; template_index < n_templates; ++template_index)
-    {
-      clusteredTemplates[template_index] = template_index;
-    }
-  }
-  else
-  {
-    PCL_INFO ("[removeOverlappingDetections] Grouping %u templates into %u clusters\n", n_templates, templateClusters.size());
-    size_t cluster_id;
-    std::map<TemplatesClusteringKey, std::vector<size_t>>::iterator tempIt;
-    for (cluster_id = 0, tempIt = templateClusters.begin(); tempIt != templateClusters.end(); ++cluster_id, ++tempIt)
-    {
-      const std::vector<size_t>& cluster = tempIt->second;
-      const size_t elements_in_cluster = cluster.size ();
-      for (size_t cluster_index = 0; cluster_index < elements_in_cluster; ++cluster_index)
-      {
-        const size_t template_index = cluster[cluster_index];
-        clusteredTemplates[template_index] = cluster_id;
-      }
-    }
-  }
+
+  // PCL_INFO ("[removeOverlappingDetections] All %u templates got grouped into %u cluster(s). Either rotation threshold=%.4f is too large, or template rotations are not set properly.\n"
+  //           "Making each template in its own cluster\n", n_templates, 1, rotation_clustering_threshold);
 
   // compute overlap between each detection
   const size_t nr_detections = detections.size ();
-
-  typedef std::tuple<size_t, size_t, size_t> ClusteringKey;
+  
+  typedef std::tuple<size_t, size_t, size_t, int> ClusteringKey;
   std::map<ClusteringKey, std::vector<size_t>> clusters;
+  std::map<ClusteringKey, int> index_to_best_score_in_cluster;
   for (size_t detection_id = 0; detection_id < nr_detections; ++detection_id)
   {
     const LINEMODDetection& d = detections[detection_id];
+
+    // use distance-in-narrow-Direction based Clustering for super long and narrow templates
+    int narrow_direction_distance_int;
+    float narrowDirection[2] = {0.0, 0.0};
+    narrowDirection[0] = templates_[d.template_id].narrowDirectionOfLongAndNarrowTemplate[0];
+    narrowDirection[1] = templates_[d.template_id].narrowDirectionOfLongAndNarrowTemplate[1];
+    float narrowDirectionDistance = static_cast< float >( d.x * narrowDirection[0] + d.y * narrowDirection[1] ); //std::sqrt(static_cast< float >( d.x * d.x + d.y * d.y ));
+    narrow_direction_distance_int = static_cast< int >(narrowDirectionDistance / translation_clustering_threshold_2D_in_narrow_direction_of_long_and_narrow_template);
+    // PCL_INFO ("[linemod2D_NarrowDirectionBasedClustering]: template %d, narrow_direction_distance_int %d, d.x / translation_clustering_threshold %d, d.y / translation_clustering_threshold %d\n", d.template_id, narrow_direction_distance_int, d.x / translation_clustering_threshold, d.y / translation_clustering_threshold);
+
     const ClusteringKey key = {
       d.x / translation_clustering_threshold,
       d.y / translation_clustering_threshold,
-      clusteredTemplates[d.template_id],
+      d.template_id,
+      narrow_direction_distance_int,
     };
 
     clusters[key].push_back(detection_id);
+    if(detections[detection_id].score > detections[clusters[key][index_to_best_score_in_cluster[key]]].score){
+      index_to_best_score_in_cluster[key] = clusters[key].size() - 1;
+    }
   }
 
   // compute detection representatives for every cluster
   std::vector<LINEMODDetection> clustered_detections;
   size_t cluster_id;
   std::map<ClusteringKey, std::vector<size_t>>::iterator it;
-  for (cluster_id = 0, it = clusters.begin(); it != clusters.end(); ++cluster_id, ++it)
+  std::map<ClusteringKey, int>::iterator itindex_to_best_score_in_cluster;
+  for (cluster_id = 0, it = clusters.begin(), itindex_to_best_score_in_cluster = index_to_best_score_in_cluster.begin(); it != clusters.end(); ++cluster_id, ++it, ++itindex_to_best_score_in_cluster)
   {
     const std::vector<size_t>& cluster = it->second;
-    float weight_sum = 0.0f;
-
-    float average_score = 0.0f;
-    float average_scale = 0.0f;
-    float average_rx = 0.0f;
-    float average_ry = 0.0f;
-    float average_rz = 0.0f;
-    float average_region_x = 0.0f;
-    float average_region_y = 0.0f;
-
-    const size_t elements_in_cluster = cluster.size ();
-    for (size_t cluster_index = 0; cluster_index < elements_in_cluster; ++cluster_index)
-    {
-      const size_t detection_id = cluster[cluster_index];
-      const LINEMODDetection& d = detections[detection_id];
-      const pcl::SparseQuantizedMultiModTemplate& template_ = templates_[d.template_id];
-
-      const float weight = d.score * d.score;
-
-      weight_sum += weight;
-
-      average_score += d.score * weight;
-      average_scale += d.scale * weight;
-      average_rx += template_.rx * weight;
-      average_ry += template_.ry * weight;
-      average_rz += template_.rz * weight;
-      average_region_x += static_cast<float>(d.x) * weight;
-      average_region_y += static_cast<float>(d.y) * weight;
-    }
-
-    const float inv_weight_sum = 1.0f / weight_sum;
-
-    average_rx *= inv_weight_sum;
-    average_ry *= inv_weight_sum;
-    average_rz *= inv_weight_sum;
-
-    float min_dist2 = std::numeric_limits<float>::max ();
-    size_t best_template_id = detections[cluster[0]].template_id;
-    for (size_t template_index = 0; template_index < n_templates; ++template_index)
-    {
-      // Skip templates that does not belong to the same cluster
-      // This is also important to protect wrong ID assignment in case all rotations are not set, thus ended up to have the same distance
-      if (clusteredTemplates[best_template_id] != clusteredTemplates[template_index]) {
-        continue;
-      }
-
-      const pcl::SparseQuantizedMultiModTemplate& template_ = templates_[template_index];
-      const float dist2 = std::pow(template_.rx - average_rx, 2) + std::pow(template_.ry - average_ry, 2) + std::pow(template_.rz - average_rz, 2);
-      if (dist2 < min_dist2)
-      {
-        min_dist2 = dist2;
-        best_template_id = template_index;
-      }
-    }
 
     LINEMODDetection detection;
-    detection.template_id = best_template_id;
-    detection.score = average_score * inv_weight_sum * std::exp(-0.5f / elements_in_cluster);
-    detection.scale = average_scale * inv_weight_sum;
-    detection.x = int (average_region_x * inv_weight_sum);
-    detection.y = int (average_region_y * inv_weight_sum);
+    detection = detections[cluster[itindex_to_best_score_in_cluster->second]];
 
     clustered_detections.push_back (detection);
   }
@@ -1063,6 +986,16 @@ pcl::LINEMOD::detectTemplatesSemiScaleInvariant (
     const float max_scale,
     const float scale_multiplier) const
 {
+#ifdef LINEMOD_USE_SEPARATE_ENERGY_MAPS
+  PCL_INFO ("linemod_detectTemplatesSemiScaleInvariant: LINEMOD_USE_SEPARATE_ENERGY_MAPS defined.\n");
+#endif
+#if defined(__AVX2__)
+  PCL_INFO ("linemod_detectTemplatesSemiScaleInvariant: __AVX2__ defined.\n");
+#endif
+#if defined(__SSE2__)
+  PCL_INFO ("linemod_detectTemplatesSemiScaleInvariant: __SSE2__ defined.\n");
+#endif
+
   // create energy maps
   std::vector<EnergyMaps> modality_energy_maps;
 #ifdef LINEMOD_USE_SEPARATE_ENERGY_MAPS