1 // Copyright (c) 2015 INRIA Sophia-Antipolis (France). 2 // All rights reserved. 3 // 4 // This file is part of CGAL (www.cgal.org). 5 // 6 // $URL: https://github.com/CGAL/cgal/blob/v5.3/Shape_detection/include/CGAL/Shape_detection/Efficient_RANSAC/Cylinder.h $ 7 // $Id: Cylinder.h 0779373 2020-03-26T13:31:46+01:00 Sébastien Loriot 8 // SPDX-License-Identifier: GPL-3.0-or-later OR LicenseRef-Commercial 9 // 10 // 11 // Author(s) : Sven Oesau, Yannick Verdie, Clément Jamin, Pierre Alliez 12 // 13 14 #ifndef CGAL_SHAPE_DETECTION_EFFICIENT_RANSAC_CYLINDER_H 15 #define CGAL_SHAPE_DETECTION_EFFICIENT_RANSAC_CYLINDER_H 16 17 #include <CGAL/license/Shape_detection.h> 18 19 #include <cmath> 20 #include <CGAL/number_utils.h> 21 #include <CGAL/Shape_detection/Efficient_RANSAC/Shape_base.h> 22 23 namespace CGAL { 24 namespace Shape_detection { 25 26 /*! 27 \brief Cylinder implements Shape_base. The cylinder is represented 28 by the axis, that is a point and direction, and the radius. The cylinder is 29 unbounded, thus caps are not modeled. 30 31 \tparam Traits must be a model of `EfficientRANSACTraits` with the additional 32 requirement for cylinders (see `EfficientRANSACTraits` documentation). 33 34 \ingroup PkgShapeDetectionRANSACShapes 35 */ 36 template <class Traits> 37 class Cylinder : public Shape_base<Traits> { 38 using Shape_base<Traits>::update_label; 39 40 public: 41 /// \cond SKIP_IN_MANUAL 42 typedef typename Traits::Point_map Point_map; 43 ///< property map to access the location of an input point. 44 typedef typename Traits::Normal_map Normal_map; 45 ///< property map to access the unoriented normal of an input point. 46 typedef typename Traits::Vector_3 Vector_3; ///< vector type. 47 typedef typename Traits::Point_3 Point_3; ///< point type. 48 typedef typename Traits::FT FT; ///< number type. 49 /// \endcond 50 51 typedef typename Traits::Line_3 Line_3; ///< Line type 52 53 public: Cylinder()54 Cylinder() : Shape_base<Traits>() {} 55 56 /*! 57 Axis of the cylinder. 58 */ axis()59 Line_3 axis() const { 60 return m_axis; 61 } 62 63 /*! 64 Radius of the cylinder. 65 */ radius()66 FT radius() const { 67 return m_radius; 68 } 69 70 /// \cond SKIP_IN_MANUAL 71 72 /*! 73 Helper function to write axis and radius of the cylinder and number of assigned points into a string. 74 */ info()75 std::string info() const { 76 std::stringstream sstr; 77 Point_3 c = this->constr_point_on(m_axis); 78 Vector_3 a = this->constr_vec(m_axis); 79 80 sstr << "Type: cylinder center: (" << this->get_x(c) << ", " << this->get_y(c) << ", " << this->get_z(c) << ") axis: (" << this->get_x(a) << ", " << this->get_y(a) << ", " << this->get_z(a) << ") radius:" << m_radius 81 << " #Pts: " << this->m_indices.size(); 82 83 return sstr.str(); 84 } 85 86 /*! 87 Computes squared Euclidean distance from query point to the shape. 88 */ squared_distance(const Point_3 & p)89 FT squared_distance(const Point_3 &p) const { 90 Vector_3 a = this->constr_vec(m_axis); 91 a = this->scale(a, (FT)1.0 / CGAL::sqrt(this->sqlen(a))); 92 93 Vector_3 v = this->constr_vec(m_point_on_axis, p); 94 v = this->sum_vectors(v, this->scale(a, -this->scalar_pdct(v, a))); 95 FT d = CGAL::sqrt(this->sqlen(v)) - m_radius; 96 97 return d * d; 98 } 99 /// \endcond 100 101 protected: 102 /// \cond SKIP_IN_MANUAL 103 104 // ------------------------------------------------------------------------ 105 // Utilities 106 // ------------------------------------------------------------------------ 107 using Shape_base<Traits>::constr_vec; constr_vec(const Line_3 & l)108 Vector_3 constr_vec(const Line_3& l) const 109 { return this->m_traits.construct_vector_3_object()(l); } constr_point_on(const Line_3 & l)110 Point_3 constr_point_on(const Line_3& l) const 111 { return this->m_traits.construct_point_on_3_object()(l, 0); } 112 create_shape(const std::vector<std::size_t> & indices)113 virtual void create_shape(const std::vector<std::size_t> &indices) { 114 Point_3 p1 = this->point(indices[0]); 115 Point_3 p2 = this->point(indices[1]); 116 117 Vector_3 n1 = this->normal(indices[0]); 118 Vector_3 n2 = this->normal(indices[1]); 119 120 Vector_3 axis = this->cross_pdct(n1, n2); 121 FT axisL = CGAL::sqrt(this->sqlen(axis)); 122 if (axisL < (FT)0.0001) { 123 return; 124 } 125 axis = this->scale(axis, FT(1.0) / axisL); 126 127 // establish two directions in the plane axis * x = 0, 128 // whereas xDir is the projected n1 129 Vector_3 xDir = this->sum_vectors( 130 n1, this->scale(axis, -this->scalar_pdct(n1, axis))); 131 xDir = this->scale(xDir, (FT)1.0 / CGAL::sqrt(this->sqlen(xDir))); 132 Vector_3 yDir = this->cross_pdct(axis, xDir); 133 yDir = this->scale(yDir, (FT)1.0 / CGAL::sqrt(this->sqlen(yDir))); 134 135 FT n2x = this->scalar_pdct(n2, yDir); 136 FT n2y = -this->scalar_pdct(n2, xDir); 137 138 Vector_3 dist = this->constr_vec(p1, p2); 139 140 FT Ox = this->scalar_pdct(xDir, dist); 141 FT Oy = this->scalar_pdct(yDir, dist); 142 143 FT lineDist = n2x * Ox + n2y * Oy; 144 145 m_radius = lineDist / n2x; 146 m_point_on_axis = this->transl(p1, this->scale(xDir, m_radius)); 147 m_radius = CGAL::abs(m_radius); 148 149 m_axis = this->m_traits.construct_line_3_object()(m_point_on_axis, axis); 150 151 if (squared_distance(p1) > this->m_epsilon || 152 (cos_to_normal(p1, n1) < this->m_normal_threshold)) 153 return; 154 155 this->m_is_valid = true; 156 this->m_wrap_u = true; 157 } 158 parameters(const std::vector<std::size_t> & indices,std::vector<std::pair<FT,FT>> & parameterSpace,FT & cluster_epsilon,FT min[2],FT max[2])159 virtual void parameters(const std::vector<std::size_t> &indices, 160 std::vector<std::pair<FT, FT> > ¶meterSpace, 161 FT &cluster_epsilon, 162 FT min[2], 163 FT max[2]) const { 164 Vector_3 d1 = this->constr_vec( 165 ORIGIN, this->constr_pt(FT(0), FT(0), FT(1))); 166 Vector_3 a = this->constr_vec(m_axis); 167 a = this->scale(a, (FT)1.0 / CGAL::sqrt(this->sqlen(a))); 168 169 Vector_3 d2 = this->cross_pdct(a, d1); 170 FT l = this->sqlen(d2); 171 if (l < (FT)0.0001) { 172 d1 = this->constr_vec(ORIGIN, this->constr_pt(FT(1), FT(0), FT(0))); 173 d2 = this->cross_pdct(this->constr_vec(m_axis), d1); 174 l = this->sqlen(d2); 175 } 176 d2 = this->scale(d2, FT(1) / CGAL::sqrt(l)); 177 178 d1 = this->cross_pdct(this->constr_vec(m_axis), d2); 179 FT length = CGAL::sqrt(this->sqlen(d1)); 180 if (length == 0) 181 return; 182 183 d1 = this->scale(d1, (FT)1.0 / length); 184 185 // first one separate for initializing min/max 186 Vector_3 vec = this->constr_vec(m_point_on_axis, this->point(indices[0])); 187 FT v = this->scalar_pdct(vec, a); 188 vec = this->sum_vectors(vec, this->scale(a, -this->scalar_pdct(vec, a))); 189 length = CGAL::sqrt(this->sqlen(vec)); 190 vec = this->scale(vec, (FT)1.0 / length); 191 192 FT a1 = this->scalar_pdct(vec, d1); 193 a1 = (a1 < (FT) -1.0) ? (FT) -1.0 : ((a1 > (FT) 1.0) ? (FT) 1.0 : a1); 194 a1 = acos(a1); 195 FT a2 = this->scalar_pdct(vec, d2); 196 a2 = (a2 < (FT) -1.0) ? (FT) -1.0 : ((a2 > (FT) 1.0) ? (FT) 1.0 : a2); 197 a2 = acos(a2); 198 199 FT u = FT((a2 < CGAL_M_PI_2) ? 2 * CGAL_PI - a1 : a1) * m_radius; 200 201 parameterSpace[0] = std::pair<FT, FT>(u, v); 202 203 min[0] = max[0] = u; 204 min[1] = max[1] = v; 205 206 for (std::size_t i = 0;i<indices.size();i++) { 207 vec = this->constr_vec(m_point_on_axis, this->point(indices[i])); 208 v = this->scalar_pdct(vec, a); 209 vec = this->sum_vectors(vec, this->scale(a, -this->scalar_pdct(vec, a))); 210 length = CGAL::sqrt(this->sqlen(vec)); 211 vec = this->scale(vec, (FT)1.0 / length); 212 213 a1 = this->scalar_pdct(vec, d1); 214 a1 = (a1 < (FT) -1.0) ? (FT) -1.0 : ((a1 > (FT) 1.0) ? (FT) 1.0 : a1); 215 a1 = acos(a1); 216 a2 = this->scalar_pdct(vec, d2); 217 a2 = (a2 < (FT) -1.0) ? (FT) -1.0 : ((a2 > (FT) 1.0) ? (FT) 1.0 : a2); 218 a2 = acos(a2); 219 220 u = FT((a2 < CGAL_M_PI_2) ? 2 * CGAL_PI - a1 : a1) * m_radius; 221 min[0] = (std::min<FT>)(min[0], u); 222 max[0] = (std::max<FT>)(max[0], u); 223 224 min[1] = (std::min<FT>)(min[1], v); 225 max[1] = (std::max<FT>)(max[1], v); 226 227 parameterSpace[i] = std::pair<FT, FT>(u, v); 228 } 229 230 // Is close to wrapping around? 231 FT diff_to_full_range = min[0] + FT(CGAL_PI * 2.0 * m_radius) - max[0]; 232 if (diff_to_full_range < cluster_epsilon) { 233 m_wrap_u = true; 234 FT frac = (max[0] - min[0]) / cluster_epsilon; 235 236 if (frac < 1) 237 return; 238 239 FT trunc = floor(frac); 240 frac = frac - trunc; 241 242 if (frac < (FT) 0.5) { 243 cluster_epsilon = (max[0] - min[0]) / (trunc * FT(0.99999)); 244 } 245 } 246 else m_wrap_u = false; 247 } 248 249 // The u coordinate corresponds to the rotation around the axis and 250 // therefore needs to be wrapped around. 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)251 virtual void post_wrap(const std::vector<unsigned int> &bitmap, 252 const std::size_t &u_extent, 253 const std::size_t &v_extent, 254 std::vector<unsigned int> &labels) const { 255 if (!m_wrap_u) 256 return; 257 258 // handle top index separately 259 unsigned int nw = bitmap[u_extent - 1]; 260 unsigned int l = bitmap[0]; 261 262 // Special case v_extent is just 1 263 if (v_extent == 1) { 264 if (nw && nw != l) { 265 l = (std::min<unsigned int>)(nw, l); 266 update_label(labels, (std::max<unsigned int>)(nw, l), l); 267 } 268 269 return; 270 } 271 272 unsigned int w = bitmap[2 * u_extent - 1]; 273 unsigned int sw; 274 275 if (l) { 276 if (nw && nw != l) { 277 l = (std::min<unsigned int>)(nw, l); 278 update_label(labels, (std::max<unsigned int>)(nw, l), l); 279 } 280 else if (w && w != l) { 281 l = (std::min<unsigned int>)(w, l); 282 update_label(labels, (std::max<unsigned int>)(w, l), l); 283 } 284 } 285 286 // handle mid indices 287 for (std::size_t y = 1;y<v_extent - 1;y++) { 288 l = bitmap[y * u_extent]; 289 if (!l) 290 continue; 291 292 nw = bitmap[y * u_extent - 1]; 293 w = bitmap[(y + 1) * u_extent - 1]; 294 sw = bitmap[(y + 2) * u_extent - 1]; 295 296 if (nw && nw != l) { 297 l = (std::min<unsigned int>)(nw, l); 298 update_label(labels, (std::max<unsigned int>)(nw, l), l); 299 } 300 if (w && w != l) { 301 l = (std::min<unsigned int>)(w, l); 302 update_label(labels, (std::max<unsigned int>)(w, l), l); 303 } 304 else if (sw && sw != l) { 305 l = (std::min<unsigned int>)(sw, l); 306 update_label(labels, (std::max<unsigned int>)(sw, l), l); 307 } 308 } 309 310 // handle last index 311 l = bitmap[(v_extent - 1) * u_extent]; 312 if (!l) 313 return; 314 315 nw = bitmap[(v_extent - 1) * u_extent - 1]; 316 w = bitmap[u_extent * v_extent - 1]; 317 318 if (nw && nw != l) { 319 l = (std::min<unsigned int>)(nw, l); 320 update_label(labels, (std::max<unsigned int>)(nw, l), l); 321 } 322 else if (w && w != l) { 323 l = (std::min<unsigned int>)(w, l); 324 update_label(labels, (std::max<unsigned int>)(w, l), l); 325 } 326 } 327 squared_distance(const std::vector<std::size_t> & indices,std::vector<FT> & dists)328 virtual void squared_distance(const std::vector<std::size_t> &indices, 329 std::vector<FT> &dists) const { 330 Vector_3 a = this->constr_vec(m_axis); 331 a = this->scale(a, (FT)1.0 / CGAL::sqrt(this->sqlen(a))); 332 for (std::size_t i = 0;i<indices.size();i++) { 333 Vector_3 v = this->constr_vec(m_point_on_axis, this->point(indices[i])); 334 335 v = this->sum_vectors(v, this->scale(a, -this->scalar_pdct(v, a))); 336 dists[i] = CGAL::sqrt(this->sqlen(v)) - m_radius; 337 dists[i] = dists[i] * dists[i]; 338 } 339 } 340 cos_to_normal(const std::vector<std::size_t> & indices,std::vector<FT> & angles)341 virtual void cos_to_normal(const std::vector<std::size_t> &indices, 342 std::vector<FT> &angles) const { 343 Vector_3 a = this->constr_vec(m_axis); 344 a = this->scale(a, (FT)1.0 / CGAL::sqrt(this->sqlen(a))); 345 for (std::size_t i = 0;i<indices.size();i++) { 346 Vector_3 v = this->constr_vec(m_point_on_axis, this->point(indices[i])); 347 348 v = this->sum_vectors(v, this->scale(a, -this->scalar_pdct(v, a))); 349 FT length = CGAL::sqrt(this->sqlen(v)); 350 if (length == 0) { 351 angles[i] = (FT)1.0; 352 continue; 353 } 354 v = this->scale(v, (FT)1.0 / length); 355 angles[i] = CGAL::abs(this->scalar_pdct(v, this->normal(indices[i]))); 356 } 357 } 358 cos_to_normal(const Point_3 & p,const Vector_3 & n)359 FT cos_to_normal(const Point_3 &p, const Vector_3 &n) const { 360 Vector_3 a = this->constr_vec(m_axis); 361 a = this->scale(a, (FT)1.0 / CGAL::sqrt(this->sqlen(a))); 362 363 Vector_3 v = this->constr_vec(m_point_on_axis, p); 364 v = this->sum_vectors(v, this->scale(a, -this->scalar_pdct(v, a))); 365 366 FT length = CGAL::sqrt(this->sqlen(v)); 367 if (length == 0) 368 return (FT)1.0; 369 370 v = this->scale(v, (FT)1.0 / length); 371 return CGAL::abs(this->scalar_pdct(v, n)); 372 } 373 minimum_sample_size()374 virtual std::size_t minimum_sample_size() const { 375 return 2; 376 } 377 supports_connected_component()378 virtual bool supports_connected_component() const { 379 return true; 380 } 381 382 private: 383 FT m_radius; 384 Line_3 m_axis; 385 Point_3 m_point_on_axis; 386 mutable bool m_wrap_u; 387 388 /// \endcond 389 }; 390 } 391 } 392 #endif 393