phynexis-deps/cgal
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
Files
329c14dd0e410d8420f95a7165ebdb94de17a512
cgal/Principal_component_analysis/include/CGAL/PCA_util.h
T
Sylvain Pion 2389cdcc1e Dimension detecting stuff rewrite. 
There is now Ambiant_dimension and Feature_dimension.
The handling of the dynamic dimension case is now done by having
the di,ension tag as the first thing provided, with the integral
constant value available only when it makes sense (INT_MAX no longer needed).
2008-04-13 12:38:48 +00:00

754 lines
28 KiB
C++
Raw Blame History

// Copyright (c) 2005 INRIA Sophia-Antipolis (France).
// All rights reserved.
//
// This file is part of CGAL (www.cgal.org); you may redistribute it under
// the terms of the Q Public License version 1.0.
// See the file LICENSE.QPL distributed with CGAL.
//
// 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: svn+ssh://gankit@scm.gforge.inria.fr/svn/cgal/trunk/Principal_component_analysis/include/CGAL/linear_least_squares_fitting_triangles.h $
// $Id: linear_least_squares_fitting_2.h 37882 2007-04-03 15:15:30Z spion $
//
// Author(s) : Pierre Alliez and Sylvain Pion and Ankit Gupta
#ifndef CGAL_LINEAR_LEAST_SQUARES_FITTING_UTIL_H
#define CGAL_LINEAR_LEAST_SQUARES_FITTING_UTIL_H
#include <CGAL/basic.h>
#include <CGAL/Object.h>
#include <CGAL/Linear_algebraCd.h>
#include <CGAL/Dimension.h>
CGAL_BEGIN_NAMESPACE
namespace CGALi {
// Initialize a matrix in n dimension by an array or numbers
template <typename K>
typename CGAL::Linear_algebraCd<typename K::FT>::Matrix
init_matrix(const int n,
typename K::FT entries[])
{
CGAL_assertion(n > 1); // dimension > 1
typedef typename CGAL::Linear_algebraCd<typename K::FT>::Matrix Matrix;
Matrix m(n);
int i,j;
for(i = 0; i < n; i++)
for(j = 0; j < n; j++)
m[i][j] = entries[i*n+j];
return m;
} // end initialization of matrix
// assemble covariance matrix from a point set
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& , // kernel
const typename K::Point_3*, // used for indirection
const CGAL::Dimension_tag<0>&)
{
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
covariance[0] = covariance[1] = covariance[2] =
covariance[3] = covariance[4] = covariance[5] = (FT)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();
}
}
// assemble covariance matrix from a triangle set
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&, // kernel
const typename K::Triangle_3*,// used for indirection
const CGAL::Dimension_tag<2>&)
{
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 CGAL::Linear_algebraCd<FT> LA;
typedef typename LA::Matrix Matrix;
// assemble covariance matrix as a semi-definite matrix.
// Matrix numbering:
// 0
// 1 2
// 3 4 5
//Final combined covariance matrix for all triangles and their combined mass
FT mass = 0.0;
// assemble 2nd order moment about the origin.
FT temp[9] = {1.0/12.0, 1.0/24.0, 1.0/24.0,
1.0/24.0, 1.0/12.0, 1.0/24.0,
1.0/24.0, 1.0/24.0, 1.0/12.0};
Matrix moment = init_matrix<K>(3,temp);
for(InputIterator it = first;
it != beyond;
it++)
{
// Now for each triangle, construct the 2nd order moment about the origin.
// assemble the transformation matrix.
const Triangle& t = *it;
// defined for convenience.
FT delta[9] = {t[0].x(), t[1].x(), t[2].x(),
t[0].y(), t[1].y(), t[2].y(),
t[0].z(), t[1].z(), t[2].z()};
Matrix transformation = init_matrix<K>(3,delta);
FT area = std::sqrt(t.squared_area());
CGAL_assertion(area != 0.0);
// Find the 2nd order moment for the triangle wrt to the origin by an affine transformation.
// Transform the standard 2nd order moment using the transformation matrix
transformation = 2 * area * transformation * moment * LA::transpose(transformation);
// and add to covariance matrix
covariance[0] += transformation[0][0];
covariance[1] += transformation[1][0];
covariance[2] += transformation[1][1];
covariance[3] += transformation[2][0];
covariance[4] += transformation[2][1];
covariance[5] += transformation[2][2];
mass += area;
}
// Translate the 2nd order moment calculated about the origin to
// the center of mass to get the covariance.
covariance[0] += mass * (-1.0 * c.x() * c.x());
covariance[1] += mass * (-1.0 * c.x() * c.y());
covariance[2] += mass * (-1.0 * c.y() * c.y());
covariance[3] += mass * (-1.0 * c.z() * c.x());
covariance[4] += mass * (-1.0 * c.z() * c.y());
covariance[5] += mass * (-1.0 * c.z() * c.z());
}
// assemble covariance matrix from a cuboid set
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& , // kernel
const typename K::Iso_cuboid_3*,// used for indirection
const CGAL::Dimension_tag<3>&)
{
typedef typename K::FT FT;
typedef typename K::Point_3 Point;
typedef typename K::Vector_3 Vector;
typedef typename K::Iso_cuboid_3 Iso_cuboid;
typedef typename CGAL::Linear_algebraCd<FT> LA;
typedef typename LA::Matrix Matrix;
// assemble covariance matrix as a semi-definite matrix.
// Matrix numbering:
// 0
// 1 2
// 3 4 5
// final combined covariance matrix for all cuboids and their combined mass
FT mass = (FT)0.0;
// assemble 2nd order moment about the origin.
FT temp[9] = {(FT)(1.0/3.0), (FT)(1.0/4.0), (FT)(1.0/4.0),
(FT)(1.0/4.0), (FT)(1.0/3.0), (FT)(1.0/4.0),
(FT)(1.0/4.0), (FT)(1.0/4.0), (FT)(1.0/3.0)};
Matrix moment = init_matrix<K>(3,temp);
for(InputIterator it = first;
it != beyond;
it++)
{
// Now for each cuboid, construct the 2nd order moment about the origin.
// assemble the transformation matrix.
const Iso_cuboid& t = *it;
// defined for convenience.
// FT example = CGAL::to_double(t[0].x());
FT x0 = t[0].x();
FT y0 = t[0].y();
FT z0 = t[0].z();
FT delta[9] = {t[1].x()-x0, t[3].x()-x0, t[5].x()-x0,
t[1].y()-y0, t[3].y()-y0, t[5].y()-y0,
t[1].z()-z0, t[3].z()-z0, t[5].z()-z0};
Matrix transformation = init_matrix<K>(3,delta);
FT volume = t.volume();
CGAL_assertion(volume != (FT)0.0);
// Find the 2nd order moment for the cuboid wrt to the origin by an affine transformation.
// Transform the standard 2nd order moment using the transformation matrix
transformation = volume * transformation * moment * LA::transpose(transformation);
// Translate the 2nd order moment to the minimum corner (x0,y0,z0) of the cuboid.
FT xav0 = (delta[0] + delta[1] + delta[2])/4.0;
FT yav0 = (delta[3] + delta[4] + delta[5])/4.0;
FT zav0 = (delta[6] + delta[7] + delta[8])/4.0;
// and add to covariance matrix
covariance[0] += transformation[0][0] + volume * (2*x0*xav0 + x0*x0);
covariance[1] += transformation[1][0] + volume * (xav0*y0 + yav0*x0 + x0*y0);
covariance[2] += transformation[1][1] + volume * (2*y0*yav0 + y0*y0);
covariance[3] += transformation[2][0] + volume * (x0*zav0 + xav0*z0 + x0*z0);
covariance[4] += transformation[2][1] + volume * (yav0*z0 + y0*zav0 + z0*y0);
covariance[5] += transformation[2][2] + volume * (2*zav0*z0 + z0*z0);
mass += volume;
}
// Translate the 2nd order moment calculated about the origin to
// the center of mass to get the covariance.
covariance[0] += mass * (- c.x() * c.x());
covariance[1] += mass * (- c.x() * c.y());
covariance[2] += mass * (- c.y() * c.y());
covariance[3] += mass * (- c.z() * c.x());
covariance[4] += mass * (- c.z() * c.y());
covariance[5] += mass * (- c.z() * c.z());
}
// assemble covariance matrix from a cuboid set
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& , // kernel
const typename K::Iso_cuboid_3*,// used for indirection
const CGAL::Dimension_tag<2>&)
{
typedef typename K::FT FT;
typedef typename K::Point_3 Point;
typedef typename K::Vector_3 Vector;
typedef typename K::Iso_cuboid_3 Iso_cuboid;
typedef typename CGAL::Linear_algebraCd<FT> LA;
typedef typename LA::Matrix Matrix;
// assemble covariance matrix as a semi-definite matrix.
// Matrix numbering:
// 0
// 1 2
// 3 4 5
//Final combined covariance matrix for all cuboids and their combined mass
FT mass = (FT)0.0;
// assemble 2nd order moment about the origin.
FT temp[9] = {(FT)(7.0/3.0), (FT)1.5, (FT)1.5,
(FT)1.5, (FT)(7.0/3.0), (FT)1.5,
(FT)1.5, (FT)1.5, (FT)(7.0/3.0)};
Matrix moment = init_matrix<K>(3,temp);
for(InputIterator it = first;
it != beyond;
it++)
{
// Now for each cuboid, construct the 2nd order moment about the origin.
// assemble the transformation matrix.
const Iso_cuboid& t = *it;
// defined for convenience.
FT x0 = t[0].x();
FT y0 = t[0].y();
FT z0 = t[0].z();
FT delta[9] = {t[1].x()-x0, t[3].x()-x0, t[5].x()-x0,
t[1].y()-y0, t[3].y()-y0, t[5].y()-y0,
t[1].z()-z0, t[3].z()-z0, t[5].z()-z0};
Matrix transformation = init_matrix<K>(3,delta);
FT area = std::pow(delta[0]*delta[0] + delta[3]*delta[3] +
delta[6]*delta[6],1/3.0)*std::pow(delta[1]*delta[1] +
delta[4]*delta[4] + delta[7]*delta[7],1/3.0)*2 +
std::pow(delta[0]*delta[0] + delta[3]*delta[3] +
delta[6]*delta[6],1/3.0)*std::pow(delta[2]*delta[2] +
delta[5]*delta[5] + delta[8]*delta[8],1/3.0)*2 +
std::pow(delta[1]*delta[1] + delta[4]*delta[4] +
delta[7]*delta[7],1/3.0)*std::pow(delta[2]*delta[2] +
delta[5]*delta[5] + delta[8]*delta[8],1/3.0)*2;
CGAL_assertion(area != (FT)0.0);
// Find the 2nd order moment for the cuboid wrt to the origin by an affine transformation.
// Transform the standard 2nd order moment using the transformation matrix
transformation = area * transformation * moment * LA::transpose(transformation);
// Translate the 2nd order moment to the minimum corner (x0,y0,z0) of the cuboid.
FT xav0 = (delta[0] + delta[1] + delta[2])/4.0;
FT yav0 = (delta[3] + delta[4] + delta[5])/4.0;
FT zav0 = (delta[6] + delta[7] + delta[8])/4.0;
// and add to covariance matrix
covariance[0] += transformation[0][0] + area * (2*x0*xav0 + x0*x0);
covariance[1] += transformation[1][0] + area * (xav0*y0 + yav0*x0 + x0*y0);
covariance[2] += transformation[1][1] + area * (2*y0*yav0 + y0*y0);
covariance[3] += transformation[2][0] + area * (x0*zav0 + xav0*z0 + x0*z0);
covariance[4] += transformation[2][1] + area * (yav0*z0 + y0*zav0 + z0*y0);
covariance[5] += transformation[2][2] + area * (2*zav0*z0 + z0*z0);
mass += area;
}
// Translate the 2nd order moment calculated about the origin to
// the center of mass to get the covariance.
covariance[0] += mass * (-1.0 * c.x() * c.x());
covariance[1] += mass * (-1.0 * c.x() * c.y());
covariance[2] += mass * (-1.0 * c.y() * c.y());
covariance[3] += mass * (-1.0 * c.z() * c.x());
covariance[4] += mass * (-1.0 * c.z() * c.y());
covariance[5] += mass * (-1.0 * c.z() * c.z());
}
// assemble covariance matrix from a sphere set
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&, // kernel
const typename K::Sphere_3*, // used for indirection
const CGAL::Dimension_tag<3>&)
{
typedef typename K::FT FT;
typedef typename K::Point_3 Point;
typedef typename K::Vector_3 Vector;
typedef typename K::Sphere_3 Sphere;
typedef typename CGAL::Linear_algebraCd<FT> LA;
typedef typename LA::Matrix Matrix;
// assemble covariance matrix as a semi-definite matrix.
// Matrix numbering:
// 0
// 1 2
// 3 4 5
//Final combined covariance matrix for all spheres and their combined mass
FT mass = 0.0;
// assemble 2nd order moment about the origin.
FT temp[9] = {4.0/15.0, 0.0, 0.0,
0.0, 4.0/15.0, 0.0,
0.0, 0.0, 4.0/15.0};
Matrix moment = init_matrix<K>(3,temp);
for(InputIterator it = first;
it != beyond;
it++)
{
// Now for each sphere, construct the 2nd order moment about the origin.
// assemble the transformation matrix.
const Sphere& t = *it;
// defined for convenience.
FT radius = std::sqrt(t.squared_radius());
FT delta[9] = {radius, 0.0, 0.0,
0.0, radius, 0.0,
0.0, 0.0, radius};
Matrix transformation = init_matrix<K>(3,delta);
FT volume = 4/3.0 * radius*t.squared_radius();
CGAL_assertion(volume != 0.0);
// Find the 2nd order moment for the sphere wrt to the origin by an affine transformation.
// Transform the standard 2nd order moment using the transformation matrix
transformation = (3.0/4.0) * volume * transformation * moment * LA::transpose(transformation);
// Translate the 2nd order moment to the center of the sphere.
FT x0 = t.center().x();
FT y0 = t.center().y();
FT z0 = t.center().z();
// and add to covariance matrix
covariance[0] += transformation[0][0] + volume * x0*x0;
covariance[1] += transformation[1][0] + volume * x0*y0;
covariance[2] += transformation[1][1] + volume * y0*y0;
covariance[3] += transformation[2][0] + volume * x0*z0;
covariance[4] += transformation[2][1] + volume * z0*y0;
covariance[5] += transformation[2][2] + volume * z0*z0;
mass += volume;
}
// Translate the 2nd order moment calculated about the origin to
// the center of mass to get the covariance.
covariance[0] += mass * (-1.0 * c.x() * c.x());
covariance[1] += mass * (-1.0 * c.x() * c.y());
covariance[2] += mass * (-1.0 * c.y() * c.y());
covariance[3] += mass * (-1.0 * c.z() * c.x());
covariance[4] += mass * (-1.0 * c.z() * c.y());
covariance[5] += mass * (-1.0 * c.z() * c.z());
}
// assemble covariance matrix from a sphere set
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&, // kernel
const typename K::Sphere_3*, // used for indirection
const CGAL::Dimension_tag<2>&)
{
typedef typename K::FT FT;
typedef typename K::Point_3 Point;
typedef typename K::Vector_3 Vector;
typedef typename K::Sphere_3 Sphere;
typedef typename CGAL::Linear_algebraCd<FT> LA;
typedef typename LA::Matrix Matrix;
// assemble covariance matrix as a semi-definite matrix.
// Matrix numbering:
// 0
// 1 2
// 3 4 5
//Final combined covariance matrix for all spheres and their combined mass
FT mass = 0.0;
// assemble 2nd order moment about the origin.
FT temp[9] = {4.0/3.0, 0.0, 0.0,
0.0, 4.0/3.0, 0.0,
0.0, 0.0, 4.0/3.0};
Matrix moment = init_matrix<K>(3,temp);
for(InputIterator it = first;
it != beyond;
it++)
{
// Now for each sphere, construct the 2nd order moment about the origin.
// assemble the transformation matrix.
const Sphere& t = *it;
// defined for convenience.
// FT example = CGAL::to_double(t[0].x());
FT radius = std::sqrt(t.squared_radius());
FT delta[9] = {radius, 0.0, 0.0,
0.0, radius, 0.0,
0.0, 0.0, radius};
Matrix transformation = init_matrix<K>(3,delta);
FT area = 4 * t.squared_radius();
CGAL_assertion(area != 0.0);
// Find the 2nd order moment for the sphere wrt to the origin by an affine transformation.
// Transform the standard 2nd order moment using the transformation matrix
transformation = (1.0/4.0) * area * transformation * moment * LA::transpose(transformation);
// Translate the 2nd order moment to the center of the sphere.
FT x0 = t.center().x();
FT y0 = t.center().y();
FT z0 = t.center().z();
// and add to covariance matrix
covariance[0] += transformation[0][0] + area * x0*x0;
covariance[1] += transformation[1][0] + area * x0*y0;
covariance[2] += transformation[1][1] + area * y0*y0;
covariance[3] += transformation[2][0] + area * x0*z0;
covariance[4] += transformation[2][1] + area * z0*y0;
covariance[5] += transformation[2][2] + area * z0*z0;
mass += area;
}
// Translate the 2nd order moment calculated about the origin to
// the center of mass to get the covariance.
covariance[0] += mass * (-1.0 * c.x() * c.x());
covariance[1] += mass * (-1.0 * c.x() * c.y());
covariance[2] += mass * (-1.0 * c.y() * c.y());
covariance[3] += mass * (-1.0 * c.z() * c.x());
covariance[4] += mass * (-1.0 * c.z() * c.y());
covariance[5] += mass * (-1.0 * c.z() * c.z());
}
// assemble covariance matrix from a tetrahedron set
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& , // kernel
const typename K::Tetrahedron_3*,// used for indirection
const CGAL::Dimension_tag<3>&)
{
typedef typename K::FT FT;
typedef typename K::Point_3 Point;
typedef typename K::Vector_3 Vector;
typedef typename K::Tetrahedron_3 Tetrahedron;
typedef typename CGAL::Linear_algebraCd<FT> LA;
typedef typename LA::Matrix Matrix;
// assemble covariance matrix as a semi-definite matrix.
// Matrix numbering:
// 0
// 1 2
// 3 4 5
//Final combined covariance matrix for all tetrahedrons and their combined mass
FT mass = 0.0;
// assemble 2nd order moment about the origin.
FT temp[9] = {1.0/60.0, 1.0/120.0, 1.0/120.0,
1.0/120.0, 1.0/60.0, 1.0/120.0,
1.0/120.0, 1.0/120.0, 1.0/60.0};
Matrix moment = init_matrix<K>(3,temp);
for(InputIterator it = first;
it != beyond;
it++)
{
// Now for each tetrahedron, construct the 2nd order moment about the origin.
// assemble the transformation matrix.
const Tetrahedron& t = *it;
// defined for convenience.
FT x0 = t[0].x();
FT y0 = t[0].y();
FT z0 = t[0].z();
FT delta[9] = {t[1].x()-x0, t[2].x()-x0, t[3].x()-x0,
t[1].y()-y0, t[2].y()-y0, t[3].y()-y0,
t[1].z()-z0, t[2].z()-z0, t[3].z()-z0};
Matrix transformation = init_matrix<K>(3,delta);
FT volume = t.volume();
CGAL_assertion(volume != 0.0);
// Find the 2nd order moment for the tetrahedron wrt to the origin by an affine transformation.
// Transform the standard 2nd order moment using the transformation matrix
transformation = 6 * volume * transformation * moment * LA::transpose(transformation);
// Translate the 2nd order moment to the center of the tetrahedron.
FT xav0 = (delta[0]+delta[1]+delta[2])/4.0;
FT yav0 = (delta[3]+delta[4]+delta[5])/4.0;
FT zav0 = (delta[6]+delta[7]+delta[8])/4.0;
// and add to covariance matrix
covariance[0] += transformation[0][0] + volume * (2*x0*xav0 + x0*x0);
covariance[1] += transformation[1][0] + volume * (xav0*y0 + yav0*x0 + x0*y0);
covariance[2] += transformation[1][1] + volume * (2*y0*yav0 + y0*y0);
covariance[3] += transformation[2][0] + volume * (x0*zav0 + xav0*z0 + x0*z0);
covariance[4] += transformation[2][1] + volume * (yav0*z0 + y0*zav0 + z0*y0);
covariance[5] += transformation[2][2] + volume * (2*zav0*z0 + z0*z0);
mass += volume;
}
// Translate the 2nd order moment calculated about the origin to
// the center of mass to get the covariance.
covariance[0] += mass * (-1.0 * c.x() * c.x());
covariance[1] += mass * (-1.0 * c.x() * c.y());
covariance[2] += mass * (-1.0 * c.y() * c.y());
covariance[3] += mass * (-1.0 * c.z() * c.x());
covariance[4] += mass * (-1.0 * c.z() * c.y());
covariance[5] += mass * (-1.0 * c.z() * c.z());
}
// assemble covariance matrix from a segment set
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& , // kernel
const typename K::Segment_3*,// used for indirection
const CGAL::Dimension_tag<1>&)
{
typedef typename K::FT FT;
typedef typename K::Point_3 Point;
typedef typename K::Vector_3 Vector;
typedef typename K::Segment_3 Segment;
typedef typename CGAL::Linear_algebraCd<FT> LA;
typedef typename LA::Matrix Matrix;
// assemble covariance matrix as a semi-definite matrix.
// Matrix numbering:
// 0
// 1 2
// 3 4 5
//Final combined covariance matrix for all segments and their combined mass
FT mass = 0.0;
// assemble 2nd order moment about the origin.
FT temp[9] = {1.0, 0.5, 0.0,
0.5, 1.0, 0.0,
0.0, 0.0, 0.0};
Matrix moment = (FT)(1.0/3.0) * init_matrix<K>(3,temp);
for(InputIterator it = first;
it != beyond;
it++)
{
// Now for each segment, construct the 2nd order moment about the origin.
// assemble the transformation matrix.
const Segment& t = *it;
// defined for convenience.
// FT example = CGAL::to_double(t[0].x());
FT delta[9] = {t[0].x(), t[1].x(), 0.0,
t[0].y(), t[1].y(), 0.0,
t[0].z(), t[1].z(), 1.0};
Matrix transformation = init_matrix<K>(3,delta);
FT length = std::sqrt(t.squared_length());
CGAL_assertion(length != 0.0);
// Find the 2nd order moment for the segment wrt to the origin by an affine transformation.
// Transform the standard 2nd order moment using the transformation matrix
transformation = length * transformation * moment * LA::transpose(transformation);
// and add to covariance matrix
covariance[0] += transformation[0][0];
covariance[1] += transformation[1][0];
covariance[2] += transformation[1][1];
covariance[3] += transformation[2][0];
covariance[4] += transformation[2][1];
covariance[5] += transformation[2][2];
mass += length;
}
// Translate the 2nd order moment calculated about the origin to
// the center of mass to get the covariance.
covariance[0] += mass * (-1.0 * c.x() * c.x());
covariance[1] += mass * (-1.0 * c.x() * c.y());
covariance[2] += mass * (-1.0 * c.y() * c.y());
covariance[3] += mass * (-1.0 * c.z() * c.x());
covariance[4] += mass * (-1.0 * c.z() * c.y());
covariance[5] += mass * (-1.0 * c.z() * c.z());
}
// compute the eigen values and vectors of the covariance
// matrix and deduces the best linear fitting plane.
// returns fitting quality
template < typename K >
typename K::FT
fitting_plane_3(const typename K::FT covariance[6], // covariance matrix
const typename K::Point_3& c, // centroid
typename K::Plane_3& plane, // best fit plane
const K& ) // kernel
{
typedef typename K::FT FT;
typedef typename K::Point_3 Point;
typedef typename K::Plane_3 Plane;
typedef typename K::Vector_3 Vector;
// 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);
// check unicity and build fitting line accordingly
if(eigen_values[0] != eigen_values[1] &&
eigen_values[0] != eigen_values[2])
{
// regular case
Vector normal(eigen_vectors[6],
eigen_vectors[7],
eigen_vectors[8]);
plane = Plane(c,normal);
return (FT)1.0 - eigen_values[2] / eigen_values[0];
} // end regular case
else
{
// isotropic case (infinite number of directions)
// by default: assemble a horizontal plane that goes
// through the centroid.
plane = Plane(c,Vector(0.0,0.0,1.0));
return (FT)0.0;
}
}
// compute the eigen values and vectors of the covariance
// matrix and deduces the best linear fitting line
// (this is an internal function)
// returns fitting quality
template < typename K >
typename K::FT
fitting_line_3(const typename K::FT covariance[6], // covariance matrix
const typename K::Point_3& c, // centroid
typename K::Line_3& line, // best fit line
const K&) // kernel
{
typedef typename K::FT FT;
typedef typename K::Point_3 Point;
typedef typename K::Line_3 Line;
typedef typename K::Vector_3 Vector;
// 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);
// check unicity and build fitting line accordingly
if(eigen_values[0] != eigen_values[1])
{
// regular case
Vector direction(eigen_vectors[0],eigen_vectors[1],eigen_vectors[2]);
line = Line(c,direction);
return (FT)1.0 - eigen_values[1] / eigen_values[0];
} // end regular case
else
{
// isotropic case (infinite number of directions)
// by default: assemble a horizontal plane that goes
// through the centroid.
line = Line(c,Vector(1.0,0.0,0.0));
return (FT)0.0;
}
}
} // end namespace CGALi
CGAL_END_NAMESPACE
#endif // CGAL_LINEAR_LEAST_SQUARES_FITTING_UTIL_H
Reference in New Issue View Git Blame Copy Permalink