Skip to content

GP3 runtime improvements #6374

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
larshg wants to merge 6 commits into
base: master
Choose a base branch
from
+153 −83
Open

GP3 runtime improvements #6374

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
12fdb33
Added gp3 benchmark
larshg Nov 27, 2025
41193b3
Compute angle only once
larshg Nov 27, 2025
f1ad81a
Use std::partition and std::sort instead of just sort
larshg Nov 27, 2025
6df5aa3
Replace repeated free-point scan with queue.
larshg Nov 27, 2025
acd094f
Cache normals, v_local, u_local and proj_qp_list instead of recomputi…
larshg Nov 27, 2025
2c172d6
Reuse vector for double_edges.
larshg Nov 27, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
6 changes: 5 additions & 1 deletion benchmarks/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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})
Expand Down Expand Up @@ -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")
60 changes: 60 additions & 0 deletions benchmarks/surface/gp3.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,60 @@
#include <pcl/filters/filter.h>
#include <pcl/io/pcd_io.h>
#include <pcl/surface/gp3.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

#include <benchmark/benchmark.h>

#include <chrono>

void
print_help()
{
std::cout << "Usage: benchmark_gp3 <pcd filename>\n";
}

static void
BM_gp3(benchmark::State& state, const pcl::PointCloud<pcl::PointNormal>::Ptr cloudIn)
{
pcl::GreedyProjectionTriangulation<pcl::PointNormal> 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<pcl::PointNormal>::Ptr cloudIn(new pcl::PointCloud<pcl::PointNormal>);
pcl::PCDReader reader;
reader.read(argv[1], *cloudIn);

// Filter out nans
pcl::PointCloud<pcl::PointNormal>::Ptr cloudFiltered(
new pcl::PointCloud<pcl::PointNormal>);
pcl::Indices indices;
pcl::removeNaNFromPointCloud(*cloudIn, *cloudFiltered, indices);

benchmark::RegisterBenchmark("gp3", &BM_gp3, cloudFiltered)
->Unit(benchmark::kMillisecond);

benchmark::Initialize(&argc, argv);
benchmark::RunSpecifiedBenchmarks();
}
23 changes: 8 additions & 15 deletions surface/include/pcl/surface/gp3.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > coords_{};
std::vector<Eigen::Vector3f> coords_{};
// Precomputed per-point data (normal, local basis u,v, and projection of point onto its normal plane)
std::vector<Eigen::Vector3f> normals_{};
std::vector<Eigen::Vector3f> u_basis_{};
std::vector<Eigen::Vector3f> v_basis_{};
std::vector<Eigen::Vector3f> proj_qp_list_{};

/** \brief A list of angles to neighbors **/
std::vector<nnAngle> angles_{};
Expand Down Expand Up @@ -369,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<doubleEdge> double_edges_{};
/** \brief The actual surface reconstruction method.
* \param[out] output the resultant polygonal mesh
*/
Expand Down Expand Up @@ -473,19 +479,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
Expand Down
147 changes: 80 additions & 67 deletions surface/include/pcl/surface/impl/gp3.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -40,6 +40,9 @@

#include <pcl/surface/gp3.h>

#include <algorithm>
#include <deque>

/////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointInT> void
pcl::GreedyProjectionTriangulation<PointInT>::performReconstruction (pcl::PolygonMesh &output)
Expand Down Expand Up @@ -120,27 +123,56 @@ pcl::GreedyProjectionTriangulation<PointInT>::reconstructPolygons (std::vector<p
state_[idx] = NONE;
}

// Saving coordinates and point to index mapping
// Saving coordinates and point to index mapping + precomputations
coords_.clear ();
coords_.reserve (indices_->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<int> point2index (input_->size (), -1);
for (int cp = 0; cp < static_cast<int> (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
int is_free=0, nr_parts=0, increase_nnn4fn=0, increase_nnn4s=0, increase_dist=0;
int nr_parts=0, increase_nnn4fn=0, increase_nnn4s=0, increase_dist=0;
angles_.resize(nnn_);
std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > 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<int> free_queue;
for (int i = 0; i < static_cast<int>(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++;
Expand All @@ -149,7 +181,7 @@ pcl::GreedyProjectionTriangulation<PointInT>::reconstructPolygons (std::vector<p
//searchForNeighbors ((*indices_)[R_], nnIdx, sqrDists);
//tree_->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++)
Expand All @@ -159,20 +191,15 @@ pcl::GreedyProjectionTriangulation<PointInT>::reconstructPolygons (std::vector<p
nnIdx[i] = point2index[nnIdx[i]];
}

// Get the normal estimate at the current point
const Eigen::Vector3f nc = (*input_)[(*indices_)[R_]].getNormalVector3fMap ();

// Get a coordinate system that lies on a plane defined by its normal
v_ = nc.unitOrthogonal ();
u_ = nc.cross (v_);

// Projecting point onto the surface
float dist = nc.dot (coords_[R_]);
proj_qp_ = coords_[R_] - dist * nc;

// Reuse precomputed normal, basis and projection
u_ = u_basis_[R_];
v_ = v_basis_[R_];
proj_qp_ = proj_qp_list_[R_];

// Converting coords, calculating angles and saving the projected near boundary edges
int nr_edge = 0;
std::vector<doubleEdge> 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
Expand Down Expand Up @@ -203,7 +230,7 @@ pcl::GreedyProjectionTriangulation<PointInT>::reconstructPolygons (std::vector<p
tmp_ = coords_[sfn_[nnIdx[i]]] - proj_qp_;
e.second[0] = tmp_.dot(u_);
e.second[1] = tmp_.dot(v_);
doubleEdges.push_back(e);
double_edges_.push_back(e);
}
}
angles_[0].visible = false;
Expand All @@ -215,12 +242,12 @@ pcl::GreedyProjectionTriangulation<PointInT>::reconstructPolygons (std::vector<p
bool visibility = true;
for (int j = 0; j < nr_edge; j++)
{
if (ffn_[nnIdx[doubleEdges[j].index]] != nnIdx[i])
visibility = isVisible(uvn_nn[i], uvn_nn[doubleEdges[j].index], doubleEdges[j].first, Eigen::Vector2f::Zero());
if (ffn_[nnIdx[double_edges_[j].index]] != nnIdx[i])
visibility = isVisible(uvn_nn[i], uvn_nn[double_edges_[j].index], double_edges_[j].first, Eigen::Vector2f::Zero());
if (!visibility)
break;
if (sfn_[nnIdx[doubleEdges[j].index]] != nnIdx[i])
visibility = isVisible(uvn_nn[i], uvn_nn[doubleEdges[j].index], doubleEdges[j].second, Eigen::Vector2f::Zero());
if (sfn_[nnIdx[double_edges_[j].index]] != nnIdx[i])
visibility = isVisible(uvn_nn[i], uvn_nn[double_edges_[j].index], double_edges_[j].second, Eigen::Vector2f::Zero());
if (!visibility)
break;
}
Expand Down Expand Up @@ -267,16 +294,6 @@ pcl::GreedyProjectionTriangulation<PointInT>::reconstructPolygons (std::vector<p
while (not_found);
}

is_free = NONE;
for (std::size_t temp = 0; temp < indices_->size (); temp++)
{
if (state_[temp] == FREE)
{
is_free = temp;
break;
}
}

bool is_fringe = true;
while (is_fringe)
{
Expand Down Expand Up @@ -311,13 +328,11 @@ pcl::GreedyProjectionTriangulation<PointInT>::reconstructPolygons (std::vector<p
// std::cerr << R_ << " [" << indices_->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)
Expand All @@ -326,29 +341,23 @@ pcl::GreedyProjectionTriangulation<PointInT>::reconstructPolygons (std::vector<p
state_[R_] = BOUNDARY;
continue;
}
double max_sqr_fns_dist = (std::max)(sqr_source_dist, max_sqr_fn_dist);
const double max_sqr_fns_dist = (std::max)(sqr_source_dist, max_sqr_fn_dist);
if (max_sqr_fns_dist > sqrDists[nnn_-1])
{
if (0 == increase_nnn4s)
PCL_WARN("Not enough neighbors are considered: source of R=%d is out of range! Consider increasing nnn_...\n", R_);
increase_nnn4s++;
}

// Get the normal estimate at the current point
const Eigen::Vector3f nc = (*input_)[(*indices_)[R_]].getNormalVector3fMap ();

// Get a coordinate system that lies on a plane defined by its normal
v_ = nc.unitOrthogonal ();
u_ = nc.cross (v_);

// Projecting point onto the surface
float dist = nc.dot (coords_[R_]);
proj_qp_ = coords_[R_] - dist * nc;
// Reuse precomputed normal, basis and projection
u_ = u_basis_[R_]; v_ = v_basis_[R_]; proj_qp_ = proj_qp_list_[R_];
const Eigen::Vector3f& nc = normals_[R_];

// Converting coords, calculating angles and saving the projected near boundary edges
int nr_edge = 0;
std::vector<doubleEdge> 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_);
Expand All @@ -370,14 +379,17 @@ pcl::GreedyProjectionTriangulation<PointInT>::reconstructPolygons (std::vector<p
if ((ffn_[R_] == nnIdx[i]) || (sfn_[R_] == nnIdx[i]))
angles_[i].visible = true;
bool same_side = true;
const Eigen::Vector3f neighbor_normal = (*input_)[(*indices_)[nnIdx[i]]].getNormalVector3fMap (); /// NOTE: nnIdx was reset
const Eigen::Vector3f neighbor_normal = normals_[nnIdx[i]];
double cosine = nc.dot (neighbor_normal);
if (cosine > 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;
Expand All @@ -394,7 +406,7 @@ pcl::GreedyProjectionTriangulation<PointInT>::reconstructPolygons (std::vector<p
tmp_ = coords_[sfn_[nnIdx[i]]] - proj_qp_;
e.second[0] = tmp_.dot(u_);
e.second[1] = tmp_.dot(v_);
doubleEdges.push_back(e);
double_edges_.push_back(e);
// Pruning by visibility criterion
if ((state_[nnIdx[i]] == FRINGE) && (ffn_[R_] != nnIdx[i]) && (sfn_[R_] != nnIdx[i]))
{
Expand Down Expand Up @@ -456,17 +468,17 @@ pcl::GreedyProjectionTriangulation<PointInT>::reconstructPolygons (std::vector<p
bool visibility = true;
for (int j = 0; j < nr_edge; j++)
{
if (doubleEdges[j].index != i)
if (double_edges_[j].index != i)
{
const auto& f = ffn_[nnIdx[doubleEdges[j].index]];
const auto& f = ffn_[nnIdx[double_edges_[j].index]];
if ((f != nnIdx[i]) && (f != R_))
visibility = isVisible(uvn_nn[i], uvn_nn[doubleEdges[j].index], doubleEdges[j].first, Eigen::Vector2f::Zero());
visibility = isVisible(uvn_nn[i], uvn_nn[double_edges_[j].index], double_edges_[j].first, Eigen::Vector2f::Zero());
if (!visibility)
break;

const auto& s = sfn_[nnIdx[doubleEdges[j].index]];
const auto& s = sfn_[nnIdx[double_edges_[j].index]];
if ((s != nnIdx[i]) && (s != R_))
visibility = isVisible(uvn_nn[i], uvn_nn[doubleEdges[j].index], doubleEdges[j].second, Eigen::Vector2f::Zero());
visibility = isVisible(uvn_nn[i], uvn_nn[double_edges_[j].index], double_edges_[j].second, Eigen::Vector2f::Zero());
if (!visibility)
break;
}
Expand All @@ -475,7 +487,8 @@ pcl::GreedyProjectionTriangulation<PointInT>::reconstructPolygons (std::vector<p
}

// Sorting angles
std::sort (angles_.begin (), angles_.end (), GreedyProjectionTriangulation<PointInT>::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)
Expand Down
Loading