diff --git a/Point_set_processing_3/include/CGAL/structure_point_set.h b/Point_set_processing_3/include/CGAL/structure_point_set.h index 811a0c8260a..5cd9a2f4dbd 100644 --- a/Point_set_processing_3/include/CGAL/structure_point_set.h +++ b/Point_set_processing_3/include/CGAL/structure_point_set.h @@ -26,6 +26,11 @@ #include #include +#include +#include +#include +#include + #include #include @@ -64,14 +69,29 @@ namespace internal { private: + class My_point_property_map{ + const std::vector& points; + public: + typedef Point value_type; + typedef const value_type& reference; + typedef std::size_t key_type; + typedef boost::lvalue_property_map_tag category; + My_point_property_map (const std::vector& pts) : points (pts) {} + reference operator[] (key_type k) const { return points[k]; } + friend inline reference get (const My_point_property_map& ppmap, key_type i) + { return ppmap[i]; } + }; + + Traits m_traits; - Input_iterator m_input_begin; - Input_iterator m_input_end; + std::vector m_points; + std::vector m_indices; Point_map m_point_pmap; Normal_map m_normal_pmap; std::vector > m_planes; + std::vector > m_pairs; public: @@ -86,8 +106,11 @@ namespace internal { const Shape_detection_3::Efficient_RANSAC& shape_detection) : m_traits (shape_detection.traits()) { - m_input_begin = begin; - m_input_end = end; + + for (Input_iterator it = begin; it != end; ++ it) + m_points.push_back (get(m_point_pmap, *begin)); + + m_indices = std::vector (m_points.size (), -1); BOOST_FOREACH (boost::shared_ptr shape, shape_detection.shapes()) { @@ -98,6 +121,9 @@ namespace internal { if (pshape == boost::shared_ptr()) continue; m_planes.push_back (pshape); + + for (std::size_t i = 0; i < pshape->indices_of_assigned_points().size (); ++ i) + m_indices[pshape->indices_of_assigned_points()[i]] = m_planes.size () - 1; } } @@ -116,6 +142,63 @@ namespace internal { void run (double radius) { + std::cerr << "Finding adjacent primitives... " << std::endl; + find_pairs_of_adjacent_primitives (radius); + std::cerr << "Found " << m_pairs.size () << " pair(s) of adjacent primitives." << std::endl; + + } + + private: + + void find_pairs_of_adjacent_primitives (double radius) + { + + typedef typename Traits::Search_traits Search_traits_base; + typedef Search_traits_adapter Search_traits; + typedef CGAL::Kd_tree Tree; + typedef CGAL::Fuzzy_sphere Fuzzy_sphere; + + My_point_property_map pmap (m_points); + Search_traits straits (pmap); + Tree tree (boost::counting_iterator (0), + boost::counting_iterator (m_points.size()), + typename Tree::Splitter(), + straits); + + std::vector > adjacency_table (m_planes.size (), + std::vector (m_planes.size (), false)); + + //compute a basic adjacency relation (two primitives are neighbors + //if at least one point of the primitive 1 is a k-nearest neighbor + //of a point of the primitive 2 and vice versa) + for (std::size_t i = 0; i < m_points.size (); ++ i) + { + int ind_i = m_indices[i]; + std::cerr << ind_i << " "; + if (ind_i == -1) + continue; + + Fuzzy_sphere query (i, radius, 0., straits); + + std::vector neighbors; + tree.search (std::back_inserter (neighbors), query); // WIP: SegFaults so far... + + + for (std::size_t k = 0; k < neighbors.size(); ++ k) + { + int ind_k = m_indices[neighbors[k]]; + if (ind_k != -1 && ind_k != ind_i) + adjacency_table[ind_i][ind_k] = true; + } + } + + //verify the symmetry and store the pairs of primitives in + //primitive_pairs + for (std::size_t i = 0; i < adjacency_table.size() - 1; ++ i) + for (std::size_t j = i + 1; j < adjacency_table[i].size(); ++ j) + if ((adjacency_table[i][j]) && (adjacency_table[j][i])) + m_pairs.push_back (std::make_pair (i, j)); + } };