From 12fdb3355c4a914fb15bc0588987966156c13c14 Mon Sep 17 00:00:00 2001 From: Lars Glud Date: Thu, 27 Nov 2025 15:53:20 +0100 Subject: [PATCH 1/6] Added gp3 benchmark --- benchmarks/CMakeLists.txt | 6 +++- benchmarks/surface/gp3.cpp | 60 ++++++++++++++++++++++++++++++++++++++ 2 files changed, 65 insertions(+), 1 deletion(-) create mode 100644 benchmarks/surface/gp3.cpp diff --git a/benchmarks/CMakeLists.txt b/benchmarks/CMakeLists.txt index 5ef8f0eb3f4..10511c02879 100644 --- a/benchmarks/CMakeLists.txt +++ b/benchmarks/CMakeLists.txt @@ -1,6 +1,6 @@ set(SUBSYS_NAME benchmarks) set(SUBSYS_DESC "Point cloud library benchmarks") -set(SUBSYS_DEPS common filters features search kdtree io sample_consensus) +set(SUBSYS_DEPS common filters features search kdtree io sample_consensus surface) PCL_SUBSYS_OPTION(build "${SUBSYS_NAME}" "${SUBSYS_DESC}" OFF) PCL_SUBSYS_DEPEND(build NAME ${SUBSYS_NAME} DEPS ${SUBSYS_DEPS}) @@ -35,3 +35,7 @@ PCL_ADD_BENCHMARK(sample_consensus_sac_model_cylinder FILES sample_consensus/sac PCL_ADD_BENCHMARK(search_radius_search FILES search/radius_search.cpp LINK_WITH pcl_io pcl_search pcl_filters ARGUMENTS "${PCL_SOURCE_DIR}/test/table_scene_mug_stereo_textured.pcd") + +PCL_ADD_BENCHMARK(gp3 FILES surface/gp3.cpp + LINK_WITH pcl_io pcl_filters pcl_surface + ARGUMENTS "${PCL_SOURCE_DIR}/test/table_scene_mug_stereo_textured.pcd") diff --git a/benchmarks/surface/gp3.cpp b/benchmarks/surface/gp3.cpp new file mode 100644 index 00000000000..d7ee5d224e5 --- /dev/null +++ b/benchmarks/surface/gp3.cpp @@ -0,0 +1,60 @@ +#include +#include +#include +#include +#include + +#include + +#include + +void +print_help() +{ + std::cout << "Usage: benchmark_gp3 \n"; +} + +static void +BM_gp3(benchmark::State& state, const pcl::PointCloud::Ptr cloudIn) +{ + pcl::GreedyProjectionTriangulation gp3; + gp3.setSearchRadius(0.025); + gp3.setMu(2.5); + gp3.setMaximumNearestNeighbors(100); + gp3.setMinimumAngle(M_PI / 18); // 10 degrees + gp3.setMaximumAngle(2 * M_PI / 3); // 120 degrees + gp3.setNormalConsistency(false); + pcl::PolygonMesh triangles; + gp3.setInputCloud(cloudIn); + + for (auto _ : state) { + gp3.reconstruct(triangles); + } +} + +int +main(int argc, char** argv) +{ + if (argc < 2) { + std::cerr << "No test file given. Please provide a PCD file for the benchmark." + << std::endl; + print_help(); + return -1; + } + + pcl::PointCloud::Ptr cloudIn(new pcl::PointCloud); + pcl::PCDReader reader; + reader.read(argv[1], *cloudIn); + + // Filter out nans + pcl::PointCloud::Ptr cloudFiltered( + new pcl::PointCloud); + pcl::Indices indices; + pcl::removeNaNFromPointCloud(*cloudIn, *cloudFiltered, indices); + + benchmark::RegisterBenchmark("gp3", &BM_gp3, cloudFiltered) + ->Unit(benchmark::kMillisecond); + + benchmark::Initialize(&argc, argv); + benchmark::RunSpecifiedBenchmarks(); +} From 41193b39a20eae5560aba54d7dccb847492ec7e0 Mon Sep 17 00:00:00 2001 From: Lars Glud Date: Thu, 27 Nov 2025 15:53:37 +0100 Subject: [PATCH 2/6] Compute angle only once --- surface/include/pcl/surface/impl/gp3.hpp | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/surface/include/pcl/surface/impl/gp3.hpp b/surface/include/pcl/surface/impl/gp3.hpp index 2b5dcf656d2..7ba2bf48814 100644 --- a/surface/include/pcl/surface/impl/gp3.hpp +++ b/surface/include/pcl/surface/impl/gp3.hpp @@ -135,6 +135,7 @@ pcl::GreedyProjectionTriangulation::reconstructPolygons (std::vector

> uvn_nn (nnn_); Eigen::Vector2f uvn_s; + const double cos_eps_angle_ = std::cos(eps_angle_); // iterating through fringe points and finishing them until everything is done while (is_free != NONE) @@ -374,10 +375,13 @@ pcl::GreedyProjectionTriangulation::reconstructPolygons (std::vector

1) cosine = 1; if (cosine < -1) cosine = -1; - double angle = std::acos (cosine); - if ((!consistent_) && (angle > M_PI/2)) - angle = M_PI - angle; - if (angle > eps_angle_) + + bool too_large_angle; + if (consistent_) + too_large_angle = (cosine < cos_eps_angle_); + else + too_large_angle = (std::fabs(cosine) < cos_eps_angle_); + if (too_large_angle) { angles_[i].visible = false; same_side = false; From f1ad81af2b92b1df32563c9ec60d763695403cb0 Mon Sep 17 00:00:00 2001 From: Lars Glud Date: Thu, 27 Nov 2025 15:53:45 +0100 Subject: [PATCH 3/6] Use std::partition and std::sort instead of just sort --- surface/include/pcl/surface/gp3.h | 13 ------------- surface/include/pcl/surface/impl/gp3.hpp | 5 ++++- 2 files changed, 4 insertions(+), 14 deletions(-) diff --git a/surface/include/pcl/surface/gp3.h b/surface/include/pcl/surface/gp3.h index 98d91fd8d16..e38d61d8cb5 100644 --- a/surface/include/pcl/surface/gp3.h +++ b/surface/include/pcl/surface/gp3.h @@ -473,19 +473,6 @@ namespace pcl part_[v] = part_[s]; fringe_queue_.push_back(v); } - - /** \brief Function for ascending sort of nnAngle, taking visibility into account - * (angles to visible neighbors will be first, to the invisible ones after). - * \param[in] a1 the first angle - * \param[in] a2 the second angle - */ - static inline bool - nnAngleSortAsc (const nnAngle& a1, const nnAngle& a2) - { - if (a1.visible == a2.visible) - return (a1.angle < a2.angle); - return a1.visible; - } }; } // namespace pcl diff --git a/surface/include/pcl/surface/impl/gp3.hpp b/surface/include/pcl/surface/impl/gp3.hpp index 7ba2bf48814..a2fa960c93f 100644 --- a/surface/include/pcl/surface/impl/gp3.hpp +++ b/surface/include/pcl/surface/impl/gp3.hpp @@ -40,6 +40,8 @@ #include +#include + ///////////////////////////////////////////////////////////////////////////////////////////// template void pcl::GreedyProjectionTriangulation::performReconstruction (pcl::PolygonMesh &output) @@ -479,7 +481,8 @@ pcl::GreedyProjectionTriangulation::reconstructPolygons (std::vector

::nnAngleSortAsc); + const auto visible_end = std::partition(angles_.begin(), angles_.end(), [](const nnAngle& a){ return a.visible; }); + std::sort(angles_.begin(), visible_end, [](const nnAngle& a, const nnAngle& b){ return a.angle < b.angle; }); // Triangulating if (angles_[2].visible == false) From 6df5aa3bdd57f1df3ac2e5934a58fa33ddbea1b3 Mon Sep 17 00:00:00 2001 From: Lars Glud Date: Thu, 27 Nov 2025 15:53:52 +0100 Subject: [PATCH 4/6] Replace repeated free-point scan with queue. --- surface/include/pcl/surface/impl/gp3.hpp | 29 ++++++++++++------------ 1 file changed, 14 insertions(+), 15 deletions(-) diff --git a/surface/include/pcl/surface/impl/gp3.hpp b/surface/include/pcl/surface/impl/gp3.hpp index a2fa960c93f..1a3b2366263 100644 --- a/surface/include/pcl/surface/impl/gp3.hpp +++ b/surface/include/pcl/surface/impl/gp3.hpp @@ -41,6 +41,7 @@ #include #include +#include ///////////////////////////////////////////////////////////////////////////////////////////// template void @@ -133,17 +134,25 @@ pcl::GreedyProjectionTriangulation::reconstructPolygons (std::vector

> uvn_nn (nnn_); Eigen::Vector2f uvn_s; const double cos_eps_angle_ = std::cos(eps_angle_); + // Initialize queue with initial FREE points (avoid repeated scans) + std::deque free_queue; + for (int i = 0; i < static_cast(indices_->size()); ++i) + if (state_[i] == FREE) + free_queue.push_back(i); + // iterating through fringe points and finishing them until everything is done - while (is_free != NONE) + while (!free_queue.empty()) { - R_ = is_free; - if (state_[R_] == FREE) + R_ = free_queue.front(); + free_queue.pop_front(); + if (state_[R_] != FREE) + continue; { state_[R_] = NONE; part_[R_] = part_index++; @@ -172,7 +181,7 @@ pcl::GreedyProjectionTriangulation::reconstructPolygons (std::vector

doubleEdges; @@ -270,16 +279,6 @@ pcl::GreedyProjectionTriangulation::reconstructPolygons (std::vector

size (); temp++) - { - if (state_[temp] == FREE) - { - is_free = temp; - break; - } - } - bool is_fringe = true; while (is_fringe) { From acd094f45db599b3d6613d33731e6e728e191560 Mon Sep 17 00:00:00 2001 From: Lars Glud Date: Thu, 27 Nov 2025 15:54:01 +0100 Subject: [PATCH 5/6] Cache normals, v_local, u_local and proj_qp_list instead of recomputing them. --- surface/include/pcl/surface/gp3.h | 7 ++- surface/include/pcl/surface/impl/gp3.hpp | 55 +++++++++++++----------- 2 files changed, 37 insertions(+), 25 deletions(-) diff --git a/surface/include/pcl/surface/gp3.h b/surface/include/pcl/surface/gp3.h index e38d61d8cb5..85317278fe8 100644 --- a/surface/include/pcl/surface/gp3.h +++ b/surface/include/pcl/surface/gp3.h @@ -309,7 +309,12 @@ namespace pcl /** \brief Temporary variable to store a triangle (as a set of point indices) **/ pcl::Vertices triangle_{}; /** \brief Temporary variable to store point coordinates **/ - std::vector > coords_{}; + std::vector coords_{}; + // Precomputed per-point data (normal, local basis u,v, and projection of point onto its normal plane) + std::vector normals_{}; + std::vector u_basis_{}; + std::vector v_basis_{}; + std::vector proj_qp_list_{}; /** \brief A list of angles to neighbors **/ std::vector angles_{}; diff --git a/surface/include/pcl/surface/impl/gp3.hpp b/surface/include/pcl/surface/impl/gp3.hpp index 1a3b2366263..177e2d91b5a 100644 --- a/surface/include/pcl/surface/impl/gp3.hpp +++ b/surface/include/pcl/surface/impl/gp3.hpp @@ -123,14 +123,34 @@ pcl::GreedyProjectionTriangulation::reconstructPolygons (std::vector

size ()); + normals_.clear (); + normals_.reserve (indices_->size ()); + u_basis_.clear (); + u_basis_.reserve (indices_->size ()); + v_basis_.clear (); + v_basis_.reserve (indices_->size ()); + proj_qp_list_.clear (); + proj_qp_list_.reserve (indices_->size ()); std::vector point2index (input_->size (), -1); for (int cp = 0; cp < static_cast (indices_->size ()); ++cp) { - coords_.push_back((*input_)[(*indices_)[cp]].getVector3fMap()); - point2index[(*indices_)[cp]] = cp; + const auto idx = (*indices_)[cp]; + const auto& p = (*input_)[idx]; + const Eigen::Vector3f xyz = p.getVector3fMap(); + coords_.push_back(xyz); + point2index[idx] = cp; + const Eigen::Vector3f n = p.getNormalVector3fMap(); + normals_.push_back(n); + // local basis + const Eigen::Vector3f v_local = n.unitOrthogonal(); + const Eigen::Vector3f u_local = n.cross(v_local); + v_basis_.push_back(v_local); + u_basis_.push_back(u_local); + const float dist = n.dot(xyz); + proj_qp_list_.emplace_back(xyz - dist * n); } // Initializing @@ -171,16 +191,10 @@ pcl::GreedyProjectionTriangulation::reconstructPolygons (std::vector

::reconstructPolygons (std::vector

::reconstructPolygons (std::vector

1) cosine = 1; if (cosine < -1) cosine = -1; From 2c172d6fda5135289930871d2d55802f1ce0d7c2 Mon Sep 17 00:00:00 2001 From: Lars Glud Date: Thu, 27 Nov 2025 15:54:10 +0100 Subject: [PATCH 6/6] Reuse vector for double_edges. --- surface/include/pcl/surface/gp3.h | 3 +- surface/include/pcl/surface/impl/gp3.hpp | 46 ++++++++++++------------ 2 files changed, 25 insertions(+), 24 deletions(-) diff --git a/surface/include/pcl/surface/gp3.h b/surface/include/pcl/surface/gp3.h index 85317278fe8..2a3f3b6a789 100644 --- a/surface/include/pcl/surface/gp3.h +++ b/surface/include/pcl/surface/gp3.h @@ -374,7 +374,8 @@ namespace pcl /** \brief Temporary variable to store 3 coordinates **/ Eigen::Vector3f tmp_; - + /** \brief Reusable buffer for projected boundary edges to avoid repeated allocations **/ + std::vector double_edges_{}; /** \brief The actual surface reconstruction method. * \param[out] output the resultant polygonal mesh */ diff --git a/surface/include/pcl/surface/impl/gp3.hpp b/surface/include/pcl/surface/impl/gp3.hpp index 177e2d91b5a..d58315e1373 100644 --- a/surface/include/pcl/surface/impl/gp3.hpp +++ b/surface/include/pcl/surface/impl/gp3.hpp @@ -181,7 +181,7 @@ pcl::GreedyProjectionTriangulation::reconstructPolygons (std::vector

nearestKSearch ((*input_)[(*indices_)[R_]], nnn_, nnIdx, sqrDists); tree_->nearestKSearch (indices_->at (R_), nnn_, nnIdx, sqrDists); - double sqr_dist_threshold = (std::min)(sqr_max_edge, sqr_mu * sqrDists[1]); + const double sqr_dist_threshold = (std::min)(sqr_max_edge, sqr_mu * sqrDists[1]); // Search tree returns indices into the original cloud, but we are working with indices. TODO: make that optional! for (int i = 1; i < nnn_; i++) @@ -198,7 +198,8 @@ pcl::GreedyProjectionTriangulation::reconstructPolygons (std::vector

doubleEdges; + double_edges_.clear(); + double_edges_.reserve(nnn_); for (int i = 1; i < nnn_; i++) // nearest neighbor with index 0 is the query point R_ itself { // Transforming coordinates @@ -229,7 +230,7 @@ pcl::GreedyProjectionTriangulation::reconstructPolygons (std::vector

::reconstructPolygons (std::vector

::reconstructPolygons (std::vector

at (R_) << "] " << i << ": " << nnIdx[i] << " / " << point2index[nnIdx[i]] << std::endl; nnIdx[i] = point2index[nnIdx[i]]; } - - // Locating FFN and SFN to adapt distance threshold - double sqr_source_dist = (coords_[R_] - coords_[source_[R_]]).squaredNorm (); - double sqr_ffn_dist = (coords_[R_] - coords_[ffn_[R_]]).squaredNorm (); - double sqr_sfn_dist = (coords_[R_] - coords_[sfn_[R_]]).squaredNorm (); - double max_sqr_fn_dist = (std::max)(sqr_ffn_dist, sqr_sfn_dist); - double sqr_dist_threshold = (std::min)(sqr_max_edge, sqr_mu * sqrDists[1]); //sqr_mu * sqr_avg_conn_dist); + const double sqr_source_dist = (coords_[R_] - coords_[source_[R_]]).squaredNorm (); + const double sqr_ffn_dist = (coords_[R_] - coords_[ffn_[R_]]).squaredNorm (); + const double sqr_sfn_dist = (coords_[R_] - coords_[sfn_[R_]]).squaredNorm (); + const double max_sqr_fn_dist = (std::max)(sqr_ffn_dist, sqr_sfn_dist); + const double sqr_dist_threshold = (std::min)(sqr_max_edge, sqr_mu * sqrDists[1]); if (max_sqr_fn_dist > sqrDists[nnn_-1]) { if (0 == increase_nnn4fn) @@ -342,7 +341,7 @@ pcl::GreedyProjectionTriangulation::reconstructPolygons (std::vector

sqrDists[nnn_-1]) { if (0 == increase_nnn4s) @@ -356,8 +355,9 @@ pcl::GreedyProjectionTriangulation::reconstructPolygons (std::vector

doubleEdges; - for (int i = 1; i < nnn_; i++) // nearest neighbor with index 0 is the query point R_ itself + double_edges_.clear(); + double_edges_.reserve(nnn_); + for (int i = 1; i < nnn_; ++i) // nearest neighbor with index 0 is the query point R_ itself { tmp_ = coords_[nnIdx[i]] - proj_qp_; uvn_nn[i][0] = tmp_.dot(u_); @@ -406,7 +406,7 @@ pcl::GreedyProjectionTriangulation::reconstructPolygons (std::vector

::reconstructPolygons (std::vector