diff --git a/filters/IterativeClosestPoint.cpp b/filters/IterativeClosestPoint.cpp index 8c9f19b8bc..d54f4389a7 100644 --- a/filters/IterativeClosestPoint.cpp +++ b/filters/IterativeClosestPoint.cpp @@ -108,19 +108,7 @@ PointViewPtr IterativeClosestPoint::icp(PointViewPtr fixed, Eigen::Matrix4d final_transformation = Eigen::Matrix4d::Identity(); // demean the fixed dataset once and only once - PointViewPtr tempFixed = fixed->makeNew(); - for (PointId i = 0; i < moving->size(); ++i) - { - double x = - fixed->getFieldAs(Dimension::Id::X, i) - centroid.x(); - double y = - fixed->getFieldAs(Dimension::Id::Y, i) - centroid.y(); - double z = - fixed->getFieldAs(Dimension::Id::Z, i) - centroid.z(); - tempFixed->setField(Dimension::Id::X, i, x); - tempFixed->setField(Dimension::Id::Y, i, y); - tempFixed->setField(Dimension::Id::Z, i, z); - } + PointViewPtr tempFixed = fixed->demeanPointView(); // construct KDtree of the fixed dataset so that the moving one can // search for the nearest point diff --git a/pdal/PointView.cpp b/pdal/PointView.cpp index 87a900c3c0..84235683a9 100644 --- a/pdal/PointView.cpp +++ b/pdal/PointView.cpp @@ -33,12 +33,16 @@ ****************************************************************************/ #include +#include +#include #include #include #include #include +#include + namespace pdal { @@ -201,6 +205,30 @@ KD2Index& PointView::build2dIndex() } +PointViewPtr PointView::demeanPointView() +{ + using namespace eigen; + using namespace Eigen; + using namespace Dimension; + + std::vector ids(size()); + std::iota(ids.begin(), ids.end(), 0); + Vector3d centroid = computeCentroid(*this, ids); + PointViewPtr outView = makeNew(); + + for (PointId idx = 0; idx < size(); idx++) + { + double x = getFieldAs(Id::X, idx) - centroid.x(); + double y = getFieldAs(Id::Y, idx) - centroid.y(); + double z = getFieldAs(Id::Z, idx) - centroid.z(); + outView->setField(Id::X, idx, x); + outView->setField(Id::Y, idx, y); + outView->setField(Id::Z, idx, z); + } + return outView; +} + + void PointView::dump(std::ostream& ostr) const { using std::endl; diff --git a/pdal/PointView.hpp b/pdal/PointView.hpp index 5f1bae6980..b645e27b1b 100644 --- a/pdal/PointView.hpp +++ b/pdal/PointView.hpp @@ -295,6 +295,8 @@ class PDAL_DLL PointView : public PointContainer KD3Index& build3dIndex(); KD2Index& build2dIndex(); + PointViewPtr demeanPointView(); + protected: PointTableRef m_pointTable; std::deque m_index; diff --git a/test/unit/PointViewTest.cpp b/test/unit/PointViewTest.cpp index ab8f2c553e..267d4b166f 100644 --- a/test/unit/PointViewTest.cpp +++ b/test/unit/PointViewTest.cpp @@ -360,3 +360,12 @@ TEST(PointViewDeathTest, out_of_bounds) EXPECT_DEATH(point_view->getFieldAs(Dimension::Id::X, 1), "< m_size"); } #endif + +TEST(PointViewTest, demeanTest) +{ + PointTable table; + PointViewPtr view = makeTestView(table); + PointViewPtr demeanView = view->demeanPointView(); + EXPECT_EQ(-80, demeanView->getFieldAs(Dimension::Id::X, 0)); + EXPECT_EQ(-800, demeanView->getFieldAs(Dimension::Id::Y, 0)); +}