Use Nanoflann to help computing the tangent plane (using PCA)

Tangential_complex/include/CGAL/Tangential_complex.h

@@ -26,11 +26,13 @@
 
 #include <CGAL/basic.h>
 #include <CGAL/tags.h>
+#include <CGAL/Dimension.h>
 
 #include <CGAL/Epick_d.h>
 #include <CGAL/Regular_triangulation_euclidean_traits.h>
 #include <CGAL/Regular_triangulation.h>
 #include <CGAL/Tangential_complex/utilities.h>
+#include <CGAL/Tangential_complex/Point_cloud.h>
 
 #include <CGAL/Mesh_3/Profiling_tools.h>
 
@@ -90,11 +92,13 @@ class Tangential_complex
   typedef typename Triangulation::Full_cell_handle    Tr_full_cell_handle;
   
   typedef typename std::vector<Vector>                Tangent_space_basis;
+  
+  typedef Point_cloud<Point>                          Points;
+  typedef Point_cloud_data_structure<Points>          Points_ds;
 
   typedef std::pair<Triangulation*, Tr_vertex_handle> Tr_and_VH;
-  typedef typename std::vector<Point>                 Point_container;
   typedef typename std::vector<Tr_and_VH>             Tr_container;
-  typedef typename std::vector<Tangent_space_basis>    TS_container;
+  typedef typename std::vector<Tangent_space_basis>   TS_container;
 
   // Stores the index of the original Point in the ambient space
   /*struct Tr_point_with_index 
@@ -115,7 +119,7 @@ public:
   template <typename InputIterator>
   Tangential_complex(InputIterator first, InputIterator last, 
                      const Kernel &k = Kernel())
-  : m_k(k), m_points(first, last) {}
+  : m_k(k), m_points(first, last, k), m_points_ds(m_points, k) {}
 
   /// Destructor
   ~Tangential_complex() {}
@@ -192,8 +196,8 @@ public:
 
     //******** VERTICES ************
 
-    Point_container::const_iterator it_p = m_points.begin();
-    Point_container::const_iterator it_p_end = m_points.end();
+    Points::const_iterator it_p = m_points.begin();
+    Points::const_iterator it_p_end = m_points.end();
     // For each point p
     for ( ; it_p != it_p_end ; ++it_p)
     {
@@ -260,7 +264,23 @@ private:
     Kernel const& m_k;
   };
 
-  
+  struct Tr_vertex_to_global_point
+  {
+    typedef typename Tr_vertex_handle	argument_type;
+    typedef typename Point	result_type;
+
+    Tr_vertex_to_global_point(Points const& points)
+      : m_points(points) {}
+
+    result_type operator()(argument_type const& vh) const
+    {
+      return m_points[vh->data()];
+    }
+
+  private:
+    Points const& m_points;
+  };
+
   struct Tr_vertex_to_bare_point
   {
     typedef typename Tr_vertex_handle	argument_type;
@@ -336,8 +356,8 @@ private:
     std::vector<Tr_point> projected_points;
     FT max_squared_weight = 0;
     projected_points.reserve(m_points.size() - 1);
-    Point_container::const_iterator it_p = m_points.begin();
-    Point_container::const_iterator it_p_end = m_points.end();
+    Points::const_iterator it_p = m_points.begin();
+    Points::const_iterator it_p_end = m_points.end();
     for (std::size_t j = 0 ; it_p != it_p_end ; ++it_p, ++j)
     {
       // ith point = p, which is already inserted
@@ -449,23 +469,20 @@ private:
     Kernel::Scalar_product_d        inner_pdct = m_k.scalar_product_d_object();
     Kernel::Difference_of_vectors_d diff_vec   = m_k.difference_of_vectors_d_object();
 
-    // CJTODO: do better than that (ANN?)
-    typedef std::set<Point, Compare_distance_to_ref_point> Sorted_points;
-    Sorted_points sorted_points(
-      Compare_distance_to_ref_point(p, m_k));
-    sorted_points.insert(m_points.begin(), m_points.end());
+    std::size_t neighbor_indices[NUM_POINTS_FOR_PCA];
+    FT squared_distance[NUM_POINTS_FOR_PCA];
+    m_points_ds.query_ANN(
+      p, NUM_POINTS_FOR_PCA, neighbor_indices, squared_distance);
 
     //******************************* PCA *************************************
 
     const int amb_dim = Ambient_dimension<Point>::value;
     // One row = one point
     Eigen::MatrixXd mat_points(NUM_POINTS_FOR_PCA, amb_dim);
-    int j = 0;
-    for (Sorted_points::const_iterator it = sorted_points.begin() ; 
-         j < NUM_POINTS_FOR_PCA ; ++it, ++j)
+    for (int j = 0 ; j < NUM_POINTS_FOR_PCA ; ++j)
     {
       for (int i = 0 ; i < amb_dim ; ++i)
-        mat_points(j, i) = (*it)[i];
+        mat_points(j, i) = m_points[neighbor_indices[j]][i]; // CJTODO: Use kernel functor
     }
     Eigen::MatrixXd centered = mat_points.rowwise() - mat_points.colwise().mean();
     Eigen::MatrixXd cov = centered.adjoint() * centered;
@@ -531,7 +548,7 @@ private:
 
     std::vector<FT> coords;
     // Ambiant-space coords of the projected point
-    std::vector<FT> p_proj(origin.cartesian_begin(), origin.cartesian_end());
+    std::vector<FT> p_proj(origin.cartesian_begin(), origin.cartesian_end()); // CJTODO: use kernel functors?
     coords.reserve(Intrinsic_dimension);
     for (std::size_t i = 0 ; i < Intrinsic_dimension ; ++i)
     {
@@ -554,7 +571,8 @@ private:
 
 private:
   const Kernel        m_k;
-  Point_container     m_points;
+  Points              m_points;
+  Points_ds           m_points_ds;
   TS_container        m_tangent_spaces;
   Tr_container        m_triangulations; // Contains the triangulations 
                                         // and their center vertex

Tangential_complex/include/CGAL/Tangential_complex/Point_cloud.h

@@ -0,0 +1,349 @@
+// Copyright (c) 2014  INRIA Sophia-Antipolis (France)
+// All rights reserved.
+//
+// This file is part of CGAL (www.cgal.org).
+// You can redistribute it and/or modify it under the terms of the GNU
+// General Public License as published by the Free Software Foundation,
+// either version 3 of the License, or (at your option) any later version.
+//
+// Licensees holding a valid commercial license may use this file in
+// accordance with the commercial license agreement provided with the software.
+//
+// This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
+// WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
+//
+// $URL: $
+// $Id: $
+//
+//
+// Author(s)     : Clement Jamin
+
+#ifndef POINT_CLOUD_H
+#define POINT_CLOUD_H
+
+#include <CGAL/basic.h>
+#include <CGAL/Bbox_3.h>
+#include <CGAL/Kernel_traits.h>
+#include <CGAL/Dimension.h>
+
+#include "nanoflann.hpp"
+
+#include <array>
+#include <utility>
+#include <limits>
+
+namespace CGAL {
+namespace Tangential_complex_ {
+
+template<typename Point_>
+class Point_cloud
+: public std::vector<Point_>
+{
+public:
+  typedef std::vector<Point_>                       Base;
+  typedef Base                                      Raw_container;
+  typedef Point_                                    Point;
+  typedef typename CGAL::Kernel_traits<Point>::type Kernel;
+  typedef typename Kernel::FT                       FT;
+  
+  //typedef typename Base::iterator                   iterator;
+  //typedef typename Base::const_iterator             const_iterator;
+  
+  static const int AMB_DIM = Ambient_dimension<Point>::value;
+
+  Point_cloud(Kernel const& k)
+  : m_k(k)
+  {
+    m_mins.fill(std::numeric_limits<FT>::max());
+    m_maxs.fill(std::numeric_limits<FT>::min());
+  }
+
+  template <class InputIterator>
+  Point_cloud(InputIterator first, InputIterator last, Kernel const& k)
+  : Base(first, last), m_k(k)
+  {
+    m_mins.fill(std::numeric_limits<FT>::max());
+    m_maxs.fill(std::numeric_limits<FT>::min());
+  }
+
+  void push_back(const Point &point, bool update_bbox = false)
+  {
+    Base::push_back(point);
+
+    // Adjust bbox?
+    if (update_bbox)
+    {
+      for (int i = 0 ; i < AMB_DIM ; ++i)
+      {
+        if (point.get_param(i) < m_mins[i])
+          m_mins[i] = point.get_param(i);
+      
+        if (point.get_param(i) > m_maxs[i])
+          m_maxs[i] = point.get_param(i);
+      }
+    }
+  }
+
+  void compute_bbox(bool only_if_not_already_done = false)
+  {
+    if (only_if_not_already_done && m_mins[0] != std::numeric_limits<FT>::max())
+      return;
+
+    // Reset
+    m_mins.fill(std::numeric_limits<FT>::max());
+    m_maxs.fill(std::numeric_limits<FT>::min());
+
+    // Adjust bbox
+    for (const auto &point : *this)
+    {
+      typedef typename Kernel::Compute_coordinate_d Ccd;
+      const Ccd ccd = m_k.compute_coordinate_d_object();
+      for (int i = 0 ; i < AMB_DIM ; ++i)
+      {
+        if (ccd(point, i) < m_mins[i])
+          m_mins[i] =ccd(point, i);
+      
+        if (ccd(point, i) > m_maxs[i])
+          m_maxs[i] = ccd(point, i);
+      }
+    }
+  }
+
+  FT get_min(int dim) const
+  {
+    return m_mins[dim];
+  }
+  
+  FT get_max(int dim) const
+  {
+    return m_maxs[dim];
+  }
+
+  FT bbox_diagonal() const
+  {
+    FT sqdiag = 0;
+    for (std::size_t i = 0 ; i < AMB_DIM ; ++i)
+    {
+      FT d = m_maxs[i] - m_mins[i];
+      sqdiag += d*d;
+    }
+    return CGAL::sqrt(sqdiag);
+  }
+
+  void recenter_points_around_origin(
+    bool compute_bbox_if_not_already_done = true)
+  {
+    // If the bounding box has not been computed already
+    if (m_mins[0] == std::numeric_limits<FT>::max())
+      if (compute_bbox_if_not_already_done)
+        compute_bbox();
+      else
+        return;
+
+    // Compute centre of bbox
+    std::array<FT, AMB_DIM> transl_array;
+    for (std::size_t i = 0 ; i < AMB_DIM ; ++i)
+      transl_array[i] = -0.5*(m_maxs[i] + m_mins[i]);
+
+    Point transl(transl_array);
+
+#ifdef CGAL_LINKED_WITH_TBB
+    tbb::parallel_for(tbb::blocked_range<size_t>(0, size()),
+      [&]( const tbb::blocked_range<size_t>& r )
+    {
+      for (auto i = r.begin(); i != r.end(); ++i)
+#else
+    for (auto i = 0; i != size(); ++i)
+#endif
+    {
+        (*this)[i] += transl;
+    }
+#ifdef CGAL_LINKED_WITH_TBB
+  });
+#endif
+
+  }
+
+protected:
+  Kernel const& m_k;      //!< A const ref to the kernel
+  // Bounding box
+  std::array<FT, AMB_DIM> m_mins;
+  std::array<FT, AMB_DIM> m_maxs;
+};
+
+
+// And this is the "dataset to kd-tree" adaptor class:
+template <typename Point_cloud_>
+class Point_cloud_adaptator
+{
+public:
+  typedef typename Point_cloud_::Kernel Kernel;
+  typedef typename Point_cloud_::Point  Point;
+  typedef typename Point_cloud_::FT     FT;
+
+  /// The constructor that sets the data set source
+  Point_cloud_adaptator(Point_cloud_ &point_cloud, Kernel const& k) 
+    : m_points(point_cloud), m_k(k)
+  {}
+
+  /// CRTP helper method
+  inline Point_cloud_ const& point_cloud() const 
+  { 
+    return m_points;
+  }  
+  inline Point_cloud_& point_cloud() 
+  {
+    return m_points;
+  }
+
+  // Must return the number of data points
+  inline size_t kdtree_get_point_count() const 
+  {
+    return point_cloud().size();
+  }
+
+  // Returns the distance between the vector "p1[0:size-1]" 
+  // and the data point with index "idx_p2" stored in the class:
+  inline FT kdtree_distance(
+    const FT *p1, const size_t idx_p2, size_t size) const
+  {
+    Point sp(p1, p1 + size);
+    return m_k.squared_distance_d_object()(sp, point_cloud()[idx_p2]);
+  }
+
+  // Returns the dim'th component of the idx'th point in the class:
+  // Since this is inlined and the "dim" argument is typically an 
+  // immediate value, the "if/else's" are actually solved at compile time.
+  inline FT kdtree_get_pt(const size_t idx, int dim) const
+  {
+    return m_k.compute_coordinate_d_object()(point_cloud()[idx], dim);
+  }
+
+  // Optional bounding-box computation: return false to default to a standard 
+  // bbox computation loop.
+  // Return true if the BBOX was already computed by the class and returned 
+  // in "bb" so it can be avoided to redo it again.
+  // Look at bb.size() to find out the expected dimensionality 
+  // (e.g. 2 or 3 for point clouds)
+  template <class Bbox>
+  bool kdtree_get_bbox(Bbox &bb) const
+  {
+    for (int i = 0 ; i < bb.size() ; ++i)
+    {
+      bb[i].low = m_points.get_min(i);
+      bb[i].high = m_points.get_max(i);
+    }
+
+    return true;
+  }
+
+  Kernel const& kernel() const
+  {
+    return m_k;
+  }
+
+protected:  
+  Point_cloud_& m_points; //!< A ref to the data set origin
+  Kernel const& m_k;      //!< A const ref to the kernel
+
+}; // end of PointCloudAdaptor
+
+template <typename Point_cloud_>
+class Point_cloud_data_structure
+{
+public:
+  typedef typename Point_cloud_::Kernel Kernel;
+  typedef typename Point_cloud_::Point  Point;
+  typedef typename Point_cloud_::FT     FT;
+
+  static const int AMB_DIM = Ambient_dimension<Point>::value;
+
+  /// Constructor
+  Point_cloud_data_structure(Point_cloud_ &cloud, Kernel const& k)
+  : m_adaptor(cloud, k),
+    m_kd_tree(AMB_DIM, 
+              m_adaptor, 
+              nanoflann::KDTreeSingleIndexAdaptorParams(10 /* max leaf */) )
+  {
+    cloud.compute_bbox(true);
+    //cloud.recenter_points_around_origin();
+    m_kd_tree.buildIndex();
+  }
+  
+  Point_cloud_ &point_cloud()
+  {
+    return m_adaptor.point_cloud();
+  }
+
+  const Point_cloud_ &point_cloud() const
+  {
+    return m_adaptor.point_cloud();
+  }
+
+  void query_ANN(const Point &sp,
+    std::size_t k,
+    size_t *neighbor_indices,
+    FT *squared_distance) const
+  {
+    /*std::vector<FT> sp_vec(
+      m_adaptor.kernel().construct_cartesian_const_iterator_d_object()(sp),
+      m_adaptor.kernel().construct_cartesian_const_iterator_d_object()(sp, 0));*/ // CJTODO remettre
+    std::vector<FT> sp_vec;
+    for (int i = 0 ; i < 4 ; ++i)
+      sp_vec.push_back(sp[i]);
+    nanoflann::KNNResultSet<FT> result_set(k);
+    result_set.init(neighbor_indices, squared_distance);
+    m_kd_tree.findNeighbors(result_set,
+                            &sp_vec[0], 
+                            nanoflann::SearchParams());
+
+    /*std::cout << "knnSearch(nn="<< num_results <<"): \n";
+    for (int i = 0 ; i < num_results ; ++i)
+    {
+      std::cout << "  * neighbor_indices = " << neighbor_indices [i]
+                << " (out_dist_sqr = " << squared_distance[i] << ")" 
+                << std::endl;
+    }*/
+  }
+  
+  void query_ball(const Point &sp, 
+                  const FT radius,
+                  std::vector<std::pair<std::size_t, FT> > &neighbors,
+                  bool sort_output = true)
+  {
+    /*std::vector<FT> sp_vec(
+      m_adaptor.kernel().construct_cartesian_const_iterator_d_object()(sp),
+      m_adaptor.kernel().construct_cartesian_const_iterator_d_object()(sp, 0));*/ // CJTODO remettre
+    std::vector<FT> sp_vec;
+    for (int i = 0 ; i < 4 ; ++i)
+      sp_vec.push_back(sp[i]);
+    m_kd_tree.radiusSearch(&sp_vec[0],
+                           radius,
+                           neighbors,
+                           nanoflann::SearchParams(32, 0.f, sort_output));
+
+    /*std::cout << "radiusSearch(num="<< neighbors.size() <<"): \n";
+    for (const auto idx_and_dist : neighbors)
+    {
+      std::cout << "  * neighbor_indices = " << idx_and_dist.first
+                << " (out_dist_sqr = " << idx_and_dist.second << ")" 
+                << std::endl;
+    }*/
+  }
+
+protected:
+  typedef Point_cloud_adaptator<Point_cloud_> Adaptor;
+  typedef nanoflann::KDTreeSingleIndexAdaptor<
+    nanoflann::L2_Simple_Adaptor<FT, Adaptor> ,
+    Adaptor,
+    AMB_DIM // dim
+    > Kd_tree;
+
+  Adaptor m_adaptor;
+  Kd_tree m_kd_tree;
+};
+
+} // namespace Tangential_complex_
+} //namespace CGAL
+
+#endif // POINT_CLOUD_H

Tangential_complex/include/CGAL/Tangential_complex/utilities.h

@@ -23,11 +23,11 @@
 
 #include <CGAL/basic.h>
 #include <CGAL/Kernel_traits.h>
+#include <CGAL/Dimension.h>
 
 #include <vector>
 
 namespace CGAL {
-
 namespace Tangential_complex_ {
 
   template <typename K>
@@ -70,7 +70,7 @@ namespace Tangential_complex_ {
     return output_basis;
   }
 
-}; // namespace Tangential_complex_
+} // namespace Tangential_complex_
 } //namespace CGAL
 
 #endif // CGAL_TC_UTILITIES_H