I factorized some functions to assemble the covariance matrix and fitting plane or line (those are internal functions)

Principal_component_analysis/include/CGAL/linear_least_squares_fitting_3.h

@@ -29,54 +29,22 @@ CGAL_BEGIN_NAMESPACE
 
 namespace CGALi {
 
-// fits a plane to a 3D point set
+// compute the eigen values and vectors of the covariance 
-// returns a fitting quality (1 - lambda_min/lambda_max):
+// matrix and deduces the best linear fitting plane
-//  1 is best (zero variance orthogonally to the fitting line)
+// (this is an internal function)
-//  0 is worst (isotropic case, returns a plane with default direction)
+// returns fitting quality
-template < typename InputIterator, 
+template < typename K >
-           typename K >
 typename K::FT
-linear_least_squares_fitting_3(InputIterator first,
+fitting_plane_3(const typename K::FT covariance[6], // covariance matrix
-                               InputIterator beyond, 
+                const typename K::Point_3& c, // centroid
                 typename K::Plane_3& plane,   // best fit plane
-                               typename K::Point_3& c,     // centroid
+                const K& k)
-                               const K& k,                 // kernel
-                               const typename K::Point_3*)
 {
   typedef typename K::FT       FT;
   typedef typename K::Point_3  Point;
   typedef typename K::Plane_3  Plane;
   typedef typename K::Vector_3 Vector;
 
-  // precondition: at least one element in the container.
-  CGAL_precondition(first != beyond);
-
-  // compute centroid
-  c = centroid(first,beyond,K());
-
-  // assemble covariance matrix as a
-  // semi-definite matrix. 
-  // Matrix numbering:
-  // 0
-  // 1 2
-  // 3 4 5
-  FT covariance[6] = {0.0, 
-                      0.0, 0.0, 
-                      0.0, 0.0, 0.0};
-  for(InputIterator it = first;
-      it != beyond;
-      it++)
-  {
-    const Point& p = *it;
-    Vector d = p - c;
-    covariance[0] += d.x() * d.x();
-    covariance[1] += d.x() * d.y();
-    covariance[2] += d.y() * d.y();
-    covariance[3] += d.x() * d.z();
-    covariance[4] += d.y() * d.z();
-    covariance[5] += d.z() * d.z();
-  }
-
   // solve for eigenvalues and eigenvectors.
   // eigen values are sorted in descending order, 
   // eigen vectors are sorted in accordance.
@@ -106,56 +74,24 @@ linear_least_squares_fitting_3(InputIterator first,
     plane = Plane(c,Vector(0.0,0.0,1.0));
     return (FT)0.0;
   } 
-} // end fit plane to point set
+}
 
-// fits a line to a 3D point set
+// compute the eigen values and vectors of the covariance 
-// returns a fitting quality (1 - lambda_min/lambda_max):
+// matrix and deduces the best linear fitting line
-//  1 is best (zero variance orthogonally to the fitting line)
+// (this is an internal function)
-//  0 is worst (isotropic case, returns a line along x axis)
+// returns fitting quality
-template < typename InputIterator, 
+template < typename K >
-           typename K >
 typename K::FT
-linear_least_squares_fitting_3(InputIterator first,
+fitting_line_3(const typename K::FT covariance[6], // covariance matrix
-                               InputIterator beyond, 
+               const typename K::Point_3& c, // centroid
                typename K::Line_3& line,   // best fit line
-                               typename K::Point_3& c,    // centroid
+               const K& k)
-                               const K& k,                // kernel
-                               const typename K::Point_3*)
 {
   typedef typename K::FT       FT;
   typedef typename K::Point_3  Point;
   typedef typename K::Line_3   Line;
   typedef typename K::Vector_3 Vector;
 
-  // precondition: at least one element in the container.
-  CGAL_precondition(first != beyond);
-
-  // compute centroid
-  c = centroid(first,beyond,K());
-
-  // assemble covariance matrix as a
-  // semi-definite matrix. 
-  // Matrix numbering:
-  // 0
-  // 1 2
-  // 3 4 5
-  FT covariance[6] = {0.0, 
-                      0.0, 0.0, 
-                      0.0, 0.0, 0.0};
-  for(InputIterator it = first;
-      it != beyond;
-      it++)
-  {
-    const Point& p = *it;
-    Vector d = p - c;
-    covariance[0] += d.x() * d.x();
-    covariance[1] += d.x() * d.y();
-    covariance[2] += d.y() * d.y();
-    covariance[3] += d.x() * d.z();
-    covariance[4] += d.y() * d.z();
-    covariance[5] += d.z() * d.z();
-  }
-
   // solve for eigenvalues and eigenvectors.
   // eigen values are sorted in descending order, 
   // eigen vectors are sorted in accordance.
@@ -182,17 +118,53 @@ linear_least_squares_fitting_3(InputIterator first,
     line = Line(c,Vector(1.0,0.0,0.0));
     return (FT)0.0;
   } 
-} // end fit line to point set
+}
 
-// fits a plane to a 3D triangle set
+// assemble covariance matrix from a point set 
+// (internal function)
 template < typename InputIterator,
            typename K >
-typename K::FT
+void
-linear_least_squares_fitting_3(InputIterator first,
+assemble_covariance_matrix_3(InputIterator first,
                              InputIterator beyond, 
-                               typename K::Plane_3& plane, // best fit plane
+                             typename K::FT covariance[6], // covariance matrix
-                               typename K::Point_3& c,     // centroid
+                             const typename K::Point_3& c, // centroid
-                               const K& k,                 // kernel
+                             const K& k,
+                             const typename K::Point_3*)
+{
+  typedef typename K::FT       FT;
+  typedef typename K::Point_3  Point;
+  typedef typename K::Vector_3 Vector;
+
+  // Matrix numbering:
+  // 0          
+  // 1 2
+  // 3 4 5          
+  for(InputIterator it = first;
+      it != beyond;
+      it++)
+  {
+    const Point& p = *it;
+    Vector d = p - c;
+    covariance[0] += d.x() * d.x();
+    covariance[1] += d.x() * d.y();
+    covariance[2] += d.y() * d.y();
+    covariance[3] += d.x() * d.z();
+    covariance[4] += d.y() * d.z();
+    covariance[5] += d.z() * d.z();
+  }
+}
+
+// assemble covariance matrix from a triangle set 
+// (internal function)
+template < typename InputIterator,
+           typename K >
+void
+assemble_covariance_matrix_3(InputIterator first,
+                             InputIterator beyond, 
+                             typename K::FT covariance[6], // covariance matrix
+                             const typename K::Point_3& c, // centroid
+                             const K& k,
                              const typename K::Triangle_3*)
 {
   typedef typename K::FT       FT;
@@ -200,21 +172,10 @@ linear_least_squares_fitting_3(InputIterator first,
   typedef typename K::Vector_3 Vector;
   typedef typename K::Triangle_3 Triangle;
 
-  // precondition: at least one element in the container.
-  CGAL_precondition(first != beyond);
-
-  // compute centroid
-  c = centroid(first,beyond,K());
-
-  // assemble covariance matrix as a
-  // semi-definite matrix. 
   // Matrix numbering:
   // 0          
   // 1 2
   // 3 4 5          
-  FT covariance[6] = {0.0, 
-                      0.0, 0.0, 
-                      0.0, 0.0, 0.0};
  
   FT sum_areas = 0.0;
   for(InputIterator it = first;
@@ -256,41 +217,138 @@ linear_least_squares_fitting_3(InputIterator first,
   covariance[3] -= sum_areas * c.z() * c.x();
   covariance[4] -= sum_areas * c.z() * c.y();
   covariance[5] -= sum_areas * c.z() * c.z();
+}
 
-  // solve for eigenvalues and eigenvectors.
-  // eigen values are sorted in descending order, 
-  // eigen vectors are sorted in accordance.
-  FT eigen_values[3];
-  FT eigen_vectors[9];
-  eigen_symmetric<FT>(covariance,3,eigen_vectors,eigen_values);
-  CGAL_assertion(eigen_values[0] >= 0.0 && 
-                 eigen_values[1] >= 0.0 &&
-                 eigen_values[2] >= 0.0);
 
-  // check unicity and build fitting plane accordingly
+// fits a plane to a 3D point set
-  if(eigen_values[0] != eigen_values[2])
+// returns a fitting quality (1 - lambda_min/lambda_max):
+//  1 is best (zero variance orthogonally to the fitting line)
+//  0 is worst (isotropic case, returns a plane with default direction)
+template < typename InputIterator, 
+           typename K >
+typename K::FT
+linear_least_squares_fitting_3(InputIterator first,
+                               InputIterator beyond, 
+                               typename K::Plane_3& plane, // best fit plane
+                               typename K::Point_3& c,     // centroid
+                               const K& k,                 // kernel
+                               const typename K::Point_3*)
 {
-    // regular case
+  typedef typename K::FT       FT;
-    Vector normal(eigen_vectors[6],
+  typedef typename K::Point_3  Point;
-                  eigen_vectors[7],
+  typedef typename K::Plane_3  Plane;
-                  eigen_vectors[8]);
+  typedef typename K::Vector_3 Vector;
-    plane = Plane(c,normal);
+
-    return (FT)1.0 - eigen_values[2] / eigen_values[0];
+  // precondition: at least one element in the container.
-  } // end regular case
+  CGAL_precondition(first != beyond);
-  else
+
+  // compute centroid
+  c = centroid(first,beyond,K());
+
+  // assemble covariance matrix
+  FT covariance[6] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
+  assemble_covariance_matrix_3(first,beyond,covariance,c,k,(Point*) NULL);
+
+  // compute fitting plane
+  return fitting_plane_3(covariance,c,plane,k);
+} // end fit plane to point set
+
+// fits a line to a 3D point set
+// returns a fitting quality (1 - lambda_min/lambda_max):
+//  1 is best (zero variance orthogonally to the fitting line)
+//  0 is worst (isotropic case, returns a line along x axis)
+template < typename InputIterator, 
+           typename K >
+typename K::FT
+linear_least_squares_fitting_3(InputIterator first,
+                               InputIterator beyond, 
+                               typename K::Line_3& line,  // best fit line
+                               typename K::Point_3& c,    // centroid
+                               const K& k,                // kernel
+                               const typename K::Point_3*)
 {
-    // isotropic case (infinite number of directions)
+  typedef typename K::FT       FT;
-    // by default: assemble a horizontal plane that goes
+  typedef typename K::Point_3  Point;
-    // through the centroid.
+  typedef typename K::Line_3   Line;
-    plane = Plane(c,Vector(0.0,0.0,1.0));
+  typedef typename K::Vector_3 Vector;
-    return (FT)0.0;
+
-  } // end isotropic case
+  // precondition: at least one element in the container.
+  CGAL_precondition(first != beyond);
+
+  // compute centroid
+  c = centroid(first,beyond,K());
+
+  // assemble covariance matrix
+  FT covariance[6] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
+  assemble_covariance_matrix_3(first,beyond,covariance,c,k,(Point*) NULL);
+
+  // compute fitting line
+  return fitting_line_3(covariance,c,line,k);
+} // end fit line to point set
+
+// fits a plane to a 3D triangle set
+template < typename InputIterator, 
+           typename K >
+typename K::FT
+linear_least_squares_fitting_3(InputIterator first,
+                               InputIterator beyond, 
+                               typename K::Plane_3& plane, // best fit plane
+                               typename K::Point_3& c,     // centroid
+                               const K& k,                 // kernel
+                               const typename K::Triangle_3*)
+{
+  typedef typename K::FT          FT;
+  typedef typename K::Point_3     Point;
+  typedef typename K::Vector_3    Vector;
+  typedef typename K::Triangle_3  Triangle;
+
+  // precondition: at least one element in the container.
+  CGAL_precondition(first != beyond);
+
+  // compute centroid
+  c = centroid(first,beyond,K());
+
+  // assemble covariance matrix
+  FT covariance[6] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
+  assemble_covariance_matrix_3(first,beyond,covariance,c,k,(Triangle*) NULL);
+
+  // compute fitting plane
+  return fitting_plane_3(covariance,c,plane,k);
+} // end linear_least_squares_fitting_3
+
+// fits a line to a 3D triangle set
+template < typename InputIterator, 
+           typename K >
+typename K::FT
+linear_least_squares_fitting_3(InputIterator first,
+                               InputIterator beyond, 
+                               typename K::Line_3& line, // best fit line
+                               typename K::Point_3& c,   // centroid
+                               const K& k,               // kernel
+                               const typename K::Triangle_3*)
+{
+  typedef typename K::FT          FT;
+  typedef typename K::Point_3     Point;
+  typedef typename K::Vector_3    Vector;
+  typedef typename K::Triangle_3  Triangle;
+  typedef typename K::Line_3      Line;
+
+  // precondition: at least one element in the container.
+  CGAL_precondition(first != beyond);
+
+  // compute centroid
+  c = centroid(first,beyond,K());
+
+  // assemble covariance matrix
+  FT covariance[6] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
+  assemble_covariance_matrix_3(first,beyond,covariance,c,k,(Triangle*) NULL);
+
+  // compute fitting plane
+  return fitting_line_3(covariance,c,line,k);
 } // end linear_least_squares_fitting_3
 
 } // end namespace CGALi
 
-
-
 template < typename InputIterator, 
            typename K >
 inline