570 lines
18 KiB
C++
Executable File
570 lines
18 KiB
C++
Executable File
// 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) : Sven Oesau, Yannick Verdie, Clément Jamin, Pierre Alliez
|
|
//
|
|
|
|
#ifndef CGAL_SHAPE_DETECTION_3_SPHERE_H
|
|
#define CGAL_SHAPE_DETECTION_3_SPHERE_H
|
|
|
|
#include <CGAL/license/Point_set_shape_detection_3.h>
|
|
|
|
|
|
#include <CGAL/Shape_detection_3/Shape_base.h>
|
|
#include <CGAL/number_utils.h>
|
|
#include <cmath>
|
|
|
|
|
|
/*!
|
|
\file Sphere.h
|
|
*/
|
|
|
|
namespace CGAL {
|
|
namespace Shape_detection_3 {
|
|
/*!
|
|
\ingroup PkgPointSetShapeDetection3Shapes
|
|
\brief Sphere implements Shape_base. The sphere is represented by its center and the radius.
|
|
\tparam Traits a model of `EfficientRANSACTraits` with the additional
|
|
requirement for spheres (see `EfficientRANSACTraits` documentation).
|
|
*/
|
|
template <class Traits>
|
|
class Sphere : public Shape_base<Traits> {
|
|
using Shape_base<Traits>::update_label;
|
|
|
|
public:
|
|
/// \cond SKIP_IN_MANUAL
|
|
typedef typename Traits::Point_map Point_map;
|
|
///< property map to access the location of an input point.
|
|
typedef typename Traits::Normal_map Normal_map;
|
|
///< property map to access the unoriented normal of an input point.
|
|
typedef typename Traits::Vector_3 Vector_3;
|
|
///< vector type.
|
|
typedef typename Traits::Sphere_3 Sphere_3;
|
|
///< sphere type.
|
|
typedef typename Traits::FT FT;
|
|
///< number type.
|
|
typedef typename Traits::Point_3 Point_3;
|
|
///< point type.
|
|
/// \endcond
|
|
|
|
public:
|
|
Sphere() : Shape_base<Traits>() {}
|
|
|
|
/*!
|
|
Conversion operator to convert to `Sphere_3` type.
|
|
*/
|
|
operator Sphere_3() const {
|
|
return m_sphere;
|
|
}
|
|
|
|
/*!
|
|
Access to the center.
|
|
*/
|
|
Point_3 center() const {
|
|
return this->sph_center(m_sphere);
|
|
}
|
|
|
|
/*!
|
|
Access to the radius of the sphere.
|
|
*/
|
|
FT radius() const {
|
|
return CGAL::sqrt(this->sqradius(m_sphere));
|
|
}
|
|
|
|
/// \cond SKIP_IN_MANUAL
|
|
/*!
|
|
Computes the squared Euclidean distance from query point to the shape.
|
|
*/
|
|
FT squared_distance(const Point_3 &p) const {
|
|
const FT d = CGAL::sqrt(
|
|
this->sqlen(this->constr_vec(
|
|
p, this->sph_center(m_sphere))))
|
|
- CGAL::sqrt(this->sqradius(m_sphere));
|
|
return d * d;
|
|
}
|
|
|
|
/*!
|
|
Helper function to write center,
|
|
radius of the sphere and number of assigned points into a string.
|
|
*/
|
|
std::string info() const {
|
|
std::stringstream sstr;
|
|
Point_3 c = this->sph_center(m_sphere);
|
|
FT r = CGAL::sqrt(this->sqradius(m_sphere));
|
|
|
|
sstr << "Type: sphere center: (" << this->get_x(c) << ", " << this->get_y(c);
|
|
sstr << ", " << this->get_z(c) << ") radius:" << r;
|
|
sstr << " #Pts: " << this->m_indices.size();
|
|
|
|
return sstr.str();
|
|
}
|
|
/// \endcond
|
|
protected:
|
|
/// \cond SKIP_IN_MANUAL
|
|
|
|
// ------------------------------------------------------------------------
|
|
// Utilities
|
|
// ------------------------------------------------------------------------
|
|
Sphere_3 constr_sphere(const Point_3& c, FT r) const
|
|
{ return this->m_traits.construct_sphere_3_object()(c, r); }
|
|
Point_3 sph_center(const Sphere_3& s) const
|
|
{ return this->m_traits.construct_center_3_object()(s); }
|
|
FT sqradius(const Sphere_3& s) const
|
|
{ return this->m_traits.compute_squared_radius_3_object()(s); }
|
|
|
|
void create_shape(const std::vector<std::size_t> &indices) {
|
|
Point_3 p1 = this->point(indices[0]);
|
|
Point_3 p2 = this->point(indices[1]);
|
|
Point_3 p3 = this->point(indices[2]);
|
|
|
|
Vector_3 n1 = this->normal(indices[0]);
|
|
Vector_3 n2 = this->normal(indices[1]);
|
|
Vector_3 n3 = this->normal(indices[2]);
|
|
|
|
|
|
// Determine center: select midpoint of shortest line segment
|
|
// between p1 and p2. Implemented from "3D game engine design" by Eberly 2001
|
|
Vector_3 diff = this->constr_vec(p2, p1);
|
|
FT a = this->scalar_pdct(n1, n1);
|
|
FT b = -this->scalar_pdct(n1, n2);
|
|
FT c = this->scalar_pdct(n2, n2);
|
|
FT d = this->scalar_pdct(n1, diff);
|
|
|
|
FT det = CGAL::abs(a * c - b * b);
|
|
|
|
// degenerated when nearly parallel
|
|
if (det < (FT)0.00001) {
|
|
this->m_is_valid = false;
|
|
return;
|
|
}
|
|
|
|
FT e = -this->scalar_pdct(n2, diff);
|
|
FT invDet = (FT) 1.0 / det;
|
|
FT s = (b * e - c * d) * invDet;
|
|
FT t = (d * b - a * e) * invDet;
|
|
|
|
Vector_3 v_transl = this->sum_vectors(
|
|
this->constr_vec(CGAL::ORIGIN, this->transl(p1, this->scale(n1, s))),
|
|
this->constr_vec(CGAL::ORIGIN, this->transl(p2, this->scale(n2, t))));
|
|
Point_3 center = this->transl(
|
|
CGAL::ORIGIN, this->scale(v_transl, (FT)0.5));
|
|
|
|
Vector_3 v1 = (this->constr_vec(center, p1));
|
|
Vector_3 v2 = (this->constr_vec(center, p2));
|
|
FT d1 = CGAL::sqrt(this->sqlen(v1));
|
|
FT d2 = CGAL::sqrt(this->sqlen(v2));
|
|
|
|
if (CGAL::abs(d1 - d2) > (FT)2.0 * this->m_epsilon) {
|
|
this->m_is_valid = false;
|
|
return;
|
|
}
|
|
|
|
v1 = this->scale(v1, (FT)1.0 / d1);
|
|
v2 = this->scale(v2, (FT)1.0 / d2);
|
|
|
|
if (this->scalar_pdct(n1, v1) < this->m_normal_threshold ||
|
|
this->scalar_pdct(n2, v2) < this->m_normal_threshold) {
|
|
this->m_is_valid = false;
|
|
return;
|
|
}
|
|
|
|
Vector_3 v3 = this->constr_vec(center, p3);
|
|
FT d3 = CGAL::sqrt(this->sqlen(v3));
|
|
v3 = this->scale(v3, (FT)1.0 / d3);
|
|
|
|
FT radius = (d1 + d2) * (FT)0.5;
|
|
|
|
if (CGAL::abs(d3 - radius) > this->m_epsilon ||
|
|
this->scalar_pdct(n3, v3) < this->m_normal_threshold) {
|
|
this->m_is_valid = false;
|
|
return;
|
|
}
|
|
|
|
this->m_is_valid = true;
|
|
|
|
m_sphere = this->constr_sphere(center, radius * radius);
|
|
}
|
|
|
|
virtual void squared_distance(const std::vector<std::size_t> &indices,
|
|
std::vector<FT> &dists) const {
|
|
|
|
FT radius = CGAL::sqrt(this->sqradius(m_sphere));
|
|
|
|
for (std::size_t i = 0;i<indices.size();i++) {
|
|
dists[i] = CGAL::sqrt(this->sqlen(this->constr_vec(
|
|
this->sph_center(m_sphere), this->point(indices[i]))))
|
|
- radius;
|
|
|
|
dists[i] = dists[i] * dists[i];
|
|
}
|
|
}
|
|
|
|
virtual void cos_to_normal(const std::vector<std::size_t> &indices,
|
|
std::vector<FT> &angles) const {
|
|
for (std::size_t i = 0;i<indices.size();i++) {
|
|
Vector_3 n = this->constr_vec(
|
|
this->point(indices[i]),
|
|
this->sph_center(m_sphere));
|
|
|
|
FT length = CGAL::sqrt(this->sqlen(n));
|
|
if (length == 0) {
|
|
angles[i] = (FT)1.0;
|
|
continue;
|
|
}
|
|
|
|
n = this->scale(n, (FT)1.0 / length);
|
|
angles[i] = CGAL::abs(this->scalar_pdct(this->normal(indices[i]), n));
|
|
}
|
|
}
|
|
|
|
virtual FT cos_to_normal(const Point_3 &p, const Vector_3 &n) const {
|
|
Vector_3 sphere_normal = this->constr_vec(p, this->sph_center(m_sphere));
|
|
FT length = (FT)(CGAL::sqrt(this->sqlen(n)));
|
|
if (length == 0)
|
|
return 1;
|
|
|
|
sphere_normal = this->scale(sphere_normal, (FT)1.0 / length);
|
|
return CGAL::abs(this->scalar_pdct(sphere_normal, n));
|
|
}
|
|
|
|
virtual std::size_t minimum_sample_size() const {
|
|
return 3;
|
|
}
|
|
|
|
// Maps to the range [-1,1]^2
|
|
static void concentric_mapping(FT phi, FT proj, FT rad, FT &x, FT &y) {
|
|
phi = (phi < FT(-CGAL_M_PI_4)) ? phi + FT(2 * CGAL_PI) : phi;
|
|
proj = (proj < FT(-1.0)) ? FT(-1.0) : ((proj > FT(1.0)) ? FT(1.0) : proj);
|
|
FT r = FT(acos(double(CGAL::abs(proj)))) / FT(CGAL_M_PI_2);
|
|
|
|
FT a = 0, b = 0;
|
|
if (phi < FT(CGAL_M_PI_4)) {
|
|
a = r;
|
|
b = phi * r / FT(CGAL_M_PI_4);
|
|
}
|
|
else if (phi < FT(3.0 * CGAL_M_PI_4)) {
|
|
a = -FT(phi - CGAL_M_PI_2) * r / FT(CGAL_M_PI_4);
|
|
b = r;
|
|
}
|
|
else if (phi < FT(5.0 * CGAL_M_PI_4)) {
|
|
a = -r;
|
|
b = (phi - FT(CGAL_PI)) * (-r) / FT(CGAL_M_PI_4);
|
|
}
|
|
else {
|
|
a = (phi - 3 * FT(CGAL_M_PI_2)) * r / FT(CGAL_M_PI_4);
|
|
b = -r;
|
|
}
|
|
|
|
x = a;
|
|
y = b;
|
|
|
|
// Map into hemisphere
|
|
if (proj >= 0)
|
|
y += 1;
|
|
else
|
|
y = -1 - y;
|
|
|
|
// Scale to surface distance
|
|
x = FT(x * CGAL_M_PI_2 * rad);
|
|
y = FT(y * CGAL_M_PI_2 * rad);
|
|
}
|
|
|
|
virtual void parameters(const std::vector<std::size_t> &indices,
|
|
std::vector<std::pair<FT, FT> > ¶meterSpace,
|
|
FT &cluster_epsilon,
|
|
FT min[2],
|
|
FT max[2]) const {
|
|
Vector_3 axis = this->constr_vec();
|
|
FT rad = radius();
|
|
// Take average normal as axis
|
|
for (std::size_t i = 0;i<indices.size();i++)
|
|
axis = this->sum_vectors(axis, this->normal(indices[i]));
|
|
axis = this->scale(axis, FT(1) / CGAL::sqrt(this->sqlen(axis)));
|
|
|
|
// create basis d1, d2
|
|
Vector_3 d1 = this->constr_vec(
|
|
ORIGIN, this->constr_pt(FT(0), FT(0), FT(1)));
|
|
Vector_3 d2 = this->cross_pdct(axis, d1);
|
|
FT l = this->sqlen(d2);
|
|
if (l < (FT)0.0001) {
|
|
d1 = this->constr_vec(ORIGIN, this->constr_pt(FT(1), FT(0), FT(0)));
|
|
d2 = this->cross_pdct(axis, d1);
|
|
l = this->sqlen(d2);
|
|
}
|
|
d2 = this->scale(d2, FT(1) / CGAL::sqrt(l));
|
|
|
|
d1 = this->cross_pdct(axis, d2);
|
|
l = CGAL::sqrt(this->sqlen(d1));
|
|
if (l == 0)
|
|
return;
|
|
|
|
d1 = this->scale(d1, (FT)1.0 / l);
|
|
|
|
// Process first point separately to initialize min/max
|
|
Vector_3 vec = this->constr_vec(
|
|
this->sph_center(m_sphere), this->point(indices[0]));
|
|
|
|
// sign indicates northern or southern hemisphere
|
|
FT proj = (this->scalar_pdct(axis, vec)) / rad;
|
|
FT phi = atan2(this->scalar_pdct(vec, d2), this->scalar_pdct(vec, d1));
|
|
FT x = FT(0), y = FT(0);
|
|
concentric_mapping(phi, proj, rad, x, y);
|
|
CGAL_assertion( x==x && y==y); // check not nan's
|
|
|
|
min[0] = max[0] = x;
|
|
min[1] = max[1] = y;
|
|
|
|
parameterSpace[0] = std::pair<FT, FT>(x, y);
|
|
|
|
for (std::size_t i = 1;i<indices.size();i++) {
|
|
Vector_3 vec = this->constr_vec(
|
|
this->sph_center(m_sphere), this->point(indices[i]));
|
|
// sign indicates northern or southern hemisphere
|
|
proj = (this->scalar_pdct(axis, vec)) / rad;
|
|
phi = atan2(this->scalar_pdct(vec, d2), this->scalar_pdct(vec, d1));
|
|
|
|
concentric_mapping(phi, proj, rad, x, y);
|
|
CGAL_assertion( x==x && y==y); // check not nan's
|
|
|
|
min[0] = (std::min<FT>)(min[0], x);
|
|
max[0] = (std::max<FT>)(max[0], x);
|
|
|
|
min[1] = (std::min<FT>)(min[1], y);
|
|
max[1] = (std::max<FT>)(max[1], y);
|
|
|
|
parameterSpace[i] = std::pair<FT, FT>(x, y);
|
|
}
|
|
|
|
// Is close to wrapping around? Check all three directions separately
|
|
m_wrap_right = abs(max[0] - CGAL_M_PI_2 * rad) < (cluster_epsilon * 0.5);
|
|
m_wrap_left = abs(min[0] + CGAL_M_PI_2 * rad) < (cluster_epsilon * 0.5);
|
|
|
|
FT diff_top = CGAL::abs(-FT(CGAL_PI * rad) - min[1])
|
|
+ FT(CGAL_PI * rad) - max[1];
|
|
m_wrap_top = diff_top < cluster_epsilon;
|
|
|
|
if (m_wrap_top || m_wrap_left || m_wrap_right) {
|
|
FT fl = FT(floor((CGAL_PI * rad) / cluster_epsilon));
|
|
|
|
if (fl > 0.9) {
|
|
FT adjusted_cf = FT(CGAL_PI * rad) / fl;
|
|
|
|
if ( (adjusted_cf < (2 * cluster_epsilon)))
|
|
cluster_epsilon = adjusted_cf;
|
|
}
|
|
|
|
// center bitmap at equator
|
|
FT required_space = ceil(
|
|
(std::max<FT>)(CGAL::abs(min[1]), max[1]) / cluster_epsilon)
|
|
* cluster_epsilon;
|
|
min[1] = -required_space;
|
|
max[1] = required_space;
|
|
}
|
|
|
|
m_equator = std::size_t((abs(min[1])) / cluster_epsilon - 0.5);
|
|
}
|
|
|
|
virtual void post_wrap(const std::vector<unsigned int> &bitmap,
|
|
const std::size_t &u_extent,
|
|
const std::size_t &v_extent,
|
|
std::vector<unsigned int> &labels) const {
|
|
unsigned int l;
|
|
unsigned int nw, n, ne;
|
|
if (m_wrap_top && v_extent > 2) {
|
|
// Handle first index separately.
|
|
l = bitmap[0];
|
|
if (l) {
|
|
n = bitmap[(v_extent - 1) * u_extent];
|
|
|
|
if (u_extent == 1) {
|
|
if (n && l != n) {
|
|
l = (std::min<unsigned int>)(n, l);
|
|
update_label(labels, (std::max<unsigned int>)(n, l), l);
|
|
return;
|
|
}
|
|
}
|
|
|
|
ne = bitmap[(v_extent - 1) * u_extent + 1];
|
|
|
|
if (n && n != l) {
|
|
l = (std::min<unsigned int>)(n, l);
|
|
update_label(labels, (std::max<unsigned int>)(n, l), l);
|
|
}
|
|
else if (ne && ne != l) {
|
|
l = (std::min<unsigned int>)(ne, l);
|
|
update_label(labels, (std::max<unsigned int>)(ne, l), l);
|
|
}
|
|
}
|
|
|
|
for (std::size_t i = 1;i<u_extent - 1;i++) {
|
|
l = bitmap[i];
|
|
if (!l)
|
|
continue;
|
|
|
|
nw = bitmap[(v_extent - 1) * u_extent + i - 1];
|
|
n = bitmap[(v_extent - 1) * u_extent + i];
|
|
ne = bitmap[(v_extent - 1) * u_extent + i + 1];
|
|
|
|
if (nw && nw != l) {
|
|
l = (std::min<unsigned int>)(nw, l);
|
|
update_label(labels, (std::max<unsigned int>)(nw, l), l);
|
|
}
|
|
if (n && n != l) {
|
|
l = (std::min<unsigned int>)(n, l);
|
|
update_label(labels, (std::max<unsigned int>)(n, l), l);
|
|
}
|
|
else if (ne && ne != l) {
|
|
l = (std::min<unsigned int>)(ne, l);
|
|
update_label(labels, (std::max<unsigned int>)(ne, l), l);
|
|
}
|
|
}
|
|
|
|
// Handle last index separately
|
|
l = bitmap[u_extent - 1];
|
|
if (l) {
|
|
n = bitmap[u_extent * v_extent - 1];
|
|
nw = bitmap[u_extent * v_extent - 2];
|
|
|
|
if (n && n != l) {
|
|
l = (std::min<unsigned int>)(n, l);
|
|
update_label(labels, (std::max<unsigned int>)(n, l), l);
|
|
}
|
|
else if (nw && nw != l) {
|
|
l = (std::min<unsigned int>)(nw, l);
|
|
update_label(labels, (std::max<unsigned int>)(nw, l), l);
|
|
}
|
|
}
|
|
}
|
|
|
|
// Walk upwards on the right side in the northern hemisphere
|
|
if (m_wrap_right && v_extent > 2) {
|
|
// First index
|
|
l = bitmap[(m_equator + 1) * u_extent - 1];
|
|
unsigned int ws = bitmap[(m_equator + 3) * u_extent - 1];
|
|
|
|
if (l && ws && l != ws) {
|
|
l = (std::min<unsigned int>)(ws, l);
|
|
update_label(labels, (std::max<unsigned int>)(ws, l), l);
|
|
}
|
|
|
|
for (std::size_t i = 1;i<(v_extent>>1) - 1;i++) {
|
|
l = bitmap[(m_equator - i + 1) * u_extent - 1];
|
|
if (!l)
|
|
continue;
|
|
|
|
unsigned int wn = bitmap[(m_equator + i) * u_extent - 1];
|
|
unsigned int w = bitmap[(m_equator + i + 1) * u_extent - 1];
|
|
ws = bitmap[(m_equator + i + 2) * u_extent - 1];
|
|
|
|
if (wn && wn != l) {
|
|
l = (std::min<unsigned int>)(wn, l);
|
|
update_label(labels, (std::max<unsigned int>)(wn, l), l);
|
|
}
|
|
if (w && w != l) {
|
|
l = (std::min<unsigned int>)(w, l);
|
|
update_label(labels, (std::max<unsigned int>)(w, l), l);
|
|
}
|
|
else if (ws && ws != l) {
|
|
l = (std::min<unsigned int>)(ws, l);
|
|
update_label(labels, (std::max<unsigned int>)(ws, l), l);
|
|
}
|
|
}
|
|
|
|
// Last index
|
|
l = bitmap[u_extent - 1];
|
|
if (l) {
|
|
unsigned int w = bitmap[u_extent * v_extent - 1];
|
|
unsigned int wn = bitmap[(v_extent - 1) * u_extent - 1];
|
|
|
|
if (w && w != l) {
|
|
l = (std::min<unsigned int>)(w, l);
|
|
update_label(labels, (std::max<unsigned int>)(w, l), l);
|
|
}
|
|
else if (wn && wn != l) {
|
|
l = (std::min<unsigned int>)(wn, l);
|
|
update_label(labels, (std::max<unsigned int>)(wn, l), l);
|
|
}
|
|
}
|
|
}
|
|
|
|
if (m_wrap_left && v_extent > 2) {
|
|
// First index
|
|
l = bitmap[(m_equator) * u_extent];
|
|
unsigned int es = bitmap[(m_equator + 2) * u_extent];
|
|
|
|
if (l && l != es) {
|
|
l = (std::min<unsigned int>)(es, l);
|
|
update_label(labels, (std::max<unsigned int>)(es, l), l);
|
|
}
|
|
|
|
for (std::size_t i = 1;i<(v_extent>>1) - 1;i++) {
|
|
l = bitmap[(m_equator - i) * u_extent];
|
|
if (!l)
|
|
continue;
|
|
|
|
unsigned int en = bitmap[(m_equator + i) * u_extent];
|
|
unsigned int e = bitmap[(m_equator + i + 1) * u_extent];
|
|
es = bitmap[(m_equator + i + 2) * u_extent];
|
|
|
|
if (en && en != l) {
|
|
l = (std::min<unsigned int>)(en, l);
|
|
update_label(labels, (std::max<unsigned int>)(en, l), l);
|
|
}
|
|
if (e && e != l) {
|
|
l = (std::min<unsigned int>)(e, l);
|
|
update_label(labels, (std::max<unsigned int>)(e, l), l);
|
|
}
|
|
else if (es && es != l) {
|
|
l = (std::min<unsigned int>)(es, l);
|
|
update_label(labels, (std::max<unsigned int>)(es, l), l);
|
|
}
|
|
}
|
|
|
|
// Last index
|
|
l = bitmap[0];
|
|
if (l) {
|
|
unsigned int w = bitmap[(v_extent - 1) * u_extent];
|
|
unsigned int wn = bitmap[(v_extent - 2) * u_extent];
|
|
|
|
if (w && w != l) {
|
|
l = (std::min<unsigned int>)(w, l);
|
|
update_label(labels, (std::max<unsigned int>)(w, l), l);
|
|
}
|
|
else if (wn && wn != l) {
|
|
l = (std::min<unsigned int>)(wn, l);
|
|
update_label(labels, (std::max<unsigned int>)(wn, l), l);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
virtual bool supports_connected_component() const {
|
|
return true;
|
|
}
|
|
|
|
private:
|
|
Sphere_3 m_sphere;
|
|
mutable bool m_wrap_right, m_wrap_top, m_wrap_left;
|
|
mutable std::size_t m_equator;
|
|
/// \endcond
|
|
};
|
|
}
|
|
}
|
|
#endif
|