From 7b891465561f1631c25dc43119886153c08e9d18 Mon Sep 17 00:00:00 2001 From: Pete Gadomski Date: Mon, 19 Jun 2017 09:38:58 -0600 Subject: [PATCH] Add ICP and CPD filters, remove CPD kernel (#1577) * Add metadata for Eigen matrices Includes to-from string conversions for these matrices. These methods are *not* generalized over all matrices, they're only for `MatrixXd` (because I'm template-dumb). * Update CPD to a filter (from a kernel) * Add ICP filter to PCL plugin --- cmake/options.cmake | 4 +- doc/stages/filters.cpd.rst | 73 +++++++ doc/stages/filters.icp.rst | 56 +++++ doc/stages/references.bib | 21 ++ pdal/EigenUtils.hpp | 46 ++++ pdal/Metadata.hpp | 2 +- plugins/cpd/CMakeLists.txt | 27 ++- plugins/cpd/filters/CpdFilter.cpp | 175 +++++++++++++++ plugins/cpd/filters/CpdFilter.hpp | 77 +++++++ plugins/cpd/kernel/CpdKernel.cpp | 187 ---------------- plugins/cpd/test/CpdFilterTest.cpp | 202 ++++++++++++++++++ plugins/pcl/CMakeLists.txt | 23 +- plugins/pcl/filters/IcpFilter.cpp | 116 ++++++++++ .../filters/IcpFilter.hpp} | 58 ++--- plugins/pcl/test/IcpFilterTest.cpp | 163 ++++++++++++++ test/data/las/100-points.las | Bin 0 -> 3627 bytes test/unit/EigenTest.cpp | 9 + 17 files changed, 1000 insertions(+), 239 deletions(-) create mode 100644 doc/stages/filters.cpd.rst create mode 100644 doc/stages/filters.icp.rst create mode 100644 doc/stages/references.bib create mode 100644 plugins/cpd/filters/CpdFilter.cpp create mode 100644 plugins/cpd/filters/CpdFilter.hpp delete mode 100644 plugins/cpd/kernel/CpdKernel.cpp create mode 100644 plugins/cpd/test/CpdFilterTest.cpp create mode 100644 plugins/pcl/filters/IcpFilter.cpp rename plugins/{cpd/kernel/CpdKernel.hpp => pcl/filters/IcpFilter.hpp} (67%) create mode 100644 plugins/pcl/test/IcpFilterTest.cpp create mode 100644 test/data/las/100-points.las diff --git a/cmake/options.cmake b/cmake/options.cmake index c30f4bf17d..add10850a9 100644 --- a/cmake/options.cmake +++ b/cmake/options.cmake @@ -8,9 +8,9 @@ add_feature_info("Bash completion" WITH_COMPLETION "completion for PDAL command line") option(BUILD_PLUGIN_CPD - "Choose if Coherent Point Drift kernel is built" FALSE) + "Choose if the cpd filter should be built" FALSE) add_feature_info("CPD plugin" BUILD_PLUGIN_CPD - "run Coherent Point Drift on two datasets") + "Coherent Point Drift (CPD) computes rigid or nonrigid transformations between point sets") option(BUILD_PLUGIN_GEOWAVE "Choose if GeoWave support should be built" FALSE) diff --git a/doc/stages/filters.cpd.rst b/doc/stages/filters.cpd.rst new file mode 100644 index 0000000000..1347c81f47 --- /dev/null +++ b/doc/stages/filters.cpd.rst @@ -0,0 +1,73 @@ +.. _filters.cpd: + +filters.cpd +============== + +The CPD filter uses the Coherent Point Drift :cite:`Myronenko` algorithm to compute a rigid, nonrigid, or affine transformation between datasets. +The rigid and affine are what you'd expect; the nonrigid transformation uses Motion Coherence Theory :cite:`Yuille1998` to "bend" the points to find a best alignment. + +.. note:: + + CPD is computationally intensive and can be slow when working with many points (i.e. > 10 000). + Nonrigid is significatly slower than rigid and affine. + +The first input to the change filter are considered the "fixed" points, and all subsequent inputs 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. +Any additional information about the cpd registration, e.g. the rigid transformation matrix, will be placed in the stage's metadata. + +Examples +-------- + +.. code-block:: json + + { + "pipeline": [ + "fixed.las", + "moving.las", + { + "type": "filters.rigid", + "method": "rigid" + }, + "output.las" + ] + } + +If "method" is not provided, the cpd filter will default to using the rigid registration method. +To get the transform matrix, you'll need to use the ``--metadata`` option: + +:: + + $ pdal pipeline cpd-pipeline.json --metadata cpd-metadata.json + +The metadata output might start something like: + +.. code-block:: json + + { + "stages": + { + "filters.cpd": + { + "iterations": 10, + "method": "rigid", + "runtime": 0.003839, + "sigma2": 5.684342128e-16, + "transform": " 1 -6.21722e-17 1.30104e-18 5.29303e-11-8.99346e-17 1 2.60209e-18 -3.49247e-10 -2.1684e-19 1.73472e-18 1 -1.53477e-12 0 0 0 1" + }, + }, + +.. seealso:: + + :ref:`filters.transformation` to apply a transform to other points. + +Options +-------- + +method + Change detection method to use. + Valid values are "rigid", "affine", and "nonrigid". + [Default: **rigid**] + +.. _Coherent Point Drift (CPD): https://github.com/gadomski/cpd + +.. bibliography:: references.bib diff --git a/doc/stages/filters.icp.rst b/doc/stages/filters.icp.rst new file mode 100644 index 0000000000..f6a1716747 --- /dev/null +++ b/doc/stages/filters.icp.rst @@ -0,0 +1,56 @@ +.. _filters.icp: + +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. + +Examples +-------- + +.. code-block:: json + + { + "pipeline": [ + "fixed.las", + "moving.las", + { + "type": "filters.icp" + }, + "output.las" + ] + } + +To get the transform matrix, you'll need to use the ``--metadata`` option: + +:: + + $ pdal pipeline icp-pipeline.json --metadata icp-metadata.json + +The metadata output might start something like: + +.. code-block:: json + + { + "stages": + { + "filters.icp": + { + "converged": true, + "fitness": 0.01953125097, + "transform": " 1 2.60209e-18 -1.97906e-09 -0.375 8.9407e-08 1 5.58794e-09 -0.5625 6.98492e -10 -5.58794e-09 1 0.00411987 0 0 0 1" + } + +.. seealso:: + + :ref:`filters.transformation` to apply a transform to other points. + +Options +-------- + +None. + +.. _PCL's Iterative Closest Point (ICP): http://docs.pointclouds.org/trunk/classpcl_1_1_iterative_closest_point.html diff --git a/doc/stages/references.bib b/doc/stages/references.bib new file mode 100644 index 0000000000..726e5ae485 --- /dev/null +++ b/doc/stages/references.bib @@ -0,0 +1,21 @@ +@article{Myronenko, +abstract = {Point set registration is a key component in many computer vision tasks. The goal of point set registration is to assign correspondences between two sets of points and to recover the transformation that maps one point set to the other. Multiple factors, including an unknown nonrigid spatial transformation, large dimensionality of point set, noise, and outliers, make the point set registration a challenging problem. We introduce a probabilistic method, called the Coherent Point Drift (CPD) algorithm, for both rigid and nonrigid point set registration. We consider the alignment of two point sets as a probability density estimation problem. We fit the Gaussian mixture model (GMM) centroids (representing the first point set) to the data (the second point set) by maximizing the likelihood. We force the GMM centroids to move coherently as a group to preserve the topological structure of the point sets. In the rigid case, we impose the coherence constraint by reparameterization of GMM centroid locations with rigid parameters and derive a closed form solution of the maximization step of the EM algorithm in arbitrary dimensions. In the nonrigid case, we impose the coherence constraint by regularizing the displacement field and using the variational calculus to derive the optimal transformation. We also introduce a fast algorithm that reduces the method computation complexity to linear. We test the CPD algorithm for both rigid and nonrigid transformations in the presence of noise, outliers, and missing points, where CPD shows accurate results and outperforms current state-of-the-art methods.}, +author = {Myronenko, Andriy and Song, Xubo}, +issn = {1939-3539}, +journal = {IEEE transactions on pattern analysis and machine intelligence}, +month = {dec}, +number = {12}, +pages = {2262--75}, +pmid = {20975122}, +publisher = {Institute of Electrical {\&} Electronics Engineers (IEEE)}, +title = {{Point set registration: coherent point drift.}}, +volume = {32}, +year = {2010} +} + +@article{Yuille1998, +author = {Yuille, Alan L. and Grzywacz, Norberto M.}, +journal = {Second International Conference on Computer Vision}, +title = {{The Motion Coherence Theory}}, +year = {1988} +} diff --git a/pdal/EigenUtils.hpp b/pdal/EigenUtils.hpp index 6bda076444..122c82bca5 100644 --- a/pdal/EigenUtils.hpp +++ b/pdal/EigenUtils.hpp @@ -36,6 +36,7 @@ #include #include +#include #include @@ -949,4 +950,49 @@ PDAL_DLL Eigen::MatrixXd computeSpline(Eigen::MatrixXd x, Eigen::MatrixXd y, } // namespace eigen +namespace Utils +{ + +template <> +inline bool fromString(const std::string& s, Eigen::MatrixXd& matrix) { + std::stringstream ss(s); + std::string line; + std::vector> rows; + while (std::getline(ss, line)) { + std::vector row; + std::stringstream ss(line); + double n; + while (ss >> n) { + row.push_back(n); + if (ss.peek() == ',' || ss.peek() == ' ') { + ss.ignore(); + } + } + if (!rows.empty() && rows.back().size() != row.size()) { + return false; + } + rows.push_back(row); + } + if (rows.empty()) { + return true; + } + size_t nrows = rows.size(); + size_t ncols = rows[0].size(); + matrix.resize(nrows, ncols); + for (size_t i = 0; i < nrows; ++i) { + for (size_t j = 0; j < ncols; ++j) { + matrix(i, j) = rows[i][j]; + } + } + return true; +} +} + +template <> +inline void MetadataNodeImpl::setValue(const Eigen::MatrixXd& matrix) +{ + m_type = "matrix"; + m_value = Utils::toString(matrix); +} + } // namespace pdal diff --git a/pdal/Metadata.hpp b/pdal/Metadata.hpp index e034d34ed4..70b528a0f3 100644 --- a/pdal/Metadata.hpp +++ b/pdal/Metadata.hpp @@ -488,7 +488,7 @@ class PDAL_DLL MetadataNode std::string v(Utils::escapeJSON(value())); if (m_impl->m_type == "string" || m_impl->m_type == "base64Binary" || - m_impl->m_type == "uuid") + m_impl->m_type == "uuid" || m_impl->m_type == "matrix") { std::string val("\""); val += escapeQuotes(v) + "\""; diff --git a/plugins/cpd/CMakeLists.txt b/plugins/cpd/CMakeLists.txt index 2d27bec118..4e66b9b009 100644 --- a/plugins/cpd/CMakeLists.txt +++ b/plugins/cpd/CMakeLists.txt @@ -1,14 +1,21 @@ -find_package(Cpd 0.5 REQUIRED) +set(Cpd_VERSION 0.5) -set_package_properties(Cpd PROPERTIES - DESCRIPTION "Coherent Point Drift" - URL "https://github.com/gadomski/cpd" - TYPE OPTIONAL - PURPOSE "Register two point sets using the Coherent Point Drift algorithm" - ) +find_package(Cpd ${Cpd_VERSION} REQUIRED) +option(BUILD_PLUGIN_CPD "Build Coherent Point Drift support" ${Cpd_FOUND}) + +set(files filters/CpdFilter.cpp) +set(include_dirs "${CMAKE_CURRENT_LIST_DIR}" "${PDAL_VENDOR_DIR}/eigen") -pdal_add_plugin(cpd_kernel_lib_name kernel cpd - FILES kernel/CpdKernel.cpp +PDAL_ADD_PLUGIN(filter_libname filter cpd + FILES filters/CpdFilter.cpp LINK_WITH Cpd::Library-C++ ) -target_include_directories(${cpd_kernel_lib_name} PRIVATE ${CMAKE_CURRENT_LIST_DIR}) +target_include_directories(${filter_libname} PRIVATE "${include_dirs}") + +if(${WITH_TESTS}) + PDAL_ADD_TEST(pdal_filters_cpd_test + FILES test/CpdFilterTest.cpp + LINK_WITH ${filter_libname} + ) + target_include_directories(pdal_filters_cpd_test PRIVATE "${include_dirs}") +endif() diff --git a/plugins/cpd/filters/CpdFilter.cpp b/plugins/cpd/filters/CpdFilter.cpp new file mode 100644 index 0000000000..69b5aaaddf --- /dev/null +++ b/plugins/cpd/filters/CpdFilter.cpp @@ -0,0 +1,175 @@ +/****************************************************************************** + * Copyright (c) 2017, Peter J. Gadomski + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following + * conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Hobu, Inc. or Flaxen Geo Consulting nor the + * names of its contributors may be used to endorse or promote + * products derived from this software without specific prior + * written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT + * OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY + * OF SUCH DAMAGE. + ****************************************************************************/ + +#include +#include +#include +#include +#include +#include + +namespace pdal +{ +namespace +{ +void movePoints(PointViewPtr moving, const cpd::Matrix& result) +{ + assert(moving->size() == result.rows()); + for (PointId i = 0; i < moving->size(); ++i) + { + moving->setField(Dimension::Id::X, i, result(i, 0)); + moving->setField(Dimension::Id::Y, i, result(i, 1)); + moving->setField(Dimension::Id::Z, i, result(i, 2)); + } +} + +void addMetadata(CpdFilter* filter, const cpd::Result& result) +{ + MetadataNode root = filter->getMetadata(); + root.add("sigma2", result.sigma2); + root.add("runtime", double(result.runtime.count()) / 1e6); + root.add("iterations", result.iterations); +} +} + +static PluginInfo const s_info = PluginInfo( + "filters.change", "CPD filter", "http://pdal.io/stages/filters.cpd.html"); + +CREATE_SHARED_PLUGIN(1, 0, CpdFilter, Filter, s_info); + +std::string CpdFilter::getName() const +{ + return s_info.name; +} + +void CpdFilter::addArgs(ProgramArgs& args) +{ + args.add("method", "CPD method (rigid, nonrigid, or affine)", m_method, + "rigid"); +} + +std::string CpdFilter::defaultMethod() +{ + return "rigid"; +} + +PointViewSet CpdFilter::run(PointViewPtr view) +{ + PointViewSet viewSet; + if (this->m_complete) + { + throw pdal_error( + "filters.change must have two point view inputs, no more, no less"); + } + else if (this->m_fixed) + { + log()->get(LogLevel::Debug2) << "Adding moving points\n"; + PointViewPtr result = this->change(this->m_fixed, view); + viewSet.insert(result); + this->m_complete = true; + log()->get(LogLevel::Debug2) << "filters.change complete\n"; + } + else + { + log()->get(LogLevel::Debug2) << "Adding fixed points\n"; + this->m_fixed = view; + } + return viewSet; +} + +void CpdFilter::done(PointTableRef _) +{ + if (!this->m_complete) + { + throw pdal_error( + "filters.change must have two point view inputs, no more, no less"); + } +} + +PointViewPtr CpdFilter::change(PointViewPtr fixed, PointViewPtr moving) +{ + MetadataNode root = this->getMetadata(); + root.add("method", this->m_method); + log()->get(LogLevel::Debug2) + << "filters.change running method:" << this->m_method << "\n"; + if (this->m_method == "rigid") + { + this->cpd_rigid(fixed, moving); + } + else if (this->m_method == "affine") + { + this->cpd_affine(fixed, moving); + } + else if (this->m_method == "nonrigid") + { + this->cpd_nonrigid(fixed, moving); + } + else + { + std::stringstream ss; + ss << "Invalid change detection method: " << this->m_method; + throw pdal_error(ss.str()); + } + return moving; +} + +void CpdFilter::cpd_rigid(PointViewPtr fixed, PointViewPtr moving) +{ + cpd::Matrix fixedMatrix = eigen::pointViewToEigen(*fixed); + cpd::Matrix movingMatrix = eigen::pointViewToEigen(*moving); + cpd::RigidResult result = cpd::rigid(fixedMatrix, movingMatrix); + movePoints(moving, result.points); + addMetadata(this, static_cast(result)); + MetadataNode root = getMetadata(); + root.add("transform", result.matrix()); +} + +void CpdFilter::cpd_affine(PointViewPtr fixed, PointViewPtr moving) +{ + cpd::Matrix fixedMatrix = eigen::pointViewToEigen(*fixed); + cpd::Matrix movingMatrix = eigen::pointViewToEigen(*moving); + cpd::AffineResult result = cpd::affine(fixedMatrix, movingMatrix); + movePoints(moving, result.points); + MetadataNode root = getMetadata(); + root.add("transform", result.matrix()); +} + +void CpdFilter::cpd_nonrigid(PointViewPtr fixed, PointViewPtr moving) +{ + cpd::Matrix fixedMatrix = eigen::pointViewToEigen(*fixed); + cpd::Matrix movingMatrix = eigen::pointViewToEigen(*moving); + cpd::NonrigidResult result = cpd::nonrigid(fixedMatrix, movingMatrix); + movePoints(moving, result.points); +} +} diff --git a/plugins/cpd/filters/CpdFilter.hpp b/plugins/cpd/filters/CpdFilter.hpp new file mode 100644 index 0000000000..a217788539 --- /dev/null +++ b/plugins/cpd/filters/CpdFilter.hpp @@ -0,0 +1,77 @@ +/****************************************************************************** + * Copyright (c) 2017, Peter J. Gadomski + * + * All rights reserved + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following + * conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Hobu, Inc. or Flaxen Geo Consulting nor the + * names of its contributors may be used to endorse or promote + * products derived from this software without specific prior + * written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT + * OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY + * OF SUCH DAMAGE. + ****************************************************************************/ + +#pragma once + +#include +#include + +extern "C" int32_t CpdFilter_ExitFunc(); +extern "C" PF_ExitFunc CpdFilter_InitPlugin(); + +namespace pdal +{ + +class PDAL_DLL CpdFilter : public Filter +{ + public: + static std::string defaultMethod(); + + CpdFilter() : Filter(), m_fixed(nullptr), m_method(""), m_complete(false) + { + } + + static void* create(); + static int32_t destroy(void*); + std::string getName() const; + + virtual PointViewSet run(PointViewPtr view); + + private: + virtual void addArgs(ProgramArgs& args); + virtual void done(PointTableRef _); + + PointViewPtr change(PointViewPtr fixed, PointViewPtr moving); + void cpd_rigid(PointViewPtr fixed, PointViewPtr moving); + void cpd_affine(PointViewPtr fixed, PointViewPtr moving); + void cpd_nonrigid(PointViewPtr fixed, PointViewPtr moving); + + PointViewPtr m_fixed; + std::string m_method; + bool m_complete; + + CpdFilter& operator=(const CpdFilter&); // not implemented + CpdFilter(const CpdFilter&); // not implemented +}; +} // namespace pdal diff --git a/plugins/cpd/kernel/CpdKernel.cpp b/plugins/cpd/kernel/CpdKernel.cpp deleted file mode 100644 index b2d5a982dd..0000000000 --- a/plugins/cpd/kernel/CpdKernel.cpp +++ /dev/null @@ -1,187 +0,0 @@ -/****************************************************************************** - * Copyright (c) 2014, Pete Gadomski (pete.gadomski@gmail.com) - * - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following - * conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Hobu, Inc. or Flaxen Geo Consulting nor the - * names of its contributors may be used to endorse or promote - * products derived from this software without specific prior - * written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT - * OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY - * OF SUCH DAMAGE. - ****************************************************************************/ - -#include "kernel/CpdKernel.hpp" -#include - -#include -#include - -#include -#include -#include -#include -#include - -namespace pdal { - -static PluginInfo const s_info = PluginInfo( - "kernels.cpd", "CPD Kernel", "http://pdal.io/kernels/kernels.cpd.html"); - -CREATE_SHARED_PLUGIN(1, 0, CpdKernel, Kernel, s_info) - -std::string CpdKernel::getName() const { return s_info.name; } - -void CpdKernel::addSwitches(ProgramArgs& args) { - Arg& method = - args.add("method,M", "registration method (rigid, nonrigid)", m_method); - method.setPositional(); - Arg& fixed = - args.add("fixed,f", "input file containing the fixed points", m_fixed); - fixed.setPositional(); - Arg& moving = args.add("moving,m", - "input file containing the moving points, " - "i.e. the points that will be registered", - m_moving); - moving.setPositional(); - Arg& output = args.add("output,o", "output file name", m_output); - output.setPositional(); - args.add("bounds", "Extent (in XYZ) to clip output to", m_bounds); - - args.add("max-iterations", "maximum number of iterations allowed", - m_max_iterations, cpd::DEFAULT_MAX_ITERATIONS); - args.add("normalize", "whether cpd should normalize the points before running", - m_normalize, true); - args.add("outliers", "a number between zero and one that represents the tolerance for outliers", - m_outliers, cpd::DEFAULT_OUTLIERS); - args.add("sigma2", "the starting sigma2 value.", - m_sigma2, cpd::DEFAULT_SIGMA2); - args.add("tolerance", "the amount the error must change to continue iterations", - m_tolerance, cpd::DEFAULT_TOLERANCE); - - args.add("reflections", "should rigid registrations allow reflections", - m_reflections, false); - args.add("scale", "should rigid registrations allow scaling", - m_reflections, false); - - args.add("beta", "beta parameter for nonrigid registrations", - m_beta, cpd::DEFAULT_BETA); - args.add("lambda", "lambda parameter for nonrigid registrations", - m_lambda, cpd::DEFAULT_LAMBDA); -} - -cpd::Matrix CpdKernel::readFile(const std::string& filename) { - Stage& reader = makeReader(filename, ""); - - PointTable table; - PointViewSet viewSet; - if (!m_bounds.empty()) { - Options boundsOptions; - boundsOptions.add("bounds", m_bounds); - - Stage& crop = makeFilter("filters.crop", reader); - crop.setOptions(boundsOptions); - crop.prepare(table); - viewSet = crop.execute(table); - } else { - reader.prepare(table); - viewSet = reader.execute(table); - } - - return eigen::pointViewToEigen(**viewSet.begin()); -} - -int CpdKernel::execute() { - cpd::Matrix fixed = readFile(m_fixed); - cpd::Matrix moving = readFile(m_moving); - - if (fixed.rows() == 0 || moving.rows() == 0) { - throw pdal_error("No points to process."); - } - - cpd::Matrix result; - if (m_method == "rigid") { - cpd::Rigid rigid; - rigid.max_iterations(m_max_iterations) - .normalize(m_normalize) - .outliers(m_outliers) - .sigma2(m_sigma2) - .tolerance(m_tolerance); - rigid.reflections(m_reflections) - .scale(m_scale); - result = rigid.run(fixed, moving).points; - } else if (m_method == "nonrigid") { - cpd::Nonrigid nonrigid; - nonrigid.max_iterations(m_max_iterations) - .normalize(m_normalize) - .outliers(m_outliers) - .sigma2(m_sigma2) - .tolerance(m_tolerance); - nonrigid.beta(m_beta).lambda(m_lambda); - result = nonrigid.run(fixed, moving).points; - } else { - std::stringstream ss; - ss << "Invalid cpd method: " << m_method << std::endl; - throw pdal_error(ss.str()); - } - - PointTable outTable; - PointLayoutPtr outLayout(outTable.layout()); - outLayout->registerDim(Dimension::Id::X); - outLayout->registerDim(Dimension::Id::Y); - outLayout->registerDim(Dimension::Id::Z); - outLayout->registerDim(Dimension::Id::XVelocity); - outLayout->registerDim(Dimension::Id::YVelocity); - outLayout->registerDim(Dimension::Id::ZVelocity); - PointViewPtr outView(new PointView(outTable)); - - size_t rows = moving.rows(); - for (size_t i = 0; i < rows; ++i) { - outView->setField(Dimension::Id::X, i, result(i, 0)); - outView->setField(Dimension::Id::Y, i, result(i, 1)); - outView->setField(Dimension::Id::Z, i, result(i, 2)); - outView->setField(Dimension::Id::XVelocity, i, - moving(i, 0) - result(i, 0)); - outView->setField(Dimension::Id::YVelocity, i, - moving(i, 1) - result(i, 1)); - outView->setField(Dimension::Id::ZVelocity, i, - moving(i, 2) - result(i, 2)); - } - - BufferReader reader; - reader.addView(outView); - - Options writerOpts; - if (StageFactory::inferReaderDriver(m_output) == "writers.text") { - writerOpts.add("order", "X,Y,Z,XVelocity,YVelocity,ZVelocity"); - writerOpts.add("keep_unspecified", false); - } - Stage& writer = makeWriter(m_output, reader, "", writerOpts); - writer.prepare(outTable); - writer.execute(outTable); - - return 0; -} - -} // namespace pdal diff --git a/plugins/cpd/test/CpdFilterTest.cpp b/plugins/cpd/test/CpdFilterTest.cpp new file mode 100644 index 0000000000..e222824c96 --- /dev/null +++ b/plugins/cpd/test/CpdFilterTest.cpp @@ -0,0 +1,202 @@ +/****************************************************************************** + * Copyright (c) 2017, Peter J. Gadomski + * + * All rights reserved + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following + * conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Hobu, Inc. or Flaxen Geo Consulting nor the + * names of its contributors may be used to endorse or promote + * products derived from this software without specific prior + * written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT + * OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY + * OF SUCH DAMAGE. + ****************************************************************************/ + +#include "Support.hpp" +#include +#include +#include +#include +#include +#include +#include +#include + +namespace pdal +{ +namespace +{ + +std::unique_ptr newReader() +{ + Options options; + options.add("filename", Support::datapath("las/100-points.las")); + std::unique_ptr reader(new LasReader()); + reader->setOptions(options); + return reader; +} + +std::unique_ptr newFilter() +{ + return std::unique_ptr(new CpdFilter()); +} + +void checkNoThrow(const std::string& method) +{ + auto reader1 = newReader(); + auto reader2 = newReader(); + auto filter = newFilter(); + filter->setInput(*reader1); + filter->setInput(*reader2); + + Options options; + options.add("method", method); + filter->setOptions(options); + + PointTable table; + filter->prepare(table); + ASSERT_NO_THROW(filter->execute(table)); +} + +void checkPointsEqualReader(const PointViewSet& pointViewSet, double tolerance) +{ + auto reader = newReader(); + PointTable table; + reader->prepare(table); + PointViewSet readerPointViewSet = reader->execute(table); + ASSERT_EQ(1u, readerPointViewSet.size()); + PointViewPtr expected = *readerPointViewSet.begin(); + ASSERT_EQ(1u, pointViewSet.size()); + PointViewPtr actual = *pointViewSet.begin(); + + ASSERT_EQ(expected->size(), actual->size()); + + for (PointId i = 0; i < expected->size(); ++i) + { + ASSERT_NEAR(expected->getFieldAs(Dimension::Id::X, i), + actual->getFieldAs(Dimension::Id::X, i), tolerance); + ASSERT_NEAR(expected->getFieldAs(Dimension::Id::Y, i), + actual->getFieldAs(Dimension::Id::Y, i), tolerance); + ASSERT_NEAR(expected->getFieldAs(Dimension::Id::Z, i), + actual->getFieldAs(Dimension::Id::Z, i), tolerance); + } +} +} + +TEST(CpdFilterTest, DefaultMethod) +{ + EXPECT_EQ("rigid", CpdFilter::defaultMethod()); +} + +TEST(CpdFilterTest, DefaultIdentity) +{ + auto reader1 = newReader(); + auto reader2 = newReader(); + auto filter = newFilter(); + filter->setInput(*reader1); + filter->setInput(*reader2); + + PointTable table; + filter->prepare(table); + PointViewSet viewSet = filter->execute(table); + EXPECT_EQ(1u, viewSet.size()); + EXPECT_EQ(100u, (*viewSet.begin())->size()); + + MetadataNode root = filter->getMetadata(); + EXPECT_EQ(CpdFilter::defaultMethod(), root.findChild("method").value()); + MetadataNode transform = root.findChild("transform"); + EXPECT_EQ("matrix", transform.type()); + Eigen::MatrixXd transformMatrix = transform.value(); + EXPECT_TRUE( + transformMatrix.isApprox(Eigen::MatrixXd::Identity(4, 4), 1e-4)); +} + +TEST(CpdFilterTest, RecoverTranslation) +{ + auto reader1 = newReader(); + auto reader2 = newReader(); + TransformationFilter transformationFilter; + Options transformationOptions; + transformationOptions.add("matrix", "1 0 0 1\n0 1 0 2\n0 0 1 3\n0 0 0 1"); + transformationFilter.setOptions(transformationOptions); + transformationFilter.setInput(*reader2); + + auto filter = newFilter(); + filter->setInput(*reader1); + filter->setInput(transformationFilter); + + PointTable table; + filter->prepare(table); + PointViewSet pointViewSet = filter->execute(table); + + MetadataNode root = filter->getMetadata(); + Eigen::MatrixXd transform = + root.findChild("transform").value(); + double tolerance = 1e-4; + EXPECT_NEAR(-1.0, transform(0, 3), tolerance); + EXPECT_NEAR(-2.0, transform(1, 3), tolerance); + EXPECT_NEAR(-3.0, transform(2, 3), tolerance); + checkPointsEqualReader(pointViewSet, tolerance); +} + +TEST(CpdFilterTest, TooFewInputs) +{ + auto reader = newReader(); + auto filter = newFilter(); + filter->setInput(*reader); + + PointTable table; + filter->prepare(table); + ASSERT_THROW(filter->execute(table), pdal_error); +} + +TEST(CpdFilterTest, TooManyInputs) +{ + auto reader1 = newReader(); + auto reader2 = newReader(); + auto reader3 = newReader(); + auto filter = newFilter(); + filter->setInput(*reader1); + filter->setInput(*reader2); + filter->setInput(*reader3); + + PointTable table; + filter->prepare(table); + ASSERT_THROW(filter->execute(table), pdal_error); +} + +TEST(CpdFilterTest, RigidNoThrow) +{ + checkNoThrow("rigid"); +} + +TEST(CpdFilterTest, AffineNoThrow) +{ + checkNoThrow("affine"); +} + +TEST(CpdFilterTest, NonrigidNoThrow) +{ + checkNoThrow("nonrigid"); +} +} // namespace pdal diff --git a/plugins/pcl/CMakeLists.txt b/plugins/pcl/CMakeLists.txt index 38a9d4ba63..168718948e 100644 --- a/plugins/pcl/CMakeLists.txt +++ b/plugins/pcl/CMakeLists.txt @@ -32,11 +32,13 @@ endif() set_package_properties(PCL PROPERTIES DESCRIPTION "Point Cloud Library" URL "http://pointclouds.org" TYPE RECOMMENDED - PURPOSE "Enables PCD reader/writer, PCLVisualizer, PCLBlock filter, and ground, pcl, smooth, and view kernels") + PURPOSE "Enables PCD reader/writer, PCLVisualizer, PCLBlock filter, ICP filter, and ground, pcl, smooth, and view kernels") set(INCLUDE_DIRS ${ROOT_DIR} - ${PCL_INCLUDE_DIRS}) + ${PCL_INCLUDE_DIRS} + ${CMAKE_CURRENT_LIST_DIR} + ) add_definitions(${PCL_DEFINITIONS}) if (NOT WIN32) @@ -120,6 +122,13 @@ PDAL_ADD_PLUGIN(greedyprojection_filter_libname filter greedyprojection target_include_directories(${greedyprojection_filter_libname} PRIVATE ${INCLUDE_DIRS}) +PDAL_ADD_PLUGIN(icp_filter_libname filter icp + FILES + filters/IcpFilter.cpp + LINK_WITH ${PCL_LIBRARIES}) +target_include_directories(${icp_filter_libname} PRIVATE + ${INCLUDE_DIRS}) + # # PCL Kernel # @@ -144,10 +153,18 @@ target_include_directories(${smooth_libname} PRIVATE ${PDAL_IO_DIR}) if (WITH_TESTS) - PDAL_ADD_TEST(pcltest + PDAL_ADD_TEST(pdal_filters_pcl_block_test FILES test/PCLBlockFilterTest.cpp LINK_WITH ${pcd_reader_libname} ${pcd_writer_libname} ${pclblock_libname} ${pcl_libname} ${smooth_libname} ) + + PDAL_ADD_TEST(pdal_filters_icp_test + FILES + test/IcpFilterTest.cpp + LINK_WITH + ${icp_filter_libname} + ) + target_include_directories(pdal_filters_icp_test PRIVATE ${INCLUDE_DIRS}) endif() diff --git a/plugins/pcl/filters/IcpFilter.cpp b/plugins/pcl/filters/IcpFilter.cpp new file mode 100644 index 0000000000..0784bd9605 --- /dev/null +++ b/plugins/pcl/filters/IcpFilter.cpp @@ -0,0 +1,116 @@ +/****************************************************************************** + * Copyright (c) 2017, Peter J. Gadomski + * + * All rights reserved + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following + * conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Hobu, Inc. or Flaxen Geo Consulting nor the + * names of its contributors may be used to endorse or promote + * products derived from this software without specific prior + * written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT + * OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY + * OF SUCH DAMAGE. + ****************************************************************************/ + +#include "PCLConversions.hpp" +#include +#include +#include +#include +#include + +namespace pdal +{ + +static PluginInfo const s_info = + PluginInfo("filters.icp", "Iterative Closest Point (ICP) filter", + "http://pdal.io/stages/filters.icp.html"); + +CREATE_SHARED_PLUGIN(1, 0, IcpFilter, Filter, s_info); + +std::string IcpFilter::getName() const +{ + return s_info.name; +} + +PointViewSet IcpFilter::run(PointViewPtr view) +{ + PointViewSet viewSet; + if (this->m_fixed) + { + log()->get(LogLevel::Debug2) << "Calculating ICP\n"; + PointViewPtr result = this->icp(this->m_fixed, view); + viewSet.insert(result); + log()->get(LogLevel::Debug2) << "ICP complete\n"; + this->m_complete = true; + } + else + { + log()->get(LogLevel::Debug2) << "Adding fixed points\n"; + this->m_fixed = view; + } + return viewSet; +} + +void IcpFilter::done(PointTableRef _) +{ + if (!this->m_complete) + { + throw pdal_error( + "filters.change must have two point view inputs, no more, no less"); + } +} + +PointViewPtr IcpFilter::icp(PointViewPtr fixed, PointViewPtr moving) const +{ + typedef pcl::PointXYZ Point; + typedef pcl::PointCloud Cloud; + Cloud::Ptr fixedCloud(new Cloud()); + pclsupport::PDALtoPCD(fixed, *fixedCloud); + Cloud::Ptr movingCloud(new Cloud()); + pclsupport::PDALtoPCD(moving, *movingCloud); + pcl::IterativeClosestPoint icp; + icp.setInputCloud(movingCloud); + icp.setInputTarget(fixedCloud); + Cloud result; + icp.align(result); + + MetadataNode root = getMetadata(); + // I couldn't figure out the template-fu to get + // `MetadataNodeImpl::setValue` to work for all Eigen matrices with one + // function, so I'm just brute-forcing the cast for now. + root.add("transform", + Eigen::MatrixXd(icp.getFinalTransformation().cast())); + root.add("converged", icp.hasConverged()); + root.add("fitness", icp.getFitnessScore()); + + assert(moving->size() == result.points.size()); + for (PointId i = 0; i < moving->size(); ++i) + { + moving->setField(Dimension::Id::X, i, result.points[i].x); + moving->setField(Dimension::Id::Y, i, result.points[i].y); + moving->setField(Dimension::Id::Z, i, result.points[i].z); + } + return moving; +} +} diff --git a/plugins/cpd/kernel/CpdKernel.hpp b/plugins/pcl/filters/IcpFilter.hpp similarity index 67% rename from plugins/cpd/kernel/CpdKernel.hpp rename to plugins/pcl/filters/IcpFilter.hpp index 19d7b4433d..477958ee4b 100644 --- a/plugins/cpd/kernel/CpdKernel.hpp +++ b/plugins/pcl/filters/IcpFilter.hpp @@ -1,7 +1,7 @@ /****************************************************************************** - * Copyright (c) 2014, Pete Gadomski (pete.gadomski@gmail.com) + * Copyright (c) 2017, Peter J. Gadomski * - * All rights reserved. + * All rights reserved * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following @@ -34,46 +34,32 @@ #pragma once -#include -#include -#include +#include +#include + +extern "C" int32_t IcpFilter_ExitFunc(); +extern "C" PF_ExitFunc IcpFilter_InitPlugin(); namespace pdal { -class PDAL_DLL CpdKernel : public Kernel +class PDAL_DLL IcpFilter : public Filter { -public: - static void *create(); - static int32_t destroy(void *); - std::string getName() const; - int execute(); - -private: - CpdKernel() : Kernel() {}; - virtual void addSwitches(ProgramArgs& args); - cpd::Matrix readFile(const std::string& filename); + public: + IcpFilter() : Filter(), m_fixed(nullptr), m_complete(false) + { + } - std::string m_method; - std::string m_fixed; - std::string m_moving; - std::string m_output; - BOX3D m_bounds; - - // cpd::Transform - size_t m_max_iterations; - bool m_normalize; - double m_outliers; - double m_sigma2; - double m_tolerance; + static void* create(); + static int32_t destroy(void*); + std::string getName() const; + virtual PointViewSet run(PointViewPtr view); - // cpd::Rigid - bool m_reflections; - bool m_scale; + private: + PointViewPtr icp(PointViewPtr fixed, PointViewPtr moving) const; + virtual void done(PointTableRef _); - // cpd::Nonrigid - double m_beta; - double m_lambda; + PointViewPtr m_fixed; + bool m_complete; }; - -} // namespace pdal +} diff --git a/plugins/pcl/test/IcpFilterTest.cpp b/plugins/pcl/test/IcpFilterTest.cpp new file mode 100644 index 0000000000..9ef6293317 --- /dev/null +++ b/plugins/pcl/test/IcpFilterTest.cpp @@ -0,0 +1,163 @@ +/****************************************************************************** + * Copyright (c) 2017, Peter J. Gadomski + * + * All rights reserved + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following + * conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Hobu, Inc. or Flaxen Geo Consulting nor the + * names of its contributors may be used to endorse or promote + * products derived from this software without specific prior + * written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT + * OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY + * OF SUCH DAMAGE. + ****************************************************************************/ + +#include "Support.hpp" +#include +#include +#include +#include +#include +#include +#include + +namespace pdal +{ +namespace +{ + +std::unique_ptr newReader() +{ + Options options; + options.add("filename", Support::datapath("las/100-points.las")); + std::unique_ptr reader(new LasReader()); + reader->setOptions(options); + return reader; +} + +std::unique_ptr newFilter() +{ + return std::unique_ptr(new IcpFilter()); +} + +void checkPointsEqualReader(const PointViewSet& pointViewSet, double tolerance) +{ + auto reader = newReader(); + PointTable table; + reader->prepare(table); + PointViewSet readerPointViewSet = reader->execute(table); + ASSERT_EQ(1u, readerPointViewSet.size()); + PointViewPtr expected = *readerPointViewSet.begin(); + ASSERT_EQ(1u, pointViewSet.size()); + PointViewPtr actual = *pointViewSet.begin(); + + ASSERT_EQ(expected->size(), actual->size()); + + for (PointId i = 0; i < expected->size(); ++i) + { + ASSERT_NEAR(expected->getFieldAs(Dimension::Id::X, i), + actual->getFieldAs(Dimension::Id::X, i), tolerance); + ASSERT_NEAR(expected->getFieldAs(Dimension::Id::Y, i), + actual->getFieldAs(Dimension::Id::Y, i), tolerance); + ASSERT_NEAR(expected->getFieldAs(Dimension::Id::Z, i), + actual->getFieldAs(Dimension::Id::Z, i), tolerance); + } +} +} + +TEST(IcpFilterTest, DefaultIdentity) +{ + auto reader1 = newReader(); + auto reader2 = newReader(); + auto filter = newFilter(); + filter->setInput(*reader1); + filter->setInput(*reader2); + + PointTable table; + filter->prepare(table); + PointViewSet viewSet = filter->execute(table); + EXPECT_EQ(1u, viewSet.size()); + EXPECT_EQ(100u, (*viewSet.begin())->size()); + + MetadataNode root = filter->getMetadata(); + MetadataNode transform = root.findChild("transform"); + EXPECT_EQ("matrix", transform.type()); + Eigen::MatrixXd transformMatrix = transform.value(); + EXPECT_TRUE(transformMatrix.isApprox(Eigen::MatrixXd::Identity(4, 4), 1.0)); +} + +TEST(IcpFilterTest, RecoverTranslation) +{ + auto reader1 = newReader(); + auto reader2 = newReader(); + TransformationFilter transformationFilter; + Options transformationOptions; + transformationOptions.add("matrix", "1 0 0 1\n0 1 0 2\n0 0 1 3\n0 0 0 1"); + transformationFilter.setOptions(transformationOptions); + transformationFilter.setInput(*reader2); + + auto filter = newFilter(); + filter->setInput(*reader1); + filter->setInput(transformationFilter); + + PointTable table; + filter->prepare(table); + PointViewSet pointViewSet = filter->execute(table); + + MetadataNode root = filter->getMetadata(); + Eigen::MatrixXd transform = + root.findChild("transform").value(); + double tolerance = 1.5; + EXPECT_NEAR(-1.0, transform(0, 3), tolerance); + EXPECT_NEAR(-2.0, transform(1, 3), tolerance); + EXPECT_NEAR(-3.0, transform(2, 3), tolerance); + checkPointsEqualReader(pointViewSet, tolerance); +} + +TEST(IcpFilterTest, TooFewInputs) +{ + auto reader = newReader(); + auto filter = newFilter(); + filter->setInput(*reader); + + PointTable table; + filter->prepare(table); + ASSERT_THROW(filter->execute(table), pdal_error); +} + +TEST(IcpFilterTest, ThreeInputs) +{ + auto reader1 = newReader(); + auto reader2 = newReader(); + auto reader3 = newReader(); + auto filter = newFilter(); + filter->setInput(*reader1); + filter->setInput(*reader2); + filter->setInput(*reader3); + + PointTable table; + filter->prepare(table); + PointViewSet pointViewSet = filter->execute(table); + EXPECT_EQ(2ul, pointViewSet.size()); +} +} diff --git a/test/data/las/100-points.las b/test/data/las/100-points.las new file mode 100644 index 0000000000000000000000000000000000000000..eb1f6b2a4fd7123d69821237051d1179b5c60076 GIT binary patch literal 3627 zcmZve2~?Ej8pogE;1nQumO$*bL@$<)MdE)Xav-v7+)bnd+;=FFKn^Zocg`}4k|wG)QZ zU;N2D;)ZEQ|HXqpe{ww!?$x7jkDkHZdPnq`o)Xdh&u6YLACS}g7Tx;eHx@!O@pUTR zK6uOi_}TTbRK0hEGc$Ye|9}1$@;_f^r=;BZa(QQMczF1MH7T97Teoc4@}2K%T9?YA zU%G4EIvWg8*USqeeLh_h6|~dF^sD@NVL8zb8tL&wS0Y+1%e`pw@zbuQIh0LDI!Y`* z#h-VoAS$BPvU)>_C?9lGx7ST9q?(D2AJigsSv=d~ab5qlc-ziVY{o8?@0h-f**%Sdk&J!fS&<3PMi z)8R>Ss+CE1;h4*FBZ?= zPiTLyuB~=4Wzt4^thck#h5mdr2u~2rhGOygjWHLWf*3&KDV8o5Ni3y}!c9y=K792h zqC<~n^L}J~9@dQ%BXo8qMaX%tN}?V#9yu)$VXEgXH$Y6JDKwX?U)$M>oBlld0+AO@ zmC3S8#KDL={0@jXN}y!w7Uf`Fa^&38OeA2X&59C{)*_`n0uhhOB~h1e988Rn^OjN~ z4Ls%WV_lzqTRnVTS&yR;6c+1XK6}s~AP|d8hU&|6s*{}oe%cB2H@x1uX=g(x%Xx7X zkvB5tX)ua_$X(6{Al{_0G@8Pr?5u3Qf=@*!zmHCS4vp9=)eX%cQcx)!-3+(0rqh?$f^>9l-?#C}|- z;2Rt+PZhFe5F7htzS0Rq6pcYNpXNwxK|2LsdxWSHa{3rO_QQR(Y6JQs8kLL#q1_Z5g$AU0D-D~YXd zQ1CK%dJ>*CARiMegBwEOhn^NfW7QNZJ5sCQlR#*YkH?ut;cHwcl0d9M7v|8s1S|U} zNy+D8D%()JhbMZyPvosBT3AV^d9ZW9YGs!@DET00gg}EpjQO@XZ9E7gt;9ZP5;j(n zsN@lQ+_jalWhSv9`ar*A5K)+y7#h6W#!?EDT*Cf9>@t>V635yWo?Zx|6rEg1r~61O zt4PUfkq;gE!xvWk28X=J0#Qa8^eGK%u`#b`74Ho{-UaboS1Q8S-_6K~@YCL=@3BbV zI%H#gZAxz0?b@H{WDpgz-rl$Z#9aJOCXMp7v3B(;-eMx`3giRC;Ht=&d=OFSpIBP* zla(!gS;fzwlP9^l5X7<7x63z!D1|~X`6pXh=SwPn7=5}1UD#+S6*~qbb~3xI%!IEa zy`iy7#fzcgOD}mmg+}wlfo-Zl#85nRSNE{8*N>`sGBg5fwKXw$Ri;^DD=+Rd<1 zK>g-e*}Lg#z7d23!c%7!SD*Jj_jeF8u`QQU-nUlP_NJPbB4h254?k$6I&1%717W6B zw2jPi8~Zt1%?H4bg@~mUh##u%Z2S(yR7#`|>Bnpv8}fsiH$me)#Nwkji)zn<$DJUm zQQ2~e+-hTcYgPR0VInz*XYgbH%-2r-3L+2HUrRS{*jT}I74M1;Qn~U0V)denXBt6l zLWkwkiFk<_wyC)2!b*!!g)0yeGZ7# zh(DiVimmK}foeV&IZa1SeV1B9?k8hgeFWkiY^e;o8g6Buhp72yAUe1Rc=~0_kp9^q zw$ciC+W(}LX@WHTF07;=>%V7|36I(Sr3>D7H2>DL zbs#b*6_xQG=wMgRD|jyQ0V_BgjDr6dmU|OhsvHxyjhG~{$|?n~MowKl)&wH1=8G$> zFt=k6|2WE9B(YKT3VsE3@x~64889#T+7)XtFNxU6^C)tVojGqQc{2Ryg6jK#NccXi z*A5VsP&HGRLOTmJD*1d6K_L9HO(M%UcS{wBL#Y04I@-y>1~n;oM|ipfezagqtu)1s zKLz3(qN}8TmO0o3Otu1^hJo+}k=VWY*aZ;f=#F9{^(qNt+i z*_$Bjv<3ZPIb>%oIZ9rESO&olo>eNA4cl~bIEZT6L>9U<%FaB_D*3-qIW;t7d8MLD z*zK5)jAs{+KqXBapeD_gFB8bh{O53TOLt+6wN`CDmkv}$!FZ>AX zcW(A<5Oa|Aboyzh#EOg5{1Q%6*G_JQi5mX#sCJ&?{Iv6N0vTxOb%~vFs@&(CfE5o+ z<(lPts{KJ!V5{$?eT@?9{9MW75Um^y&2}lVl5;B0;Rjov(mS7#*w!X8#>%oPr;eZ5HJ<1LI=ceq@u8bdMb@B~Hyl&>wF@ z!z;@y>V4X68wX+p?l6<7rq02pwpH^zZckgw+U8os^6nnjY7<;JU5zv9?oM=Y^>}fa>q0_8-{UURa5M zA6=p0^RY!V&GC;90#O1h8)(-Ni8W5w@HOxw7O^y8e?;B={_v|H(s0LFOgC3aZ0s^M zZ*!c;1`Q6Pxm!+${vcuyNj!OE+S#sqYWGxjL9~x`W#XM@;h!DBWbc9vBOR@9uxB-D zJ{(pmLDU(_gmdk_g6kk+aJPsjQ>Bf0W@-5AAQbSUInyGh?b%*23RW^P4LT}1VPzYB z)40!uK=@G)qF>isLn?@2P#8;ry{ybpsNoOx6Zv8G8?l4Velg=*BCN#1%4C`qW@CNo zHC#d`yKbMfv`lmjKR#kHh>vmJ=24MGVzuJ}cnESj9JgQ(#Ij@MQ`09P(%iS4EvIZO z=?O9h0_VQWM^`SKPF;A;GFP9Pa5jXUv9h$+131U+6SrUwrY{%!isPQ`1d)nOx0Jf+ z9W3gYhBsoL>Y>qSu!u36mOi_W+J1=hCy#2^+1aK;8h!%#Fu4e1J@tM{U|U3c6x(z= zO)ikwp}qn9TFHOzxH9b1_i7eh*$hu-;#5zk$!0sNdO3jS!AdkV7>N9J7keEBVZihC zB+qrQD?2p&7i>${&7fIdCfbe;U3CeB4z(|!d`v@ChX5W5jakr;ttc0*9icn>B9?J9 h4R+25JA0@J;F-urDl+DceC!*h@Js+PgJ#3dzX6BcSP}pL literal 0 HcmV?d00001 diff --git a/test/unit/EigenTest.cpp b/test/unit/EigenTest.cpp index ec8c9692b6..a32455ce32 100644 --- a/test/unit/EigenTest.cpp +++ b/test/unit/EigenTest.cpp @@ -250,3 +250,12 @@ TEST(EigenTest, Morphological) EXPECT_EQ(1, Fv[12]); EXPECT_EQ(0, Fv2[12]); } + +TEST(EigenTest, RoundtripString) +{ + Eigen::MatrixXd identity = Eigen::MatrixXd::Identity(4, 4); + Eigen::MatrixXd target; + Utils::fromString(Utils::toString(identity), target); + ASSERT_EQ(identity.size(), target.size()); + EXPECT_EQ(identity, target); +}