songsenand
/
cgal
mirror of https://github.com/CGAL/cgal
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
cgal/Point_set_shape_detection_3/include/CGAL/Cylinder.h

259 lines
9.2 KiB
C++
Raw Blame History

#ifndef CGAL_EFFICIENT_RANSAC_CYLINDER_H
#define CGAL_EFFICIENT_RANSAC_CYLINDER_H
#include "Primitive.h"
#include <set>
#include <math.h>
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif
#ifndef M_PI_2
#define M_PI_2 1.57079632679489661923
#endif
namespace CGAL {
namespace Efficient_ransac {
template <typename Kernel, class inputDataType>
class Cylinder : public Primitive_ab<Kernel, inputDataType>
{
public:
typedef typename Kernel::FT FT;
typedef typename Kernel::Line_3 Line;
typedef typename Kernel::Point_3 Point;
typedef typename Kernel::Vector_3 Vector;
typedef typename Kernel::Plane_3 Plane_3;
typedef typename Kernel::Sphere_3 Sphere_3;
FT m_radius;
Line m_axis;
Point m_point_on_axis;
public:
Cylinder() : Primitive_ab<Kernel, inputDataType>(0.1, 0.9) {m_type = CYLINDER; m_type_name ="Cylinder";}
Cylinder(FT _a, FT _b) : Primitive_ab<Kernel, inputDataType>(_a, _b) {m_type = CYLINDER;m_type_name ="Cylinder";}
void compute(std::set<int> &l_list_index_selected, InputConstIterator &m_it_Point_Normal) {
if ( l_list_index_selected.size() < 3) {
m_isValid = false;
return;
}
std::vector<int> output(l_list_index_selected.begin(), l_list_index_selected.end());
Point p1 = (m_it_Point_Normal + output[0])->first;
Vector n1 = (m_it_Point_Normal + output[0])->second;
Point p2 = (m_it_Point_Normal + output[1])->first;
Vector n2 = (m_it_Point_Normal + output[1])->second;
Point p3 = (m_it_Point_Normal + output[2])->first;
Vector n3 = (m_it_Point_Normal + output[2])->second;
Vector axis = CGAL::cross_product((m_it_Point_Normal + output[0])->second, (m_it_Point_Normal + output[1])->second);
FT axisL = sqrt(axis.squared_length());
if (axisL < 0.001) {
m_isValid = false;
return;
}
axis = axis * (1.0 / axisL);
// establish two directions in the plane axis * x = 0, whereas xDir is the projected n1
Vector xDir = (m_it_Point_Normal + output[0])->second - ((m_it_Point_Normal + output[0])->second * axis) * axis;
xDir = xDir * (1.0 / sqrt(xDir.squared_length()));
Vector yDir = CGAL::cross_product(axis, xDir);
yDir = yDir * (1.0 / sqrt(yDir.squared_length()));
FT n2x = (m_it_Point_Normal + output[1])->second * yDir;
FT n2y = -((m_it_Point_Normal + output[1])->second * xDir);
Vector dist = (m_it_Point_Normal + output[1])->first - (m_it_Point_Normal + output[0])->first;
FT Ox = xDir * dist;
FT Oy = yDir * dist;
FT lineDist = n2x * Ox + n2y * Oy;
m_radius = lineDist / n2x;
m_point_on_axis = (m_it_Point_Normal + output[0])->first + m_radius * xDir;
m_radius = abs(m_radius);
m_axis = Line(m_point_on_axis, axis);
if (squared_distance((m_it_Point_Normal + output[0])->first) > m_epsilon || (cos_to_normal((m_it_Point_Normal + output[0])->first, (m_it_Point_Normal + output[0])->second) < m_normalThresh)) {
m_isValid = false;
return;
}
/* FT xDist = dist * n2y;
Point c = p1 + xDist * xDir;
Point proj1 = (m_it_Point_Normal + output[0])->first - (((m_it_Point_Normal + output[0])->first - CGAL::ORIGIN) * axis) * axis;
Point proj2 = (m_it_Point_Normal + output[1])->first - (((m_it_Point_Normal + output[1])->first - CGAL::ORIGIN) * axis) * axis;
*/
}
std::string info() {
std::stringstream sstr;
Point c = m_axis.point();
Vector a = m_axis.to_vector();
sstr << "Type: " << m_type_name << " c: (" << c.x() << ", " << c.y() << ", " << c.z() << ") a: (" << a.x() << ", " << a.y() << ", " << a.z() << ") r:" << m_radius
<< " ev: " << ExpectedValue() << " s: " << m_nb_subset_used << " #Pts: " << m_indices.size() << std::endl;
return sstr.str();
}
std::string type_str() const {return m_type_name;}
void parameters(InputConstIterator first, std::vector<std::pair<FT, FT>> &parameterSpace, const std::vector<int> &indices, FT min[2], FT max[2]) const {
Vector d1 = Vector(0, 0, 1);
Vector a = m_axis.to_vector();
a = a * (1.0 / sqrt(a.squared_length()));
Vector d2 = CGAL::cross_product(a, d1);
FT l = d2.squared_length();
if (l < 0.0001) {
d1 = Vector(1, 0, 0);
d2 = CGAL::cross_product(m_axis.to_vector(), d1);
l = d2.squared_length();
if (l < 0.0001) {
std::cout << "Cylinder::pointOnPrimitive() construction failed!" << std::endl;
}
}
d1 = CGAL::cross_product(m_axis.to_vector(), d2);
d1 = d1 * (1.0 / sqrt(d1.squared_length()));
// first one separate for initializing min/max
Vector vec = first[indices[0]].first - m_point_on_axis;
FT v = vec * a;
vec = vec - ((vec * a) * a);
vec = vec * (1.0 / sqrt(vec.squared_length()));
FT a1 = acos(vec * d1);
FT a2 = acos(vec * d2);
FT u = (a2 < M_PI_2) ? 2 * M_PI - a1 : a1;
parameterSpace[0] = std::pair<FT, FT>(u, v);
min[0] = max[0] = u;
min[1] = max[1] = v;
for (unsigned int i = 0;i<indices.size();i++) {
Vector vec = first[indices[i]].first - m_point_on_axis;
FT v = vec * a;
vec = vec - ((vec * a) * a);
vec = vec * (1.0 / sqrt(vec.squared_length()));
FT a1 = acos(vec * d1);
FT a2 = acos(vec * d2);
FT u = (a2 < M_PI_2) ? 2 * M_PI - a1 : a1;
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);
}
}
void parameterExtend(const Point &center, FT width, FT min[2], FT max[2]) const {
//V length of axis in box? not enough
FT maxLambda = std::numeric_limits<double>::max(), minLambda = -std::numeric_limits<double>::max();
Vector a = m_axis.to_vector();
Point p = m_point_on_axis;
for (unsigned int i = 0;i<3;i++) {
if (abs(a[i]) > 0.001) {
FT l1 = (center[i] + width + m_radius - p[i]) / a[i];
FT l2 = (center[i] - width - m_radius - p[i]) / a[i];
if (l1 * l2 > 0) {
std::cout << "Cylinder::parameterExtend(): dim 0, l1*l2 > 0" << std::endl;
}
minLambda = std::max<FT>(minLambda, std::min<FT>(l1, l2));
maxLambda = std::min<FT>(maxLambda, std::max<FT>(l1, l2));
}
}
min[1] = minLambda;
max[1] = maxLambda;
//U circumfence
min[0] = 0;
max[0] = (2 * M_PI * m_radius);
}
Point pointOnPrimitive() const {
Vector d1 = Vector(0, 0, 1);
Vector d2 = Vector::cross_product(m_axis.to_vector(), d1);
FT l = d2.squared_length();
if (l < 0.0001) {
d1 = Vector(1, 0, 0);
d2 = Vector::cross_product(m_axis.to_vector(), d1);
l = d2.squared_length();
if (l < 0.0001) {
std::cout << "Cylinder::pointOnPrimitive() construction failed!" << std::endl;
}
}
return m_point_on_axis + d2 * m_radius / sqrt(l);
}
FT squared_distance(const Point &_p) const {
Vector a = m_axis.to_vector();
a = a * (1.0 / sqrt(a.squared_length()));
Vector v = _p - m_point_on_axis;
v = v - ((v * a) * a);
FT d = sqrt(v.squared_length()) - m_radius;
return d * d;
}
void squared_distance(InputConstIterator first, std::vector<FT> &dists, const std::vector<int> &shapeIndex, const std::vector<unsigned int> &indices) {
Vector a = m_axis.to_vector();
a = a * (1.0 / sqrt(a.squared_length()));
for (unsigned int i = 0;i<indices.size();i++) {
if (shapeIndex[indices[i]] == -1) {
Vector v = first[indices[i]].first - m_point_on_axis;
v = v - ((v * a) * a);
dists[i] = sqrt(v.squared_length()) - m_radius;
dists[i] = dists[i] * dists[i];
}
}
}
void cos_to_normal(InputConstIterator first, std::vector<FT> &angles, const std::vector<int> &shapeIndex, const std::vector<unsigned int> &indices) const {
Vector a = m_axis.to_vector();
a = a * (1.0 / sqrt(a.squared_length()));
for (unsigned int i = 0;i<indices.size();i++) {
if (shapeIndex[indices[i]] == -1) {
Vector v = first[indices[i]].first - m_point_on_axis;
v = v - ((v * a) * a);
v = v * (1.0 / sqrt(v.squared_length()));
angles[i] = abs(v * first[indices[i]].second);
}
}
}
FT cos_to_normal(const Point &_p, const Vector &_n) const {
Vector a = m_axis.to_vector();
a = a * (1.0 / sqrt(a.squared_length()));
Vector v = _p - m_point_on_axis;
v = v - ((v * a) * a);
v = v * (1.0 / sqrt(v.squared_length()));
return abs(v * _n);
}
virtual bool supportsConnectedComponent() {return true;}
// U is longitude
virtual bool wrapsU() const {return true;}
// V is between caps
virtual bool wrapsV() const {return false;}
};
}
}
#endif
Reference in New Issue View Git Blame Copy Permalink