1 // Copyright (c) 2020 GeometryFactory Sarl (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/Point_set_processing_3/include/CGAL/scanline_orient_normals.h $
7 // $Id: scanline_orient_normals.h bce99e7 2020-11-09T08:36:45+01:00 Simon Giraudot
8 // SPDX-License-Identifier: GPL-3.0-or-later OR LicenseRef-Commercial
9 //
10 // Author(s) : Simon Giraudot
11
12 #ifndef CGAL_SCANLINE_ORIENT_NORMALS_H
13 #define CGAL_SCANLINE_ORIENT_NORMALS_H
14
15 #include <CGAL/license/Point_set_processing_3.h>
16
17 #include <CGAL/squared_distance_3.h>
18
19 #include <CGAL/boost/graph/Named_function_parameters.h>
20 #include <CGAL/boost/graph/named_params_helper.h>
21
22 #include <CGAL/linear_least_squares_fitting_3.h>
23
24 #include <boost/iterator/transform_iterator.hpp>
25
26 #if defined(CGAL_EIGEN3_ENABLED) || defined(DOXYGEN_RUNNING)
27
28 #include <Eigen/Dense>
29 #include <Eigen/IterativeLinearSolvers>
30
31 //#define CGAL_SCANLINE_ORIENT_VERBOSE
32
33 namespace CGAL
34 {
35
36 /// \cond SKIP_IN_MANUAL
37 namespace Point_set_processing_3
38 {
39
40 namespace internal
41 {
42
43 template <typename Vector_3>
vertical_vector()44 const Vector_3& vertical_vector()
45 {
46 static Vector_3 v(0, 0, 1);
47 return v;
48 }
49
50 template <typename Iterator, typename PointMap,
51 typename ScanlineIDMap>
is_end_of_scanline(Iterator scanline_begin,Iterator it,PointMap,ScanlineIDMap scanline_id_map,const Tag_false &)52 bool is_end_of_scanline (Iterator scanline_begin, Iterator it,
53 PointMap,
54 ScanlineIDMap scanline_id_map,
55 const Tag_false&) // no fallback
56 {
57 return (get (scanline_id_map, *scanline_begin)
58 != get (scanline_id_map, *it));
59 }
60
61 template <typename Iterator, typename PointMap,
62 typename ScanlineIDMap>
is_end_of_scanline(Iterator scanline_begin,Iterator it,PointMap point_map,ScanlineIDMap,const Tag_true &)63 bool is_end_of_scanline (Iterator scanline_begin, Iterator it,
64 PointMap point_map,
65 ScanlineIDMap,
66 const Tag_true&) // fallback
67 {
68 using Point_3 = typename boost::property_traits<PointMap>::value_type;
69 using Vector_3 = typename Kernel_traits<Point_3>::Kernel::Vector_3;
70
71 if (std::distance (scanline_begin, it) < 3)
72 return false;
73
74 Iterator n_minus_1 = it; -- n_minus_1;
75 Iterator n_minus_2 = n_minus_1; -- n_minus_2;
76 Iterator n_minus_3 = n_minus_2; -- n_minus_3;
77
78 const Point_3& p_minus_1 = get(point_map, *n_minus_1);
79 const Point_3& p_minus_2 = get(point_map, *n_minus_2);
80 const Point_3& p_minus_3 = get(point_map, *n_minus_3);
81
82 // End of scanline reached if inversion of direction
83 Vector_3 v32 (p_minus_3, p_minus_2);
84 v32 = Vector_3 (v32.x(), v32.y(), 0);
85 Vector_3 v21 (p_minus_2, p_minus_1);
86 v21 = Vector_3 (v21.x(), v21.y(), 0);
87
88 return (v32 * v21 < 0);
89 }
90
91 template <typename Iterator, typename PointMap>
92 std::pair<typename Kernel_traits<typename boost::property_traits
93 <PointMap>::value_type>::Kernel::Vector_3,
94 typename Kernel_traits<typename boost::property_traits
95 <PointMap>::value_type>::Kernel::Vector_3>
scanline_base(Iterator begin,Iterator end,PointMap point_map)96 scanline_base (Iterator begin, Iterator end,
97 PointMap point_map)
98 {
99 using Point_3 = typename boost::property_traits<PointMap>::value_type;
100 using Kernel = typename Kernel_traits<Point_3>::Kernel;
101 using Vector_3 = typename Kernel::Vector_3;
102
103 using Line_3 = typename Kernel::Line_3;
104 using Plane_3 = typename Kernel::Plane_3;
105
106 const double limit = CGAL_PI * 30. / 180.;
107
108 Line_3 pca2;
109 linear_least_squares_fitting_3
110 (boost::make_transform_iterator
111 (begin, Property_map_to_unary_function<PointMap>(point_map)),
112 boost::make_transform_iterator
113 (end, Property_map_to_unary_function<PointMap>(point_map)),
114 pca2, Dimension_tag<0>());
115
116 Vector_3 pca_direction = pca2.to_vector();
117 pca_direction = Vector_3 (pca_direction.x(), pca_direction.y(), 0);
118 pca_direction = pca_direction / CGAL::approximate_sqrt(pca_direction.squared_length());
119
120 Plane_3 pca3;
121 linear_least_squares_fitting_3
122 (boost::make_transform_iterator
123 (begin, Property_map_to_unary_function<PointMap>(point_map)),
124 boost::make_transform_iterator
125 (end, Property_map_to_unary_function<PointMap>(point_map)),
126 pca3, Dimension_tag<0>());
127
128 Vector_3 orthogonal = pca3.orthogonal_vector();
129 Vector_3 vertical = CGAL::cross_product (pca_direction, orthogonal);
130 if (vertical * vertical_vector<Vector_3>() < 0)
131 vertical = -vertical;
132
133 vertical = vertical / CGAL::approximate_sqrt(vertical.squared_length());
134
135 if (std::acos(vertical * vertical_vector<Vector_3>()) < limit)
136 return std::make_pair (pca_direction, vertical);
137
138 // if plane diverges from the vertical more than 30 degrees, then
139 // fallback to 0 0 1 vertical vector
140
141 // Dummy begin->end vector version
142 Iterator last = end; -- last;
143 const Point_3& pbegin = get (point_map, *begin);
144 const Point_3& plast = get (point_map, *last);
145 Vector_3 direction (Point_3 (pbegin.x(), pbegin.y(), 0),
146 Point_3 (plast.x(), plast.y(), 0));
147 direction = direction / CGAL::approximate_sqrt (direction.squared_length());
148
149 return std::make_pair (direction, vertical_vector<Vector_3>());
150 }
151
152 template <typename Vector_3>
normal_along_scanline_is_inverted(const Vector_3 & normal,const Vector_3 & line_of_sight)153 bool normal_along_scanline_is_inverted
154 (const Vector_3& normal, const Vector_3& line_of_sight)
155 {
156 return (line_of_sight * normal < 0);
157 }
158
159 template <typename Iterator, typename PointMap, typename Vector_3>
160 std::pair<typename boost::property_traits<PointMap>::value_type, bool>
estimate_scan_position(Iterator begin,Iterator end,PointMap point_map,const std::vector<Vector_3> & lines_of_sight)161 estimate_scan_position (Iterator begin, Iterator end, PointMap point_map,
162 const std::vector<Vector_3>& lines_of_sight)
163 {
164 using Point_3 = typename boost::property_traits<PointMap>::value_type;
165 typedef Eigen::Matrix3d Matrix;
166 typedef Eigen::Vector3d Vector;
167 typedef Eigen::ConjugateGradient<Matrix> Solver;
168
169 Matrix R;
170 R << 0, 0, 0, 0, 0, 0, 0, 0, 0;
171 Vector Q;
172 Q << 0, 0, 0;
173
174 std::size_t idx = 0;
175 for (Iterator it = begin; it != end; ++ it, ++ idx)
176 {
177 const Point_3& p = get (point_map, *it);
178 const Vector_3& v = lines_of_sight[idx];
179
180 Vector n;
181 n << v.x(), v.y(), v.z();
182
183 Matrix I_nnt = Matrix::Identity() - n * n.transpose();
184 Vector a;
185 a << p.x(), p.y(), p.z();
186
187 R += I_nnt;
188 Q += I_nnt * a;
189 }
190
191 Solver solver(R);
192
193 if (solver.info() != Eigen::Success)
194 return std::make_pair (ORIGIN, false);
195
196 Vector p = solver.solve(Q);
197 if (solver.info() != Eigen::Success)
198 return std::make_pair (ORIGIN, false);
199
200 return std::make_pair (Point_3(p(0), p(1), p(2)), true);
201 }
202
203
204 template <typename Iterator, typename PointMap, typename NormalMap,
205 typename ScanAngleMap>
orient_scanline(Iterator begin,Iterator end,PointMap point_map,NormalMap normal_map,ScanAngleMap scan_angle_map,const Tag_false &)206 void orient_scanline (Iterator begin, Iterator end,
207 PointMap point_map,
208 NormalMap normal_map,
209 ScanAngleMap scan_angle_map,
210 const Tag_false&) // no fallback scan angle
211 {
212 using Point_3 = typename boost::property_traits<PointMap>::value_type;
213 using Vector_3 = typename boost::property_traits<NormalMap>::value_type;
214
215 Vector_3 direction;
216 Vector_3 vertical;
217 std::tie (direction, vertical)
218 = scanline_base (begin, end, point_map);
219
220 std::vector<Vector_3> lines_of_sight;
221 lines_of_sight.reserve (std::distance (begin, end));
222
223 double mean_z = 0;
224 for (Iterator it = begin; it != end; ++ it)
225 {
226 double angle = CGAL_PI * static_cast<double>(get (scan_angle_map, *it)) / 180.;
227 Vector_3 los = direction * std::sin(angle) + vertical * std::cos(angle);
228 lines_of_sight.push_back (los);
229 mean_z += get (point_map, *it).z();
230 }
231 mean_z /= std::distance (begin, end);
232
233 #ifdef CGAL_SCANORIENT_DUMP_RANDOM_SCANLINES
234 if (rand() % 1000 == 0 && std::distance(begin, end) > 10)
235 {
236 std::ofstream ofile ("scanline.polylines.txt");
237 ofile.precision(18);
238 ofile << std::distance (begin, end);
239 for (Iterator it = begin; it != end; ++ it)
240 ofile << " " << get(point_map, *it);
241 ofile << std::endl;
242
243 std::ofstream ofile2 ("base.polylines.txt");
244 Point_3 orig = get (point_map, *(begin + std::distance(begin, end) / 2));
245 double dist = CGAL::approximate_sqrt (CGAL::squared_distance
246 (get(point_map, *begin), orig));
247
248 ofile2.precision(18);
249 ofile2 << "2 " << orig << " " << orig + direction * dist << std::endl
250 << "2 " << orig << " " << orig + vertical * dist << std::endl;
251 }
252 #endif
253
254 Point_3 scan_position;
255 bool solver_success;
256 std::tie (scan_position, solver_success)
257 = estimate_scan_position (begin, end, point_map, lines_of_sight);
258
259 // If solver failed OR if scan position is detected under the
260 // scanline (obviously wrong)
261 if (!solver_success || scan_position.z() < mean_z)
262 {
263 #ifdef CGAL_SCANLINE_ORIENT_VERBOSE
264 if (!solver_success)
265 std::cerr << "Inverting because olver failed: ";
266 else
267 std::cerr << "Inverting because scanner under scanline: ";
268 #endif
269
270 direction = -direction;
271 std::size_t idx = 0;
272 for (Iterator it = begin; it != end; ++ it, ++ idx)
273 {
274 double angle = CGAL_PI * static_cast<double>(get (scan_angle_map, *it)) / 180.;
275 Vector_3 los = direction * std::sin(angle) + vertical * std::cos(angle);
276 lines_of_sight[idx] = los;
277 }
278
279 std::tie (scan_position, solver_success)
280 = estimate_scan_position (begin, end, point_map, lines_of_sight);
281
282 #ifdef CGAL_SCANLINE_ORIENT_VERBOSE
283 if (solver_success && scan_position.z() > mean_z)
284 std::cerr << "SOLVED" << std::endl;
285 else if (!solver_success)
286 std::cerr << "FAILED, solver failure" << std::endl;
287 else
288 std::cerr << "FAILED, scanner under scanline" << std::endl;
289 #endif
290 }
291
292 std::size_t idx = 0;
293 for (Iterator it = begin; it != end; ++ it, ++ idx)
294 {
295 const Vector_3 los = lines_of_sight[idx];
296 const Vector_3& normal = get (normal_map, *it);
297 if (normal_along_scanline_is_inverted (normal, los))
298 put (normal_map, *it, -normal);
299 }
300 }
301
302 template <typename Iterator, typename PointMap, typename NormalMap,
303 typename ScanAngleMap>
orient_scanline(Iterator begin,Iterator end,PointMap point_map,NormalMap normal_map,ScanAngleMap,const Tag_true &)304 void orient_scanline (Iterator begin, Iterator end,
305 PointMap point_map,
306 NormalMap normal_map,
307 ScanAngleMap,
308 const Tag_true&) // fallback scan angle
309 {
310 using Point_3 = typename boost::property_traits<PointMap>::value_type;
311 using Vector_3 = typename boost::property_traits<NormalMap>::value_type;
312
313 Vector_3 direction;
314 Vector_3 vertical;
315 std::tie (direction, vertical)
316 = scanline_base (begin, end, point_map);
317
318 // Estimate scanner position:
319 // average XY-projected point, located above
320 double mean_x = 0.;
321 double mean_y = 0.;
322 double max_z = -(std::numeric_limits<double>::max)();
323 std::size_t nb = 0;
324
325 for (Iterator it = begin; it != end; ++ it)
326 {
327 const Point_3& p = get (point_map, *it);
328 mean_x += p.x();
329 mean_y += p.y();
330 max_z = (std::max)(max_z, p.z());
331 ++ nb;
332 }
333
334 Iterator last = end; -- last;
335 double length = CGAL::approximate_sqrt (CGAL::squared_distance
336 (get (point_map, *begin),
337 get (point_map, *last)));
338
339 Point_3 scan_position (mean_x / nb, mean_y / nb,
340 max_z + length * 2);
341
342 for (Iterator it = begin; it != end; ++ it)
343 {
344 Vector_3 line_of_sight (get(point_map, *it), scan_position);
345 const Vector_3& normal = get (normal_map, *it);
346 if (normal_along_scanline_is_inverted (normal, line_of_sight))
347 put (normal_map, *it, -normal);
348 }
349 }
350
351 } // namespace internal
352
353 } // namespace Point_set_processing_3
354 /// \endcond
355
356 // ----------------------------------------------------------------------------
357 // Public section
358 // ----------------------------------------------------------------------------
359
360 /**
361 \ingroup PkgPointSetProcessing3Algorithms
362
363 orients the normals of the range of `points` by estimating a line
364 of sight and checking its consistency with the current normal orientation.
365
366 \warning This function requires the input `points` to be ordered
367 along scanlines aligned on the XY-plane. It is typically designed
368 for 2.5D urban datasets acquired through, for example, airborne
369 LIDAR devices.
370
371 First, scanlines are estimated as subranges of `points` by
372 iterating on `points`:
373
374 - if the named parameter `scanline_id_map` is provided, the range
375 is cutted everytime the id changes.
376
377 - if no scanline ID map is provided, a fallback method simply cuts
378 the range everytime 3 consecutive points form an acute angle on
379 the projected XY-plane. This fallback method gives suboptimal
380 results.
381
382 Then, the line of sight (estimated vector between a point and the
383 position of the scanner at its time of acquisition) is estimated:
384
385 - if `scan_angle` is provided, the line of sight can be directly
386 computed as a combination of the estimated scanline and of the
387 scan angle.
388
389 - if no scan angle map is provided, then for each scanline, the
390 position of the scanner is estimated as being above of the
391 barycenter of the points of the scanline projected on the
392 XY-plane. This fallback method gives suboptimal results.
393
394 Once the line of sight is estimated for each point, the normals are
395 oriented by checking, for each of them, if the line of sight and the
396 normal vector give a positive scalar product. If they don't, then
397 the normal vector is inverted.
398
399 \note This method gives optimal results when `scanline_id_map`
400 and `scan_angle` are provided. Correct results may still be
401 produced in the absence of either one or both of these properties,
402 as long as the point set is ordered in 2.5D scanlines.
403
404 \tparam PointRange is a model of `Range`. The value type of
405 its iterator is the key type of the named parameter `point_map`.
406
407 \param points input point range.
408 \param np an optional sequence of \ref bgl_namedparameters "Named Parameters" among the ones listed below
409
410 \cgalNamedParamsBegin
411 \cgalParamNBegin{point_map}
412 \cgalParamDescription{a property map associating points to the elements of the point set `points`}
413 \cgalParamType{a model of `ReadablePropertyMap` whose key type is the value type
414 of the iterator of `PointRange` and whose value type is `geom_traits::Point_3`}
415 \cgalParamDefault{`CGAL::Identity_property_map<geom_traits::Point_3>`}
416 \cgalParamNEnd
417
418 \cgalParamNBegin{normal_map}
419 \cgalParamDescription{a property map associating normals to the
420 elements of the point set `points`}
421 \cgalParamType{a model of `WritablePropertyMap` whose key type
422 is the value type of the iterator of
423 `PointRange` and whose value type is
424 `geom_traits::Vector_3`}
425 \cgalParamNEnd
426
427 \cgalParamNBegin{scan_angle_map}
428 \cgalParamDescription{a property map associating the angle of
429 acquisition (in degrees) to the elements of the point set
430 `points`}
431 \cgalParamType{a model of `ReadablePropertyMap` whose key type
432 is the value type of the iterator of
433 `PointRange` and whose value type is convertible
434 to `double`}
435 \cgalParamNEnd
436
437 \cgalParamNBegin{scanline_id_map}
438 \cgalParamDescription{a property map associating a scanline ID
439 to the elements of the point set `points`. A scanline is
440 detected as a consecutive subrange of items in the input range
441 `point` whose ID are identical. IDs do not need to be unique,
442 they just need to be different for two consecutive
443 scanlines. The LAS property `scan_direction_flag` (whose values
444 are either 0 or 1 depending on the direction of the scanner)
445 can be used.}
446 \cgalParamType{a model of `ReadablePropertyMap` whose key type
447 is the value type of the iterator of
448 `PointRange` and whose value type is a model of
449 `EqualityComparable`}
450 \cgalParamNEnd
451
452 \cgalParamNBegin{geom_traits}
453 \cgalParamDescription{an instance of a geometric traits class}
454 \cgalParamType{a model of `Kernel`}
455 \cgalParamDefault{a \cgal Kernel deduced from the point type, using `CGAL::Kernel_traits`}
456 \cgalParamNEnd
457 \cgalNamedParamsEnd
458 */
459 template <typename PointRange, typename NamedParameters>
scanline_orient_normals(PointRange & points,const NamedParameters & np)460 void scanline_orient_normals (PointRange& points, const NamedParameters& np)
461 {
462 using parameters::choose_parameter;
463 using parameters::get_parameter;
464
465 using Iterator = typename PointRange::iterator;
466 using Value_type = typename std::iterator_traits<Iterator>::value_type;
467 using PointMap = typename CGAL::GetPointMap<PointRange, NamedParameters>::type;
468 using NormalMap = typename Point_set_processing_3::GetNormalMap<PointRange, NamedParameters>::type;
469
470 using No_map = Constant_property_map<Value_type, int>;
471
472 using ScanAngleMap = typename internal_np::Lookup_named_param_def
473 <internal_np::scan_angle_t, NamedParameters, No_map>::type;
474 using Fallback_scan_angle = Boolean_tag<std::is_same<ScanAngleMap, No_map>::value>;
475
476 using ScanlineIDMap = typename internal_np::Lookup_named_param_def
477 <internal_np::scanline_id_t, NamedParameters, No_map>::type;
478 using Fallback_scanline_ID = Boolean_tag<std::is_same<ScanlineIDMap, No_map>::value>;
479
480 CGAL_static_assertion_msg(!(std::is_same<NormalMap,
481 typename Point_set_processing_3::GetNormalMap
482 <PointRange, NamedParameters>::NoMap>::value),
483 "Error: no normal map");
484
485 PointMap point_map = choose_parameter<PointMap>(get_parameter(np, internal_np::point_map));
486 NormalMap normal_map = choose_parameter<NormalMap>(get_parameter(np, internal_np::normal_map));
487 ScanAngleMap scan_angle_map = choose_parameter<ScanAngleMap>
488 (get_parameter(np, internal_np::scan_angle_map));
489 ScanlineIDMap scanline_id_map = choose_parameter<ScanlineIDMap>
490 (get_parameter(np, internal_np::scanline_id_map));
491
492 std::size_t nb_scanlines = 1;
493
494 #ifdef CGAL_SCANORIENT_DUMP_RANDOM_SCANLINES
495 std::ofstream ofile ("scanlines.polylines.txt");
496 ofile.precision(18);
497 #endif
498
499 CGAL::Bbox_3 bbox = CGAL::bbox_3
500 (boost::make_transform_iterator
501 (points.begin(), Property_map_to_unary_function<PointMap>(point_map)),
502 boost::make_transform_iterator
503 (points.end(), Property_map_to_unary_function<PointMap>(point_map)));
504
505 double bbox_diagonal
506 = CGAL::approximate_sqrt((bbox.xmax() - bbox.xmin()) * (bbox.xmax() - bbox.xmin())
507 + (bbox.ymax() - bbox.ymin()) * (bbox.ymax() - bbox.ymin())
508 + (bbox.zmax() - bbox.zmin()) * (bbox.zmax() - bbox.zmin()));
509 double limit = 0.05 * bbox_diagonal;
510
511 Iterator scanline_begin = points.begin();
512 for (Iterator it = points.begin(); it != points.end(); ++ it)
513 {
514 bool force_cut = false;
515 if (it != points.begin())
516 {
517 Iterator prev = it; -- prev;
518 force_cut = (CGAL::squared_distance (get (point_map, *prev),
519 get (point_map, *it)) > limit * limit);
520 }
521
522 if (Point_set_processing_3::internal::is_end_of_scanline
523 (scanline_begin, it, point_map, scanline_id_map,
524 Fallback_scanline_ID()) || force_cut)
525 {
526 Point_set_processing_3::internal::orient_scanline
527 (scanline_begin, it, point_map, normal_map,
528 scan_angle_map, Fallback_scan_angle());
529
530 #ifdef CGAL_SCANORIENT_DUMP_RANDOM_SCANLINES
531 ofile << std::distance (scanline_begin, it);
532 for (Iterator it2 = scanline_begin; it2 != it; ++ it2)
533 ofile << " " << get (point_map, *it2);
534 ofile << std::endl;
535 #endif
536
537 scanline_begin = it;
538 ++ nb_scanlines;
539 }
540 }
541
542 Point_set_processing_3::internal::orient_scanline
543 (scanline_begin, points.end(), point_map, normal_map,
544 scan_angle_map, Fallback_scan_angle());
545
546 #ifdef CGAL_SCANLINE_ORIENT_VERBOSE
547 std::cerr << nb_scanlines << " scanline(s) identified (mean length = "
548 << std::size_t(points.size() / double(nb_scanlines))
549 << " point(s))" << std::endl;
550 #endif
551 }
552
553 template <typename PointRange>
scanline_orient_normals(PointRange & points)554 void scanline_orient_normals (PointRange& points)
555 {
556 return scanline_orient_normals (points,
557 CGAL::Point_set_processing_3::parameters::all_default(points));
558 }
559
560 } // namespace CGAL
561
562 #endif // CGAL_EIGEN3_ENABLED
563
564 #endif // CGAL_SCANLINE_ORIENT_NORMALS_H
565