Initial commit of native ICP

doc/stages/filters.icp.rst

@@ -3,37 +3,35 @@
 filters.icp
 ==============
 
-The **ICP filter** uses the `PCL's Iterative Closest Point (ICP)`_ algorithm to
-calculate a **rigid** (rotation and translation) transformation that best aligns
-two datasets.  The first input to the ICP filter is considered the "fixed"
-points, and all subsequent points are "moving" points.  The output from the
-change filter are the "moving" points after the calculated transformation has
-been applied, one point view per input.  The transformation matrix is inserted
-into the stage's metadata.
+The **ICP filter** uses the Iterative Closest Point (ICP) algorithm to
+calculate a **rigid** (rotation and translation) transformation that best
+aligns two datasets.  The first input to the ICP filter is considered the
+"fixed" points, and all subsequent points are "moving" points.  The output from
+the filter are the "moving" points after the calculated transformation has been
+applied, one point view per input.  The transformation matrix is inserted into
+the stage's metadata.
 
 .. note::
 
-    ICP requires that the initial pose of the two point sets to be adequately
-    close, which are not always available, especially when transformation is
-    non-rigid. ICP can handle limited nonrigid transformations but be aware
+    ICP requires the initial pose of the two point sets to be adequately close,
+    which is not always possible, especially when the transformation is
+    non-rigid.  ICP can handle limited non-rigid transformations but be aware
     ICP may be unable to escape a local minimum. Consider using CPD instead.
 
     From :cite:`Xuechen2019`:
 
     ICP starts with an initial guess of the transformation between the two
     point sets and then iterates between finding the correspondence under the
-    current transformation and updating the transformation with the newly
-    found correspondence. ICP is widely used because it is rather
-    straightforward and easy to implement in practice; however, its biggest
-    problem is that it does not guarantee finding the globally optimal
-    transformation. In fact, ICP converges within a very small basin in the
-    parameter space, and it easily becomes trapped in local minima. Therefore,
-    the results of ICP are very sensitive to the initialization, especially
-    when high levels of noise and large proportions of outliers exist.
+    current transformation and updating the transformation with the newly found
+    correspondence. ICP is widely used because it is rather straightforward and
+    easy to implement in practice; however, its biggest problem is that it does
+    not guarantee finding the globally optimal transformation. In fact, ICP
+    converges within a very small basin in the parameter space, and it easily
+    becomes trapped in local minima. Therefore, the results of ICP are very
+    sensitive to the initialization, especially when high levels of noise and
+    large proportions of outliers exist.
 
 
-.. plugin::
-
 Examples
 --------
 
@@ -78,6 +76,17 @@ The metadata output might start something like:
 Options
 --------
 
-None.
+max_iter
+  Maximum number of iterations. [Default: **100**]
+
+max_similar
+  Max number of similar transforms to consider converged. [Default: **0**]
+
+mse_abs
+  Absolute threshold for MSE. [Default: **1e-12**]
+
+rt
+  Rotation threshold. [Default: **0.99999**]
 
-.. _PCL's Iterative Closest Point (ICP): http://docs.pointclouds.org/trunk/classpcl_1_1_iterative_closest_point.html
+tt
+  Translation threshold. [Default: **9e-8**]

filters/ApproximateCoplanarFilter.cpp

@@ -87,7 +87,7 @@ void ApproximateCoplanarFilter::filter(PointView& view)
         auto ids = kdi.neighbors(i, m_knn);
 
         // compute covariance of the neighborhood
-        auto B = eigen::computeCovariance(view, ids);
+        auto B = computeCovariance(view, ids);
 
         // perform the eigen decomposition
         SelfAdjointEigenSolver<Matrix3d> solver(B);

filters/CovarianceFeaturesFilter.cpp

@@ -110,7 +110,7 @@ void CovarianceFeaturesFilter::setDimensionality(PointView &view, const PointId
     auto ids = kid.neighbors(id, m_knn + 1);
 
     // compute covariance of the neighborhood
-    auto B = eigen::computeCovariance(view, ids);
+    auto B = computeCovariance(view, ids);
 
     // perform the eigen decomposition
     SelfAdjointEigenSolver<Matrix3d> solver(B);

filters/ELMFilter.cpp

@@ -39,6 +39,8 @@
 
 #include "ELMFilter.hpp"
 
+#include <pdal/EigenUtils.hpp>
+
 #include <map>
 #include <string>
 
@@ -74,7 +76,7 @@ void ELMFilter::addDimensions(PointLayoutPtr layout)
 void ELMFilter::filter(PointView& view)
 {
     BOX2D bounds;
-    view.calculateBounds(bounds);
+    calculateBounds(view, bounds);
 
     size_t cols =
         static_cast<size_t>(((bounds.maxx - bounds.minx) / m_cell) + 1);

filters/EigenvaluesFilter.cpp

@@ -86,7 +86,7 @@ void EigenvaluesFilter::filter(PointView& view)
         auto ids = kdi.neighbors(i, m_knn);
 
         // compute covariance of the neighborhood
-        auto B = eigen::computeCovariance(view, ids);
+        auto B = computeCovariance(view, ids);
 
         // perform the eigen decomposition
         SelfAdjointEigenSolver<Matrix3d> solver(B);

filters/EstimateRankFilter.cpp

@@ -80,7 +80,7 @@ void EstimateRankFilter::filter(PointView& view)
         // find the k-nearest neighbors
         auto ids = kdi.neighbors(i, m_knn);
 
-        view.setField(m_rank, i, eigen::computeRank(view, ids, m_thresh));
+        view.setField(m_rank, i, computeRank(view, ids, m_thresh));
     }
 }