833 lines
31 KiB
C
833 lines
31 KiB
C
|
// Copyright (c) 2015 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$
|
||
|
|
// SPDX-License-Identifier: GPL-3.0+
|
||
|
|
//
|
||
|
|
//
|
||
|
|
// Author(s) : Florent Lafarge, Simon Giraudot
|
||
|
|
//
|
||
|
|
|
||
|
|
/**
|
||
|
|
* \ingroup PkgPointSetShapeDetection3
|
||
|
|
* \file CGAL/regularize_planes.h
|
||
|
|
*
|
||
|
|
*/
|
||
|
|
|
||
|
|
|
||
|
|
#ifndef CGAL_REGULARIZE_PLANES_H
|
||
|
|
#define CGAL_REGULARIZE_PLANES_H
|
||
|
|
|
||
|
|
#include <CGAL/license/Point_set_shape_detection_3.h>
|
||
|
|
|
||
|
|
|
||
|
|
#include <CGAL/Shape_detection_3.h>
|
||
|
|
#include <CGAL/centroid.h>
|
||
|
|
#include <CGAL/squared_distance_3.h>
|
||
|
|
|
||
|
|
#include <boost/foreach.hpp>
|
||
|
|
|
||
|
|
|
||
|
|
namespace CGAL {
|
||
|
|
|
||
|
|
// ----------------------------------------------------------------------------
|
||
|
|
// Private section
|
||
|
|
// ----------------------------------------------------------------------------
|
||
|
|
/// \cond SKIP_IN_MANUAL
|
||
|
|
namespace internal {
|
||
|
|
namespace PlaneRegularization {
|
||
|
|
|
||
|
|
template <typename Traits>
|
||
|
|
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;
|
||
|
|
typename Traits::Vector_3 normal;
|
||
|
|
typename Traits::FT cosangle_symmetry;
|
||
|
|
typename Traits::FT area;
|
||
|
|
typename Traits::FT cosangle_centroid;
|
||
|
|
|
||
|
|
Plane_cluster()
|
||
|
|
: is_free (true)
|
||
|
|
, normal (typename Traits::FT(0.),
|
||
|
|
typename Traits::FT(0.),
|
||
|
|
typename Traits::FT(1.))
|
||
|
|
, cosangle_symmetry (typename Traits::FT(0.))
|
||
|
|
, area (typename Traits::FT(0.))
|
||
|
|
, cosangle_centroid (typename Traits::FT(0.))
|
||
|
|
{ }
|
||
|
|
};
|
||
|
|
|
||
|
|
|
||
|
|
template <typename Traits>
|
||
|
|
typename Traits::Vector_3 regularize_normal
|
||
|
|
(const typename Traits::Vector_3& n,
|
||
|
|
const typename Traits::Vector_3& symmetry_direction,
|
||
|
|
typename Traits::FT cos_symmetry)
|
||
|
|
{
|
||
|
|
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;
|
||
|
|
|
||
|
|
Point pt_symmetry = CGAL::ORIGIN + cos_symmetry* symmetry_direction;
|
||
|
|
|
||
|
|
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);
|
||
|
|
|
||
|
|
Point projected_origin = line.projection (CGAL::ORIGIN);
|
||
|
|
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;
|
||
|
|
|
||
|
|
if (CGAL::squared_distance (pt_normal, pt1) <= CGAL::squared_distance (pt_normal, pt2))
|
||
|
|
return Vector (CGAL::ORIGIN, pt1);
|
||
|
|
else
|
||
|
|
return Vector (CGAL::ORIGIN, pt2);
|
||
|
|
|
||
|
|
}
|
||
|
|
else
|
||
|
|
return n;
|
||
|
|
}
|
||
|
|
|
||
|
|
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,
|
||
|
|
typename Traits::FT cos_symmetry)
|
||
|
|
{
|
||
|
|
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;
|
||
|
|
|
||
|
|
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 PointRange,
|
||
|
|
typename PointMap,
|
||
|
|
typename IndexMap>
|
||
|
|
void compute_centroids_and_areas (const PointRange& points,
|
||
|
|
PointMap point_map,
|
||
|
|
std::size_t nb_planes,
|
||
|
|
IndexMap index_map,
|
||
|
|
std::vector<typename Traits::Point_3>& centroids,
|
||
|
|
std::vector<typename Traits::FT>& areas)
|
||
|
|
{
|
||
|
|
typedef typename Traits::FT FT;
|
||
|
|
typedef typename Traits::Point_3 Point;
|
||
|
|
|
||
|
|
std::vector<std::vector<Point> > listp (nb_planes);
|
||
|
|
|
||
|
|
for (std::size_t i = 0; i < points.size(); ++ i)
|
||
|
|
{
|
||
|
|
int idx = get (index_map, i);
|
||
|
|
if (idx != -1)
|
||
|
|
listp[std::size_t(idx)].push_back (get(point_map, *(points.begin() + i)));
|
||
|
|
}
|
||
|
|
|
||
|
|
centroids.reserve (nb_planes);
|
||
|
|
areas.reserve (nb_planes);
|
||
|
|
for (std::size_t i = 0; i < nb_planes; ++ i)
|
||
|
|
{
|
||
|
|
centroids.push_back (CGAL::centroid (listp[i].begin (), listp[i].end ()));
|
||
|
|
areas.push_back ((FT)(listp[i].size() / (FT)100.));
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
template <typename Traits,
|
||
|
|
typename PlaneRange,
|
||
|
|
typename PlaneMap>
|
||
|
|
void compute_parallel_clusters (PlaneRange& planes,
|
||
|
|
PlaneMap plane_map,
|
||
|
|
std::vector<Plane_cluster<Traits> >& clusters,
|
||
|
|
std::vector<typename Traits::FT>& areas,
|
||
|
|
typename Traits::FT tolerance_cosangle,
|
||
|
|
const typename Traits::Vector_3& symmetry_direction)
|
||
|
|
{
|
||
|
|
|
||
|
|
typedef typename Traits::FT FT;
|
||
|
|
typedef typename Traits::Vector_3 Vector;
|
||
|
|
|
||
|
|
// find pairs of epsilon-parallel primitives and store them in parallel_planes
|
||
|
|
std::vector<std::vector<std::size_t> > parallel_planes (planes.size ());
|
||
|
|
for (std::size_t i = 0; i < std::size_t(planes.size ()); ++ i)
|
||
|
|
{
|
||
|
|
typename PlaneRange::iterator it = planes.begin() + i;
|
||
|
|
Vector v1 = get(plane_map, *it).orthogonal_vector();
|
||
|
|
|
||
|
|
for (std::size_t j = 0; j < std::size_t(planes.size()); ++ j)
|
||
|
|
{
|
||
|
|
if (i == j)
|
||
|
|
continue;
|
||
|
|
|
||
|
|
typename PlaneRange::iterator it2 = planes.begin() + j;
|
||
|
|
Vector v2 = get(plane_map, *it2).orthogonal_vector();
|
||
|
|
|
||
|
|
if (std::fabs (v1 * v2) > 1. - tolerance_cosangle)
|
||
|
|
parallel_planes[i].push_back (j);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
std::vector<bool> is_available (planes.size (), true);
|
||
|
|
|
||
|
|
for (std::size_t i = 0; i < std::size_t(planes.size()); ++ i)
|
||
|
|
{
|
||
|
|
|
||
|
|
if(is_available[i])
|
||
|
|
{
|
||
|
|
const typename Traits::Plane_3& plane = get(plane_map, *(planes.begin() + i));
|
||
|
|
|
||
|
|
is_available[i] = false;
|
||
|
|
|
||
|
|
clusters.push_back (Plane_cluster<Traits>());
|
||
|
|
Plane_cluster<Traits>& clu = clusters.back ();
|
||
|
|
|
||
|
|
//initialization containers
|
||
|
|
clu.planes.push_back (i);
|
||
|
|
|
||
|
|
std::vector<std::size_t> index_container_former_ring_parallel;
|
||
|
|
index_container_former_ring_parallel.push_back(i);
|
||
|
|
|
||
|
|
std::list<std::size_t> index_container_current_ring_parallel;
|
||
|
|
|
||
|
|
//propagation over the pairs of epsilon-parallel primitives
|
||
|
|
bool propagation=true;
|
||
|
|
clu.normal = plane.orthogonal_vector ();
|
||
|
|
clu.area = areas[i];
|
||
|
|
|
||
|
|
do
|
||
|
|
{
|
||
|
|
propagation = false;
|
||
|
|
|
||
|
|
for (std::size_t k = 0; k < index_container_former_ring_parallel.size(); ++ k)
|
||
|
|
{
|
||
|
|
|
||
|
|
std::size_t plane_index = index_container_former_ring_parallel[k];
|
||
|
|
|
||
|
|
for (std::size_t l = 0; l < parallel_planes[plane_index].size(); ++ l)
|
||
|
|
{
|
||
|
|
std::size_t it = parallel_planes[plane_index][l];
|
||
|
|
|
||
|
|
Vector normal_it = get(plane_map, *(planes.begin() + it)).orthogonal_vector ();
|
||
|
|
|
||
|
|
if(is_available[it]
|
||
|
|
&& std::fabs (normal_it*clu.normal) > 1. - tolerance_cosangle )
|
||
|
|
{
|
||
|
|
propagation = true;
|
||
|
|
index_container_current_ring_parallel.push_back(it);
|
||
|
|
is_available[it]=false;
|
||
|
|
|
||
|
|
if(clu.normal * normal_it <0)
|
||
|
|
normal_it = -normal_it;
|
||
|
|
|
||
|
|
clu.normal = (FT)clu.area * clu.normal
|
||
|
|
+ (FT)areas[it] * normal_it;
|
||
|
|
FT norm = (FT)1. / std::sqrt (clu.normal.squared_length());
|
||
|
|
clu.normal = norm * clu.normal;
|
||
|
|
clu.area += areas[it];
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
//update containers
|
||
|
|
index_container_former_ring_parallel.clear();
|
||
|
|
for (std::list<std::size_t>::iterator it = index_container_current_ring_parallel.begin();
|
||
|
|
it != index_container_current_ring_parallel.end(); ++it)
|
||
|
|
{
|
||
|
|
index_container_former_ring_parallel.push_back(*it);
|
||
|
|
clu.planes.push_back(*it);
|
||
|
|
}
|
||
|
|
index_container_current_ring_parallel.clear();
|
||
|
|
|
||
|
|
}
|
||
|
|
while(propagation);
|
||
|
|
|
||
|
|
if (symmetry_direction != CGAL::NULL_VECTOR)
|
||
|
|
{
|
||
|
|
clu.cosangle_symmetry = symmetry_direction * clu.normal;
|
||
|
|
if (clu.cosangle_symmetry < 0.)
|
||
|
|
{
|
||
|
|
clu.normal = -clu.normal;
|
||
|
|
clu.cosangle_symmetry = -clu.cosangle_symmetry;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
is_available.clear();
|
||
|
|
}
|
||
|
|
|
||
|
|
template <typename Traits>
|
||
|
|
void cluster_symmetric_cosangles (std::vector<Plane_cluster<Traits> >& clusters,
|
||
|
|
typename Traits::FT tolerance_cosangle,
|
||
|
|
typename Traits::FT tolerance_cosangle_ortho)
|
||
|
|
{
|
||
|
|
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)
|
||
|
|
list_cluster_index.push_back(static_cast<std::size_t>(-1));
|
||
|
|
|
||
|
|
std::size_t mean_index = 0;
|
||
|
|
for (std::size_t i = 0; i < clusters.size(); ++ i)
|
||
|
|
{
|
||
|
|
if(list_cluster_index[i] == static_cast<std::size_t>(-1))
|
||
|
|
{
|
||
|
|
list_cluster_index[i] = mean_index;
|
||
|
|
FT mean = clusters[i].area * clusters[i].cosangle_symmetry;
|
||
|
|
FT mean_area = clusters[i].area;
|
||
|
|
|
||
|
|
for (std::size_t j = i+1; j < clusters.size(); ++ j)
|
||
|
|
{
|
||
|
|
if (list_cluster_index[j] == static_cast<std::size_t>(-1)
|
||
|
|
&& std::fabs (clusters[j].cosangle_symmetry -
|
||
|
|
mean / mean_area) < tolerance_cosangle_ortho)
|
||
|
|
{
|
||
|
|
list_cluster_index[j] = mean_index;
|
||
|
|
mean_area += clusters[j].area;
|
||
|
|
mean += clusters[j].area * clusters[j].cosangle_symmetry;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
++ mean_index;
|
||
|
|
mean /= mean_area;
|
||
|
|
cosangle_centroids.push_back (mean);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
for (std::size_t i = 0; i < cosangle_centroids.size(); ++ i)
|
||
|
|
{
|
||
|
|
if (cosangle_centroids[i] < tolerance_cosangle_ortho)
|
||
|
|
cosangle_centroids[i] = 0;
|
||
|
|
else if (cosangle_centroids[i] > 1. - tolerance_cosangle)
|
||
|
|
cosangle_centroids[i] = 1;
|
||
|
|
}
|
||
|
|
for (std::size_t i = 0; i < clusters.size(); ++ i)
|
||
|
|
clusters[i].cosangle_symmetry = cosangle_centroids[list_cluster_index[i]];
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
template <typename Traits>
|
||
|
|
void subgraph_mutually_orthogonal_clusters (std::vector<Plane_cluster<Traits> >& 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;
|
||
|
|
|
||
|
|
for (std::size_t i = 0; i < clusters.size(); ++ i)
|
||
|
|
clusters[i].is_free = true;
|
||
|
|
|
||
|
|
for (std::size_t i = 0; i < clusters.size(); ++ i)
|
||
|
|
{
|
||
|
|
if(clusters[i].is_free)
|
||
|
|
{
|
||
|
|
clusters[i].is_free = false;
|
||
|
|
FT max_area = clusters[i].area;
|
||
|
|
std::size_t index_max_area = i;
|
||
|
|
|
||
|
|
//initialization containers
|
||
|
|
std::vector < std::size_t > index_container;
|
||
|
|
index_container.push_back(i);
|
||
|
|
std::vector < std::size_t > index_container_former_ring;
|
||
|
|
index_container_former_ring.push_back(i);
|
||
|
|
std::list < std::size_t > index_container_current_ring;
|
||
|
|
|
||
|
|
//propagation
|
||
|
|
bool propagation=true;
|
||
|
|
do
|
||
|
|
{
|
||
|
|
propagation=false;
|
||
|
|
|
||
|
|
//neighbors
|
||
|
|
for (std::size_t k=0;k<index_container_former_ring.size();k++)
|
||
|
|
{
|
||
|
|
|
||
|
|
std::size_t cluster_index=index_container_former_ring[k];
|
||
|
|
|
||
|
|
for (std::size_t j = 0; j < clusters[cluster_index].orthogonal_clusters.size(); ++ j)
|
||
|
|
{
|
||
|
|
std::size_t cluster_index_2 = clusters[cluster_index].orthogonal_clusters[j];
|
||
|
|
if(clusters[cluster_index_2].is_free)
|
||
|
|
{
|
||
|
|
propagation = true;
|
||
|
|
index_container_current_ring.push_back(cluster_index_2);
|
||
|
|
clusters[cluster_index_2].is_free = false;
|
||
|
|
|
||
|
|
if(max_area < clusters[cluster_index_2].area)
|
||
|
|
{
|
||
|
|
max_area = clusters[cluster_index_2].area;
|
||
|
|
index_max_area = cluster_index_2;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
//update containers
|
||
|
|
index_container_former_ring.clear();
|
||
|
|
for(std::list < std::size_t>::iterator it = index_container_current_ring.begin();
|
||
|
|
it != index_container_current_ring.end(); ++it)
|
||
|
|
{
|
||
|
|
index_container_former_ring.push_back(*it);
|
||
|
|
index_container.push_back(*it);
|
||
|
|
}
|
||
|
|
index_container_current_ring.clear();
|
||
|
|
|
||
|
|
}
|
||
|
|
while(propagation);
|
||
|
|
subgraph_clusters.push_back(index_container);
|
||
|
|
subgraph_clusters_max_area_index.push_back(index_max_area);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
//create subgraphs of mutually orthogonal clusters in which the
|
||
|
|
//largest cluster is excluded and store in
|
||
|
|
//subgraph_clusters_prop
|
||
|
|
std::vector < std::vector < std::size_t> > subgraph_clusters_prop;
|
||
|
|
for (std::size_t i=0;i<subgraph_clusters.size(); i++)
|
||
|
|
{
|
||
|
|
std::size_t index=subgraph_clusters_max_area_index[i];
|
||
|
|
std::vector < std::size_t> subgraph_clusters_prop_temp;
|
||
|
|
for (std::size_t j=0;j<subgraph_clusters[i].size(); j++)
|
||
|
|
if(subgraph_clusters[i][j]!=index)
|
||
|
|
subgraph_clusters_prop_temp.push_back(subgraph_clusters[i][j]);
|
||
|
|
|
||
|
|
subgraph_clusters_prop.push_back(subgraph_clusters_prop_temp);
|
||
|
|
}
|
||
|
|
|
||
|
|
//regularization of cluster normals : in eachsubgraph, we start
|
||
|
|
//from the largest area cluster and we propage over the subgraph
|
||
|
|
//by regularizing the normals of the clusters accorting to
|
||
|
|
//orthogonality and cosangle to symmetry direction
|
||
|
|
|
||
|
|
for (std::size_t i = 0; i < clusters.size(); ++ i)
|
||
|
|
clusters[i].is_free = true;
|
||
|
|
|
||
|
|
for (std::size_t i = 0; i < subgraph_clusters_prop.size(); ++ i)
|
||
|
|
{
|
||
|
|
|
||
|
|
std::size_t index_current=subgraph_clusters_max_area_index[i];
|
||
|
|
|
||
|
|
Vector vec_current=regularize_normal<Traits>
|
||
|
|
(clusters[index_current].normal,
|
||
|
|
symmetry_direction,
|
||
|
|
clusters[index_current].cosangle_symmetry);
|
||
|
|
clusters[index_current].normal = vec_current;
|
||
|
|
clusters[index_current].is_free = false;
|
||
|
|
|
||
|
|
//initialization containers
|
||
|
|
std::vector < std::size_t> index_container;
|
||
|
|
index_container.push_back(index_current);
|
||
|
|
std::vector < std::size_t> index_container_former_ring;
|
||
|
|
index_container_former_ring.push_back(index_current);
|
||
|
|
std::list < std::size_t> index_container_current_ring;
|
||
|
|
|
||
|
|
//propagation
|
||
|
|
bool propagation=true;
|
||
|
|
do
|
||
|
|
{
|
||
|
|
propagation=false;
|
||
|
|
|
||
|
|
//neighbors
|
||
|
|
for (std::size_t k=0;k<index_container_former_ring.size();k++)
|
||
|
|
{
|
||
|
|
|
||
|
|
std::size_t cluster_index=index_container_former_ring[k];
|
||
|
|
|
||
|
|
for (std::size_t j = 0; j < clusters[cluster_index].orthogonal_clusters.size(); ++ j)
|
||
|
|
{
|
||
|
|
std::size_t cluster_index_2 = clusters[cluster_index].orthogonal_clusters[j];
|
||
|
|
if(clusters[cluster_index_2].is_free)
|
||
|
|
{
|
||
|
|
propagation = true;
|
||
|
|
index_container_current_ring.push_back(cluster_index_2);
|
||
|
|
clusters[cluster_index_2].is_free = false;
|
||
|
|
|
||
|
|
Vector new_vect=regularize_normals_from_prior<Traits>
|
||
|
|
(clusters[cluster_index].normal,
|
||
|
|
clusters[cluster_index_2].normal,
|
||
|
|
symmetry_direction,
|
||
|
|
clusters[cluster_index_2].cosangle_symmetry);
|
||
|
|
clusters[cluster_index_2].normal = new_vect;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
//update containers
|
||
|
|
index_container_former_ring.clear();
|
||
|
|
for(std::list < std::size_t>::iterator it = index_container_current_ring.begin();
|
||
|
|
it != index_container_current_ring.end(); ++it)
|
||
|
|
{
|
||
|
|
index_container_former_ring.push_back(*it);
|
||
|
|
index_container.push_back(*it);
|
||
|
|
}
|
||
|
|
index_container_current_ring.clear();
|
||
|
|
}while(propagation);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
} // namespace PlaneRegularization
|
||
|
|
} // namespace internal
|
||
|
|
/// \endcond
|
||
|
|
|
||
|
|
|
||
|
|
// ----------------------------------------------------------------------------
|
||
|
|
// Public section
|
||
|
|
// ----------------------------------------------------------------------------
|
||
|
|
|
||
|
|
/// \ingroup PkgPointSetShapeDetection3
|
||
|
|
|
||
|
|
/*!
|
||
|
|
|
||
|
|
Given a set of detected planes with their respective inlier sets,
|
||
|
|
this function enables to regularize the planes:
|
||
|
|
|
||
|
|
- Planes near parallel can be made exactly parallel.
|
||
|
|
|
||
|
|
- Planes near orthogonal can be made exactly orthogonal.
|
||
|
|
|
||
|
|
- Planes parallel and near coplanar can be made exactly coplanar.
|
||
|
|
|
||
|
|
- Planes near 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 PointRange range of points, model of `ConstRange`
|
||
|
|
\tparam PointPMap is a model of `ReadablePropertyMap` with value type `Kernel::Point_3`.
|
||
|
|
It can be omitted if the value type of the iterator of `PointRange` is convertible to `Point_3<Kernel>`.
|
||
|
|
\tparam PlaneRange range of planes, model of `Range`
|
||
|
|
\tparam PlaneMap is a model of `WritablePropertyMap` with value type `Kernel::Plane_3`.
|
||
|
|
It can be omitted if the value type of the iterator of `PlaneRange` is convertible to `Plane_3<Kernel>`.
|
||
|
|
\tparam IndexMap is a model of `ReadablePropertyMap` with value type `int`.
|
||
|
|
\tparam Kernel Geometric traits class.
|
||
|
|
It can be omitted and deduced automatically from the value type of `PointMap`.
|
||
|
|
|
||
|
|
\param points range of points.
|
||
|
|
\param point_map property map: value_type of `typename PointRange::const_iterator` -> `Point_3`
|
||
|
|
\param planes range of planes.
|
||
|
|
\param plane_map property map: value_type of `typename PlaneRange::iterator` -> `Plane_3`
|
||
|
|
\param index_map property map: index of point `std::size_t` -> index of plane `int` (-1 if the point is not assigned to a plane)
|
||
|
|
|
||
|
|
\param regularize_parallelism Select whether parallelism is
|
||
|
|
regularized or not.
|
||
|
|
|
||
|
|
\param regularize_orthogonality Select whether orthogonality is
|
||
|
|
regularized or not.
|
||
|
|
|
||
|
|
\param regularize_coplanarity Select whether coplanarity is
|
||
|
|
regularized or not.
|
||
|
|
|
||
|
|
\param regularize_axis_symmetry Select whether axis symmetry is
|
||
|
|
regularized or not.
|
||
|
|
|
||
|
|
\param tolerance_angle Tolerance of deviation between normal
|
||
|
|
vectors of planes (in degrees) used for parallelism, orthogonality
|
||
|
|
and axis symmetry. Default value is 25 degrees.
|
||
|
|
|
||
|
|
\param tolerance_coplanarity Maximal distance between two parallel
|
||
|
|
planes such that they are considered coplanar. Default value is
|
||
|
|
0.01.
|
||
|
|
|
||
|
|
\param symmetry_direction Chosen axis for symmetry
|
||
|
|
regularization. Default value is the Z axis.
|
||
|
|
*/
|
||
|
|
|
||
|
|
// This variant requires all parameters
|
||
|
|
template <typename PointRange,
|
||
|
|
typename PointMap,
|
||
|
|
typename PlaneRange,
|
||
|
|
typename PlaneMap,
|
||
|
|
typename IndexMap,
|
||
|
|
typename Kernel>
|
||
|
|
void regularize_planes (const PointRange& points,
|
||
|
|
PointMap point_map,
|
||
|
|
PlaneRange& planes,
|
||
|
|
PlaneMap plane_map,
|
||
|
|
IndexMap index_map,
|
||
|
|
const Kernel&,
|
||
|
|
bool regularize_parallelism,
|
||
|
|
bool regularize_orthogonality,
|
||
|
|
bool regularize_coplanarity,
|
||
|
|
bool regularize_axis_symmetry,
|
||
|
|
double tolerance_angle = 25.0,
|
||
|
|
double tolerance_coplanarity = 0.01,
|
||
|
|
typename Kernel::Vector_3 symmetry_direction
|
||
|
|
= typename Kernel::Vector_3 (0., 0., 1.))
|
||
|
|
{
|
||
|
|
typedef typename Kernel::FT FT;
|
||
|
|
typedef typename Kernel::Point_3 Point;
|
||
|
|
typedef typename Kernel::Vector_3 Vector;
|
||
|
|
typedef typename Kernel::Plane_3 Plane;
|
||
|
|
|
||
|
|
typedef typename internal::PlaneRegularization::Plane_cluster<Kernel>
|
||
|
|
Plane_cluster;
|
||
|
|
|
||
|
|
/*
|
||
|
|
* Compute centroids and areas
|
||
|
|
*/
|
||
|
|
std::vector<Point> centroids;
|
||
|
|
std::vector<FT> areas;
|
||
|
|
internal::PlaneRegularization::compute_centroids_and_areas<Kernel>
|
||
|
|
(points, point_map, planes.size(), index_map, centroids, areas);
|
||
|
|
|
||
|
|
tolerance_angle = tolerance_angle * (FT)CGAL_PI / (FT)(180);
|
||
|
|
FT tolerance_cosangle = (FT)(1. - std::cos (tolerance_angle));
|
||
|
|
FT tolerance_cosangle_ortho = (FT)(std::cos ((FT)0.5 * (FT)CGAL_PI - (FT)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<Kernel>
|
||
|
|
(planes, plane_map, clusters, areas,
|
||
|
|
(regularize_parallelism ? tolerance_cosangle : (FT)0.0),
|
||
|
|
(regularize_axis_symmetry ? symmetry_direction : CGAL::NULL_VECTOR));
|
||
|
|
|
||
|
|
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_ortho)
|
||
|
|
{
|
||
|
|
clusters[i].orthogonal_clusters.push_back (j);
|
||
|
|
clusters[j].orthogonal_clusters.push_back (i);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
if (regularize_axis_symmetry)
|
||
|
|
{
|
||
|
|
//clustering the symmetry cosangle and store their centroids in
|
||
|
|
//cosangle_centroids and the centroid index of each cluster in
|
||
|
|
//list_cluster_index
|
||
|
|
internal::PlaneRegularization::cluster_symmetric_cosangles<Kernel>
|
||
|
|
(clusters, tolerance_cosangle, tolerance_cosangle_ortho);
|
||
|
|
}
|
||
|
|
|
||
|
|
//find subgraphs of mutually orthogonal clusters (store index of
|
||
|
|
//clusters in subgraph_clusters), and select the cluster of
|
||
|
|
//largest area
|
||
|
|
if (regularize_orthogonality || regularize_axis_symmetry)
|
||
|
|
internal::PlaneRegularization::subgraph_mutually_orthogonal_clusters<Kernel>
|
||
|
|
(clusters, (regularize_axis_symmetry ? symmetry_direction : CGAL::NULL_VECTOR));
|
||
|
|
|
||
|
|
//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];
|
||
|
|
const Plane& plane = get(plane_map, *(planes.begin() + index_prim));
|
||
|
|
|
||
|
|
Point pt_reg = plane.projection (centroids[index_prim]);
|
||
|
|
if(plane.orthogonal_vector () * vec_reg < 0)
|
||
|
|
vec_reg=-vec_reg;
|
||
|
|
Plane plane_reg(pt_reg,vec_reg);
|
||
|
|
|
||
|
|
if( std::fabs(plane.orthogonal_vector () * vec_reg) > 1. - tolerance_cosangle)
|
||
|
|
put(plane_map, *(planes.begin() + index_prim), plane_reg);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
if (regularize_coplanarity)
|
||
|
|
{
|
||
|
|
//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))
|
||
|
|
{
|
||
|
|
const Plane& plane = get(plane_map, *(planes.begin() + index_prim));
|
||
|
|
|
||
|
|
clusters[i].coplanar_group[j] = cop_index;
|
||
|
|
|
||
|
|
Point pt_reg = plane.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];
|
||
|
|
const Plane& plane_next = get(plane_map, *(planes.begin() + index_prim_next));
|
||
|
|
Point pt_reg_next = plane_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 = get(plane_map, *(planes.begin()+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 (get(plane_map, *(planes.begin() + index_prim)).orthogonal_vector ()
|
||
|
|
* plane_reg.orthogonal_vector() < 0)
|
||
|
|
put(plane_map, *(planes.begin() + index_prim), plane_reg.opposite());
|
||
|
|
else
|
||
|
|
put(plane_map, *(planes.begin() + index_prim), plane_reg);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
/// \cond SKIP_IN_MANUAL
|
||
|
|
|
||
|
|
|
||
|
|
// This variant deduces the kernel from the point property map.
|
||
|
|
template <typename PointRange,
|
||
|
|
typename PointMap,
|
||
|
|
typename PlaneRange,
|
||
|
|
typename PlaneMap,
|
||
|
|
typename IndexMap>
|
||
|
|
void regularize_planes (const PointRange& points,
|
||
|
|
PointMap point_map,
|
||
|
|
PlaneRange& planes,
|
||
|
|
PlaneMap plane_map,
|
||
|
|
IndexMap index_map,
|
||
|
|
bool regularize_parallelism,
|
||
|
|
bool regularize_orthogonality,
|
||
|
|
bool regularize_coplanarity,
|
||
|
|
bool regularize_axis_symmetry,
|
||
|
|
double tolerance_angle = 25.0,
|
||
|
|
double tolerance_coplanarity = 0.01,
|
||
|
|
typename Kernel_traits
|
||
|
|
<typename boost::property_traits
|
||
|
|
<PointMap>::value_type>::Kernel::Vector_3 symmetry_direction
|
||
|
|
= typename Kernel_traits
|
||
|
|
<typename boost::property_traits
|
||
|
|
<PointMap>::value_type>::Kernel::Vector_3
|
||
|
|
(typename Kernel_traits
|
||
|
|
<typename boost::property_traits
|
||
|
|
<PointMap>::value_type>::Kernel::FT(0.),
|
||
|
|
typename Kernel_traits
|
||
|
|
<typename boost::property_traits
|
||
|
|
<PointMap>::value_type>::Kernel::FT(0.),
|
||
|
|
typename Kernel_traits
|
||
|
|
<typename boost::property_traits
|
||
|
|
<PointMap>::value_type>::Kernel::FT(1.)))
|
||
|
|
{
|
||
|
|
typedef typename boost::property_traits<PointMap>::value_type Point;
|
||
|
|
typedef typename Kernel_traits<Point>::Kernel Kernel;
|
||
|
|
|
||
|
|
regularize_planes (points, point_map, planes, plane_map, index_map, Kernel(),
|
||
|
|
regularize_parallelism, regularize_orthogonality,
|
||
|
|
regularize_coplanarity, regularize_axis_symmetry,
|
||
|
|
tolerance_angle, tolerance_coplanarity, symmetry_direction);
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
/// \endcond
|
||
|
|
|
||
|
|
} // namespace CGAL
|
||
|
|
|
||
|
|
#endif // CGAL_REGULARIZE_PLANES_H
|