diff --git a/filters/VoxelCentroidNearestNeighborFilter.cpp b/filters/VoxelCentroidNearestNeighborFilter.cpp index d48908ec8e..9232edff1c 100644 --- a/filters/VoxelCentroidNearestNeighborFilter.cpp +++ b/filters/VoxelCentroidNearestNeighborFilter.cpp @@ -94,7 +94,7 @@ PointViewSet VoxelCentroidNearestNeighborFilter::run(PointViewPtr view) PointViewPtr output = view->makeNew(); for (auto const& t : populated_voxel_ids) { - Eigen::Vector3f centroid = eigen::computeCentroid(*view, t.second); + Eigen::Vector3d centroid = eigen::computeCentroid(*view, t.second); std::vector neighbors = kdi.neighbors(centroid[0], centroid[1], centroid[2], 1); output->appendPoint(*view, neighbors[0]); diff --git a/pdal/EigenUtils.cpp b/pdal/EigenUtils.cpp index b2d262cb66..8332836dec 100644 --- a/pdal/EigenUtils.cpp +++ b/pdal/EigenUtils.cpp @@ -52,23 +52,30 @@ namespace pdal namespace eigen { -Eigen::Vector3f computeCentroid(PointView& view, std::vector ids) +Eigen::Vector3d computeCentroid(PointView& view, std::vector ids) { using namespace Eigen; - - auto n = ids.size(); - + double mx, my, mz; mx = my = mz = 0.0; + point_count_t n(0); for (auto const& j : ids) { - mx += view.getFieldAs(Dimension::Id::X, j); - my += view.getFieldAs(Dimension::Id::Y, j); - mz += view.getFieldAs(Dimension::Id::Z, j); + auto update = [&n](double value, double average) + { + double delta, delta_n; + delta = value - average; + delta_n = delta / n; + return average + delta_n; + }; + n++; + mx = update(view.getFieldAs(Dimension::Id::X, j), mx); + my = update(view.getFieldAs(Dimension::Id::Y, j), my); + mz = update(view.getFieldAs(Dimension::Id::Z, j), mz); } - Vector3f centroid; - centroid << mx/n, my/n, mz/n; + Vector3d centroid; + centroid << mx, my, mz; return centroid; } @@ -79,7 +86,7 @@ Eigen::Matrix3f computeCovariance(PointView& view, std::vector ids) auto n = ids.size(); - Vector3f centroid = computeCentroid(view, ids); + Vector3d centroid = computeCentroid(view, ids); // demean the neighborhood MatrixXf A(3, n); diff --git a/pdal/EigenUtils.hpp b/pdal/EigenUtils.hpp index 0bce5349a8..036c7608d5 100644 --- a/pdal/EigenUtils.hpp +++ b/pdal/EigenUtils.hpp @@ -75,7 +75,7 @@ namespace eigen \param ids a vector of PointIds specifying a subset of points. \return the 3D centroid of the XYZ dimensions. */ -PDAL_DLL Eigen::Vector3f computeCentroid(PointView& view, +PDAL_DLL Eigen::Vector3d computeCentroid(PointView& view, std::vector ids); /**