diff --git a/recognition/include/pcl/recognition/linemod.h b/recognition/include/pcl/recognition/linemod.h index e9fc5b88c1d..065208560c4 100644 --- a/recognition/include/pcl/recognition/linemod.h +++ b/recognition/include/pcl/recognition/linemod.h @@ -369,10 +369,17 @@ 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] 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) 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 & 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..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), 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,6 +117,9 @@ namespace pcl /** \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; 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 eb017d54b3c..ad2f13935eb 100644 --- a/recognition/src/linemod.cpp +++ b/recognition/src/linemod.cpp @@ -176,146 +176,69 @@ void pcl::LINEMOD::removeOverlappingDetections ( std::vector & 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::epsilon(); - } + // if (rotation_clustering_threshold == 0.f) { + // rotation_clustering_threshold = std::numeric_limits::epsilon(); + // } - 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; + + typedef std::tuple ClusteringKey; std::map> clusters; + 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 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 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 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[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 modality_energy_maps; #ifdef LINEMOD_USE_SEPARATE_ENERGY_MAPS