WIP: finding pairs of adjacent primitives

Point_set_processing_3/include/CGAL/structure_point_set.h

@@ -26,6 +26,11 @@
 #include <CGAL/point_set_processing_assertions.h>
 #include <CGAL/assertions.h>
 
+#include <CGAL/Kd_tree.h>
+#include <CGAL/Fuzzy_sphere.h>
+#include <CGAL/Fuzzy_iso_box.h>
+#include <CGAL/Search_traits_d.h>
+
 #include <iterator>
 #include <list>
 
@@ -64,14 +69,29 @@ namespace internal {
 
   private:
 
+    class My_point_property_map{
+      const std::vector<Point>& 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<Point>& 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<Point> m_points;
+    std::vector<int> m_indices;
     Point_map m_point_pmap;
     Normal_map m_normal_pmap;
     
     std::vector<boost::shared_ptr<Plane_shape> > m_planes;
+    std::vector<std::pair<std::size_t, std::size_t> > m_pairs;
     
   public:
 
@@ -86,8 +106,11 @@ namespace internal {
                            const Shape_detection_3::Efficient_RANSAC<Traits>& 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<int> (m_points.size (), -1);
 
       BOOST_FOREACH (boost::shared_ptr<Shape> shape, shape_detection.shapes())
         {
@@ -98,6 +121,9 @@ namespace internal {
           if (pshape == boost::shared_ptr<Plane_shape>())
             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 <std::size_t, My_point_property_map, Search_traits_base> Search_traits;
+      typedef CGAL::Kd_tree<Search_traits> Tree;
+      typedef CGAL::Fuzzy_sphere<Search_traits> Fuzzy_sphere;
+
+      My_point_property_map pmap (m_points);
+      Search_traits straits (pmap);      
+      Tree tree (boost::counting_iterator<std::size_t> (0),
+                 boost::counting_iterator<std::size_t> (m_points.size()),
+                 typename Tree::Splitter(),
+                 straits);
+        
+      std::vector<std::vector<bool> > adjacency_table (m_planes.size (),
+                                                       std::vector<bool> (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<std::size_t> 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));
+
     }
     
   };