added the key part of upsample_point_set

considering how to use back_inserter

Point_set_processing_3/examples/Point_set_processing_3/upsample_point_set_example.cpp

@@ -46,14 +46,16 @@ int main(void)
   task_timer.start();
   std::cout << "Run upsample algorithm example: " << std::endl;
 
-  // Run algorithm using ball-tree
-   //CGAL::upsample_point_set(
-   //         points.begin(), 
-   //         points.end(), 
-   //         sharpness_sigma, 
-   //         edge_senstivity,
-   //         neighbor_radius,
-   //         number_of_output_points);
+   //Run algorithm using ball-tree
+   CGAL::upsample_point_set(
+            points.begin(), 
+            points.end(), 
+            CGAL::First_of_pair_property_map<PointVectorPair>(),
+            CGAL::Second_of_pair_property_map<PointVectorPair>(),
+            sharpness_sigma, 
+            edge_senstivity,
+            neighbor_radius,
+            number_of_output_points);
 
   long memory = CGAL::Memory_sizer().virtual_size();
   std::cout << "done: " << task_timer.time() << " seconds, " 

Point_set_processing_3/include/CGAL/Rich_grid.h

@@ -69,6 +69,7 @@ public:
              const Vector& v = CGAL::NULL_VECTOR
              ):pt(p),index(i),normal(v){} 
 
+public:
   Point pt;
   Vector normal;
   unsigned int index;

Point_set_processing_3/include/CGAL/denoise_point_set.h

@@ -283,10 +283,9 @@ compute_max_spacing(
 
 // This variant requires all parameters.
 template <typename ForwardIterator,
-  typename PointPMap,
-  typename NormalPMap,
-  typename Kernel
->
+          typename PointPMap,
+          typename NormalPMap,
+          typename Kernel>
 double
 denoise_points_with_normals(
   ForwardIterator first,  ///< iterator over the first input point.
@@ -433,7 +432,7 @@ denoise_points_with_normals(
   ForwardIterator first, ///< first input point.
   ForwardIterator beyond, ///< past-the-end input point.
   PointPMap point_pmap, ///< property map OutputIterator -> Point_3.
-  NormalPMap normal_pmap,
+  NormalPMap normal_pmap, ///< property map ForwardIterator -> Vector_3.
   const unsigned int k, ///< number of neighbors.
   double sharpness_sigma  ///< control sharpness(0-90)
 ) ///< property map OutputIterator -> Vector_3.

Point_set_processing_3/include/CGAL/regularize_and_simplify_point_set_using_balltree.h

@@ -63,7 +63,7 @@ template <typename Kernel>
 typename Kernel::Vector_3
 compute_average_term(
   const typename Kernel::Point_3& query, ///< 3D point to project
-  const std::vector<rich_grid_internel::Rich_point<Kernel> > 
+  const std::vector<rich_grid_internel::Rich_point<Kernel> >& 
                     neighbor_original_points,///< neighbor sample points
   const typename Kernel::FT radius, ///<accept neighborhood radius
   const std::vector<typename Kernel::FT>& density_weight_set ///< densities
@@ -117,7 +117,7 @@ template <typename Kernel>
 typename Kernel::Vector_3
 compute_repulsion_term(
   const typename Kernel::Point_3& query, ///< 3D point to project
-  const std::vector<rich_grid_internel::Rich_point<Kernel> > 
+  const std::vector<rich_grid_internel::Rich_point<Kernel> >& 
              neighbor_sample_points, ///< neighbor sample points
   const typename Kernel::FT radius, ///<accept neighborhood radius
   const std::vector<typename Kernel::FT>& density_weight_set ///< densities
@@ -179,7 +179,7 @@ template <typename Kernel>
 typename Kernel::FT
 compute_density_weight_for_original_point(
   const typename Kernel::Point_3& query, ///< 3D point to project
-  const std::vector<typename Kernel::Point_3> neighbor_original_points, ///< 
+  const std::vector<typename Kernel::Point_3>& neighbor_original_points, ///< 
   const typename Kernel::FT radius ///<accept neighborhood radius
 )
 {
@@ -219,7 +219,7 @@ template <typename Kernel>
 typename Kernel::FT
 compute_density_weight_for_sample_point(
   const typename Kernel::Point_3& query, ///< 3D point to project
-  const std::vector<typename Kernel::Point_3> neighbor_sample_points, ///< 
+  const std::vector<typename Kernel::Point_3>& neighbor_sample_points, ///< 
   const typename Kernel::FT radius
 )
 {

Point_set_processing_3/include/CGAL/upsample_point_set.h

@@ -47,6 +47,71 @@ namespace CGAL {
 namespace upsample_internal{
 
 
+/// For each query point, select a best "base point" in its neighborhoods.
+/// Then,a new point will be interpolated between query point and "base point".
+/// This is the key part of the upsample algorithm 
+/// 
+/// \pre `radius > 0`
+///
+/// @tparam Kernel Geometric traits class.
+///
+/// @return local density length
+template <typename Kernel>
+typename Kernel::FT
+base_point_selection(
+  const rich_grid_internel::Rich_point<Kernel>& query, ///< 3D point to project
+  const std::vector<rich_grid_internel::Rich_point<Kernel> >& 
+                    neighbor_points,///< neighbor sample points
+  const typename Kernel::FT edge_senstivity,
+  unsigned int& output_base_index ///< base point index
+  )
+{
+  // basic geometric types
+  typedef typename Kernel::Point_3 Point;
+  typedef typename Kernel::Vector_3 Vector;
+  typedef typename Kernel::FT FT;
+  typedef typename rich_grid_internel::Rich_point<Kernel> Rich_point;
+
+
+  FT best_dist2 = -10.0;
+  Rich_point& v = query;
+  for (unsigned int i = 0; i < neighbor_points.size(); i++)
+  {
+    Rich_point& t = neighbor_points[i];
+    Point mid_point = (v.pt + t.pt) / FT(2.0);
+
+    Vector& vm = v.normal;
+    Vector& tm = t.normal;
+    FT dot_produce = pow((FT(2.0) - vm * tm), edge_senstivity);
+
+    Vector diff_t_mid = mid_point - t.pt;
+    FT project_t = diff_t_mid * tm;
+    FT min_dist2 = diff_t_mid.squared_length() - project_t * project_t;
+
+    for (unsigned int j = 0; j < neighbor_points.size(); j++)
+    {
+      Rich_point& s = neighbor_points[j];
+      Vector diff_s_mid = mid_point - s.pt;
+      FT prject_s = diff_s_mid * s.normal;
+
+      FT proj_min2 = diff_s_mid.squared_length() - project_s * project_s;
+
+      if (proj_min2 < min_dist2)
+      {
+        min_dist2 = proj_min2;
+      }
+    }
+    min_dist2 *= dot_produce;
+
+    if (min_dist2 > best_dist2)
+    {
+      best_dist2 = min_dist2;
+      output_base_index = neighbor_points[i].index;
+    }
+  }
+
+  return best_dist2;
+}
 
 } // namespace upsample_internal
 
@@ -72,16 +137,20 @@ namespace upsample_internal{
 /// @return iterator of the first point to downsampled points.
 
 // This variant requires all parameters.
-template <typename ForwardIterator, typename PointPMap, typename Kernel>
+template <typename ForwardIterator, 
+          typename PointPMap, 
+          typename NormalPMap,
+          typename Kernel>
 ForwardIterator
 upsample_point_set(
   ForwardIterator first,  ///< iterator over the first input point.
   ForwardIterator beyond, ///< past-the-end iterator over the input points.
   PointPMap point_pmap, ///< property map ForwardIterator -> Point_3
+  NormalPMap normal_pmap, ///< property map ForwardIterator -> Vector_3.
   const typename Kernel::FT sharpness_sigma,  ///< control sharpness(0-90)
   const typename Kernel::FT edge_senstivity,  ///< edge senstivity(0-5)
   const typename Kernel::FT neighbor_radius, ///< initial size of neighbors.
-  const unsigned int number_of_output_points,///< number of iterations.                            
+  const unsigned int number_of_output,///< number of iterations.                            
   const Kernel& /*kernel*/ ///< geometric traits.
 )
 {
@@ -93,6 +162,46 @@ upsample_point_set(
   typedef typename Kernel::Vector_3 Vector;
   typedef typename Kernel::FT FT;
   typedef typename rich_grid_internel::Rich_point<Kernel> Rich_point;
+  typedef typename rich_grid_internel::Rich_box<Kernel> Rich_box;
+
+
+  // preconditions
+  CGAL_point_set_processing_precondition(first != beyond);
+  CGAL_point_set_processing_precondition(sharpness_sigma >= 0 
+                                       &&sharpness_sigma <= 90);
+  CGAL_point_set_processing_precondition(edge_senstivity >= 0 
+                                       &&edge_senstivity <= 5);
+  CGAL_point_set_processing_precondition(neighbor_radius > 0);
+
+  std::size_t number_of_input = std::distance(first, beyond);
+  CGAL_point_set_processing_precondition(number_of_output > number_of_input);
+
+  // copy rich point set
+  ForwardIterator it;// point iterator
+  unsigned int i; 
+  std::vector<Rich_point> rich_point_set(number_of_input);
+  Rich_box box;
+  for(it = first, i = 0; it != beyond; ++it, i++)
+  {
+#ifdef CGAL_USE_PROPERTY_MAPS_API_V1
+    rich_point_set[i].pt = get(point_pmap, it);
+    rich_point_set[i].normal = get(normal_pmap, it);
+#else
+    rich_point_set[i].pt = get(point_pmap, *it);
+    rich_point_set[i].normal = get(normal_pmap, *it);
+#endif
+
+    rich_point_set[i].index = i;
+    box.add_point(rich_point_set[i].pt);
+  }
+
+  // compute neighborhood
+  rich_grid_internel::compute_ball_neighbors_one_self(rich_point_set,
+                                                      box,
+                                                      neighbor_radius);
+
+
+
 
 
   return first;
@@ -100,12 +209,13 @@ upsample_point_set(
 
 /// @cond SKIP_IN_MANUAL
 // This variant deduces the kernel from the iterator type.
-template <typename ForwardIterator, typename PointPMap>
+template <typename ForwardIterator, typename PointPMap, typename NormalPMap>
 ForwardIterator
 upsample_point_set(
   ForwardIterator first, ///< iterator over the first input point
   ForwardIterator beyond, ///< past-the-end iterator
   PointPMap point_pmap, ///< property map ForwardIterator -> Point_3
+  NormalPMap normal_pmap, ///< property map ForwardIterator -> Vector_3.
   double sharpness_sigma,  ///< control sharpness(0-90)
   double edge_senstivity,  ///< edge senstivity(0-5)
   double neighbor_radius, ///< initial size of neighbors.
@@ -117,6 +227,7 @@ upsample_point_set(
   return upsample_point_set(
     first, beyond,
     point_pmap,
+    normal_pmap,
     sharpness_sigma,
     edge_senstivity,
     neighbor_radius,
@@ -127,11 +238,12 @@ upsample_point_set(
 
 /// @cond SKIP_IN_MANUAL
 // This variant creates a default point property map = Dereference_property_map.
-template <typename ForwardIterator>
+template <typename ForwardIterator, typename NormalPMap>
 ForwardIterator
 upsample_point_set(
   ForwardIterator first, ///< iterator over the first input point
   ForwardIterator beyond, ///< past-the-end iterator
+  NormalPMap normal_pmap, ///< property map ForwardIterator -> Vector_3.
   double sharpness_sigma,  ///< control sharpness(0-90)
   double edge_senstivity,  ///< edge senstivity(0-5)
   double neighbor_radius, ///< initial size of neighbors.
@@ -146,6 +258,7 @@ upsample_point_set(
     make_identity_property_map(typename std::iterator_traits<ForwardIterator>::
                                value_type()),
 #endif
+    normal_pmap,
     sharpness_sigma, 
     edge_senstivity,
     neighbor_radius,