CGAL 5.6.1 - Shape Detection
Shape_detection/include/efficient_RANSAC_with_custom_shape.h
#ifndef MY_PLANE_SHAPE_H
#define MY_PLANE_SHAPE_H
#include <CGAL/number_utils.h>
// My_Plane is derived from Shape_base. The plane is represented by
// its normal vector and distance to the origin.
template <class Traits>
class My_Plane : public CGAL::Shape_detection::Shape_base<Traits> {
public:
typedef typename Traits::FT FT; // number type
typedef typename Traits::Point_3 Point; // point type
typedef typename Traits::Vector_3 Vector; // vector type
My_Plane() :
{ }
// Compute squared Euclidean distance from query point to the shape.
virtual FT squared_distance(const Point& p) const {
const FT sd = (this->constr_vec(m_point_on_primitive, p)) * m_normal;
return sd * sd;
}
Vector plane_normal() const {
return m_normal;
}
FT d() const {
return m_d;
}
// Return a string with shape parameters.
virtual 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();
}
protected:
// Construct shape base on a minimal set of samples from the input data.
virtual void create_shape(const std::vector<std::size_t>& indices) {
const Point p1 = this->point(indices[0]);
const Point p2 = this->point(indices[1]);
const Point p3 = this->point(indices[2]);
m_normal = this->cross_pdct(p1 - p2, p1 - p3);
m_normal = m_normal * (1.0 / sqrt(this->sqlen(m_normal)));
m_d = -(p1[0] * m_normal[0] + p1[1] * m_normal[1] + p1[2] * m_normal[2]);
m_point_on_primitive = p1;
this->m_is_valid = true;
}
// Compute squared Euclidean distance from a set of points.
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 sd = (this->point(indices[i]) - m_point_on_primitive) * m_normal;
dists[i] = sd * sd;
}
}
// Compute the normal deviation between a shape and
// a set of points with normals.
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->normal(indices[i]) * m_normal);
}
// Return the number of required samples for construction.
virtual std::size_t minimum_sample_size() const {
return 3;
}
private:
Point m_point_on_primitive;
Vector m_normal;
FT m_d;
};
#endif // MY_PLANE_SHAPE_H