Add demeanPointView to PointView

PDAL · May 22, 2019 · 93e0be0 · 93e0be0
1 parent a047425
commit 93e0be0
Show file tree

Hide file tree

Showing 4 changed files with 40 additions and 13 deletions.
diff --git 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<double>(Dimension::Id::X, i) - centroid.x();
-        double y =
-            fixed->getFieldAs<double>(Dimension::Id::Y, i) - centroid.y();
-        double z =
-            fixed->getFieldAs<double>(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
@@ -33,12 +33,16 @@
 ****************************************************************************/
 
 #include <iomanip>
+#include <numeric>
 
+#include <pdal/EigenUtils.hpp>
 #include <pdal/KDIndex.hpp>
 #include <pdal/PointView.hpp>
 #include <pdal/PointViewIter.hpp>
 #include <pdal/util/Algorithm.hpp>
 
+#include <Eigen/Dense>
+
 namespace pdal
 {
 
@@ -201,6 +205,30 @@ KD2Index& PointView::build2dIndex()
 }
 
 
+PointViewPtr PointView::demeanPointView()
+{
+    using namespace eigen;
+    using namespace Eigen;
+    using namespace Dimension;
+
+    std::vector<PointId> 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<double>(Id::X, idx) - centroid.x();
+        double y = getFieldAs<double>(Id::Y, idx) - centroid.y();
+        double z = getFieldAs<double>(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
@@ -295,6 +295,8 @@ class PDAL_DLL PointView : public PointContainer
     KD3Index& build3dIndex();
     KD2Index& build2dIndex();
 
+    PointViewPtr demeanPointView();
+
 protected:
     PointTableRef m_pointTable;
     std::deque<PointId> m_index;

diff --git a/test/unit/PointViewTest.cpp b/test/unit/PointViewTest.cpp
@@ -360,3 +360,12 @@ TEST(PointViewDeathTest, out_of_bounds)
     EXPECT_DEATH(point_view->getFieldAs<uint8_t>(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<double>(Dimension::Id::X, 0));
+    EXPECT_EQ(-800, demeanView->getFieldAs<double>(Dimension::Id::Y, 0));
+}