244 lines
7.6 KiB
C++
Executable File
244 lines
7.6 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_PLANE_H
|
|
#define CGAL_SHAPE_DETECTION_3_PLANE_H
|
|
|
|
#include <CGAL/license/Point_set_shape_detection_3.h>
|
|
|
|
|
|
#include <CGAL/Shape_detection_3/Shape_base.h>
|
|
#include <CGAL/number_utils.h>
|
|
|
|
/*!
|
|
\file Plane.h
|
|
*/
|
|
namespace CGAL {
|
|
namespace Shape_detection_3 {
|
|
/*!
|
|
\ingroup PkgPointSetShapeDetection3Shapes
|
|
\brief Plane implements Shape_base. The plane is represented by the normal vector and the distance to the origin.
|
|
\tparam Traits a model of `EfficientRANSACTraits` with the additional
|
|
requirement for planes (see `EfficientRANSACTraits` documentation).
|
|
*/
|
|
template <class Traits>
|
|
class Plane : public Shape_base<Traits> {
|
|
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::FT FT; ///< number type.
|
|
typedef typename Traits::Point_3 Point_3; ///< point type.
|
|
typedef typename Traits::Point_2 Point_2; ///< point 2D type.
|
|
typedef typename Traits::Vector_3 Vector_3;
|
|
/// \endcond
|
|
|
|
typedef typename Traits::Plane_3 Plane_3;///< plane type for conversion operator.
|
|
|
|
public:
|
|
Plane() : Shape_base<Traits>() {}
|
|
|
|
/*!
|
|
Conversion operator to `Plane_3` type.
|
|
*/
|
|
operator Plane_3() const {
|
|
return Plane_3(this->get_x(m_normal), this->get_y(m_normal), this->get_z(m_normal), m_d);
|
|
}
|
|
|
|
/*!
|
|
Normal vector of the plane.
|
|
*/
|
|
Vector_3 plane_normal() const {
|
|
return m_normal;
|
|
}
|
|
|
|
/*!
|
|
Signed distance from the origin.
|
|
*/
|
|
FT d() const {
|
|
return m_d;
|
|
}
|
|
|
|
/// \cond SKIP_IN_MANUAL
|
|
/*!
|
|
Computes squared Euclidean distance from query point to the shape.
|
|
*/
|
|
FT squared_distance(const Point_3 &p) const {
|
|
FT d = this->scalar_pdct(
|
|
this->constr_vec(p, m_point_on_primitive), m_normal);
|
|
return d * d;
|
|
}
|
|
|
|
/*!
|
|
Computes the orthogonal projection of a query point on the shape.
|
|
*/
|
|
Point_3 projection (const Point_3& p) const {
|
|
return to_3d (to_2d (p));
|
|
}
|
|
|
|
Point_2 to_2d (const Point_3& p) const {
|
|
Vector_3 v (m_point_on_primitive, p);
|
|
return Point_2 (v * m_base1, v * m_base2);
|
|
}
|
|
|
|
Point_3 to_3d (const Point_2& p) const {
|
|
return m_point_on_primitive + p.x () * m_base1 + p.y () * m_base2;
|
|
}
|
|
|
|
/*!
|
|
Replaces the plane by p
|
|
*/
|
|
void update (const Plane_3& p) {
|
|
m_base1 = p.base1 () / std::sqrt (p.base1() * p.base1 ());
|
|
m_base2 = p.base2 () / std::sqrt (p.base2() * p.base2 ());
|
|
m_normal = p.orthogonal_vector () / std::sqrt (p.orthogonal_vector () * p.orthogonal_vector ());
|
|
|
|
m_d = p.d();
|
|
|
|
}
|
|
|
|
/*!
|
|
Helper function to write the plane equation and
|
|
number of assigned points into a string.
|
|
*/
|
|
std::string info() const {
|
|
std::stringstream sstr;
|
|
sstr << "Type: plane (" << this->get_x(m_normal) << ", " << this->get_y(m_normal)
|
|
<< ", " << this->get_z(m_normal) << ")x - " << m_d << "= 0"
|
|
<< " #Pts: " << this->m_indices.size();
|
|
|
|
return sstr.str();
|
|
}
|
|
/// \endcond
|
|
|
|
protected:
|
|
/// \cond SKIP_IN_MANUAL
|
|
virtual 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]);
|
|
|
|
m_normal = this->cross_pdct(
|
|
this->constr_vec(p2, p1), this->constr_vec(p3, p1));
|
|
|
|
FT length = CGAL::sqrt(this->sqlen(m_normal));
|
|
|
|
// Are the points almost singular?
|
|
if (length < (FT)0.0001) {
|
|
return;
|
|
}
|
|
|
|
m_normal = this->scale(m_normal, (FT)1.0 / length);
|
|
m_d = -(this->get_x(p1) * this->get_x(m_normal)
|
|
+ this->get_y(p1) * this->get_y(m_normal)
|
|
+ this->get_z(p1) * this->get_z(m_normal));
|
|
|
|
//check deviation of the 3 normal
|
|
Vector_3 l_v = this->constr_vec();
|
|
for (std::size_t i = 0;i<3;i++) {
|
|
l_v = this->normal(indices[i]);
|
|
|
|
if (CGAL::abs(this->scalar_pdct(l_v, m_normal))
|
|
< this->m_normal_threshold * CGAL::sqrt(this->sqlen(l_v))) {
|
|
this->m_is_valid = false;
|
|
return;
|
|
}
|
|
|
|
m_point_on_primitive = p1;
|
|
m_base1 = this->cross_pdct(this->constr_vec(p2, p1), m_normal);
|
|
m_base1 = this->scale(m_base1, ((FT)1.0 / CGAL::sqrt(this->sqlen(m_base1))));
|
|
|
|
m_base2 = this->cross_pdct(m_base1, m_normal);
|
|
m_base2 = this->scale(m_base2, ((FT)1.0 / CGAL::sqrt(this->sqlen(m_base2))));
|
|
}
|
|
|
|
this->m_is_valid = true;
|
|
}
|
|
|
|
virtual void parameters(const std::vector<std::size_t> &indices,
|
|
std::vector<std::pair<FT, FT> > ¶meterSpace,
|
|
FT &,
|
|
FT min[2],
|
|
FT max[2]) const {
|
|
// Transform first point before to initialize min/max
|
|
Vector_3 p = this->constr_vec(
|
|
m_point_on_primitive, this->point(indices[0]));
|
|
FT u = this->scalar_pdct(p, m_base1);
|
|
FT v = this->scalar_pdct(p, m_base2);
|
|
parameterSpace[0] = std::pair<FT, FT>(u, v);
|
|
min[0] = max[0] = u;
|
|
min[1] = max[1] = v;
|
|
|
|
for (std::size_t i = 1;i<indices.size();i++) {
|
|
p = this->constr_vec(m_point_on_primitive, this->point(indices[i]));
|
|
u = this->scalar_pdct(p, m_base1);
|
|
v = this->scalar_pdct(p, m_base2);
|
|
min[0] = (std::min<FT>)(min[0], u);
|
|
max[0] = (std::max<FT>)(max[0], u);
|
|
min[1] = (std::min<FT>)(min[1], v);
|
|
max[1] = (std::max<FT>)(max[1], v);
|
|
parameterSpace[i] = std::pair<FT, FT>(u, v);
|
|
}
|
|
}
|
|
|
|
virtual void squared_distance(const std::vector<std::size_t> &indices,
|
|
std::vector<FT> &dists) const {
|
|
for (std::size_t i = 0;i<indices.size();i++) {
|
|
const FT d = this->scalar_pdct(
|
|
this->constr_vec(m_point_on_primitive, this->point(indices[i])),
|
|
m_normal);
|
|
dists[i] = d * d;
|
|
}
|
|
}
|
|
|
|
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++) {
|
|
angles[i] = CGAL::abs(
|
|
this->scalar_pdct(this->normal(indices[i]), m_normal));
|
|
}
|
|
}
|
|
|
|
FT cos_to_normal(const Point_3 &, const Vector_3 &n) const{
|
|
return CGAL::abs(this->scalar_pdct(n, m_normal));
|
|
}
|
|
|
|
virtual std::size_t minimum_sample_size() const {
|
|
return 3;
|
|
}
|
|
|
|
virtual bool supports_connected_component() const {
|
|
return true;
|
|
}
|
|
|
|
private:
|
|
Point_3 m_point_on_primitive;
|
|
Vector_3 m_base1, m_base2, m_normal;
|
|
FT m_d;
|
|
/// \endcond
|
|
};
|
|
}
|
|
}
|
|
#endif
|