// Copyright (c) 2020 GeometryFactory Sarl (France). // All rights reserved. // // This file is part of CGAL (www.cgal.org). // // $URL: https://github.com/CGAL/cgal/blob/v5.2/Point_set_processing_3/include/CGAL/scanline_orient_normals.h $ // $Id: scanline_orient_normals.h bce99e7 2020-11-09T08:36:45+01:00 Simon Giraudot // SPDX-License-Identifier: GPL-3.0-or-later OR LicenseRef-Commercial // // Author(s) : Simon Giraudot #ifndef CGAL_SCANLINE_ORIENT_NORMALS_H #define CGAL_SCANLINE_ORIENT_NORMALS_H #include #include #include #include #include #include #if defined(CGAL_EIGEN3_ENABLED) || defined(DOXYGEN_RUNNING) #include #include //#define CGAL_SCANLINE_ORIENT_VERBOSE namespace CGAL { /// \cond SKIP_IN_MANUAL namespace Point_set_processing_3 { namespace internal { template const Vector_3& vertical_vector() { static Vector_3 v(0, 0, 1); return v; } template bool is_end_of_scanline (Iterator scanline_begin, Iterator it, PointMap, ScanlineIDMap scanline_id_map, const Tag_false&) // no fallback { return (get (scanline_id_map, *scanline_begin) != get (scanline_id_map, *it)); } template bool is_end_of_scanline (Iterator scanline_begin, Iterator it, PointMap point_map, ScanlineIDMap, const Tag_true&) // fallback { using Point_3 = typename boost::property_traits::value_type; using Vector_3 = typename Kernel_traits::Kernel::Vector_3; if (std::distance (scanline_begin, it) < 3) return false; Iterator n_minus_1 = it; -- n_minus_1; Iterator n_minus_2 = n_minus_1; -- n_minus_2; Iterator n_minus_3 = n_minus_2; -- n_minus_3; const Point_3& p_minus_1 = get(point_map, *n_minus_1); const Point_3& p_minus_2 = get(point_map, *n_minus_2); const Point_3& p_minus_3 = get(point_map, *n_minus_3); // End of scanline reached if inversion of direction Vector_3 v32 (p_minus_3, p_minus_2); v32 = Vector_3 (v32.x(), v32.y(), 0); Vector_3 v21 (p_minus_2, p_minus_1); v21 = Vector_3 (v21.x(), v21.y(), 0); return (v32 * v21 < 0); } template std::pair::value_type>::Kernel::Vector_3, typename Kernel_traits::value_type>::Kernel::Vector_3> scanline_base (Iterator begin, Iterator end, PointMap point_map) { using Point_3 = typename boost::property_traits::value_type; using Kernel = typename Kernel_traits::Kernel; using Vector_3 = typename Kernel::Vector_3; using Line_3 = typename Kernel::Line_3; using Plane_3 = typename Kernel::Plane_3; const double limit = CGAL_PI * 30. / 180.; Line_3 pca2; linear_least_squares_fitting_3 (boost::make_transform_iterator (begin, Property_map_to_unary_function(point_map)), boost::make_transform_iterator (end, Property_map_to_unary_function(point_map)), pca2, Dimension_tag<0>()); Vector_3 pca_direction = pca2.to_vector(); pca_direction = Vector_3 (pca_direction.x(), pca_direction.y(), 0); pca_direction = pca_direction / CGAL::approximate_sqrt(pca_direction.squared_length()); Plane_3 pca3; linear_least_squares_fitting_3 (boost::make_transform_iterator (begin, Property_map_to_unary_function(point_map)), boost::make_transform_iterator (end, Property_map_to_unary_function(point_map)), pca3, Dimension_tag<0>()); Vector_3 orthogonal = pca3.orthogonal_vector(); Vector_3 vertical = CGAL::cross_product (pca_direction, orthogonal); if (vertical * vertical_vector() < 0) vertical = -vertical; vertical = vertical / CGAL::approximate_sqrt(vertical.squared_length()); if (std::acos(vertical * vertical_vector()) < limit) return std::make_pair (pca_direction, vertical); // if plane diverges from the vertical more than 30 degrees, then // fallback to 0 0 1 vertical vector // Dummy begin->end vector version Iterator last = end; -- last; const Point_3& pbegin = get (point_map, *begin); const Point_3& plast = get (point_map, *last); Vector_3 direction (Point_3 (pbegin.x(), pbegin.y(), 0), Point_3 (plast.x(), plast.y(), 0)); direction = direction / CGAL::approximate_sqrt (direction.squared_length()); return std::make_pair (direction, vertical_vector()); } template bool normal_along_scanline_is_inverted (const Vector_3& normal, const Vector_3& line_of_sight) { return (line_of_sight * normal < 0); } template std::pair::value_type, bool> estimate_scan_position (Iterator begin, Iterator end, PointMap point_map, const std::vector& lines_of_sight) { using Point_3 = typename boost::property_traits::value_type; typedef Eigen::Matrix3d Matrix; typedef Eigen::Vector3d Vector; typedef Eigen::ConjugateGradient Solver; Matrix R; R << 0, 0, 0, 0, 0, 0, 0, 0, 0; Vector Q; Q << 0, 0, 0; std::size_t idx = 0; for (Iterator it = begin; it != end; ++ it, ++ idx) { const Point_3& p = get (point_map, *it); const Vector_3& v = lines_of_sight[idx]; Vector n; n << v.x(), v.y(), v.z(); Matrix I_nnt = Matrix::Identity() - n * n.transpose(); Vector a; a << p.x(), p.y(), p.z(); R += I_nnt; Q += I_nnt * a; } Solver solver(R); if (solver.info() != Eigen::Success) return std::make_pair (ORIGIN, false); Vector p = solver.solve(Q); if (solver.info() != Eigen::Success) return std::make_pair (ORIGIN, false); return std::make_pair (Point_3(p(0), p(1), p(2)), true); } template void orient_scanline (Iterator begin, Iterator end, PointMap point_map, NormalMap normal_map, ScanAngleMap scan_angle_map, const Tag_false&) // no fallback scan angle { using Point_3 = typename boost::property_traits::value_type; using Vector_3 = typename boost::property_traits::value_type; Vector_3 direction; Vector_3 vertical; std::tie (direction, vertical) = scanline_base (begin, end, point_map); std::vector lines_of_sight; lines_of_sight.reserve (std::distance (begin, end)); double mean_z = 0; for (Iterator it = begin; it != end; ++ it) { double angle = CGAL_PI * static_cast(get (scan_angle_map, *it)) / 180.; Vector_3 los = direction * std::sin(angle) + vertical * std::cos(angle); lines_of_sight.push_back (los); mean_z += get (point_map, *it).z(); } mean_z /= std::distance (begin, end); #ifdef CGAL_SCANORIENT_DUMP_RANDOM_SCANLINES if (rand() % 1000 == 0 && std::distance(begin, end) > 10) { std::ofstream ofile ("scanline.polylines.txt"); ofile.precision(18); ofile << std::distance (begin, end); for (Iterator it = begin; it != end; ++ it) ofile << " " << get(point_map, *it); ofile << std::endl; std::ofstream ofile2 ("base.polylines.txt"); Point_3 orig = get (point_map, *(begin + std::distance(begin, end) / 2)); double dist = CGAL::approximate_sqrt (CGAL::squared_distance (get(point_map, *begin), orig)); ofile2.precision(18); ofile2 << "2 " << orig << " " << orig + direction * dist << std::endl << "2 " << orig << " " << orig + vertical * dist << std::endl; } #endif Point_3 scan_position; bool solver_success; std::tie (scan_position, solver_success) = estimate_scan_position (begin, end, point_map, lines_of_sight); // If solver failed OR if scan position is detected under the // scanline (obviously wrong) if (!solver_success || scan_position.z() < mean_z) { #ifdef CGAL_SCANLINE_ORIENT_VERBOSE if (!solver_success) std::cerr << "Inverting because olver failed: "; else std::cerr << "Inverting because scanner under scanline: "; #endif direction = -direction; std::size_t idx = 0; for (Iterator it = begin; it != end; ++ it, ++ idx) { double angle = CGAL_PI * static_cast(get (scan_angle_map, *it)) / 180.; Vector_3 los = direction * std::sin(angle) + vertical * std::cos(angle); lines_of_sight[idx] = los; } std::tie (scan_position, solver_success) = estimate_scan_position (begin, end, point_map, lines_of_sight); #ifdef CGAL_SCANLINE_ORIENT_VERBOSE if (solver_success && scan_position.z() > mean_z) std::cerr << "SOLVED" << std::endl; else if (!solver_success) std::cerr << "FAILED, solver failure" << std::endl; else std::cerr << "FAILED, scanner under scanline" << std::endl; #endif } std::size_t idx = 0; for (Iterator it = begin; it != end; ++ it, ++ idx) { const Vector_3 los = lines_of_sight[idx]; const Vector_3& normal = get (normal_map, *it); if (normal_along_scanline_is_inverted (normal, los)) put (normal_map, *it, -normal); } } template void orient_scanline (Iterator begin, Iterator end, PointMap point_map, NormalMap normal_map, ScanAngleMap, const Tag_true&) // fallback scan angle { using Point_3 = typename boost::property_traits::value_type; using Vector_3 = typename boost::property_traits::value_type; Vector_3 direction; Vector_3 vertical; std::tie (direction, vertical) = scanline_base (begin, end, point_map); // Estimate scanner position: // average XY-projected point, located above double mean_x = 0.; double mean_y = 0.; double max_z = -(std::numeric_limits::max)(); std::size_t nb = 0; for (Iterator it = begin; it != end; ++ it) { const Point_3& p = get (point_map, *it); mean_x += p.x(); mean_y += p.y(); max_z = (std::max)(max_z, p.z()); ++ nb; } Iterator last = end; -- last; double length = CGAL::approximate_sqrt (CGAL::squared_distance (get (point_map, *begin), get (point_map, *last))); Point_3 scan_position (mean_x / nb, mean_y / nb, max_z + length * 2); for (Iterator it = begin; it != end; ++ it) { Vector_3 line_of_sight (get(point_map, *it), scan_position); const Vector_3& normal = get (normal_map, *it); if (normal_along_scanline_is_inverted (normal, line_of_sight)) put (normal_map, *it, -normal); } } } // namespace internal } // namespace Point_set_processing_3 /// \endcond // ---------------------------------------------------------------------------- // Public section // ---------------------------------------------------------------------------- /** \ingroup PkgPointSetProcessing3Algorithms orients the normals of the range of `points` by estimating a line of sight and checking its consistency with the current normal orientation. \warning This function requires the input `points` to be ordered along scanlines aligned on the XY-plane. It is typically designed for 2.5D urban datasets acquired through, for example, airborne LIDAR devices. First, scanlines are estimated as subranges of `points` by iterating on `points`: - if the named parameter `scanline_id_map` is provided, the range is cutted everytime the id changes. - if no scanline ID map is provided, a fallback method simply cuts the range everytime 3 consecutive points form an acute angle on the projected XY-plane. This fallback method gives suboptimal results. Then, the line of sight (estimated vector between a point and the position of the scanner at its time of acquisition) is estimated: - if `scan_angle` is provided, the line of sight can be directly computed as a combination of the estimated scanline and of the scan angle. - if no scan angle map is provided, then for each scanline, the position of the scanner is estimated as being above of the barycenter of the points of the scanline projected on the XY-plane. This fallback method gives suboptimal results. Once the line of sight is estimated for each point, the normals are oriented by checking, for each of them, if the line of sight and the normal vector give a positive scalar product. If they don't, then the normal vector is inverted. \note This method gives optimal results when `scanline_id_map` and `scan_angle` are provided. Correct results may still be produced in the absence of either one or both of these properties, as long as the point set is ordered in 2.5D scanlines. \tparam PointRange is a model of `Range`. The value type of its iterator is the key type of the named parameter `point_map`. \param points input point range. \param np an optional sequence of \ref bgl_namedparameters "Named Parameters" among the ones listed below \cgalNamedParamsBegin \cgalParamNBegin{point_map} \cgalParamDescription{a property map associating points to the elements of the point set `points`} \cgalParamType{a model of `ReadablePropertyMap` whose key type is the value type of the iterator of `PointRange` and whose value type is `geom_traits::Point_3`} \cgalParamDefault{`CGAL::Identity_property_map`} \cgalParamNEnd \cgalParamNBegin{normal_map} \cgalParamDescription{a property map associating normals to the elements of the point set `points`} \cgalParamType{a model of `WritablePropertyMap` whose key type is the value type of the iterator of `PointRange` and whose value type is `geom_traits::Vector_3`} \cgalParamNEnd \cgalParamNBegin{scan_angle_map} \cgalParamDescription{a property map associating the angle of acquisition (in degrees) to the elements of the point set `points`} \cgalParamType{a model of `ReadablePropertyMap` whose key type is the value type of the iterator of `PointRange` and whose value type is convertible to `double`} \cgalParamNEnd \cgalParamNBegin{scanline_id_map} \cgalParamDescription{a property map associating a scanline ID to the elements of the point set `points`. A scanline is detected as a consecutive subrange of items in the input range `point` whose ID are identical. IDs do not need to be unique, they just need to be different for two consecutive scanlines. The LAS property `scan_direction_flag` (whose values are either 0 or 1 depending on the direction of the scanner) can be used.} \cgalParamType{a model of `ReadablePropertyMap` whose key type is the value type of the iterator of `PointRange` and whose value type is a model of `EqualityComparable`} \cgalParamNEnd \cgalParamNBegin{geom_traits} \cgalParamDescription{an instance of a geometric traits class} \cgalParamType{a model of `Kernel`} \cgalParamDefault{a \cgal Kernel deduced from the point type, using `CGAL::Kernel_traits`} \cgalParamNEnd \cgalNamedParamsEnd */ template void scanline_orient_normals (PointRange& points, const NamedParameters& np) { using parameters::choose_parameter; using parameters::get_parameter; using Iterator = typename PointRange::iterator; using Value_type = typename std::iterator_traits::value_type; using PointMap = typename CGAL::GetPointMap::type; using NormalMap = typename Point_set_processing_3::GetNormalMap::type; using No_map = Constant_property_map; using ScanAngleMap = typename internal_np::Lookup_named_param_def ::type; using Fallback_scan_angle = Boolean_tag::value>; using ScanlineIDMap = typename internal_np::Lookup_named_param_def ::type; using Fallback_scanline_ID = Boolean_tag::value>; CGAL_static_assertion_msg(!(std::is_same::NoMap>::value), "Error: no normal map"); PointMap point_map = choose_parameter(get_parameter(np, internal_np::point_map)); NormalMap normal_map = choose_parameter(get_parameter(np, internal_np::normal_map)); ScanAngleMap scan_angle_map = choose_parameter (get_parameter(np, internal_np::scan_angle_map)); ScanlineIDMap scanline_id_map = choose_parameter (get_parameter(np, internal_np::scanline_id_map)); std::size_t nb_scanlines = 1; #ifdef CGAL_SCANORIENT_DUMP_RANDOM_SCANLINES std::ofstream ofile ("scanlines.polylines.txt"); ofile.precision(18); #endif CGAL::Bbox_3 bbox = CGAL::bbox_3 (boost::make_transform_iterator (points.begin(), Property_map_to_unary_function(point_map)), boost::make_transform_iterator (points.end(), Property_map_to_unary_function(point_map))); double bbox_diagonal = CGAL::approximate_sqrt((bbox.xmax() - bbox.xmin()) * (bbox.xmax() - bbox.xmin()) + (bbox.ymax() - bbox.ymin()) * (bbox.ymax() - bbox.ymin()) + (bbox.zmax() - bbox.zmin()) * (bbox.zmax() - bbox.zmin())); double limit = 0.05 * bbox_diagonal; Iterator scanline_begin = points.begin(); for (Iterator it = points.begin(); it != points.end(); ++ it) { bool force_cut = false; if (it != points.begin()) { Iterator prev = it; -- prev; force_cut = (CGAL::squared_distance (get (point_map, *prev), get (point_map, *it)) > limit * limit); } if (Point_set_processing_3::internal::is_end_of_scanline (scanline_begin, it, point_map, scanline_id_map, Fallback_scanline_ID()) || force_cut) { Point_set_processing_3::internal::orient_scanline (scanline_begin, it, point_map, normal_map, scan_angle_map, Fallback_scan_angle()); #ifdef CGAL_SCANORIENT_DUMP_RANDOM_SCANLINES ofile << std::distance (scanline_begin, it); for (Iterator it2 = scanline_begin; it2 != it; ++ it2) ofile << " " << get (point_map, *it2); ofile << std::endl; #endif scanline_begin = it; ++ nb_scanlines; } } Point_set_processing_3::internal::orient_scanline (scanline_begin, points.end(), point_map, normal_map, scan_angle_map, Fallback_scan_angle()); #ifdef CGAL_SCANLINE_ORIENT_VERBOSE std::cerr << nb_scanlines << " scanline(s) identified (mean length = " << std::size_t(points.size() / double(nb_scanlines)) << " point(s))" << std::endl; #endif } template void scanline_orient_normals (PointRange& points) { return scanline_orient_normals (points, CGAL::Point_set_processing_3::parameters::all_default(points)); } } // namespace CGAL #endif // CGAL_EIGEN3_ENABLED #endif // CGAL_SCANLINE_ORIENT_NORMALS_H