Plane_regularization class is now a simple function regularize_planes

Point_set_shape_detection_3/include/CGAL/Plane_regularization.h

@@ -37,361 +37,175 @@
 
 namespace CGAL {
   
+namespace internal {
+namespace PlaneRegularization {
 
-/*!
-\ingroup PkgPointSetShapeDetection3
-\brief A plane regularization algorithm applied as a post-processing
-to a shape detection algorithm.
-
-Given a set of detected planes with their respective inlier sets, this
-class enables to regularize the planes: planes almost parallel are
-made exactly parallel. In addition, some additional regularization can
-be performed:
-
-- Plane clusters that are almost orthogonal can be made exactly
-  orthogonal.
-
-- Planes that are parallel and almost coplanar can be made exactly
-  coplanar.
-
-- Planes that are almost symmetrical with a user-defined axis can be
-  made exactly symmetrical.
-
-Planes are directly modified. Points are left unaltered, as well as
-their relationships to planes (no transfer of point from a primitive
-plane to another).
-
-The implementation follows \cgalCite{cgal:vla-lod-15}.
-
-\tparam Traits a model of `EfficientRANSACTraits`
-
-*/
 template <typename Traits>
-  class Plane_regularization
-  {
-  public:
-
-    /// \cond SKIP_IN_MANUAL
-    typedef Plane_regularization<Traits> Self;
-    /// \endcond
-
-    /// \name Types 
-    /// @{
-    /// \cond SKIP_IN_MANUAL
-    typedef typename Traits::FT FT;
-    typedef typename Traits::Point_3 Point;
-    typedef typename Traits::Vector_3 Vector;
-    typedef typename Traits::Line_3 Line;
-
-    /// \endcond
-    typedef typename Traits::Plane_3 Plane; ///< Raw plane type
-
-    typedef typename Traits::Point_map Point_map;
-    ///< property map to access the location of an input point.
-    typedef typename Traits::Normal_map Normal_map;
-    ///< property map to access the unoriented normal of an input point
-    typedef typename Traits::Input_range Input_range;
-    ///< Model of the concept `Range` with random access iterators, providing input points and normals
-    /// through the following two property maps.
-
-    typedef typename Input_range::iterator Input_iterator; ///< Iterator on input data
-
-    typedef Shape_detection_3::Shape_base<Traits> Shape; ///< Shape type.
-    typedef Shape_detection_3::Plane<Traits> Plane_shape; ///< Plane type.
-    /// @}
-
-  private:
-
 struct Plane_cluster
 {
   bool is_free;
   std::vector<std::size_t> planes;
   std::vector<std::size_t> coplanar_group;
   std::vector<std::size_t> orthogonal_clusters;
-      Vector normal;
+  typename Traits::Vector_3 normal;
-      FT cosangle_symmetry;
+  typename Traits::FT cosangle_symmetry;
-      FT area;
+  typename Traits::FT area;
-      FT cosangle_centroid;
+  typename Traits::FT cosangle_centroid;
 };
 
-    Traits m_traits;
   
-    Input_iterator m_input_begin;
+template <typename Traits>
-    Input_iterator m_input_end;
+typename Traits::Vector_3 regularize_normal
-    Point_map m_point_pmap;
+  (const typename Traits::Vector_3& n,
-    Normal_map m_normal_pmap;
+   const typename Traits::Vector_3& symmetry_direction,
-
+   typename Traits::FT cos_symmetry)
-    std::vector<boost::shared_ptr<Plane_shape> > m_planes;
-    std::vector<Point> m_centroids;
-    std::vector<FT> m_areas;
-
-  public:
-
-    /// \name Initialization 
-    /// @{
-    /*! 
-      Constructs an empty plane regularization engine.
-    */ 
-    Plane_regularization (Traits t = Traits ())
-      : m_traits (t)
 {
+  typedef typename Traits::FT FT;
+  typedef typename Traits::Point_3 Point;
+  typedef typename Traits::Vector_3 Vector;
+  typedef typename Traits::Line_3 Line;
+  typedef typename Traits::Plane_3 Plane;
 
-    }
+  if (symmetry_direction == CGAL::NULL_VECTOR)
+    return n;
       
-    /*! 
+  Point pt_symmetry = CGAL::ORIGIN + cos_symmetry* symmetry_direction;
 
-      Constructs a plane regularization engine base on an input range
+  Plane plane_symmetry (pt_symmetry, symmetry_direction);
-      of points with its related shape detection engine.
+  Point pt_normal = CGAL::ORIGIN + n;
 
-      \param input_range Range of input data.
+  if (n != symmetry_direction || n != -symmetry_direction)
-
-      \param shape_detection Shape detection engine used to detect
-      shapes from the input data. This engine may handle any types of
-      primitive shapes but only planes will be regularized.
-
-      \warning The `shape_detection` parameter must have already
-      detected shapes and must have been using `input_range` as input.
-
-    */ 
-    Plane_regularization (Input_range& input_range,
-                          const Shape_detection_3::Efficient_RANSAC<Traits>& shape_detection)
-      : m_traits (shape_detection.traits())
     {
-      m_input_begin = input_range.begin ();
+      Plane plane_cut (CGAL::ORIGIN, pt_normal, CGAL::ORIGIN + symmetry_direction);
-      m_input_end = input_range.end ();
+      Line line;
+      CGAL::Object ob_1 = CGAL::intersection(plane_cut, plane_symmetry);
+      if (!assign(line, ob_1))
+        return n;
 
-      BOOST_FOREACH (boost::shared_ptr<Shape> shape, shape_detection.shapes())
+      FT delta = std::sqrt ((FT)1. - cos_symmetry * cos_symmetry);
-        {
-          boost::shared_ptr<Plane_shape> pshape
-            = boost::dynamic_pointer_cast<Plane_shape>(shape);
 
-          // Ignore all shapes other than plane
+      Point projected_origin = line.projection (CGAL::ORIGIN);
-          if (pshape == boost::shared_ptr<Plane_shape>())
+      Vector line_vector (line);
-            continue;
+      line_vector = line_vector / std::sqrt (line_vector * line_vector);
-          m_planes.push_back (pshape);
+      Point pt1 = projected_origin + delta * line_vector;
-        }
+      Point pt2 = projected_origin - delta * line_vector;
 
-    }
+      if (CGAL::squared_distance (pt_normal, pt1) <= CGAL::squared_distance (pt_normal, pt2))
-
+        return Vector (CGAL::ORIGIN, pt1);
-    /*! 
-      Releases all memory allocated by this instance.
-    */ 
-    virtual ~Plane_regularization ()
-    {
-      clear ();
-    }
-    /// @}
-
-    /// \name Memory Management
-    /// @{
-    /*!
-      Clear all internal structures.
-     */ 
-    void clear ()
-    {
-      std::vector<boost::shared_ptr<Plane_shape> > ().swap (m_planes);
-      std::vector<Point> ().swap (m_centroids);
-      std::vector<FT> ().swap (m_areas);
-    }
-    /// @}
-
-    /// \name Regularization 
-    /// @{
-    /*! 
-
-      Performs the plane regularization. Planes are directly modified.
-
-      \param tolerance_angle Tolerance of deviation between normal
-      vectors of planes so that they are considered parallel (in
-      degrees).
-
-      \param tolerance_coplanarity Maximal distance between two
-      parallel planes such that they are considered coplanar. The
-      default value is 0, meaning that coplanarity is not taken into
-      account for regularization.
-
-      \param regularize_orthogonality Make almost orthogonal clusters
-      of plane exactly orthogonal.
-
-      \param symmetry_direction Make clusters that are almost
-      symmetrical in the symmetry direction exactly symmetrical. This
-      parameter is ignored if it is equal to `CGAL::NULL_VECTOR`
-      (default value).
-
-      \return The number of clusters of parallel planes found.
-    */ 
-
-    std::size_t run (FT tolerance_angle = (FT)25.0,
-                     FT tolerance_coplanarity = (FT)0.0,
-                     bool regularize_orthogonality = true,
-                     Vector symmetry_direction = CGAL::NULL_VECTOR)
-    {
-      compute_centroids_and_areas ();
-
-      FT tolerance_cosangle = (FT)1. - std::cos (tolerance_angle);
-      
-      // clustering the parallel primitives and store them in clusters
-      // & compute the normal, size and cos angle to the symmetry
-      // direction of each cluster
-      std::vector<Plane_cluster> clusters;
-      compute_parallel_clusters (clusters, tolerance_cosangle, symmetry_direction);
-
-      if (regularize_orthogonality)
-        {
-          //discovery orthogonal relationship between clusters 
-          for (std::size_t i = 0; i < clusters.size(); ++ i)
-            {
-              for (std::size_t j = i + 1; j < clusters.size(); ++ j)
-                {
-              
-                  if (std::fabs (clusters[i].normal * clusters[j].normal) < tolerance_cosangle)
-                    {
-                      clusters[i].orthogonal_clusters.push_back (j);
-                      clusters[j].orthogonal_clusters.push_back (i);
-                    }
-                }
-            }
-        }
-      
-      //clustering the symmetry cosangle and store their centroids in
-      //cosangle_centroids and the centroid index of each cluster in
-      //list_cluster_index
-      if (symmetry_direction != CGAL::NULL_VECTOR)
-        cluster_symmetric_cosangles (clusters, tolerance_cosangle);
-
-      //find subgraphs of mutually orthogonal clusters (store index of
-      //clusters in subgraph_clusters), and select the cluster of
-      //largest area
-      if (regularize_orthogonality)
-        subgraph_mutually_orthogonal_clusters (clusters, symmetry_direction);
-      
-      //recompute optimal plane for each primitive after normal regularization
-      for (std::size_t i=0; i < clusters.size(); ++ i)
-        {
-
-          Vector vec_reg = clusters[i].normal;
-
-          for (std::size_t j = 0; j < clusters[i].planes.size(); ++ j)
-            {
-              std::size_t index_prim = clusters[i].planes[j];
-              Point pt_reg = m_planes[index_prim]->projection (m_centroids[index_prim]);
-              if( m_planes[index_prim]->plane_normal () * vec_reg < 0)
-                vec_reg=-vec_reg;
-              Plane plane_reg(pt_reg,vec_reg);
-              
-              if( std::fabs(m_planes[index_prim]->plane_normal () * plane_reg.orthogonal_vector ()) > 1. - tolerance_cosangle)
-                m_planes[index_prim]->update (plane_reg);
-            }
-        }
-
-
-      //detecting co-planarity and store in list_coplanar_prim
-      for (std::size_t i = 0; i < clusters.size(); ++ i)
-        {
-          Vector vec_reg = clusters[i].normal;
-
-          for (std::size_t ip = 0; ip < clusters[i].planes.size(); ++ ip)
-            clusters[i].coplanar_group.push_back (static_cast<std::size_t>(-1));
-
-          std::size_t cop_index=0;
-
-          for (std::size_t j = 0; j < clusters[i].planes.size(); ++ j)
-            {
-              std::size_t index_prim = clusters[i].planes[j];
-
-              if (clusters[i].coplanar_group[j] == static_cast<std::size_t>(-1))
-                {
-                  clusters[i].coplanar_group[j] = cop_index;
-			
-                  Point pt_reg = m_planes[index_prim]->projection(m_centroids[index_prim]);
-                  Plane plan_reg(pt_reg,vec_reg);
-
-                  for (std::size_t k = j + 1; k < clusters[i].planes.size(); ++ k)
-                    {
-                      if (clusters[i].coplanar_group[k] == static_cast<std::size_t>(-1))
-                        {
-                          std::size_t index_prim_next = clusters[i].planes[k];
-                          Point pt_reg_next = m_planes[index_prim_next]->projection(m_centroids[index_prim_next]);
-                          Point pt_proj=plan_reg.projection(pt_reg_next);
-                          FT distance=distance_Point(pt_reg_next,pt_proj);
-					
-                          if (distance < tolerance_coplanarity)
-                            clusters[i].coplanar_group[k] = cop_index;
-                        }
-                    }
-                  cop_index++; 
-                }
-            }
-
-          //regularize primitive position by computing barycenter of cplanar planes
-          std::vector<Point> pt_bary (cop_index, Point ((FT)0., (FT)0., (FT)0.));
-          std::vector<FT> area (cop_index, 0.);
-      
-          for (std::size_t j = 0; j < clusters[i].planes.size (); ++ j)
-            {
-              std::size_t index_prim = clusters[i].planes[j];
-              std::size_t group = clusters[i].coplanar_group[j];
-              
-              Point pt_reg = m_planes[index_prim]->projection(m_centroids[index_prim]);
-
-              pt_bary[group] = CGAL::barycenter (pt_bary[group], area[group], pt_reg, m_areas[index_prim]); 
-              area[group] += m_areas[index_prim];
-            }
-
-
-          for (std::size_t j = 0; j < clusters[i].planes.size (); ++ j)
-            {
-              std::size_t index_prim = clusters[i].planes[j];
-              std::size_t group = clusters[i].coplanar_group[j];
-              
-              Plane plane_reg (pt_bary[group], vec_reg);
-              
-              if (m_planes[index_prim]->plane_normal ()
-                  * plane_reg.orthogonal_vector() < 0)
-                m_planes[index_prim]->update (plane_reg.opposite());
       else
-                m_planes[index_prim]->update (plane_reg);
+        return Vector (CGAL::ORIGIN, pt2);
+
     }
+  else
+    return n;
 }
 
-      return clusters.size ();
+template <typename Traits>  
-    }
+typename Traits::Vector_3 regularize_normals_from_prior
-    /// @}
+  (const typename Traits::Vector_3& np,
-
+   const typename Traits::Vector_3& n,
-
+   const typename Traits::Vector_3& symmetry_direction,
-  private:
+   typename Traits::FT cos_symmetry)
-    
-    void compute_centroids_and_areas ()
 {
-      for (std::size_t i = 0; i < m_planes.size (); ++ i)
+  typedef typename Traits::FT FT;
+  typedef typename Traits::Point_3 Point;
+  typedef typename Traits::Vector_3 Vector;
+  typedef typename Traits::Line_3 Line;
+  typedef typename Traits::Plane_3 Plane;
+
+  if (symmetry_direction == CGAL::NULL_VECTOR)
+    return n;
+
+  Plane plane_orthogonality (CGAL::ORIGIN, np);
+  Point pt_symmetry = CGAL::ORIGIN + cos_symmetry* symmetry_direction;
+
+  Plane plane_symmetry (pt_symmetry, symmetry_direction);
+		
+  Line line;
+  CGAL::Object ob_1 = CGAL::intersection (plane_orthogonality, plane_symmetry);
+  if (!assign(line, ob_1))
+    return regularize_normal<Traits> (n, symmetry_direction, cos_symmetry);
+
+  Point projected_origin = line.projection (CGAL::ORIGIN);
+  FT R = CGAL::squared_distance (Point (CGAL::ORIGIN), projected_origin);
+
+  if (R <= 1)  // 2 (or 1) possible points intersecting the unit sphere and line
+    {
+      FT delta = std::sqrt ((FT)1. - R);
+      Vector line_vector(line); 
+      line_vector = line_vector / std::sqrt (line_vector * line_vector);
+      Point pt1 = projected_origin + delta * line_vector;
+      Point pt2 = projected_origin - delta * line_vector;
+			
+      Point pt_n = CGAL::ORIGIN + n;
+      if (CGAL::squared_distance (pt_n, pt1) <= CGAL::squared_distance (pt_n, pt2))
+        return Vector (CGAL::ORIGIN, pt1);
+      else
+        return Vector (CGAL::ORIGIN, pt2);
+    }
+  else //no point intersecting the unit sphere and line
+    return regularize_normal<Traits> (n,symmetry_direction, cos_symmetry);
+
+}
+
+template <typename Traits,
+          typename RandomAccessIterator,
+          typename PlaneContainer,
+          typename PointPMap,
+          typename CentroidContainer,
+          typename AreaContainer>
+void compute_centroids_and_areas (RandomAccessIterator input_begin,
+                                  PlaneContainer& planes,
+                                  PointPMap point_pmap,
+                                  CentroidContainer& centroids,
+                                  AreaContainer& areas)
+{
+  typedef typename Traits::FT FT;
+  typedef typename Traits::Point_3 Point;
+  
+  for (std::size_t i = 0; i < planes.size (); ++ i)
     {
       std::vector < Point > listp;
-          for (std::size_t j = 0; j < m_planes[i]->indices_of_assigned_points ().size (); ++ j)
+      for (std::size_t j = 0; j < planes[i]->indices_of_assigned_points ().size (); ++ j)
         {
-              std::size_t yy = m_planes[i]->indices_of_assigned_points()[j];
+          std::size_t yy = planes[i]->indices_of_assigned_points()[j];
-              Point pt = get (m_point_pmap, *(m_input_begin + yy));
+          Point pt = get (point_pmap, *(input_begin + yy));
           listp.push_back(pt);
         }
-          m_centroids.push_back (CGAL::centroid (listp.begin (), listp.end ()));
+      centroids.push_back (CGAL::centroid (listp.begin (), listp.end ()));
-          m_areas.push_back ((FT)(m_planes[i]->indices_of_assigned_points().size()) / (FT)100.);
+      areas.push_back ((FT)(planes[i]->indices_of_assigned_points().size()) / (FT)100.);
     }
 }
 
-    void compute_parallel_clusters (std::vector<Plane_cluster>& clusters, FT tolerance_cosangle,
+
-                                    const Vector& symmetry_direction)
+template <typename Traits,
+          typename PlaneContainer,
+          typename PlaneClusterContainer,
+          typename AreaContainer>
+void compute_parallel_clusters (PlaneContainer& planes,
+                                PlaneClusterContainer& clusters,
+                                AreaContainer& areas,
+                                typename Traits::FT tolerance_cosangle,
+                                const typename Traits::Vector_3& symmetry_direction)
 {
+
+  typedef typename Traits::FT FT;
+  typedef typename Traits::Vector_3 Vector;
+  
+  typedef typename PlaneClusterContainer::value_type Plane_cluster;
+  
   // find pairs of epsilon-parallel primitives and store them in parallel_planes
-      std::vector<std::vector<std::size_t> > parallel_planes (m_planes.size ());
+  std::vector<std::vector<std::size_t> > parallel_planes (planes.size ());
-      for (std::size_t i = 0; i < m_planes.size (); ++ i)
+  for (std::size_t i = 0; i < planes.size (); ++ i)
     {
-          Vector v1 = m_planes[i]->plane_normal ();
+      Vector v1 = planes[i]->plane_normal ();
           
-          for (std::size_t j = 0; j < m_planes.size(); ++ j)
+      for (std::size_t j = 0; j < planes.size(); ++ j)
         {
           if (i == j)
             continue;
               
-              Vector v2 = m_planes[i]->plane_normal ();
+          Vector v2 = planes[i]->plane_normal ();
               
           if (std::fabs (v1 * v2) > 1. - tolerance_cosangle)
             parallel_planes[i].push_back (j);
@@ -399,9 +213,9 @@ The implementation follows \cgalCite{cgal:vla-lod-15}.
     }
 
 
-      std::vector<bool> is_available (m_planes.size (), true);
+  std::vector<bool> is_available (planes.size (), true);
       
-      for (std::size_t i = 0; i < m_planes.size(); ++ i)
+  for (std::size_t i = 0; i < planes.size(); ++ i)
     {
 
       if(is_available[i])
@@ -421,8 +235,8 @@ The implementation follows \cgalCite{cgal:vla-lod-15}.
 
           //propagation over the pairs of epsilon-parallel primitives
           bool propagation=true;
-              clu.normal = m_planes[i]->plane_normal ();
+          clu.normal = planes[i]->plane_normal ();
-              clu.area = m_areas[i];
+          clu.area = areas[i];
 			
           do
             {
@@ -437,7 +251,7 @@ The implementation follows \cgalCite{cgal:vla-lod-15}.
                     {
                       std::size_t it = parallel_planes[plane_index][l];
                           
-                          Vector normal_it =  m_planes[it]->plane_normal ();
+                      Vector normal_it =  planes[it]->plane_normal ();
 
                       if(is_available[it]
                          && std::fabs (normal_it*clu.normal) > 1. - tolerance_cosangle )
@@ -450,10 +264,10 @@ The implementation follows \cgalCite{cgal:vla-lod-15}.
                             normal_it = -normal_it;
 
                           clu.normal = (FT)clu.area * clu.normal
-                                + (FT)m_areas[it] * normal_it;
+                            + (FT)areas[it] * normal_it;
                           FT norm = (FT)1. / std::sqrt (clu.normal.squared_length()); 
                           clu.normal = norm * clu.normal;
-                              clu.area += m_areas[it];
+                          clu.area += areas[it];
                         }	
                     }
                 }
@@ -478,8 +292,13 @@ The implementation follows \cgalCite{cgal:vla-lod-15}.
   is_available.clear();
 }
 
-    void cluster_symmetric_cosangles (std::vector<Plane_cluster>& clusters, FT tolerance_cosangle)
+template <typename Traits,
+          typename PlaneClusterContainer>
+void cluster_symmetric_cosangles (PlaneClusterContainer& clusters,
+                                  typename Traits::FT tolerance_cosangle)
 {
+  typedef typename Traits::FT FT;
+  
   std::vector < FT > cosangle_centroids;
   std::vector < std::size_t> list_cluster_index;
   for( std::size_t i = 0; i < clusters.size(); ++ i)
@@ -522,9 +341,15 @@ The implementation follows \cgalCite{cgal:vla-lod-15}.
     clusters[i].cosangle_symmetry = cosangle_centroids[list_cluster_index[i]];
 }
 
-    void subgraph_mutually_orthogonal_clusters (std::vector<Plane_cluster>& clusters,
+
-                                                const Vector& symmetry_direction)
+template <typename Traits,
+          typename PlaneClusterContainer>
+void subgraph_mutually_orthogonal_clusters (PlaneClusterContainer& clusters,
+                                            const typename Traits::Vector_3& symmetry_direction)
 {
+  typedef typename Traits::FT FT;
+  typedef typename Traits::Vector_3 Vector;
+  
   std::vector < std::vector < std::size_t> > subgraph_clusters;
   std::vector < std::size_t> subgraph_clusters_max_area_index;
 
@@ -622,7 +447,8 @@ The implementation follows \cgalCite{cgal:vla-lod-15}.
     {
 	
       std::size_t index_current=subgraph_clusters_max_area_index[i];
-          Vector vec_current=regularize_normal(clusters[index_current].normal,
+      Vector vec_current=regularize_normal<Traits>
+        (clusters[index_current].normal,
          symmetry_direction,
          clusters[index_current].cosangle_symmetry);
       clusters[index_current].normal = vec_current;
@@ -657,7 +483,8 @@ The implementation follows \cgalCite{cgal:vla-lod-15}.
                       index_container_current_ring.push_back(j);
                       clusters[j].is_free = false;
 
-                          Vector new_vect=regularize_normals_from_prior(clusters[cluster_index].normal,
+                      Vector new_vect=regularize_normals_from_prior<Traits>
+                        (clusters[cluster_index].normal,
                          clusters[j].normal,
                          symmetry_direction,
                          clusters[j].cosangle_symmetry);
@@ -680,92 +507,240 @@ The implementation follows \cgalCite{cgal:vla-lod-15}.
 }
                                     
 
-    FT distance_Point (const Point& a, const Point& b)
+
+} // namespace PlaneRegularization
+} // namespace internal
+
+  
+  /*! 
+
+    Given a set of detected planes with their respective inlier sets,
+    this function enables to regularize the planes: planes almost
+    parallel are made exactly parallel. In addition, some additional
+    regularization can be performed:
+
+    - Plane clusters that are almost orthogonal can be made exactly
+    orthogonal.
+
+    - Planes that are parallel and almost coplanar can be made exactly
+    coplanar.
+
+    - Planes that are almost symmetrical with a user-defined axis can be
+    made exactly symmetrical.
+
+    Planes are directly modified. Points are left unaltered, as well as
+    their relationships to planes (no transfer of point from a primitive
+    plane to another).
+
+    The implementation follows \cgalCite{cgal:vla-lod-15}.
+
+    \tparam Traits a model of `EfficientRANSACTraits`
+
+    \param shape_detection Shape detection engine used to detect
+    shapes from the input data. This engine may handle any types of
+    primitive shapes but only planes will be regularized.
+
+    \warning The `shape_detection` parameter must have already
+    detected shapes and must have been using `input_range` as input.
+
+    \param tolerance_angle Tolerance of deviation between normal
+    vectors of planes so that they are considered parallel (in
+    degrees).
+
+    \param tolerance_coplanarity Maximal distance between two
+    parallel planes such that they are considered coplanar. The
+    default value is 0, meaning that coplanarity is not taken into
+    account for regularization.
+
+    \param regularize_orthogonality Make almost orthogonal clusters
+    of plane exactly orthogonal.
+
+    \param symmetry_direction Make clusters that are almost
+    symmetrical in the symmetry direction exactly symmetrical. This
+    parameter is ignored if it is equal to `CGAL::NULL_VECTOR`
+  (default value).
+
+  \return The number of clusters of parallel planes found.
+*/ 
+
+template <typename RandomAccessIterator,
+          typename EfficientRANSACTraits>
+void regularize_planes (RandomAccessIterator input_begin,
+                        RandomAccessIterator /*input_end*/,
+                        const Shape_detection_3::Efficient_RANSAC<EfficientRANSACTraits>& shape_detection,
+                        typename EfficientRANSACTraits::FT tolerance_angle
+                        = (typename EfficientRANSACTraits::FT)25.0,
+                        typename EfficientRANSACTraits::FT tolerance_coplanarity
+                        = (typename EfficientRANSACTraits::FT)0.0,
+                        bool regularize_orthogonality = true,
+                        typename EfficientRANSACTraits::Vector_3 symmetry_direction
+                        = CGAL::NULL_VECTOR)
 {
-      return std::sqrt (CGAL::squared_distance (a, b));
+  typedef typename EfficientRANSACTraits::FT FT;
+  typedef typename EfficientRANSACTraits::Point_3 Point;
+  typedef typename EfficientRANSACTraits::Vector_3 Vector;
+  typedef typename EfficientRANSACTraits::Plane_3 Plane;
+  typedef typename EfficientRANSACTraits::Point_map Point_map;
+
+  typedef Shape_detection_3::Shape_base<EfficientRANSACTraits> Shape;
+  typedef Shape_detection_3::Plane<EfficientRANSACTraits> Plane_shape;
+
+  typedef typename internal::PlaneRegularization::Plane_cluster<EfficientRANSACTraits>
+    Plane_cluster;
+
+  std::vector<boost::shared_ptr<Plane_shape> > planes;
+    
+  BOOST_FOREACH (boost::shared_ptr<Shape> shape, shape_detection.shapes())
+    {
+      boost::shared_ptr<Plane_shape> pshape
+        = boost::dynamic_pointer_cast<Plane_shape>(shape);
+        
+      // Ignore all shapes other than plane
+      if (pshape == boost::shared_ptr<Plane_shape>())
+        continue;
+      planes.push_back (pshape);
     }
 
-    Vector regularize_normal (const Vector& n, const Vector& symmetry_direction,
+
-                              FT cos_symmetry)
+  /*
+   * Compute centroids and areas
+   */
+  std::vector<Point> centroids;
+  std::vector<FT> areas;
+  internal::PlaneRegularization::compute_centroids_and_areas<EfficientRANSACTraits>
+    (input_begin, planes, Point_map(), centroids, areas);
+
+  FT tolerance_cosangle = (FT)1. - std::cos (tolerance_angle);
+      
+  // clustering the parallel primitives and store them in clusters
+  // & compute the normal, size and cos angle to the symmetry
+  // direction of each cluster
+  std::vector<Plane_cluster> clusters;
+  internal::PlaneRegularization::compute_parallel_clusters<EfficientRANSACTraits>
+    (planes, clusters, areas, tolerance_cosangle, symmetry_direction);
+
+  if (regularize_orthogonality)
     {
-      if (symmetry_direction == CGAL::NULL_VECTOR)
+      //discovery orthogonal relationship between clusters 
-        return n;
+      for (std::size_t i = 0; i < clusters.size(); ++ i)
-      
+        {
-      Point pt_symmetry = CGAL::ORIGIN + cos_symmetry* symmetry_direction;
+          for (std::size_t j = i + 1; j < clusters.size(); ++ j)
-
-      Plane plane_symmetry (pt_symmetry, symmetry_direction);
-      Point pt_normal = CGAL::ORIGIN + n;
-
-      if (n != symmetry_direction || n != -symmetry_direction)
             {
-          Plane plane_cut (CGAL::ORIGIN, pt_normal, CGAL::ORIGIN + symmetry_direction);
-          Line line;
-          CGAL::Object ob_1 = CGAL::intersection(plane_cut, plane_symmetry);
-          if (!assign(line, ob_1))
-            return n;
               
-          FT delta = std::sqrt ((FT)1. - cos_symmetry * cos_symmetry);
+              if (std::fabs (clusters[i].normal * clusters[j].normal) < tolerance_cosangle)
+                {
+                  clusters[i].orthogonal_clusters.push_back (j);
+                  clusters[j].orthogonal_clusters.push_back (i);
+                }
+            }
+        }
+    }
       
-          Point projected_origin = line.projection (CGAL::ORIGIN);
+  //clustering the symmetry cosangle and store their centroids in
-          Vector line_vector (line);
+  //cosangle_centroids and the centroid index of each cluster in
-          line_vector = line_vector / std::sqrt (line_vector * line_vector);
+  //list_cluster_index
-          Point pt1 = projected_origin + delta * line_vector;
+  if (symmetry_direction != CGAL::NULL_VECTOR)
-          Point pt2 = projected_origin - delta * line_vector;
+    internal::PlaneRegularization::cluster_symmetric_cosangles<EfficientRANSACTraits>
+      (clusters, tolerance_cosangle);
 
-          if (CGAL::squared_distance (pt_normal, pt1) <= CGAL::squared_distance (pt_normal, pt2))
+  //find subgraphs of mutually orthogonal clusters (store index of
-            return Vector (CGAL::ORIGIN, pt1);
+  //clusters in subgraph_clusters), and select the cluster of
+  //largest area
+  if (regularize_orthogonality)
+    internal::PlaneRegularization::subgraph_mutually_orthogonal_clusters<EfficientRANSACTraits>
+      (clusters, symmetry_direction);
+      
+  //recompute optimal plane for each primitive after normal regularization
+  for (std::size_t i=0; i < clusters.size(); ++ i)
+    {
+
+      Vector vec_reg = clusters[i].normal;
+
+      for (std::size_t j = 0; j < clusters[i].planes.size(); ++ j)
+        {
+          std::size_t index_prim = clusters[i].planes[j];
+          Point pt_reg = planes[index_prim]->projection (centroids[index_prim]);
+          if( planes[index_prim]->plane_normal () * vec_reg < 0)
+            vec_reg=-vec_reg;
+          Plane plane_reg(pt_reg,vec_reg);
+              
+          if( std::fabs(planes[index_prim]->plane_normal () * plane_reg.orthogonal_vector ()) > 1. - tolerance_cosangle)
+            planes[index_prim]->update (plane_reg);
+        }
+    }
+
+
+  //detecting co-planarity and store in list_coplanar_prim
+  for (std::size_t i = 0; i < clusters.size(); ++ i)
+    {
+      Vector vec_reg = clusters[i].normal;
+
+      for (std::size_t ip = 0; ip < clusters[i].planes.size(); ++ ip)
+        clusters[i].coplanar_group.push_back (static_cast<std::size_t>(-1));
+
+      std::size_t cop_index=0;
+
+      for (std::size_t j = 0; j < clusters[i].planes.size(); ++ j)
+        {
+          std::size_t index_prim = clusters[i].planes[j];
+
+          if (clusters[i].coplanar_group[j] == static_cast<std::size_t>(-1))
+            {
+              clusters[i].coplanar_group[j] = cop_index;
+			
+              Point pt_reg = planes[index_prim]->projection(centroids[index_prim]);
+              Plane plan_reg(pt_reg,vec_reg);
+
+              for (std::size_t k = j + 1; k < clusters[i].planes.size(); ++ k)
+                {
+                  if (clusters[i].coplanar_group[k] == static_cast<std::size_t>(-1))
+                    {
+                      std::size_t index_prim_next = clusters[i].planes[k];
+                      Point pt_reg_next = planes[index_prim_next]->projection(centroids[index_prim_next]);
+                      Point pt_proj=plan_reg.projection(pt_reg_next);
+                      FT distance = std::sqrt (CGAL::squared_distance(pt_reg_next,pt_proj));
+					
+                      if (distance < tolerance_coplanarity)
+                        clusters[i].coplanar_group[k] = cop_index;
+                    }
+                }
+              cop_index++; 
+            }
+        }
+
+      //regularize primitive position by computing barycenter of cplanar planes
+      std::vector<Point> pt_bary (cop_index, Point ((FT)0., (FT)0., (FT)0.));
+      std::vector<FT> area (cop_index, 0.);
+      
+      for (std::size_t j = 0; j < clusters[i].planes.size (); ++ j)
+        {
+          std::size_t index_prim = clusters[i].planes[j];
+          std::size_t group = clusters[i].coplanar_group[j];
+              
+          Point pt_reg = planes[index_prim]->projection(centroids[index_prim]);
+
+          pt_bary[group] = CGAL::barycenter (pt_bary[group], area[group], pt_reg, areas[index_prim]); 
+          area[group] += areas[index_prim];
+        }
+
+
+      for (std::size_t j = 0; j < clusters[i].planes.size (); ++ j)
+        {
+          std::size_t index_prim = clusters[i].planes[j];
+          std::size_t group = clusters[i].coplanar_group[j];
+              
+          Plane plane_reg (pt_bary[group], vec_reg);
+              
+          if (planes[index_prim]->plane_normal ()
+              * plane_reg.orthogonal_vector() < 0)
+            planes[index_prim]->update (plane_reg.opposite());
           else
-            return Vector (CGAL::ORIGIN, pt2);
+            planes[index_prim]->update (plane_reg);
-
         }
-      else
-        return n;
     }
       
-  
-    Vector regularize_normals_from_prior (const Vector& np,
-                                          const Vector& n,
-                                          const Vector& symmetry_direction,
-                                          FT cos_symmetry)
-    {
-      if (symmetry_direction == CGAL::NULL_VECTOR)
-        return n;
-
-      Plane plane_orthogonality (CGAL::ORIGIN, np);
-      Point pt_symmetry = CGAL::ORIGIN + cos_symmetry* symmetry_direction;
-
-      Plane plane_symmetry (pt_symmetry, symmetry_direction);
-		
-      Line line;
-      CGAL::Object ob_1 = CGAL::intersection (plane_orthogonality, plane_symmetry);
-      if (!assign(line, ob_1))
-        return regularize_normal (n, symmetry_direction, cos_symmetry);
-
-      Point projected_origin = line.projection (CGAL::ORIGIN);
-      FT R = CGAL::squared_distance (Point (CGAL::ORIGIN), projected_origin);
-
-      if (R <= 1)  // 2 (or 1) possible points intersecting the unit sphere and line
-        {
-          FT delta = std::sqrt ((FT)1. - R);
-          Vector line_vector(line); 
-          line_vector = line_vector / std::sqrt (line_vector * line_vector);
-          Point pt1 = projected_origin + delta * line_vector;
-          Point pt2 = projected_origin - delta * line_vector;
-			
-          Point pt_n = CGAL::ORIGIN + n;
-          if (CGAL::squared_distance (pt_n, pt1) <= CGAL::squared_distance (pt_n, pt2))
-            return Vector (CGAL::ORIGIN, pt1);
-          else
-            return Vector (CGAL::ORIGIN, pt2);
 }
-      else //no point intersecting the unit sphere and line
+  /// @}
-        return regularize_normal (n,symmetry_direction, cos_symmetry);
-
-    }
-
-  
-
-  };
 
 
 } // namespace CGAL