From 5a23d97c06ec720f4d802e8084b59245e8bc350f Mon Sep 17 00:00:00 2001 From: Runqiu Bao Date: Mon, 21 Mar 2022 18:21:16 +0900 Subject: [PATCH 1/7] fix linemod2D clustering: get the highest score out of each clusters --- recognition/src/linemod.cpp | 19 +++++++++++++++++-- 1 file changed, 17 insertions(+), 2 deletions(-) diff --git a/recognition/src/linemod.cpp b/recognition/src/linemod.cpp index eb017d54b3c..9618e2e0642 100644 --- a/recognition/src/linemod.cpp +++ b/recognition/src/linemod.cpp @@ -236,6 +236,7 @@ pcl::LINEMOD::removeOverlappingDetections ( typedef std::tuple ClusteringKey; std::map> clusters; + std::map indexToBestScoreInCluster; for (size_t detection_id = 0; detection_id < nr_detections; ++detection_id) { const LINEMODDetection& d = detections[detection_id]; @@ -246,13 +247,17 @@ pcl::LINEMOD::removeOverlappingDetections ( }; clusters[key].push_back(detection_id); + if(detections[detection_id].score > detections[clusters[key][indexToBestScoreInCluster[key]]].score){ + indexToBestScoreInCluster[key] = clusters[key].size() - 1; + } } // compute detection representatives for every cluster std::vector clustered_detections; size_t cluster_id; std::map>::iterator it; - for (cluster_id = 0, it = clusters.begin(); it != clusters.end(); ++cluster_id, ++it) + std::map::iterator itIndexToBestScoreInCluster; + for (cluster_id = 0, it = clusters.begin(), itIndexToBestScoreInCluster = indexToBestScoreInCluster.begin(); it != clusters.end(); ++cluster_id, ++it, ++itIndexToBestScoreInCluster) { const std::vector& cluster = it->second; float weight_sum = 0.0f; @@ -292,7 +297,7 @@ pcl::LINEMOD::removeOverlappingDetections ( average_rz *= inv_weight_sum; float min_dist2 = std::numeric_limits::max (); - size_t best_template_id = detections[cluster[0]].template_id; + size_t best_template_id = detections[cluster[itIndexToBestScoreInCluster->second]].template_id; for (size_t template_index = 0; template_index < n_templates; ++template_index) { // Skip templates that does not belong to the same cluster @@ -1063,6 +1068,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 modality_energy_maps; #ifdef LINEMOD_USE_SEPARATE_ENERGY_MAPS From 7ebafec6438b32315e877e6d8fd6b103c0e1dc44 Mon Sep 17 00:00:00 2001 From: Runqiu Bao Date: Fri, 25 Mar 2022 17:32:09 +0900 Subject: [PATCH 2/7] change linemod2D clustering: return highest score in a cluster --- recognition/src/linemod.cpp | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/recognition/src/linemod.cpp b/recognition/src/linemod.cpp index 9618e2e0642..64eaac030e0 100644 --- a/recognition/src/linemod.cpp +++ b/recognition/src/linemod.cpp @@ -316,11 +316,14 @@ pcl::LINEMOD::removeOverlappingDetections ( } 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[itIndexToBestScoreInCluster->second]]; + // TODO:add also distant one in shortest extents' direction (assume always the same template in one cluster) + + // 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); clustered_detections.push_back (detection); } From 0042664578c992333bb76cac2ab3a439436548a4 Mon Sep 17 00:00:00 2001 From: Runqiu Bao Date: Wed, 30 Mar 2022 13:04:46 +0900 Subject: [PATCH 3/7] add critical direction in linemod2D clustering --- recognition/include/pcl/recognition/linemod.h | 4 ++- .../sparse_quantized_multi_mod_template.h | 3 ++ recognition/src/linemod.cpp | 30 +++++++++++++++++-- 3 files changed, 33 insertions(+), 4 deletions(-) diff --git a/recognition/include/pcl/recognition/linemod.h b/recognition/include/pcl/recognition/linemod.h index e9fc5b88c1d..9e0076c2606 100644 --- a/recognition/include/pcl/recognition/linemod.h +++ b/recognition/include/pcl/recognition/linemod.h @@ -372,7 +372,9 @@ namespace pcl void removeOverlappingDetections (std::vector & detections, size_t translation_clustering_threshold, - float rotation_clustering_threshold) const; + float rotation_clustering_threshold, + bool useCriticalDirectionIn2DClustering = false, + size_t translationClusteringThreshold2DInCriticalDirection = 0) const; void sortDetections (std::vector & detections) const; diff --git a/recognition/include/pcl/recognition/sparse_quantized_multi_mod_template.h b/recognition/include/pcl/recognition/sparse_quantized_multi_mod_template.h index 6faf03ad236..62fd4614219 100644 --- a/recognition/include/pcl/recognition/sparse_quantized_multi_mod_template.h +++ b/recognition/include/pcl/recognition/sparse_quantized_multi_mod_template.h @@ -117,6 +117,9 @@ namespace pcl /** \brief The rotations assigned to the template. */ float rx, ry, rz; + /** \brief for super long object, the direction perpendiculr to longest axis is critical (should lower clustering threshold)*/ + float criticalDirection[2] = {0.0, 0.0}; + /** \brief The region assigned to the template. */ RegionXY region; diff --git a/recognition/src/linemod.cpp b/recognition/src/linemod.cpp index 64eaac030e0..bee95a34a51 100644 --- a/recognition/src/linemod.cpp +++ b/recognition/src/linemod.cpp @@ -176,7 +176,9 @@ void pcl::LINEMOD::removeOverlappingDetections ( std::vector & detections, size_t translation_clustering_threshold, - float rotation_clustering_threshold) const + float rotation_clustering_threshold, + bool useCriticalDirectionIn2DClustering, + size_t translationClusteringThreshold2DInCriticalDirection) const { // check if clustering is disabled if (translation_clustering_threshold == 0 && rotation_clustering_threshold == 0.f) { @@ -189,6 +191,9 @@ pcl::LINEMOD::removeOverlappingDetections ( if (rotation_clustering_threshold == 0.f) { rotation_clustering_threshold = std::numeric_limits::epsilon(); } + if (useCriticalDirectionIn2DClustering && translationClusteringThreshold2DInCriticalDirection==0){ + translationClusteringThreshold2DInCriticalDirection = 1; + } typedef std::tuple TemplatesClusteringKey; std::map> templateClusters; @@ -233,17 +238,33 @@ pcl::LINEMOD::removeOverlappingDetections ( // compute overlap between each detection const size_t nr_detections = detections.size (); - - typedef std::tuple ClusteringKey; + + typedef std::tuple ClusteringKey; std::map> clusters; std::map indexToBestScoreInCluster; for (size_t detection_id = 0; detection_id < nr_detections; ++detection_id) { const LINEMODDetection& d = detections[detection_id]; + + // use criticalDirectionBasedClustering for super long object + int criticalDistanceInt; + if(useCriticalDirectionIn2DClustering){ + float criticalDirection[2] = {0.0, 0.0}; + criticalDirection[0] = templates_[clusteredTemplates[d.template_id]].criticalDirection[0]; + criticalDirection[1] = templates_[clusteredTemplates[d.template_id]].criticalDirection[1]; + float criticalDistance = static_cast< float >( d.x * criticalDirection[0] + d.y * criticalDirection[1] ); //std::sqrt(static_cast< float >( d.x * d.x + d.y * d.y )); + criticalDistanceInt = static_cast< int >(criticalDistance / translationClusteringThreshold2DInCriticalDirection); + PCL_INFO ("[linemod2D_CriticalDirectionBasedClustering]: template %d, criticalDistanceInt %d, translation_clustering_threshold %d\n", d.template_id, criticalDistanceInt, translation_clustering_threshold); + } + else{ + criticalDistanceInt = 0; + } + const ClusteringKey key = { d.x / translation_clustering_threshold, d.y / translation_clustering_threshold, clusteredTemplates[d.template_id], + criticalDistanceInt, }; clusters[key].push_back(detection_id); @@ -271,6 +292,7 @@ pcl::LINEMOD::removeOverlappingDetections ( float average_region_y = 0.0f; const size_t elements_in_cluster = cluster.size (); + float sum_score = 0.0f; for (size_t cluster_index = 0; cluster_index < elements_in_cluster; ++cluster_index) { const size_t detection_id = cluster[cluster_index]; @@ -278,6 +300,7 @@ pcl::LINEMOD::removeOverlappingDetections ( const pcl::SparseQuantizedMultiModTemplate& template_ = templates_[d.template_id]; const float weight = d.score * d.score; + sum_score += d.score; weight_sum += weight; @@ -289,6 +312,7 @@ pcl::LINEMOD::removeOverlappingDetections ( average_region_x += static_cast(d.x) * weight; average_region_y += static_cast(d.y) * weight; } + // PCL_INFO ("[bao]: cluster_index %d, numInCluster %d, averageScore %g, bestScore %g\n", cluster_id, elements_in_cluster, sum_score/elements_in_cluster, detections[cluster[itIndexToBestScoreInCluster->second]].score); const float inv_weight_sum = 1.0f / weight_sum; From 4a86a069ee6ca4866966a178118b3821a3d6ca8c Mon Sep 17 00:00:00 2001 From: Runqiu Bao Date: Thu, 31 Mar 2022 17:04:04 +0900 Subject: [PATCH 4/7] minor clean-up for previous commit --- recognition/src/linemod.cpp | 14 ++------------ 1 file changed, 2 insertions(+), 12 deletions(-) diff --git a/recognition/src/linemod.cpp b/recognition/src/linemod.cpp index bee95a34a51..dbe61ff3a99 100644 --- a/recognition/src/linemod.cpp +++ b/recognition/src/linemod.cpp @@ -254,10 +254,10 @@ pcl::LINEMOD::removeOverlappingDetections ( criticalDirection[1] = templates_[clusteredTemplates[d.template_id]].criticalDirection[1]; float criticalDistance = static_cast< float >( d.x * criticalDirection[0] + d.y * criticalDirection[1] ); //std::sqrt(static_cast< float >( d.x * d.x + d.y * d.y )); criticalDistanceInt = static_cast< int >(criticalDistance / translationClusteringThreshold2DInCriticalDirection); - PCL_INFO ("[linemod2D_CriticalDirectionBasedClustering]: template %d, criticalDistanceInt %d, translation_clustering_threshold %d\n", d.template_id, criticalDistanceInt, translation_clustering_threshold); + // PCL_INFO ("[linemod2D_CriticalDirectionBasedClustering]: template %d, criticalDistanceInt %d, translation_clustering_threshold %d\n", d.template_id, criticalDistanceInt, translation_clustering_threshold); } else{ - criticalDistanceInt = 0; + criticalDistanceInt = 0; //not using critical-direction } const ClusteringKey key = { @@ -292,7 +292,6 @@ pcl::LINEMOD::removeOverlappingDetections ( float average_region_y = 0.0f; const size_t elements_in_cluster = cluster.size (); - float sum_score = 0.0f; for (size_t cluster_index = 0; cluster_index < elements_in_cluster; ++cluster_index) { const size_t detection_id = cluster[cluster_index]; @@ -300,7 +299,6 @@ pcl::LINEMOD::removeOverlappingDetections ( const pcl::SparseQuantizedMultiModTemplate& template_ = templates_[d.template_id]; const float weight = d.score * d.score; - sum_score += d.score; weight_sum += weight; @@ -312,7 +310,6 @@ pcl::LINEMOD::removeOverlappingDetections ( average_region_x += static_cast(d.x) * weight; average_region_y += static_cast(d.y) * weight; } - // PCL_INFO ("[bao]: cluster_index %d, numInCluster %d, averageScore %g, bestScore %g\n", cluster_id, elements_in_cluster, sum_score/elements_in_cluster, detections[cluster[itIndexToBestScoreInCluster->second]].score); const float inv_weight_sum = 1.0f / weight_sum; @@ -341,13 +338,6 @@ pcl::LINEMOD::removeOverlappingDetections ( LINEMODDetection detection; detection = detections[cluster[itIndexToBestScoreInCluster->second]]; - // TODO:add also distant one in shortest extents' direction (assume always the same template in one cluster) - - // 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); clustered_detections.push_back (detection); } From eb8269ad8f7798bf4274ee64cb3fe5c3e8ba7854 Mon Sep 17 00:00:00 2001 From: Runqiu Bao Date: Fri, 8 Apr 2022 18:51:57 +0900 Subject: [PATCH 5/7] linemod2D clustering: add docs ; delete rotational clustering which is not used --- recognition/include/pcl/recognition/linemod.h | 11 +- .../sparse_quantized_multi_mod_template.h | 6 +- recognition/src/linemod.cpp | 136 +++--------------- 3 files changed, 33 insertions(+), 120 deletions(-) diff --git a/recognition/include/pcl/recognition/linemod.h b/recognition/include/pcl/recognition/linemod.h index 9e0076c2606..4b04a7146bb 100644 --- a/recognition/include/pcl/recognition/linemod.h +++ b/recognition/include/pcl/recognition/linemod.h @@ -369,12 +369,19 @@ namespace pcl std::vector>& 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] use_critical_direction_in_2D_clustering enable finer clustering in critical direction. For super long object, the direction perpendicular to longest axis is critical. + * \param[in] translation_clustering_threshold_2D_in_critical_direction a (usually much smaller than translation_clustering_threshold) threshold of translation in critical direction. + */ void removeOverlappingDetections (std::vector & detections, size_t translation_clustering_threshold, float rotation_clustering_threshold, - bool useCriticalDirectionIn2DClustering = false, - size_t translationClusteringThreshold2DInCriticalDirection = 0) const; + bool use_critical_direction_in_2D_clustering = false, + size_t translation_clustering_threshold_2D_in_critical_direction = 1) const; void sortDetections (std::vector & detections) const; diff --git a/recognition/include/pcl/recognition/sparse_quantized_multi_mod_template.h b/recognition/include/pcl/recognition/sparse_quantized_multi_mod_template.h index 62fd4614219..8fb9dd15566 100644 --- a/recognition/include/pcl/recognition/sparse_quantized_multi_mod_template.h +++ b/recognition/include/pcl/recognition/sparse_quantized_multi_mod_template.h @@ -109,7 +109,7 @@ 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), criticalDirection{0.f, 0.f}, region () {} /** \brief The storage for the multi-modality features. */ std::vector features; @@ -117,8 +117,8 @@ namespace pcl /** \brief The rotations assigned to the template. */ float rx, ry, rz; - /** \brief for super long object, the direction perpendiculr to longest axis is critical (should lower clustering threshold)*/ - float criticalDirection[2] = {0.0, 0.0}; + /** \brief for super long object, the direction perpendicular to longest axis is critical (should lower clustering threshold)*/ + float criticalDirection[2]; /** \brief The region assigned to the template. */ RegionXY region; diff --git a/recognition/src/linemod.cpp b/recognition/src/linemod.cpp index dbe61ff3a99..7135d36d4af 100644 --- a/recognition/src/linemod.cpp +++ b/recognition/src/linemod.cpp @@ -177,99 +177,59 @@ pcl::LINEMOD::removeOverlappingDetections ( std::vector & detections, size_t translation_clustering_threshold, float rotation_clustering_threshold, - bool useCriticalDirectionIn2DClustering, - size_t translationClusteringThreshold2DInCriticalDirection) const + bool use_critical_direction_in_2D_clustering, + size_t translation_clustering_threshold_2D_in_critical_direction) const { // check if clustering is disabled if (translation_clustering_threshold == 0 && rotation_clustering_threshold == 0.f) { return; } - if (translation_clustering_threshold == 0) { translation_clustering_threshold = 1; } if (rotation_clustering_threshold == 0.f) { rotation_clustering_threshold = std::numeric_limits::epsilon(); } - if (useCriticalDirectionIn2DClustering && translationClusteringThreshold2DInCriticalDirection==0){ - translationClusteringThreshold2DInCriticalDirection = 1; - } - typedef std::tuple TemplatesClusteringKey; - std::map> 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(template_.rx / rotation_clustering_threshold), - static_cast(template_.ry / rotation_clustering_threshold), - static_cast(template_.rz / rotation_clustering_threshold), - }; - - templateClusters[key].push_back(template_index); - } - - std::vector clusteredTemplates(n_templates, std::numeric_limits::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>::iterator tempIt; - for (cluster_id = 0, tempIt = templateClusters.begin(); tempIt != templateClusters.end(); ++cluster_id, ++tempIt) - { - const std::vector& 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 ClusteringKey; std::map> clusters; - std::map indexToBestScoreInCluster; + std::map 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 criticalDirectionBasedClustering for super long object - int criticalDistanceInt; - if(useCriticalDirectionIn2DClustering){ + // use critical Direction Based Clustering for super long object + int critical_distance_int; + if(use_critical_direction_in_2D_clustering){ float criticalDirection[2] = {0.0, 0.0}; - criticalDirection[0] = templates_[clusteredTemplates[d.template_id]].criticalDirection[0]; - criticalDirection[1] = templates_[clusteredTemplates[d.template_id]].criticalDirection[1]; + criticalDirection[0] = templates_[d.template_id].criticalDirection[0]; + criticalDirection[1] = templates_[d.template_id].criticalDirection[1]; float criticalDistance = static_cast< float >( d.x * criticalDirection[0] + d.y * criticalDirection[1] ); //std::sqrt(static_cast< float >( d.x * d.x + d.y * d.y )); - criticalDistanceInt = static_cast< int >(criticalDistance / translationClusteringThreshold2DInCriticalDirection); - // PCL_INFO ("[linemod2D_CriticalDirectionBasedClustering]: template %d, criticalDistanceInt %d, translation_clustering_threshold %d\n", d.template_id, criticalDistanceInt, translation_clustering_threshold); + critical_distance_int = static_cast< int >(criticalDistance / translation_clustering_threshold_2D_in_critical_direction); + // PCL_INFO ("[linemod2D_CriticalDirectionBasedClustering]: template %d, critical_distance_int %d, translation_clustering_threshold %d\n", d.template_id, critical_distance_int, translation_clustering_threshold); } else{ - criticalDistanceInt = 0; //not using critical-direction + critical_distance_int = 0; //not using critical-direction } const ClusteringKey key = { d.x / translation_clustering_threshold, d.y / translation_clustering_threshold, - clusteredTemplates[d.template_id], - criticalDistanceInt, + d.template_id, + critical_distance_int, }; clusters[key].push_back(detection_id); - if(detections[detection_id].score > detections[clusters[key][indexToBestScoreInCluster[key]]].score){ - indexToBestScoreInCluster[key] = clusters[key].size() - 1; + 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; } } @@ -277,67 +237,13 @@ pcl::LINEMOD::removeOverlappingDetections ( std::vector clustered_detections; size_t cluster_id; std::map>::iterator it; - std::map::iterator itIndexToBestScoreInCluster; - for (cluster_id = 0, it = clusters.begin(), itIndexToBestScoreInCluster = indexToBestScoreInCluster.begin(); it != clusters.end(); ++cluster_id, ++it, ++itIndexToBestScoreInCluster) + std::map::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& 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(d.x) * weight; - average_region_y += static_cast(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::max (); - size_t best_template_id = detections[cluster[itIndexToBestScoreInCluster->second]].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 = detections[cluster[itIndexToBestScoreInCluster->second]]; + detection = detections[cluster[itindex_to_best_score_in_cluster->second]]; clustered_detections.push_back (detection); } From 6074b88c5f017fbe7e908208401507c2f8bfab24 Mon Sep 17 00:00:00 2001 From: Runqiu Bao Date: Mon, 11 Apr 2022 14:16:52 +0900 Subject: [PATCH 6/7] linemod2D clustering: clean-up and error message --- recognition/src/linemod.cpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/recognition/src/linemod.cpp b/recognition/src/linemod.cpp index 7135d36d4af..4083b95f1de 100644 --- a/recognition/src/linemod.cpp +++ b/recognition/src/linemod.cpp @@ -182,19 +182,20 @@ pcl::LINEMOD::removeOverlappingDetections ( { // 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::epsilon(); - } + // if (rotation_clustering_threshold == 0.f) { + // rotation_clustering_threshold = std::numeric_limits::epsilon(); + // } const size_t n_templates = templates_.size (); - 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); + // 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 (); From 6396f03573cee65902fb54cdef43028fb23a77fa Mon Sep 17 00:00:00 2001 From: Runqiu Bao Date: Mon, 11 Jul 2022 17:35:57 +0900 Subject: [PATCH 7/7] make longAndNarrow as a property of template; rename criticalDirection to narrowDirection --- recognition/include/pcl/recognition/linemod.h | 6 ++--- .../sparse_quantized_multi_mod_template.h | 6 ++--- .../pcl/recognition/surface_normal_modality.h | 3 ++- recognition/src/linemod.cpp | 26 +++++++------------ 4 files changed, 17 insertions(+), 24 deletions(-) diff --git a/recognition/include/pcl/recognition/linemod.h b/recognition/include/pcl/recognition/linemod.h index 4b04a7146bb..065208560c4 100644 --- a/recognition/include/pcl/recognition/linemod.h +++ b/recognition/include/pcl/recognition/linemod.h @@ -373,15 +373,13 @@ namespace pcl * \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] use_critical_direction_in_2D_clustering enable finer clustering in critical direction. For super long object, the direction perpendicular to longest axis is critical. - * \param[in] translation_clustering_threshold_2D_in_critical_direction a (usually much smaller than translation_clustering_threshold) threshold of translation in critical direction. + * \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 & detections, size_t translation_clustering_threshold, float rotation_clustering_threshold, - bool use_critical_direction_in_2D_clustering = false, - size_t translation_clustering_threshold_2D_in_critical_direction = 1) const; + size_t translation_clustering_threshold_2D_in_narrow_direction_of_long_and_narrow_template = 1) const; void sortDetections (std::vector & detections) const; diff --git a/recognition/include/pcl/recognition/sparse_quantized_multi_mod_template.h b/recognition/include/pcl/recognition/sparse_quantized_multi_mod_template.h index 8fb9dd15566..398dc2ecfba 100644 --- a/recognition/include/pcl/recognition/sparse_quantized_multi_mod_template.h +++ b/recognition/include/pcl/recognition/sparse_quantized_multi_mod_template.h @@ -109,7 +109,7 @@ namespace pcl struct SparseQuantizedMultiModTemplate { /** \brief Constructor. */ - SparseQuantizedMultiModTemplate () : features (), rx (0.f), ry (0.f), rz (0.f), criticalDirection{0.f, 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 features; @@ -117,8 +117,8 @@ namespace pcl /** \brief The rotations assigned to the template. */ float rx, ry, rz; - /** \brief for super long object, the direction perpendicular to longest axis is critical (should lower clustering threshold)*/ - float criticalDirection[2]; + /** \brief for super long and narrow template*/ + float narrowDirectionOfLongAndNarrowTemplate[2]; /** \brief The region assigned to the template. */ RegionXY region; diff --git a/recognition/include/pcl/recognition/surface_normal_modality.h b/recognition/include/pcl/recognition/surface_normal_modality.h index 675d14fa322..b395ec06832 100644 --- a/recognition/include/pcl/recognition/surface_normal_modality.h +++ b/recognition/include/pcl/recognition/surface_normal_modality.h @@ -1416,10 +1416,11 @@ pcl::SurfaceNormalModality::quantizeSurfaceNormals () int bin_index = static_cast (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 (bin_index); } diff --git a/recognition/src/linemod.cpp b/recognition/src/linemod.cpp index 4083b95f1de..ad2f13935eb 100644 --- a/recognition/src/linemod.cpp +++ b/recognition/src/linemod.cpp @@ -177,8 +177,7 @@ pcl::LINEMOD::removeOverlappingDetections ( std::vector & detections, size_t translation_clustering_threshold, float rotation_clustering_threshold, - bool use_critical_direction_in_2D_clustering, - size_t translation_clustering_threshold_2D_in_critical_direction) const + 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) { @@ -207,25 +206,20 @@ pcl::LINEMOD::removeOverlappingDetections ( { const LINEMODDetection& d = detections[detection_id]; - // use critical Direction Based Clustering for super long object - int critical_distance_int; - if(use_critical_direction_in_2D_clustering){ - float criticalDirection[2] = {0.0, 0.0}; - criticalDirection[0] = templates_[d.template_id].criticalDirection[0]; - criticalDirection[1] = templates_[d.template_id].criticalDirection[1]; - float criticalDistance = static_cast< float >( d.x * criticalDirection[0] + d.y * criticalDirection[1] ); //std::sqrt(static_cast< float >( d.x * d.x + d.y * d.y )); - critical_distance_int = static_cast< int >(criticalDistance / translation_clustering_threshold_2D_in_critical_direction); - // PCL_INFO ("[linemod2D_CriticalDirectionBasedClustering]: template %d, critical_distance_int %d, translation_clustering_threshold %d\n", d.template_id, critical_distance_int, translation_clustering_threshold); - } - else{ - critical_distance_int = 0; //not using critical-direction - } + // 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, d.template_id, - critical_distance_int, + narrow_direction_distance_int, }; clusters[key].push_back(detection_id);