diff --git a/BGL/include/CGAL/boost/graph/named_params_helper.h b/BGL/include/CGAL/boost/graph/named_params_helper.h index 4f589134439..19efd08118b 100644 --- a/BGL/include/CGAL/boost/graph/named_params_helper.h +++ b/BGL/include/CGAL/boost/graph/named_params_helper.h @@ -503,6 +503,50 @@ CGAL_DEF_GET_INITIALIZED_INDEX_MAP(face, typename boost::graph_traits::fa > ::type type; }; + template + class GetScanAngleMap + { + public: + struct NoMap + { + typedef typename std::iterator_traits::value_type key_type; + typedef float value_type; + typedef value_type reference; + typedef boost::read_write_property_map_tag category; + + typedef NoMap Self; + friend reference get(const Self&, const key_type&) { return 0.f; } + friend void put(const Self&, const key_type&, const value_type&) { } + }; + typedef typename internal_np::Lookup_named_param_def < + internal_np::scan_angle_t, + NamedParameters, + NoMap//default + > ::type type; + }; + + template + class GetScanDirectionFlag + { + public: + struct NoMap + { + typedef typename std::iterator_traits::value_type key_type; + typedef unsigned char value_type; + typedef value_type reference; + typedef boost::read_write_property_map_tag category; + + typedef NoMap Self; + friend reference get(const Self&, const key_type&) { return 0; } + friend void put(const Self&, const key_type&, const value_type&) { } + }; + typedef typename internal_np::Lookup_named_param_def < + internal_np::scan_direction_flag_t, + NamedParameters, + NoMap//default + > ::type type; + }; + } // namespace Point_set_processing_3 template diff --git a/BGL/include/CGAL/boost/graph/parameters_interface.h b/BGL/include/CGAL/boost/graph/parameters_interface.h index 3e25ee8ff6c..2de74be459b 100644 --- a/BGL/include/CGAL/boost/graph/parameters_interface.h +++ b/BGL/include/CGAL/boost/graph/parameters_interface.h @@ -150,6 +150,8 @@ CGAL_add_named_parameter(inspector_t, inspector, inspector) CGAL_add_named_parameter(logger_t, logger, logger) CGAL_add_named_parameter(pointmatcher_config_t, pointmatcher_config, pointmatcher_config) CGAL_add_named_parameter(adjacencies_t, adjacencies, adjacencies) +CGAL_add_named_parameter(scan_angle_t, scan_angle, scan_angle) +CGAL_add_named_parameter(scan_direction_flag_t, scan_direction_flag, scan_direction_flag) // List of named parameters used in Surface_mesh_approximation package CGAL_add_named_parameter(verbose_level_t, verbose_level, verbose_level) diff --git a/Point_set_processing_3/examples/Point_set_processing_3/CMakeLists.txt b/Point_set_processing_3/examples/Point_set_processing_3/CMakeLists.txt index 83253368016..bee5fc22af0 100644 --- a/Point_set_processing_3/examples/Point_set_processing_3/CMakeLists.txt +++ b/Point_set_processing_3/examples/Point_set_processing_3/CMakeLists.txt @@ -59,18 +59,24 @@ if ( CGAL_FOUND ) target_link_libraries(${target} ${CGAL_libs}) endforeach() + # Use Eigen + find_package(Eigen3 3.1.0) #(requires 3.1.0 or greater) + include(CGAL_Eigen_support) + find_package(LASLIB) include(CGAL_LASLIB_support) if (TARGET CGAL::LASLIB_support) add_executable( read_las_example "read_las_example.cpp" ) target_link_libraries(read_las_example ${CGAL_libs} CGAL::LASLIB_support) + if (TARGET CGAL::Eigen_support) + add_executable( orient_lidar_example "orient_lidar_example.cpp" ) + target_link_libraries(orient_lidar_example ${CGAL_libs} + CGAL::Eigen_support CGAL::LASLIB_support) + endif() else() message(STATUS "NOTICE : the LAS reader test requires LASlib and will not be compiled.") endif() - # Use Eigen - find_package(Eigen3 3.1.0) #(requires 3.1.0 or greater) - include(CGAL_Eigen_support) if (TARGET CGAL::Eigen_support) set(CGAL_libs ${CGAL_libs} CGAL::Eigen_support) diff --git a/Point_set_processing_3/examples/Point_set_processing_3/orient_lidar_example.cpp b/Point_set_processing_3/examples/Point_set_processing_3/orient_lidar_example.cpp new file mode 100644 index 00000000000..aae675e0088 --- /dev/null +++ b/Point_set_processing_3/examples/Point_set_processing_3/orient_lidar_example.cpp @@ -0,0 +1,89 @@ +#include +#include +#include +#include +#include + +using Kernel = CGAL::Simple_cartesian; +using Point_3 = Kernel::Point_3; +using Vector_3 = Kernel::Vector_3; +using Point_with_info = std::tuple; +using Point_map = CGAL::Nth_of_tuple_property_map<0, Point_with_info>; +using Normal_map = CGAL::Nth_of_tuple_property_map<1, Point_with_info>; +using Scan_angle_map = CGAL::Nth_of_tuple_property_map<2, Point_with_info>; +using Scan_direction_flag_map = CGAL::Nth_of_tuple_property_map<3, Point_with_info>; + +void dump (const char* filename, const std::vector& points) +{ + std::ofstream ofile (filename, std::ios::binary); + CGAL::set_binary_mode(ofile); + CGAL::write_ply_points + (ofile, points, + CGAL::parameters::point_map (Point_map()). + normal_map (Normal_map())); + +} + +int main (int argc, char** argv) +{ + std::string fname (argc > 1 ? argv[1] : "data/test.las"); + + std::vector points; + + std::cerr << "Reading input file " << fname << std::endl; + std::ifstream ifile (fname, std::ios::binary); + if (!ifile || + !CGAL::read_las_points_with_properties + (ifile, std::back_inserter (points), + CGAL::make_las_point_reader (Point_map()), + std::make_pair (Scan_angle_map(), + CGAL::LAS_property::Scan_angle()), + std::make_pair (Scan_direction_flag_map(), + CGAL::LAS_property::Scan_direction_flag()))) + { + std::cerr << "Can't read " << fname << std::endl; + return EXIT_FAILURE; + } + + points.resize(100000); + + std::cerr << "Estimating normals" << std::endl; + CGAL::jet_estimate_normals + (points, 12, + CGAL::parameters::point_map (Point_map()). + normal_map (Normal_map())); + + std::cerr << "Orienting normals using scan angle and direction flag" << std::endl; + CGAL::lidar_orient_normals + (points, + CGAL::parameters::point_map (Point_map()). + normal_map (Normal_map()). + scan_angle (Scan_angle_map()). + scan_direction_flag (Scan_direction_flag_map())); + dump("out_angle_and_flag.ply", points); + + std::cerr << "Orienting normals using scan direction flag only" << std::endl; + CGAL::lidar_orient_normals + (points, + CGAL::parameters::point_map (Point_map()). + normal_map (Normal_map()). + scan_direction_flag (Scan_direction_flag_map())); + dump("out_flag.ply", points); + + std::cerr << "Orienting normals using scan angle only" << std::endl; + CGAL::lidar_orient_normals + (points, + CGAL::parameters::point_map (Point_map()). + normal_map (Normal_map()). + scan_angle (Scan_angle_map())); + dump("out_angle.ply", points); + + std::cerr << "Orienting normals using no additional info" << std::endl; + CGAL::lidar_orient_normals + (points, + CGAL::parameters::point_map (Point_map()). + normal_map (Normal_map())); + dump("out_nothing.ply", points); + + return EXIT_SUCCESS; +} diff --git a/Point_set_processing_3/include/CGAL/lidar_orient_normals.h b/Point_set_processing_3/include/CGAL/lidar_orient_normals.h new file mode 100644 index 00000000000..c198c1beed7 --- /dev/null +++ b/Point_set_processing_3/include/CGAL/lidar_orient_normals.h @@ -0,0 +1,302 @@ +// Copyright (c) 2020 GeometryFactory Sarl (France). +// All rights reserved. +// +// This file is part of CGAL (www.cgal.org). +// +// $URL$ +// $Id$ +// SPDX-License-Identifier: GPL-3.0-or-later OR LicenseRef-Commercial +// +// Author(s) : Simon Giraudot + +#ifndef CGAL_LIDAR_ORIENT_NORMALS_H +#define CGAL_LIDAR_ORIENT_NORMALS_H + +#include + +#include + +#include +#include + +#include + +namespace CGAL +{ + +/// \cond SKIP_IN_MANUAL +namespace Point_set_processing_3 +{ + +namespace internal +{ + +template +bool is_end_of_scanline (Iterator scanline_begin, Iterator it, + PointMap, + ScanDirectionFlagMap scan_direction_flag_map, + const Tag_false&) // no fallback +{ + return (get (scan_direction_flag_map, *scanline_begin) + != get (scan_direction_flag_map, *it)); +} + +template +bool is_end_of_scanline (Iterator scanline_begin, Iterator it, + PointMap point_map, + ScanDirectionFlagMap, + 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 +typename Kernel_traits::value_type>::Kernel::Vector_3 +scanline_direction (Iterator begin, Iterator end, + PointMap point_map) +{ + using Vector_3 = typename Kernel_traits::value_type>::Kernel::Vector_3; + Iterator last = end; -- last; + Vector_3 direction (get (point_map, *begin), + get (point_map, *last)); + direction = Vector_3 (direction.x(), direction.y(), 0); + direction = direction / CGAL::approximate_sqrt (direction.squared_length()); + return direction; +} + +template +void orient_scanline (Iterator begin, Iterator end, + PointMap point_map, + NormalMap normal_map, + ScanDirectionFlagMap scan_direction_flag, + ScanAngleMap scan_angle_map, + const Tag_false&, // no fallback direction flag + const Tag_false&) // no fallback scan angle +{ + using Vector_3 = typename boost::property_traits::value_type; + + static const Vector_3 vertical (0, 0, 1); + + Vector_3 direction + = Point_set_processing_3::internal::scanline_direction + (begin, end, point_map); + if (get (scan_direction_flag, *begin) == 1) + direction = -direction; + + for (Iterator it = begin; it != end; ++ it) + { + double angle = CGAL_PI * double(get (scan_angle_map, *it)) / 180.; + Vector_3 line_of_sight + = direction * std::sin(angle) + vertical * std::cos(angle); + const Vector_3& normal = get (normal_map, *it); + if (line_of_sight * normal < 0) + put (normal_map, *it, -normal); + } +} + +template +void orient_scanline (Iterator begin, Iterator end, + PointMap point_map, + NormalMap normal_map, + ScanDirectionFlagMap, + ScanAngleMap scan_angle_map, + const Tag_true&, // no fallback direction flag + const Tag_false&) // fallback scan angle +{ + using Point_3 = typename boost::property_traits::value_type; + using Vector_3 = typename boost::property_traits::value_type; + + // Estimate scanner position: + // above point with minimum scan angle + float min_scan_angle = (std::numeric_limits::max)(); + Iterator chosen = begin; + double min_z = (std::numeric_limits::max)(); + double max_z = -(std::numeric_limits::max)(); + + for (Iterator it = begin; it != end; ++ it) + { + const Point_3& p = get (point_map, *it); + min_z = (std::min(min_z, p.z())); + max_z = (std::max(max_z, p.z())); + float scan_angle = get (scan_angle_map, *it); + if (scan_angle < min_scan_angle) + { + min_scan_angle = scan_angle; + chosen = it; + } + } + + const Point_3& chosen_point = get (point_map, *chosen); + Point_3 scan_position (chosen_point.x(), + chosen_point.y(), + min_z + 10 * (max_z - min_z)); + + 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 (line_of_sight * normal < 0) + put (normal_map, *it, -normal); + } +} + +template +void orient_scanline (Iterator begin, Iterator end, + PointMap point_map, + NormalMap normal_map, + ScanDirectionFlagMap, + ScanAngleMap, + const FallbackFlag&, // either fallback direction flag or not + const Tag_true&) // fallback scan angle +{ + using Point_3 = typename boost::property_traits::value_type; + using Vector_3 = typename boost::property_traits::value_type; + + // Estimate scanner position: + // average XY-projected point, located above + double mean_x = 0.; + double mean_y = 0.; + double min_z = (std::numeric_limits::max)(); + 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(); + min_z = (std::min(min_z, p.z())); + max_z = (std::max(max_z, p.z())); + ++ nb; + } + + Point_3 scan_position (mean_x / nb, mean_y / nb, + min_z + 10 * (max_z - min_z)); + + 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 (line_of_sight * normal < 0) + put (normal_map, *it, -normal); + } +} + +} // namespace internal + +} // namespace Point_set_processing_3 +/// \endcond + +// ---------------------------------------------------------------------------- +// Public section +// ---------------------------------------------------------------------------- + +template +void lidar_orient_normals (PointRange& points, const NamedParameters& np) +{ + using parameters::choose_parameter; + using parameters::get_parameter; + + using Iterator = typename PointRange::iterator; + using PointMap = typename CGAL::GetPointMap::type; + using NormalMap = typename Point_set_processing_3::GetNormalMap::type; + using Kernel = typename Point_set_processing_3::GetK::Kernel; + using Point_3 = typename Kernel::Point_3; + using Vector_3 = typename Kernel::Vector_3; + using ScanAngleMap = typename Point_set_processing_3::GetScanAngleMap + ::type; + using ScanDirectionFlagMap = typename Point_set_processing_3::GetScanDirectionFlag + ::type; + + CGAL_static_assertion_msg(!(std::is_same::NoMap>::value), + "Error: no normal map"); + + using Fallback_scan_angle + = Boolean_tag + ::NoMap>::value>; + using Fallback_scan_direction_flag + = Boolean_tag + ::NoMap>::value>; + + + 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)); + ScanDirectionFlagMap scan_direction_flag_map = choose_parameter + (get_parameter(np, internal_np::scan_direction_flag)); + + std::size_t nb_scanlines = 1; + + Iterator scanline_begin = points.begin(); + for (Iterator it = points.begin(); it != points.end(); ++ it) + { + if (Point_set_processing_3::internal::is_end_of_scanline + (scanline_begin, it, point_map, scan_direction_flag_map, + Fallback_scan_direction_flag())) + { + Point_set_processing_3::internal::orient_scanline + (scanline_begin, it, point_map, normal_map, + scan_direction_flag_map, scan_angle_map, + Fallback_scan_direction_flag(), Fallback_scan_angle()); + + scanline_begin = it; + ++ nb_scanlines; + } + } + + Point_set_processing_3::internal::orient_scanline + (scanline_begin, points.end(), point_map, normal_map, + scan_direction_flag_map, scan_angle_map, + Fallback_scan_direction_flag(), Fallback_scan_angle()); + + std::cerr << nb_scanlines << " scanline(s) identified (mean length = " + << std::size_t(points.size() / double(nb_scanlines)) + << " point(s))" << std::endl; +} + +template +void lidar_orient_normals (PointRange& points) +{ + return lidar_orient_normals (points, + CGAL::Point_set_processing_3::parameters::all_default(points)); +} + +} // namespace CGAL + + +#endif // CGAL_LIDAR_ORIENT_NORMALS_H