diff --git a/.github/workflows/ubuntu.yml b/.github/workflows/ubuntu.yml index a1dd223b..1e66e0fc 100644 --- a/.github/workflows/ubuntu.yml +++ b/.github/workflows/ubuntu.yml @@ -23,7 +23,7 @@ jobs: strategy: fail-fast: false matrix: - distro: [focal] + distro: [focal, jammy] container: image: ubuntu:${{ matrix.distro }} env: @@ -37,9 +37,10 @@ jobs: path: target_ws/src - name: Build and Tests - uses: tesseract-robotics/colcon-action@v5 + uses: marip8/colcon-action@009925185785f6c8e8228b859d64c731ab7a62ac with: ccache-prefix: ${{ matrix.distro }} vcs-file: dependencies.repos + rosdep-install-args: '--skip-keys libvtk' target-path: target_ws/src target-args: --cmake-args -DCMAKE_BUILD_TYPE=Debug diff --git a/noether_filtering/CMakeLists.txt b/noether_filtering/CMakeLists.txt deleted file mode 100644 index 6bc4b3ea..00000000 --- a/noether_filtering/CMakeLists.txt +++ /dev/null @@ -1,157 +0,0 @@ -cmake_minimum_required(VERSION 3.5.0) - -find_package(ros_industrial_cmake_boilerplate REQUIRED) -extract_package_metadata(pkg) - -project(${pkg_extracted_name} VERSION ${pkg_extracted_version} LANGUAGES CXX) - -find_package(PCL REQUIRED COMPONENTS common filters surface segmentation) -if(PCL_FOUND AND ("${PCL_VERSION}" VERSION_LESS 1.9)) - message(WARNING "The minimum required version of PCL is 1.9, but found ${PCL_VERSION} in path first. Checking for exactly 1.9") - find_package(PCL REQUIRED 1.9 COMPONENTS common filters surface segmentation) -else() - add_definitions(${PCL_DEFINITIONS}) -endif() -find_package(console_bridge REQUIRED) -find_package(pluginlib REQUIRED) -find_package(xmlrpcpp REQUIRED) - -########### -## Build ## -########### - -add_library(${PROJECT_NAME} SHARED - src/filter_group.cpp - src/filter_manager.cpp - src/subset_extraction/subset_extractor.cpp - src/subset_extraction/extruded_polygon_subset_extractor.cpp -) -target_link_libraries(${PROJECT_NAME} PUBLIC - ${Boost_LIBRARIES} - ${pluginlib_LIBRARIES} - ${PCL_LIBRARIES} - ${xmlrpcpp_LIBRARIES} -) -target_include_directories(${PROJECT_NAME} PUBLIC - "$" - "$") -target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC - ${Boost_INCLUDE_DIRS} - ${pluginlib_INCLUDE_DIRS} - ${PCL_INCLUDE_DIRS} - ${xmlrpcpp_INCLUDE_DIRS} -) -target_cxx_version(${PROJECT_NAME} PUBLIC VERSION 14) -set_target_properties(${PROJECT_NAME} PROPERTIES NO_SYSTEM_FROM_IMPORTED 1) - -if(${BUILD_MESH_PLUGINS}) - # Mesh Filters library - message("Building mesh plugins") - set(MESH_FILTERS_SRC - src/mesh/euclidean_clustering.cpp - src/mesh/clean_data.cpp - src/mesh/fill_holes.cpp - src/mesh/windowed_sinc_smoothing.cpp - ) - set(MESH_FILTERS_COMPILE_DEFINITIONS "") - - # Check if PCL was compiled with OpenNURBS - if(NOT DEFINED NURBS_FOUND AND ${CMAKE_VERSION} VERSION_GREATER 3.15) - try_compile( - NURBS_FOUND - ${CMAKE_CURRENT_BINARY_DIR}/pcl_nurbs_try_compile - ${CMAKE_CURRENT_SOURCE_DIR}/cmake/test_scripts/check_pcl_nurbs.cpp - CMAKE_FLAGS ${CMAKE_FLAGS} -DINCLUDE_DIRECTORIES=${PCL_INCLUDE_DIRS} - LINK_LIBRARIES ${PCL_LIBRARIES} - ) - if (NOT ${NURBS_FOUND}) - message("PCL NURBS NOT FOUND") - else() - message("PCL NURBS FOUND") - endif() - endif() - - if (${NURBS_FOUND}) - list(APPEND MESH_FILTERS_SRC src/mesh/bspline_reconstruction.cpp) - list(APPEND MESH_FILTERS_COMPILE_DEFINITIONS ENABLE_NURBS) - endif() - - add_library(${PROJECT_NAME}_mesh_filters SHARED ${MESH_FILTERS_SRC}) - target_compile_definitions(${PROJECT_NAME}_mesh_filters PUBLIC ${MESH_FILTERS_COMPILE_DEFINITIONS}) - - target_link_libraries(${PROJECT_NAME}_mesh_filters PUBLIC ${PROJECT_NAME}) - target_include_directories(${PROJECT_NAME}_mesh_filters PUBLIC - "$" - "$") - target_cxx_version(${PROJECT_NAME}_mesh_filters PUBLIC VERSION 14) - set_target_properties(${PROJECT_NAME}_mesh_filters PROPERTIES NO_SYSTEM_FROM_IMPORTED 1) - - # Mesh Filter Plugins Library - add_library(${PROJECT_NAME}_mesh_filter_plugins SHARED src/mesh/plugins.cpp) - target_link_libraries(${PROJECT_NAME}_mesh_filter_plugins PUBLIC - ${PROJECT_NAME}_mesh_filters - ) - target_cxx_version(${PROJECT_NAME}_mesh_filter_plugins PUBLIC VERSION 14) - set_target_properties(${PROJECT_NAME}_mesh_filter_plugins PROPERTIES NO_SYSTEM_FROM_IMPORTED 1) -endif() - -# Cloud Filters library -add_library(${PROJECT_NAME}_cloud_filters SHARED src/cloud/filters.cpp) -target_link_libraries(${PROJECT_NAME}_cloud_filters PUBLIC ${PROJECT_NAME}) -target_include_directories(${PROJECT_NAME}_cloud_filters PUBLIC - "$" - "$") -target_cxx_version(${PROJECT_NAME}_cloud_filters PUBLIC VERSION 14) -set_target_properties(${PROJECT_NAME}_cloud_filters PROPERTIES NO_SYSTEM_FROM_IMPORTED 1) - -# Cloud Filter Plugins Library -add_library(${PROJECT_NAME}_cloud_filter_plugins SHARED src/cloud/plugins.cpp) -target_link_libraries(${PROJECT_NAME}_cloud_filter_plugins PUBLIC - ${PROJECT_NAME}_cloud_filters -) -target_cxx_version(${PROJECT_NAME}_cloud_filter_plugins PUBLIC VERSION 14) -set_target_properties(${PROJECT_NAME}_cloud_filter_plugins PROPERTIES NO_SYSTEM_FROM_IMPORTED 1) - -############# -## Install ## -############# -if(${BUILD_MESH_PLUGINS}) - list(APPEND PACKAGE_LIBRARIES ${PROJECT_NAME} ${PROJECT_NAME}_mesh_filters ${PROJECT_NAME}_mesh_filter_plugins ${PROJECT_NAME}_cloud_filters ${PROJECT_NAME}_cloud_filter_plugins) -else() - list(APPEND PACKAGE_LIBRARIES ${PROJECT_NAME} ${PROJECT_NAME}_cloud_filters ${PROJECT_NAME}_cloud_filter_plugins) -endif() - -# Install headers -install(DIRECTORY include/ DESTINATION include/) - -# Install files -install(FILES package.xml cloud_filter_plugins.xml mesh_filter_plugins.xml - DESTINATION share/${PROJECT_NAME}) - -############# -## Testing ## -############# -if(${ENABLE_TESTS}) - enable_testing() - find_gtest() - - add_run_tests_target() - - add_executable(${PROJECT_NAME}_cloud_unit src/test/cloud_utest.cpp) - target_link_libraries(${PROJECT_NAME}_cloud_unit PUBLIC - ${PROJECT_NAME}_cloud_filter_plugins - GTest::GTest - GTest::Main - ) - target_cxx_version(${PROJECT_NAME}_cloud_unit PUBLIC VERSION 14) - set_target_properties(${PROJECT_NAME}_cloud_unit PROPERTIES NO_SYSTEM_FROM_IMPORTED 1) - add_gtest_discover_tests(${PROJECT_NAME}_cloud_unit) - add_dependencies(run_tests ${PROJECT_NAME}) -endif() - -# Configure package -configure_package( - NAMESPACE noether - TARGETS ${PACKAGE_LIBRARIES} - DEPENDENCIES "PCL REQUIRED COMPONENTS common filters surface segmentation" pluginlib console_bridge xmlrpcpp -) diff --git a/noether_filtering/README.md b/noether_filtering/README.md deleted file mode 100644 index 818b6b74..00000000 --- a/noether_filtering/README.md +++ /dev/null @@ -1,72 +0,0 @@ -# Noether Filtering - -This package provides a generic manager for dynamically constucting and running filter pipelines. -At run-time, a user can specify the configuration of various filter groups (i.e. pipelines) to the manager using XmlRpc values. -A filter group is comprised of a serial chain of filters through which to run data. The manager loads the filters -from plugins and performs the filtering operations on an input data object. - -The target applications for this pipeline are mesh and point cloud types, but could be -extended to any other classes where filter pipelines apply (such as 2D images). - -## Usage - -The filter manager is configured using XmlRpc value organized in the structure shown below: - -``` -- filter_groups: - - group_name: (my_group) - filters: - - name: (filter_1) - type: noether_filtering::(FilterClass) - config: - - param_1: (my_param_1) - param_2: (my_param_2) - - name: (filter_2) - type: noether_filtering::(FilterClass) - config: - - param_a: (my_param_a) - param_b: (my_param_b) -``` - -Required Configuration Parameters: - - Filter manager configuration - - `filter_groups`: array, filter group configurations - - Filter group configuration: - - `group_name`: string, unique name of the filter group - - `continue_on_failure`: boolean, whether or not the chain should continue after failure of one filter - - `filters`: array, filter configurations - - Filter configuration - - `name`: string, unique name of the filter - - `type`: string, fully qualified filter class name (see Available Plugins below) - - `config`: XmlRpc value, structure containing configuration data of the individual filter - -## Supported Types - -This package compiles various filters for the `pcl::PolygonMesh` type and for -[all PCL XYZ point types](https://github.com/PointCloudLibrary/pcl/blob/a8f6435a1a6635656327d5347fe81b1876a11dea/common/include/pcl/impl/point_types.hpp#L112) - -## Available Plugins - -The following plugins are currently available and are listed by filter data type - -### `pcl::PolygonMesh` -- `noether_filtering::BSplineReconstruction` -- `noether_filtering::EuclideanClustering` -- `noether_filtering::mesh::CleanData` -- `noether_filtering::mesh::WindowedSincSmoothing` -- `noether_filtering::mesh::FillHoles` - -> Note: PCL 1.9 and VTK 8.2 are required to compile the mesh filtering plugins. If these system dependencies do not exist, the mesh filtering plugins will not be built - -### `pcl::PointCloud` -- `noether_filtering::CropBoxFilter` -- `noether_filtering::PassThrough` -- `noether_filtering::RadiusOutlierFilter` -- `noether_filtering::StatisticalOutlierFilter` -- `noether_filtering::VoxelGridFilter` -- `noether_filtering::MLSSmoothingFilter` - -> Note: a valid `PointT` type must be provided in the specification of the name of the plugin -(i.e. `pcl::PointXYZ`) - - diff --git a/noether_filtering/cloud_filter_plugins.xml b/noether_filtering/cloud_filter_plugins.xml deleted file mode 100644 index 14d14d4b..00000000 --- a/noether_filtering/cloud_filter_plugins.xml +++ /dev/null @@ -1,153 +0,0 @@ - - - - - Voxel grid filter for PointXYZ type - - - - - Statistical outlier filter for PointXYZ type - - - - - Pass through filter for PointXYZ type - - - - - Radius outlier filter for PointXYZ type - - - - - Crop box filter for PointXYZ type - - - - - MLS smoothing filter for PointXYZ type - - - - - - - Voxel grid filter for PointNormal type - - - - - Statistical outlier filter for PointNormal type - - - - - Pass through filter for PointNormal type - - - - - Radius outlier filter for PointNormal type - - - - - Crop box filter for PointNormal type - - - - - MLS smoothing filter for PointNormal type - - - - - - - Voxel grid filter for PointXYZRGB type - - - - - Statistical outlier filter for PointXYZRGB type - - - - - Pass through filter for PointXYZRGB type - - - - - Radius outlier filter for PointXYZRGB type - - - - - Crop box filter for PointXYZRGB type - - - - - MLS smoothing filter for PointXYZRGB type - - - - - - - Voxel grid filter for PointXYZI type - - - - - Statistical outlier filter for PointXYZI type - - - - - Pass through filter for PointXYZI type - - - - - Radius outlier filter for PointXYZI type - - - - - Crop box filter for PointXYZI type - - - - - MLS smoothing filter for PointXYZI type - - - diff --git a/noether_filtering/colcon.pkg b/noether_filtering/colcon.pkg deleted file mode 100644 index 05946594..00000000 --- a/noether_filtering/colcon.pkg +++ /dev/null @@ -1,3 +0,0 @@ -{ - "hooks": ["share/noether_filtering/hook/ament_prefix_path.dsv", "share/noether_filtering/hook/ros_package_path.dsv"] -} diff --git a/noether_filtering/include/noether_filtering/cloud/cloud_filter_manager.h b/noether_filtering/include/noether_filtering/cloud/cloud_filter_manager.h deleted file mode 100644 index 36c1b377..00000000 --- a/noether_filtering/include/noether_filtering/cloud/cloud_filter_manager.h +++ /dev/null @@ -1,27 +0,0 @@ -#ifndef NOETHER_FILTERING_CLOUD_CLOUD_FILTER_MANAGER_H -#define NOETHER_FILTERING_CLOUD_CLOUD_FILTER_MANAGER_H - -#include "noether_filtering/filter_manager.h" -#include - -// Forward declare PCL PointCloud -namespace pcl -{ -template -class PointCloud; -} - -namespace noether_filtering -{ -namespace cloud -{ -template -using CloudFilterGroup = FilterManager>; - -template -using CloudFilterManager = FilterManager>; - -} // namespace cloud -} // namespace noether_filtering - -#endif // NOETHER_FILTERING_CLOUD_CLOUD_FILTER_MANAGER_H diff --git a/noether_filtering/include/noether_filtering/cloud/crop_box_filter.h b/noether_filtering/include/noether_filtering/cloud/crop_box_filter.h deleted file mode 100644 index 1d51fd9c..00000000 --- a/noether_filtering/include/noether_filtering/cloud/crop_box_filter.h +++ /dev/null @@ -1,65 +0,0 @@ -#ifndef NOETHER_FILTERING_CLOUD_CROP_BOX_FILTER_H -#define NOETHER_FILTERING_CLOUD_CROP_BOX_FILTER_H - -#include "noether_filtering/filter_base.h" -#include -#include - -namespace noether_filtering -{ -namespace cloud -{ -template -class CropBoxFilter : public FilterBase> -{ -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - using T = typename pcl::PointCloud; - - using FilterBase::FilterBase; - - /** - * @brief configure - * @param config: XmlRpc value - * - config: - * min_pt: - * x: (double) - * y: (double) - * z: (double) - * max_pt: - * x: (double) - * y: (double) - * z: (double) - * transform: - * x: (double) - * y: (double) - * z: (double) - * rx: (double) - * ry: (double) - * rz: (double) - * crop_outside: (bool) - * @return - */ - bool configure(XmlRpc::XmlRpcValue config) override final; - bool filter(const T& input, T& output) override final; - std::string getName() const override final; - - struct Params - { - Eigen::Vector4f min_pt = -1.0 * Eigen::Vector4f::Ones(); - Eigen::Vector4f max_pt = Eigen::Vector4f::Ones(); - Eigen::Affine3f transform = Eigen::Affine3f::Identity(); - bool crop_outside = false; - }; - Params params; - - static const std::string MAX; - static const std::string MIN; - static const std::string TRANSFORM; - static const std::string CROP_OUTSIDE; -}; - -} // namespace cloud -} // namespace noether_filtering - -#endif // NOETHER_FILTERING_CLOUD_CROP_BOX_FILTER_H diff --git a/noether_filtering/include/noether_filtering/cloud/impl/crop_box_filter.hpp b/noether_filtering/include/noether_filtering/cloud/impl/crop_box_filter.hpp deleted file mode 100644 index 19ade854..00000000 --- a/noether_filtering/include/noether_filtering/cloud/impl/crop_box_filter.hpp +++ /dev/null @@ -1,149 +0,0 @@ -#ifndef NOETHER_FILTERING_CLOUD_IMPL_CROP_BOX_FILTER_HPP -#define NOETHER_FILTERING_CLOUD_IMPL_CROP_BOX_FILTER_HPP - -#include "noether_filtering/cloud/crop_box_filter.h" -#include "noether_filtering/utils.h" -#include -#include -#include - -namespace -{ -bool fromXmlRpc(XmlRpc::XmlRpcValue value, Eigen::Ref out) -{ - try - { - out.x() = static_cast(value["x"]); - out.y() = static_cast(value["y"]); - out.z() = static_cast(value["z"]); - } - catch (const XmlRpc::XmlRpcException& ex) - { - CONSOLE_BRIDGE_logError("%s", ex.getMessage().c_str()); - return false; - } - return true; -} - -bool fromXmlRpc(XmlRpc::XmlRpcValue value, Eigen::Affine3f& out) -{ - out = Eigen::Affine3f::Identity(); - try - { - out.translation().x() = static_cast(value["x"]); - out.translation().y() = static_cast(value["y"]); - out.translation().z() = static_cast(value["z"]); - - Eigen::AngleAxisf rx(static_cast(value["rx"]), Eigen::Vector3f::UnitX()); - Eigen::AngleAxisf ry(static_cast(value["ry"]), Eigen::Vector3f::UnitY()); - Eigen::AngleAxisf rz(static_cast(value["rz"]), Eigen::Vector3f::UnitZ()); - out.rotate(rx * ry * rz); - } - catch (const XmlRpc::XmlRpcException& ex) - { - CONSOLE_BRIDGE_logError("%s", ex.getMessage().c_str()); - return false; - } - return true; -} - -} // namespace - -namespace noether_filtering -{ -namespace cloud -{ -template -const std::string CropBoxFilter::MAX = "max"; - -template -const std::string CropBoxFilter::MIN = "min"; - -template -const std::string CropBoxFilter::TRANSFORM = "transform"; - -template -const std::string CropBoxFilter::CROP_OUTSIDE = "crop_outside"; - -template -bool CropBoxFilter::configure(XmlRpc::XmlRpcValue value) -{ - std::string error; - if (!value.hasMember(MIN)) - error += MIN + ", "; - if (!value.hasMember(MAX)) - error += MAX + ", "; - if (!value.hasMember(TRANSFORM)) - error += TRANSFORM + ", "; - - if (!error.empty()) - { - CONSOLE_BRIDGE_logError("Filter configuration missing required parameters: %s", error.c_str()); - return false; - } - - // Required parameter(s) - try - { - bool success = true; - success &= fromXmlRpc(value[MIN], params.min_pt); - success &= fromXmlRpc(value[MAX], params.max_pt); - success &= fromXmlRpc(value[TRANSFORM], params.transform); - if (!success) - { - return false; - } - } - catch (const XmlRpc::XmlRpcException& ex) - { - CONSOLE_BRIDGE_logError("Failed to load required parameter(s) for pass through filter: '%s'", - ex.getMessage().c_str()); - return false; - } - - // Optional parameter(s) - if (value.hasMember(CROP_OUTSIDE)) - { - try - { - params.crop_outside = static_cast(value[CROP_OUTSIDE]); - } - catch (const XmlRpc::XmlRpcException& ex) - { - CONSOLE_BRIDGE_logWarn("Failed to load optional parameter(s) for crop box filter: '%s'", ex.getMessage().c_str()); - } - } - - return true; -} - -template -bool CropBoxFilter::filter(const T& input, T& output) -{ - // Create a shared pointer to the input object with a "destructor" function that does not delete the raw pointer - auto cloud = boost::shared_ptr(&input, [](const T*) {}); - - // Set the parameters - pcl::CropBox f(params.crop_outside); - f.setMin(params.min_pt); - f.setMax(params.max_pt); - f.setTransform(params.transform); - - f.setInputCloud(cloud); - f.filter(output); - - return true; -} - -template -std::string CropBoxFilter::getName() const -{ - return utils::getClassName(); -} - -} // namespace cloud -} // namespace noether_filtering - -#define PCL_INSTANTIATE_CropBoxFilter(T) template class PCL_EXPORTS noether_filtering::cloud::CropBoxFilter; - -#endif // NOETHER_FILTERING_CLOUD_IMPL_CROP_BOX_FILTER_HPP diff --git a/noether_filtering/include/noether_filtering/cloud/impl/mls_smoothing_filter.hpp b/noether_filtering/include/noether_filtering/cloud/impl/mls_smoothing_filter.hpp deleted file mode 100644 index e3d802c9..00000000 --- a/noether_filtering/include/noether_filtering/cloud/impl/mls_smoothing_filter.hpp +++ /dev/null @@ -1,122 +0,0 @@ -#ifndef NOETHER_FILTERING_CLOUD_IMPL_MLS_SMOOTHING_FILTER_HPP -#define NOETHER_FILTERING_CLOUD_IMPL_MLS_SMOOTHING_FILTER_HPP - -#include "noether_filtering/cloud/mls_smoothing_filter.h" -#include "noether_filtering/utils.h" -#include -#include -#include -#include - -namespace noether_filtering -{ -namespace cloud -{ -template -const std::string MLSSmoothingFilter::POLYNOMIAL_ORDER = "polynomial_order"; - -template -const std::string MLSSmoothingFilter::SEARCH_RADIUS = "search_radius"; - -template -bool MLSSmoothingFilter::configure(XmlRpc::XmlRpcValue value) -{ - std::string error; - if (!value.hasMember(SEARCH_RADIUS)) - error += SEARCH_RADIUS + ", "; - if (!value.hasMember(POLYNOMIAL_ORDER)) - error += POLYNOMIAL_ORDER + ", "; - - if (!error.empty()) - { - CONSOLE_BRIDGE_logError("Failed to find required configuration parameters: %s", error.c_str()); - return false; - } - - try - { - params.search_radius = static_cast(value[SEARCH_RADIUS]); - params.polynomial_order = static_cast(value[POLYNOMIAL_ORDER]); - } - catch (const XmlRpc::XmlRpcException& ex) - { - CONSOLE_BRIDGE_logError("Error configuring MLS Smoothing Filter: '%s'", ex.getMessage().c_str()); - return false; - } - - return true; -} - -template -bool MLSSmoothingFilter::filter(const T& input, T& output) -{ - // Convert the input cloud to XYZ type - pcl::PointCloud::Ptr cloud(new pcl::PointCloud()); - pcl::copyPointCloud(input, *cloud); - - typename pcl::search::KdTree::Ptr tree(new pcl::search::KdTree()); - - pcl::MovingLeastSquares mls; - mls.setInputCloud(cloud); - mls.setSearchMethod(tree); - - mls.setSearchRadius(params.search_radius); - mls.setSqrGaussParam(params.search_radius * params.search_radius); - mls.setPolynomialOrder(params.polynomial_order); - - // Set the computation of normals false since we are only instantiating PCL XYZ point types as inputs/outputs - mls.setComputeNormals(false); - - // Run the filter - pcl::PointCloud filtered_cloud; - mls.process(filtered_cloud); - - // Convert the output cloud back into the template format - pcl::copyPointCloud(filtered_cloud, output); - - return true; -} - -/** - * Provide an override specifically for the Point Normal template type that will calculate normals - */ -template <> -bool MLSSmoothingFilter::filter(const pcl::PointCloud& input, - pcl::PointCloud& output) -{ - // Convert the input cloud to XYZ type - pcl::PointCloud::Ptr cloud(new pcl::PointCloud()); - pcl::copyPointCloud(input, *cloud); - - typename pcl::search::KdTree::Ptr tree(new pcl::search::KdTree()); - - pcl::MovingLeastSquares mls; - mls.setInputCloud(cloud); - mls.setSearchMethod(tree); - - mls.setSearchRadius(params.search_radius); - mls.setSqrGaussParam(params.search_radius * params.search_radius); - mls.setPolynomialOrder(params.polynomial_order); - - // Set the computation of normals true since the incoming cloud has normals - mls.setComputeNormals(true); - - // Run the filter - mls.process(output); - - return true; -} - -template -std::string MLSSmoothingFilter::getName() const -{ - return utils::getClassName(); -} - -} // namespace cloud -} // namespace noether_filtering - -#define PCL_INSTANTIATE_MLSSmoothingFilter(T) \ - template class PCL_EXPORTS noether_filtering::cloud::MLSSmoothingFilter; - -#endif // NOETHER_FILTERING_CLOUD_IMPL_MLS_SMOOTHING_FILTER_HPP diff --git a/noether_filtering/include/noether_filtering/cloud/impl/pass_through_filter.hpp b/noether_filtering/include/noether_filtering/cloud/impl/pass_through_filter.hpp deleted file mode 100644 index 6ee6af48..00000000 --- a/noether_filtering/include/noether_filtering/cloud/impl/pass_through_filter.hpp +++ /dev/null @@ -1,112 +0,0 @@ -#ifndef NOETHER_FILTERING_CLOUD_IMPL_PASS_THROUGH_FILTER_HPP -#define NOETHER_FILTERING_CLOUD_IMPL_PASS_THROUGH_FILTER_HPP - -#include -#include "noether_filtering/cloud/pass_through_filter.h" -#include "noether_filtering/utils.h" -#include -#include - -namespace noether_filtering -{ -namespace cloud -{ -template -const std::string PassThroughFilter::FILTER_FIELD_NAME = "filter_field_name"; - -template -const std::string PassThroughFilter::MIN_LIMIT = "min_limit"; - -template -const std::string PassThroughFilter::MAX_LIMIT = "max_limit"; - -template -const std::string PassThroughFilter::NEGATIVE = "negative"; - -template -bool PassThroughFilter::configure(XmlRpc::XmlRpcValue value) -{ - std::string error; - if (!value.hasMember(FILTER_FIELD_NAME)) - error += FILTER_FIELD_NAME + ", "; - if (!value.hasMember(MIN_LIMIT)) - error += MIN_LIMIT + ", "; - if (!value.hasMember(MAX_LIMIT)) - error += MAX_LIMIT + ", "; - - if (!error.empty()) - { - CONSOLE_BRIDGE_logError("Pass through filter configuration missing required parameters: %s", error.c_str()); - return false; - } - - // Required parameter(s) - try - { - params.filter_field_name = static_cast(value[FILTER_FIELD_NAME]); - params.min_limit = static_cast(value[MIN_LIMIT]); - params.max_limit = static_cast(value[MAX_LIMIT]); - } - catch (const XmlRpc::XmlRpcException& ex) - { - CONSOLE_BRIDGE_logError("Failed to load required parameter(s) for pass through filter: '%s'", - ex.getMessage().c_str()); - return false; - } - - if (value.hasMember(NEGATIVE)) - { - // Optional parameter(s) - try - { - params.negative = static_cast(value[NEGATIVE]); - } - catch (const XmlRpc::XmlRpcException& ex) - { - CONSOLE_BRIDGE_logWarn("Failed to load optional parameter(s) for pass through filter: '%s'", - ex.getMessage().c_str()); - } - } - - return true; -} - -template -bool PassThroughFilter::filter(const T& input, T& output) -{ - // Create a shared pointer to the input object with a "destructor" function that does not delete the raw pointer - auto cloud = boost::shared_ptr(&input, [](const T*) {}); - - pcl::PassThrough f; - - // Set the parameters - if (!params.filter_field_name.empty()) - { - f.setFilterFieldName(params.filter_field_name); - f.setFilterLimits(params.min_limit, params.max_limit); - f.setFilterLimitsNegative(params.negative); - } - else - { - CONSOLE_BRIDGE_logError("No filter field name set for pass through filter"); - return false; - } - - f.setInputCloud(cloud); - f.filter(output); - - return true; -} - -template -std::string PassThroughFilter::getName() const -{ - return utils::getClassName(); -} - -} // namespace cloud -} // namespace noether_filtering - -#define PCL_INSTANTIATE_PassThroughFilter(T) template class PCL_EXPORTS noether_filtering::cloud::PassThroughFilter; - -#endif // NOETHER_FILTERING_CLOUD_IMPL_PASS_THROUGH_FILTER_HPP diff --git a/noether_filtering/include/noether_filtering/cloud/impl/radius_outlier_filter.hpp b/noether_filtering/include/noether_filtering/cloud/impl/radius_outlier_filter.hpp deleted file mode 100644 index 235d7a97..00000000 --- a/noether_filtering/include/noether_filtering/cloud/impl/radius_outlier_filter.hpp +++ /dev/null @@ -1,78 +0,0 @@ -#ifndef NOETHER_FILTERING_CLOUD_IMPL_RADIUS_OUTLIER_FILTER_HPP -#define NOETHER_FILTERING_CLOUD_IMPL_RADIUS_OUTLIER_FILTER_HPP - -#include "noether_filtering/cloud/radius_outlier_filter.h" -#include "noether_filtering/utils.h" -#include -#include -#include - -namespace noether_filtering -{ -namespace cloud -{ -template -const std::string RadiusOutlierFilter::RADIUS = "radius"; - -template -const std::string RadiusOutlierFilter::MIN_PTS = "min_pts"; - -template -bool RadiusOutlierFilter::configure(XmlRpc::XmlRpcValue config) -{ - std::string error; - if (!config.hasMember("radius")) - error += RADIUS + ", "; - if (!config.hasMember("min_pts")) - error += MIN_PTS + ", "; - - if (!error.empty()) - { - CONSOLE_BRIDGE_logError("Radius outlier filter configuration missing parameters: %s", error.c_str()); - return false; - } - - try - { - params.radius = static_cast(config[RADIUS]); - params.min_pts = static_cast(config[MIN_PTS]); - } - catch (const XmlRpc::XmlRpcException& ex) - { - CONSOLE_BRIDGE_logError("Error configuring radius outlier filter: '%s'", ex.getMessage().c_str()); - return false; - } - return true; -} - -template -bool RadiusOutlierFilter::filter(const T& input, T& output) -{ - // Create a shared pointer to the input object with a "destructor" function that does not delete the raw pointer - auto cloud = boost::shared_ptr(&input, [](const T*) {}); - - pcl::RadiusOutlierRemoval f; - - // Set the parameters - f.setRadiusSearch(params.radius); - f.setMinNeighborsInRadius(params.min_pts); - - f.setInputCloud(cloud); - f.filter(output); - - return true; -} - -template -std::string RadiusOutlierFilter::getName() const -{ - return utils::getClassName(); -} - -} // namespace cloud -} // namespace noether_filtering - -#define PCL_INSTANTIATE_RadiusOutlierFilter(T) \ - template class PCL_EXPORTS noether_filtering::cloud::RadiusOutlierFilter; - -#endif // NOETHER_FILTERING_CLOUD_IMPL_RADIUS_OUTLIER_FILTER_HPP diff --git a/noether_filtering/include/noether_filtering/cloud/impl/statistical_outlier_filter.hpp b/noether_filtering/include/noether_filtering/cloud/impl/statistical_outlier_filter.hpp deleted file mode 100644 index 1fc2b73e..00000000 --- a/noether_filtering/include/noether_filtering/cloud/impl/statistical_outlier_filter.hpp +++ /dev/null @@ -1,78 +0,0 @@ -#ifndef STATISTICAL_OUTLIER_FILTER_HPP -#define STATISTICAL_OUTLIER_FILTER_HPP - -#include -#include "noether_filtering/cloud/statistical_outlier_filter.h" -#include "noether_filtering/utils.h" -#include -#include - -namespace noether_filtering -{ -namespace cloud -{ -template -const std::string StatisticalOutlierFilter::MEAN_K = "mean_k"; - -template -const std::string StatisticalOutlierFilter::STD_DEV_MULT = "std_dev_mult"; - -template -bool StatisticalOutlierFilter::configure(XmlRpc::XmlRpcValue value) -{ - std::string error; - if (!value.hasMember(MEAN_K)) - error += MEAN_K + ", "; - if (!value.hasMember(STD_DEV_MULT)) - error += STD_DEV_MULT + ", "; - - if (!error.empty()) - { - CONSOLE_BRIDGE_logError("Statistical outlier filter missing required parameters: %s", error.c_str()); - return false; - } - - try - { - params.mean_k = static_cast(value[MEAN_K]); - params.std_dev_mult = static_cast(value[STD_DEV_MULT]); - } - catch (const XmlRpc::XmlRpcException& ex) - { - CONSOLE_BRIDGE_logError("Error configuring statistical outlier filter: '%s'", ex.getMessage().c_str()); - return false; - } - return true; -} - -template -bool StatisticalOutlierFilter::filter(const T& input, T& output) -{ - // Create a shared pointer to the input object with a "destructor" function that does not delete the raw pointer - auto cloud = boost::shared_ptr(&input, [](const T*) {}); - - pcl::StatisticalOutlierRemoval f; - - // Apply the parameters - f.setMeanK(params.mean_k); - f.setStddevMulThresh(params.std_dev_mult); - - f.setInputCloud(cloud); - f.filter(output); - - return true; -} - -template -std::string StatisticalOutlierFilter::getName() const -{ - return utils::getClassName(); -} - -} // namespace cloud -} // namespace noether_filtering - -#define PCL_INSTANTIATE_StatisticalOutlierFilter(T) \ - template class PCL_EXPORTS noether_filtering::cloud::StatisticalOutlierFilter; - -#endif // STATISTICAL_OUTLIER_FILTER_HPP diff --git a/noether_filtering/include/noether_filtering/cloud/impl/voxel_grid_filter.hpp b/noether_filtering/include/noether_filtering/cloud/impl/voxel_grid_filter.hpp deleted file mode 100644 index c974eab7..00000000 --- a/noether_filtering/include/noether_filtering/cloud/impl/voxel_grid_filter.hpp +++ /dev/null @@ -1,111 +0,0 @@ -#ifndef NOETHER_FILTERING_CLOUD_IMPL_VOXEL_GRID_FILTER_HPP -#define NOETHER_FILTERING_CLOUD_IMPL_VOXEL_GRID_FILTER_HPP - -#include -#include "noether_filtering/cloud/voxel_grid_filter.h" -#include "noether_filtering/utils.h" -#include -#include - -namespace noether_filtering -{ -namespace cloud -{ -template -const std::string VoxelGridFilter::LEAF_SIZE = "leaf_size"; - -template -const std::string VoxelGridFilter::FILTER_FIELD_NAME = "filter_field_name"; - -template -const std::string VoxelGridFilter::MIN_LIMIT = "min_limit"; - -template -const std::string VoxelGridFilter::MAX_LIMIT = "max_limit"; - -template -const std::string VoxelGridFilter::FILTER_LIMITS_NEGATIVE = "filter_limits_negative"; - -template -const std::string VoxelGridFilter::MIN_PTS_PER_VOXEL = "min_pts_per_voxel"; - -template -bool VoxelGridFilter::configure(XmlRpc::XmlRpcValue value) -{ - if (!value.hasMember(LEAF_SIZE)) - { - CONSOLE_BRIDGE_logError("Voxel grid filter missing required configuration parameter: %s", LEAF_SIZE.c_str()); - return false; - } - - // Required parameter(s) - try - { - params.leaf_size = static_cast(value[LEAF_SIZE]); - } - catch (const XmlRpc::XmlRpcException& ex) - { - CONSOLE_BRIDGE_logError("Failed to load required parameter(s) for voxel grid filter: '%s'", - ex.getMessage().c_str()); - return false; - } - - // Optional parameter(s) - if (value.hasMember(FILTER_FIELD_NAME) && value.hasMember(MIN_LIMIT) && value.hasMember(MAX_LIMIT) && - value.hasMember(FILTER_LIMITS_NEGATIVE) && value.hasMember(MIN_PTS_PER_VOXEL)) - { - try - { - params.filter_field_name = static_cast(value[FILTER_FIELD_NAME]); - params.min_limit = static_cast(value[MIN_LIMIT]); - params.max_limit = static_cast(value[MAX_LIMIT]); - params.filter_limits_negative = static_cast(value[FILTER_LIMITS_NEGATIVE]); - params.min_pts_per_voxel = static_cast(value[MIN_PTS_PER_VOXEL]); - } - catch (const XmlRpc::XmlRpcException& ex) - { - CONSOLE_BRIDGE_logWarn("Failed to load optional parameter(s) for voxel grid filter: '%s'", - ex.getMessage().c_str()); - } - } - - return true; -} - -template -bool VoxelGridFilter::filter(const T& input, T& output) -{ - // Create a shared pointer to the input object with a "destructor" function that does not delete the raw pointer - auto cloud = boost::shared_ptr(&input, [](const T*) {}); - - pcl::VoxelGrid f; - - // Set the parameters - f.setLeafSize(params.leaf_size, params.leaf_size, params.leaf_size); - f.setMinimumPointsNumberPerVoxel(params.min_pts_per_voxel); - - if (!params.filter_field_name.empty()) - { - f.setFilterFieldName(params.filter_field_name); - f.setFilterLimits(params.min_limit, params.max_limit); - f.setFilterLimitsNegative(params.filter_limits_negative); - } - - f.setInputCloud(cloud); - f.filter(output); - - return true; -} - -template -std::string VoxelGridFilter::getName() const -{ - return utils::getClassName(); -} - -} // namespace cloud -} // namespace noether_filtering - -#define PCL_INSTANTIATE_VoxelGridFilter(T) template class PCL_EXPORTS noether_filtering::cloud::VoxelGridFilter; - -#endif // NOETHER_FILTERING_CLOUD_IMPL_VOXEL_GRID_FILTER_HPP diff --git a/noether_filtering/include/noether_filtering/cloud/mls_smoothing_filter.h b/noether_filtering/include/noether_filtering/cloud/mls_smoothing_filter.h deleted file mode 100644 index 70a356eb..00000000 --- a/noether_filtering/include/noether_filtering/cloud/mls_smoothing_filter.h +++ /dev/null @@ -1,44 +0,0 @@ -#ifndef NOETHER_FILTERING_CLOUD_MLS_SMOOTHING_FILTER_H -#define NOETHER_FILTERING_CLOUD_MLS_SMOOTHING_FILTER_H - -#include "noether_filtering/filter_base.h" - -namespace noether_filtering -{ -namespace cloud -{ -template -class MLSSmoothingFilter : public FilterBase> -{ -public: - using T = typename pcl::PointCloud; - - using FilterBase::FilterBase; - - /** - * @brief configure - * @param config: XmlRpc value - * - config: - * polynomial_order: - * search_radius: - * @return - */ - bool configure(XmlRpc::XmlRpcValue config) override final; - bool filter(const T& input, T& output) override final; - std::string getName() const override final; - - struct Params - { - int polynomial_order = 2; - double search_radius = 0.1; - }; - Params params; - - static const std::string POLYNOMIAL_ORDER; - static const std::string SEARCH_RADIUS; -}; - -} // namespace cloud -} // namespace noether_filtering - -#endif // NOETHER_FILTERING_CLOUD_MLS_SMOOTHING_FILTER_H diff --git a/noether_filtering/include/noether_filtering/cloud/pass_through_filter.h b/noether_filtering/include/noether_filtering/cloud/pass_through_filter.h deleted file mode 100644 index 1a4ae420..00000000 --- a/noether_filtering/include/noether_filtering/cloud/pass_through_filter.h +++ /dev/null @@ -1,50 +0,0 @@ -#ifndef NOETHER_FILTERING_CLOUD_PASS_THROUGH_FILTER_H -#define NOETHER_FILTERING_CLOUD_PASS_THROUGH_FILTER_H - -#include "noether_filtering/filter_base.h" -#include - -namespace noether_filtering -{ -namespace cloud -{ -template -class PassThroughFilter : public FilterBase> -{ -public: - using T = pcl::PointCloud; - - using FilterBase::FilterBase; - - /** - * @brief configure - * @param config: XmlRpc value - * - config: - * filter_field_name: (string) - * min_limit: (double) - * max_limit: (double) - * negative: (bool) - * @return - */ - bool configure(XmlRpc::XmlRpcValue config) override final; - bool filter(const T& input, T& output) override final; - std::string getName() const override final; - - struct Params - { - std::string filter_field_name = "x"; - float min_limit = -std::numeric_limits::max(); - float max_limit = std::numeric_limits::max(); - bool negative = false; - }; - Params params; - - static const std::string FILTER_FIELD_NAME; - static const std::string MIN_LIMIT; - static const std::string MAX_LIMIT; - static const std::string NEGATIVE; -}; -} // namespace cloud -} // namespace noether_filtering - -#endif // NOETHER_FILTERING_CLOUD_PASS_THROUGH_FILTER_H diff --git a/noether_filtering/include/noether_filtering/cloud/radius_outlier_filter.h b/noether_filtering/include/noether_filtering/cloud/radius_outlier_filter.h deleted file mode 100644 index 2f4f3d6e..00000000 --- a/noether_filtering/include/noether_filtering/cloud/radius_outlier_filter.h +++ /dev/null @@ -1,44 +0,0 @@ -#ifndef NOETHER_FILTERING_CLOUD_RADIUS_OUTLIER_FILTER_H -#define NOETHER_FILTERING_CLOUD_RADIUS_OUTLIER_FILTER_H - -#include "noether_filtering/filter_base.h" -#include - -namespace noether_filtering -{ -namespace cloud -{ -template -class RadiusOutlierFilter : public FilterBase> -{ -public: - using T = pcl::PointCloud; - - using FilterBase::FilterBase; - - /** - * @brief configure - * @param config: XmlRpc value - * - config: - * radius: (double) - * min_pts: (int) - * @return - */ - virtual bool configure(XmlRpc::XmlRpcValue config) override final; - virtual bool filter(const T& input, T& output) override final; - virtual std::string getName() const override final; - - struct Params - { - double radius = 0.1; - int min_pts = 10; - }; - Params params; - - static const std::string RADIUS; - static const std::string MIN_PTS; -}; -} // namespace cloud -} // namespace noether_filtering - -#endif // NOETHER_FILTERING_CLOUD_RADIUS_OUTLIER_FILTER_H diff --git a/noether_filtering/include/noether_filtering/cloud/statistical_outlier_filter.h b/noether_filtering/include/noether_filtering/cloud/statistical_outlier_filter.h deleted file mode 100644 index bcd34c8f..00000000 --- a/noether_filtering/include/noether_filtering/cloud/statistical_outlier_filter.h +++ /dev/null @@ -1,44 +0,0 @@ -#ifndef STATISTICAL_OUTLIER_FILTER_H -#define STATISTICAL_OUTLIER_FILTER_H - -#include "noether_filtering/filter_base.h" -#include - -namespace noether_filtering -{ -namespace cloud -{ -template -class StatisticalOutlierFilter : public FilterBase> -{ -public: - using T = pcl::PointCloud; - - using FilterBase::FilterBase; - - /** - * @brief configure - * @param config: XmlRpc value - * - config: - * mean_k: (int) - * std_dev_mult: (double) - * @return - */ - virtual bool configure(XmlRpc::XmlRpcValue config) override final; - virtual bool filter(const T& input, T& output) override final; - virtual std::string getName() const override final; - - struct Params - { - int mean_k = 10; - double std_dev_mult = 1.0; - }; - Params params; - - static const std::string MEAN_K; - static const std::string STD_DEV_MULT; -}; -} // namespace cloud -} // namespace noether_filtering - -#endif // STATISTICAL_OUTLIER_FILTER_H diff --git a/noether_filtering/include/noether_filtering/cloud/voxel_grid_filter.h b/noether_filtering/include/noether_filtering/cloud/voxel_grid_filter.h deleted file mode 100644 index 528afe39..00000000 --- a/noether_filtering/include/noether_filtering/cloud/voxel_grid_filter.h +++ /dev/null @@ -1,56 +0,0 @@ -#ifndef NOETHER_FILTERING_CLOUD_VOXEL_GRID_FILTER_H -#define NOETHER_FILTERING_CLOUD_VOXEL_GRID_FILTER_H - -#include "noether_filtering/filter_base.h" -#include - -namespace noether_filtering -{ -namespace cloud -{ -template -class VoxelGridFilter : public FilterBase> -{ -public: - using T = pcl::PointCloud; - - using FilterBase::FilterBase; - - /** - * @brief configure - * @param config: XmlRpc value - * - config: - * leaf_size: (string) - * min_limit: (double, optional) - * max_limit: (double, optional) - * filter_limits_negative: (bool, optional) - * min_pts_per_voxel: (int, optional) - * filter_field_name: (string, optional) - * @return - */ - bool configure(XmlRpc::XmlRpcValue config) override final; - bool filter(const T& input, T& output) override final; - std::string getName() const override final; - - struct Params - { - float leaf_size = 0.01; - float min_limit = -std::numeric_limits::max(); - float max_limit = std::numeric_limits::max(); - bool filter_limits_negative = false; - unsigned int min_pts_per_voxel = 1; - std::string filter_field_name = ""; - }; - Params params; - - static const std::string LEAF_SIZE; - static const std::string FILTER_FIELD_NAME; - static const std::string MIN_LIMIT; - static const std::string MAX_LIMIT; - static const std::string FILTER_LIMITS_NEGATIVE; - static const std::string MIN_PTS_PER_VOXEL; -}; -} // namespace cloud -} // namespace noether_filtering - -#endif // NOETHER_FILTERING_CLOUD_VOXEL_GRID_FILTER_H diff --git a/noether_filtering/include/noether_filtering/filter_base.h b/noether_filtering/include/noether_filtering/filter_base.h deleted file mode 100644 index 1040e5f9..00000000 --- a/noether_filtering/include/noether_filtering/filter_base.h +++ /dev/null @@ -1,29 +0,0 @@ -/* - * mesh_filter_base.h - * - * Created on: Oct 9, 2019 - * Author: jrgnicho - */ -#ifndef INCLUDE_NOETHER_FILTERING_MESH_FILTER_BASE_H_ -#define INCLUDE_NOETHER_FILTERING_MESH_FILTER_BASE_H_ - -#include -#include - -namespace noether_filtering -{ -template -class FilterBase -{ -public: - FilterBase() = default; - virtual ~FilterBase() = default; - - virtual bool configure(XmlRpc::XmlRpcValue config) = 0; - virtual bool filter(const T& input, T& output) = 0; - virtual std::string getName() const = 0; -}; - -} // namespace noether_filtering - -#endif /* INCLUDE_NOETHER_FILTERING_MESH_FILTER_BASE_H_ */ diff --git a/noether_filtering/include/noether_filtering/filter_group.h b/noether_filtering/include/noether_filtering/filter_group.h deleted file mode 100644 index 8a06b5c1..00000000 --- a/noether_filtering/include/noether_filtering/filter_group.h +++ /dev/null @@ -1,90 +0,0 @@ -/* - * filter_manager.hpp - * - * Created on: Oct 14, 2019 - * Author: jrgnicho - */ -#ifndef INCLUDE_NOETHER_FILTERING_FILTER_GROUP_H_ -#define INCLUDE_NOETHER_FILTERING_FILTER_GROUP_H_ - -#include -#include -#include -#include -#include "noether_filtering/filter_base.h" - -namespace noether_filtering -{ -namespace config_fields -{ -namespace filter -{ -static const std::string TYPE_NAME = "type"; -static const std::string NAME = "name"; -static const std::string CONFIG = "config"; -} // namespace filter - -namespace group -{ -static const std::string GROUP_NAME = "group_name"; -static const std::string CONTINUE_ON_FAILURE = "continue_on_failure"; -static const std::string VERBOSITY_ON = "verbosity_on"; -static const std::string FILTERS = "filters"; -} // namespace group -} // namespace config_fields - -/** - * @brief The FilterManager class - */ -template -class FilterGroup -{ -public: - FilterGroup(const std::string& base_class_name); - virtual ~FilterGroup() = default; - - /** - * @details Initializes the filter chain and loads all the filter plugins from a yaml structured parameter. - * When 'continue_on_failure = true' then it only takes one filter to succeed for the whole pass through the group to - * succeed. When 'continue_on_failure = false' then the algorithm returns false as soon as one filter fails. The - * parameter must conform to the following syntax: - * - * continue_on_failure: True - * mesh_filters: - * - type: DemoFilter1 - * name: demo_filter_1 - * config: - * param1: 20 - * param2: 'optimize' - * . - * . - * . - * - type: DemoFilter2 - * name: demo_filter_2 - * config: - * x: 1.0 - * y: 3.5 - * . - * . - * . - * @param config - * @return - */ - bool init(XmlRpc::XmlRpcValue config); - bool applyFilters(const F& input, F& output, std::string& err_msg); - bool applyFilters(const std::vector& filters, const F& input, F& output, std::string& err_msg); - -protected: - using FilterT = FilterBase; - typedef typename std::unique_ptr FilterBasePtr; - - pluginlib::ClassLoader filter_loader_; - std::vector filters_loaded_; - std::map filters_map_; - bool continue_on_failure_; - bool verbosity_on_; -}; - -} /* namespace noether_filtering */ - -#endif /* INCLUDE_NOETHER_FILTERING_FILTER_GROUP_H_ */ diff --git a/noether_filtering/include/noether_filtering/filter_manager.h b/noether_filtering/include/noether_filtering/filter_manager.h deleted file mode 100644 index f82a267a..00000000 --- a/noether_filtering/include/noether_filtering/filter_manager.h +++ /dev/null @@ -1,90 +0,0 @@ -/* - * filter_manager.h - * - * Created on: Oct 10, 2019 - * Author: jrgnicho - */ -#ifndef NOETHER_FILTERING_FILTER_MANAGER_H -#define NOETHER_FILTERING_FILTER_MANAGER_H - -#include "noether_filtering/filter_group.h" - -namespace noether_filtering -{ -namespace config_fields -{ -namespace manager -{ -static const std::string FILTER_GROUPS = "filter_groups"; -static const std::string GROUP_NAME = "group_name"; -} // namespace manager -} // namespace config_fields - -/** - * @brief Filter Manager class - */ -template -class FilterManager -{ -public: - FilterManager(const std::string& base_class_name); - virtual ~FilterManager() = default; - - /** - * @details loads several filter chain configurations. The yaml structure is expected to be as follows: - * filter_groups: - * - group_name: DEFAULT - * continue_on_failure: False - * mesh_filters: - * - type: DemoFilter1 - * name: demo_filter_1 - * config: - * param1: 20 - * param2: 'optimize' - * . - * . - * . - * - type: DemoFilter2 - * name: demo_filter_2 - * config: - * x: 1.0 - * y: 3.5 - * - group_name: GROUP_1 - * continue_on_failure: False - * mesh_filters: - * - type: DemoFilterX - * name: demo_filter_x - * config: - * param_x: 20 - * . - * . - * . - * - type: DemoFilterY - * name: demo_filter_y - * config: - * a: True - * b: 20 - * . - * . - * . - * - * @param config The configuration - * @return True on success, false otherwise - */ - bool init(XmlRpc::XmlRpcValue config); - - /** - * @brief gets the requested filter group - * @param name the name of the group - * @return the requested filter group, returns a nullptr when the requested group isn't recognized - */ - std::shared_ptr> getFilterGroup(const std::string& name) const; - -protected: - const std::string base_class_name_; - std::map>> filter_groups_map_; -}; - -} /* namespace noether_filtering */ - -#endif /* NOETHER_FILTERING_FILTER_MANAGER_H */ diff --git a/noether_filtering/include/noether_filtering/impl/filter_group.hpp b/noether_filtering/include/noether_filtering/impl/filter_group.hpp deleted file mode 100644 index b1c60637..00000000 --- a/noether_filtering/include/noether_filtering/impl/filter_group.hpp +++ /dev/null @@ -1,221 +0,0 @@ -/* - * filter_manager.cpp - * - * Created on: Oct 14, 2019 - * Author: jrgnicho - */ -#ifndef NOETHER_FILTERING_FILTER_GROUP_HPP_ -#define NOETHER_FILTERING_FILTER_GROUP_HPP_ - -#include "noether_filtering/filter_group.h" -#include "noether_filtering/utils.h" -#include -#include -#include - -static const std::string PACKAGE_NAME = "noether_filtering"; - -struct FilterInfo -{ - std::string type_name; /** @brief the filter type */ - std::string name; /** @brief an alias for the filter */ - XmlRpc::XmlRpcValue config; /** @brief the filter configuration */ -}; - -namespace noether_filtering -{ -static bool loadFilterInfos(XmlRpc::XmlRpcValue filter_configs, std::vector& filter_infos) -{ - if (filter_configs.getType() != XmlRpc::XmlRpcValue::TypeArray) - { - CONSOLE_BRIDGE_logError("The filter group configuration is not an array of filters"); - return false; - } - - for (int i = 0; i < filter_configs.size(); i++) - { - using namespace noether_filtering::config_fields; - FilterInfo fi; - XmlRpc::XmlRpcValue fi_config = filter_configs[i]; - fi.type_name = static_cast(fi_config[filter::TYPE_NAME]); - fi.name = static_cast(fi_config[filter::NAME]); - fi.config = fi_config[filter::CONFIG]; - filter_infos.push_back(std::move(fi)); - } - return !filter_infos.empty(); -} - -template -FilterGroup::FilterGroup(const std::string& base_class_name) - : continue_on_failure_(false), filter_loader_(PACKAGE_NAME, base_class_name) -{ - std::vector filters = filter_loader_.getDeclaredClasses(); - std::string out = "Available plugins for base class '" + base_class_name + "' :"; - for (const std::string& f : filters) - { - out += "\n\t\t" + f; - } - CONSOLE_BRIDGE_logInform("%s", out.c_str()); -} - -template -bool FilterGroup::init(XmlRpc::XmlRpcValue config) -{ - using namespace config_fields::group; - - // checking config fields - const std::vector REQUIRED_FIELDS = { CONTINUE_ON_FAILURE, VERBOSITY_ON, FILTERS }; - - auto check_fn = [&config](const std::string& f) { - if (!config.hasMember(f)) - { - CONSOLE_BRIDGE_logError( - "The '%s' field was not found in the %s config", f.c_str(), utils::getClassName>().c_str()); - return false; - } - return true; - }; - - if (!std::all_of(REQUIRED_FIELDS.begin(), REQUIRED_FIELDS.end(), check_fn)) - { - return false; - } - - // load manager parameters - try - { - continue_on_failure_ = static_cast(config[CONTINUE_ON_FAILURE]); - verbosity_on_ = static_cast(config[VERBOSITY_ON]); - } - catch (const XmlRpc::XmlRpcException& ex) - { - CONSOLE_BRIDGE_logWarn( - "Failed to parse '%s' parameter: '%s'", CONTINUE_ON_FAILURE.c_str(), ex.getMessage().c_str()); - continue_on_failure_ = false; - } - - // load filter infos - std::vector filter_infos; - XmlRpc::XmlRpcValue filter_configs = config[FILTERS]; - if (!loadFilterInfos(filter_configs, filter_infos)) - { - CONSOLE_BRIDGE_logError("Failed to load filters"); - return false; - } - - // instantiating filters - for (FilterInfo& fi : filter_infos) - { - if (filters_map_.count(fi.name) > 0) - { - CONSOLE_BRIDGE_logError( - "The %s plugin '%s' has already been added", utils::getClassName().c_str(), fi.name.c_str()); - return false; - } - - FilterBasePtr plugin; - try - { - // plugin = filter_loader_.createUniqueInstance>(fi.type_name); - plugin.reset(filter_loader_.createUnmanagedInstance(fi.type_name)); - if (!plugin->configure(fi.config)) - { - CONSOLE_BRIDGE_logError( - "%s plugin '%s' failed to load configuration", utils::getClassName().c_str(), fi.name.c_str()); - return false; - } - - // storing plugin - filters_loaded_.push_back(fi.name); - filters_map_.insert(std::make_pair(fi.name, std::move(plugin))); - } - catch (const class_loader::ClassLoaderException& ex) - { - CONSOLE_BRIDGE_logError("%s plugin '%s' could not be created", utils::getClassName().c_str(), fi.name.c_str()); - return false; - } - } - - return true; -} - -template -bool FilterGroup::applyFilters(const F& input, F& output, std::string& err_msg) -{ - // applying all loaded filters - return applyFilters(filters_loaded_, input, output, err_msg); -} - -template -bool FilterGroup::applyFilters(const std::vector& filters, - const F& input, - F& output, - std::string& err_msg) -{ - std::vector selected_filters = filters; - namespace chrono = std::chrono; - if (selected_filters.empty()) - { - CONSOLE_BRIDGE_logWarn("%s received empty list of filters, using all filters loaded", - utils::getClassName>().c_str()); - std::copy(filters_loaded_.begin(), filters_loaded_.end(), std::back_inserter(selected_filters)); - } - - // check all filters exists - auto check_fn = [this, &err_msg](const std::string& f) { - if (filters_map_.count(f) == 0) - { - err_msg = boost::str(boost::format("The filter %s was not found") % f); - CONSOLE_BRIDGE_logError("%s", err_msg.c_str()); - return false; - } - return true; - }; - - if (!std::all_of(filters.begin(), filters.end(), check_fn)) - { - return false; - } - - // apply filters - bool success = false; - F temp = input; - chrono::time_point start_time; - chrono::duration filter_dur; - for (const std::string& fname : selected_filters) - { - F temp_output; - start_time = chrono::system_clock::now(); - bool current_filter_succeeded = filters_map_.at(fname)->filter(temp, temp_output); - - if (verbosity_on_) - { - filter_dur = chrono::system_clock::now() - start_time; - CONSOLE_BRIDGE_logInform("Filter '%s' took %f seconds", fname.c_str(), filter_dur.count()); - } - - if (!current_filter_succeeded) - { - err_msg = boost::str(boost::format("The filter %s failed") % fname); - CONSOLE_BRIDGE_logError("%s", err_msg.c_str()); - - if (!continue_on_failure_) - { - return false; - } - } - else - { - // save output onto the input for the next filter - temp = std::move(temp_output); - } - - success |= current_filter_succeeded; - } - output = std::move(temp); - return success; -} - -} /* namespace noether_filtering */ - -#endif // NOETHER_FILTERING_FILTER_GROUP_HPP_ diff --git a/noether_filtering/include/noether_filtering/impl/filter_manager.hpp b/noether_filtering/include/noether_filtering/impl/filter_manager.hpp deleted file mode 100644 index 366d2dbd..00000000 --- a/noether_filtering/include/noether_filtering/impl/filter_manager.hpp +++ /dev/null @@ -1,95 +0,0 @@ -#ifndef NOETHER_FILTERING_FILTER_MANAGER_HPP -#define NOETHER_FILTERING_FILTER_MANAGER_HPP - -#include "noether_filtering/filter_manager.h" -#include "noether_filtering/utils.h" -#include -#include - -static const std::string DEFAULT_FILTER_CHAIN_NAME = "Default"; - -namespace noether_filtering -{ -template -FilterManager::FilterManager(const std::string& base_class_name) : base_class_name_(base_class_name) -{ -} - -template -bool FilterManager::init(XmlRpc::XmlRpcValue config) -{ - using namespace config_fields; - filter_groups_map_.clear(); - if (!config.hasMember(manager::FILTER_GROUPS)) - { - CONSOLE_BRIDGE_logError("The '%s' field was not found, %s failed to load configuration", - manager::FILTER_GROUPS.c_str(), - utils::getClassName().c_str()); - return false; - } - - XmlRpc::XmlRpcValue filter_groups_config = config[manager::FILTER_GROUPS]; - if (filter_groups_config.getType() != XmlRpc::XmlRpcValue::TypeArray) - { - CONSOLE_BRIDGE_logError("The '%s' field is not an array, %s failed to load configuration", - manager::FILTER_GROUPS.c_str(), - utils::getClassName().c_str()); - return false; - } - - for (int i = 0; i < filter_groups_config.size(); i++) - { - try - { - // Create the filter group object - std::shared_ptr> filter_group = std::make_shared>(base_class_name_); - - // Load the group configuration - XmlRpc::XmlRpcValue group_config = filter_groups_config[i]; - std::string group_name = static_cast(group_config[manager::GROUP_NAME]); - - if (filter_groups_map_.find(group_name) != filter_groups_map_.end()) - { - CONSOLE_BRIDGE_logError("The filter group '%s' already exists in %s", - group_name.c_str(), - utils::getClassName().c_str()); - return false; - } - - if (!filter_group->init(group_config)) - { - CONSOLE_BRIDGE_logError("Failed to initialize filter group '%s'", group_name.c_str()); - return false; - } - - filter_groups_map_.insert(std::make_pair(group_name, std::move(filter_group))); - CONSOLE_BRIDGE_logInform( - "%s loaded filter group '%s'", utils::getClassName().c_str(), group_name.c_str()); - } - catch (XmlRpc::XmlRpcException& e) - { - CONSOLE_BRIDGE_logError("Failure while loading %s configuration, error msg: '%s'", - utils::getClassName().c_str(), - e.getMessage().c_str()); - return false; - } - } - - return true; -} - -template -std::shared_ptr> FilterManager::getFilterGroup(const std::string& name) const -{ - std::string n = name.empty() ? DEFAULT_FILTER_CHAIN_NAME : name; - if (filter_groups_map_.find(n) == filter_groups_map_.end()) - { - CONSOLE_BRIDGE_logError("Filter chain '%s' was not found", n.c_str()); - return nullptr; - } - return filter_groups_map_.at(n); -} - -} // namespace noether_filtering - -#endif // NOETHER_FILTERING_FILTER_MANAGER_HPP diff --git a/noether_filtering/include/noether_filtering/mesh/bspline_reconstruction.h b/noether_filtering/include/noether_filtering/mesh/bspline_reconstruction.h deleted file mode 100644 index e5502682..00000000 --- a/noether_filtering/include/noether_filtering/mesh/bspline_reconstruction.h +++ /dev/null @@ -1,112 +0,0 @@ -/** - * @author Jorge Nicho - * @file bspline_reconstruction.cpp - * @date Oct 10, 2019 - * @copyright Copyright (c) 2019, Southwest Research Institute - * - * @par License - * Software License Agreement (Apache License) - * @par - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * http://www.apache.org/licenses/LICENSE-2.0 - * @par - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#ifndef INCLUDE_NOETHER_FILTERING_MESH_FILTERS_BSPLINE_RECONSTRUCTION_H_ -#define INCLUDE_NOETHER_FILTERING_MESH_FILTERS_BSPLINE_RECONSTRUCTION_H_ - -#include "noether_filtering/mesh/mesh_filter_base.h" -#include -#include - -namespace noether_filtering -{ -namespace mesh -{ -/** - * @class noether_filtering::filters::BSplineReconstruction - * @details: Smoothes a mesh by fitting a bspline surface to it. The details of this implementation' - * can be in found in http://pointclouds.org/documentation/tutorials/bspline_fitting.php - */ -class BSplineReconstruction : public MeshFilterBase -{ -public: - enum class SurfInitMethod : int - { - PCA = 1, - PCA_BB, - CUSTOM_PLANE, // not implemented yet but will use pcl::on_nurbs::FittingSurface::initNurbs4Corners - }; - - struct Parameters - { - bool verbosity_on = false; /** @brief print more info */ - int order = 3; /** @brief is the polynomial order of the B-spline surface. */ - int refinement = 3; /** @brief refinement is the number of refinement iterations, where for each iteration - control-points are inserted*/ - unsigned iterations = 1; /** @brief is the number of iterations that are performed after refinement is completed */ - unsigned mesh_resolution = 50; /** @brief the number of vertices in each parametric direction, used for - triangulation of the B-spline surface.*/ - SurfInitMethod surf_init_method = - SurfInitMethod::PCA_BB; /** @brief method for creating the initial surface to be fit */ - pcl::on_nurbs::FittingSurface::Parameter surface_params; /** @brief parameters to fit the surface**/ - - bool clip_boundary_curve = true; /** @brief whether to fit the boundary curve and clip every that extends past it*/ - int boundary_fit_order = 2; /** @brief applicable only when clip_boundary_curve = true */ - int boundary_startCPs = 0; /** @brief initial number of control points */ - bool boundary_clipping_required = - false; /** @brief if True then algorithm will fail if the boundary could not be clipped */ - pcl::on_nurbs::FittingCurve2dAPDM::FitParameter boundary_curve_params; - }; - - using FilterBase::FilterBase; - - /** @details - * Loads the configuration from a yaml object with the following structure: - * order: 3 - * refinement: 3 - * iterations: 1 - * mesh_resolution: 50 - * surf_init_method: 2 - * fit_surface_parameters: - * interior_smoothness: 0.2 - * interior_weight: 1.0 - * boundary_smoothness: 0.2 - * boundary_weight: 0.0 - * clip_boundary_curve: True - * boundary_fit_order: 2 # applicable only when clip_boundary_curve: True - * boundary_curve_parameters: - * addCPsAccuracy: 5e-2 - * addCPsIteration: 3 - * maxCPs: 200 - * accuracy: 1e-3 - * iterations: 100 - * closest_point_resolution: 0 - * closest_point_weight: 1.0 - * closest_point_sigma2: 0.1 - * interior_sigma2: 0.00001 - * smooth_concavity: 1.0 - * smoothness: 1.0 - * - * @param config The configuration - * @return True on success, false otherwise - */ - bool configure(XmlRpc::XmlRpcValue config) override final; - bool filter(const pcl::PolygonMesh& mesh_in, pcl::PolygonMesh& mesh_out) override final; - std::string getName() const override final; - -protected: - Parameters parameters_; -}; - -} /* namespace mesh */ -} /* namespace noether_filtering */ - -#endif /* INCLUDE_NOETHER_FILTERING_MESH_FILTERS_BSPLINE_RECONSTRUCTION_H_ */ diff --git a/noether_filtering/include/noether_filtering/mesh/mesh_filter_base.h b/noether_filtering/include/noether_filtering/mesh/mesh_filter_base.h deleted file mode 100644 index d5cb877b..00000000 --- a/noether_filtering/include/noether_filtering/mesh/mesh_filter_base.h +++ /dev/null @@ -1,17 +0,0 @@ -#ifndef NOETHER_FILTERING_MESH_MESH_FILTER_BASE_H -#define NOETHER_FILTERING_MESH_MESH_FILTER_BASE_H - -#include -#include "noether_filtering/filter_base.h" - -namespace noether_filtering -{ -namespace mesh -{ -// Provide alias for mesh filter base -using MeshFilterBase = FilterBase; - -} /* namespace mesh */ -} /* namespace noether_filtering */ - -#endif // NOETHER_FILTERING_MESH_MESH_FILTER_BASE_H diff --git a/noether_filtering/include/noether_filtering/mesh/mesh_filter_manager.h b/noether_filtering/include/noether_filtering/mesh/mesh_filter_manager.h deleted file mode 100644 index c6c1179a..00000000 --- a/noether_filtering/include/noether_filtering/mesh/mesh_filter_manager.h +++ /dev/null @@ -1,24 +0,0 @@ -#ifndef NOETHER_FILTERING_MESH_MESH_FILTER_H -#define NOETHER_FILTERING_MESH_MESH_FILTER_H - -#include -#include "noether_filtering/filter_manager.h" - -namespace noether_filtering -{ -namespace mesh -{ -// Provide aliases for specific types -using MeshFilterGroup = FilterGroup; -class MeshFilterManager : public FilterManager -{ -public: - MeshFilterManager() : FilterManager("noether_filtering::mesh::MeshFilterBase") {} - - virtual ~MeshFilterManager() = default; -}; - -} /* namespace mesh */ -} /* namespace noether_filtering */ - -#endif // NOETHER_FILTERING_MESH_MESH_FILTER_H diff --git a/noether_filtering/include/noether_filtering/mesh/windowed_sinc_smoothing.h b/noether_filtering/include/noether_filtering/mesh/windowed_sinc_smoothing.h deleted file mode 100644 index f4e9f308..00000000 --- a/noether_filtering/include/noether_filtering/mesh/windowed_sinc_smoothing.h +++ /dev/null @@ -1,79 +0,0 @@ -/* - * window_sinc_smoothing.h - * - * Created on: Dec 6, 2019 - * Author: jrgnicho - */ - -#ifndef SRC_MESH_WINDOW_SINC_SMOOTHING_H_ -#define SRC_MESH_WINDOW_SINC_SMOOTHING_H_ - -#include "noether_filtering/mesh/mesh_filter_base.h" - -namespace noether_filtering -{ -namespace mesh -{ -/** - * @class noether_filtering:;mesh::WindowedSincSmoothing - * @details uses the vtkWindowedSincPolyDataFilter to smooth out the mesh, details of the implementation - * and the configuration parameters can be found here - * https://vtk.org/doc/nightly/html/classvtkWindowedSincPolyDataFilter.html - */ -class WindowedSincSmoothing : public MeshFilterBase -{ -public: - struct Config - { - std::size_t num_iter = 100; - bool enable_boundary_smoothing = true; /**@brief Set to true to enable */ - bool enable_feature_edge_smoothing = false; /**@brief Set to true to enable */ - bool enable_non_manifold_smoothing = true; /**@brief Set to true to enable */ - bool enable_normalize_coordinates = true; /**@brief Set to true to enable */ - double feature_angle = 10.0; /**@brief degrees, only applicable when feature_edge_smoothing = true */ - double edge_angle = 150.0; /**@brief degrees, only applicable when feature_edge_smoothing = true */ - double pass_band = 0.01; /**@brief PassBand for the windowed sinc filter, see explanation in reference link */ - }; - - WindowedSincSmoothing(); - virtual ~WindowedSincSmoothing(); - - /** - * @details Loads a configuration from yaml of the following form: - * - * num_iter: 100 - * enable_boundary_smoothing: true - * enable_feature_edge_smoothing: false - * enable_non_manifold_smoothing: true - * enable_normalize_coordinates: true - * feature_angle: 10.0 - * edge_angle: 150.0 - * pass_band: 0.01 - * - * @param config - * @return - */ - bool configure(XmlRpc::XmlRpcValue config) override final; - - /** - * @brief applies the filtering method - * @param mesh_in Input mesh - * @param mesh_out Output mesh - * @return True on success, false otherwise - */ - bool filter(const pcl::PolygonMesh& mesh_in, pcl::PolygonMesh& mesh_out) override final; - - /** - * @brief returns the type name of the filter - * @return A string containing the filter type name - */ - std::string getName() const override final; - -private: - Config config_; -}; - -} /* namespace mesh */ -} /* namespace noether_filtering */ - -#endif /* SRC_MESH_WINDOW_SINC_SMOOTHING_H_ */ diff --git a/noether_filtering/include/noether_filtering/utils.h b/noether_filtering/include/noether_filtering/utils.h deleted file mode 100644 index 2e7959a4..00000000 --- a/noether_filtering/include/noether_filtering/utils.h +++ /dev/null @@ -1,34 +0,0 @@ -/* - * utils.h - * - * Created on: Oct 10, 2019 - * Author: jrgnicho - */ - -#ifndef INCLUDE_NOETHER_FILTERING_UTILS_H_ -#define INCLUDE_NOETHER_FILTERING_UTILS_H_ - -#include -#include -#include -#include - -namespace noether_filtering -{ -namespace utils -{ -template -static std::string getClassName() -{ - int status = -4; // some arbitrary value to eliminate the compiler warning - const char* mangled_name = typeid(C).name(); - - // enable c++11 by passing the flag -std=c++11 to g++ - std::unique_ptr res{ abi::__cxa_demangle(mangled_name, NULL, NULL, &status), std::free }; - return (status == 0) ? res.get() : mangled_name; -} - -} // namespace utils -} // namespace noether_filtering - -#endif /* INCLUDE_NOETHER_FILTERING_UTILS_H_ */ diff --git a/noether_filtering/mesh_filter_plugins.xml b/noether_filtering/mesh_filter_plugins.xml deleted file mode 100644 index d942a655..00000000 --- a/noether_filtering/mesh_filter_plugins.xml +++ /dev/null @@ -1,42 +0,0 @@ - - - - Fits a nurve surface to a polygonal mesh and clips the boundaries if requested - - - - - - Runs euclidean clustering and removes all the points that don't fit the constraints - - - - - - Merges duplicate points, and/or remove unused points and/or remove degenerate cells using VTK, - see https://vtk.org/doc/nightly/html/classvtkCleanPolyData.html#details for more details - - - - - - Adjust point positions using a windowed sinc function interpolation kernel using VTK see - https://vtk.org/doc/nightly/html/classvtkWindowedSincPolyDataFilter.html#details for - more details - - - - - Filter that identifies and fills holes in input meshes. - Holes are identified by locating boundary edges, linking them together into loops, - and then triangulating the resulting loops. - Note that you can specify an approximate limit to the size of the hole that can be filled. - See https://vtk.org/doc/nightly/html/classvtkFillHolesFilter.html#details - - - diff --git a/noether_filtering/package.xml b/noether_filtering/package.xml deleted file mode 100644 index 26781f2a..00000000 --- a/noether_filtering/package.xml +++ /dev/null @@ -1,19 +0,0 @@ - - - noether_filtering - 0.0.0 - The noether_filtering package - Jorge Nicho - Apache 2.0 - Jorge Nicho - - libconsole-bridge-dev - pluginlib - libpcl-all-dev - ros_industrial_cmake_boilerplate - - cmake - - - - diff --git a/noether_filtering/src/cloud/filters.cpp b/noether_filtering/src/cloud/filters.cpp deleted file mode 100644 index dbd813d0..00000000 --- a/noether_filtering/src/cloud/filters.cpp +++ /dev/null @@ -1,20 +0,0 @@ -#include "noether_filtering/cloud/impl/voxel_grid_filter.hpp" -#include "noether_filtering/cloud/impl/statistical_outlier_filter.hpp" -#include "noether_filtering/cloud/impl/crop_box_filter.hpp" -#include "noether_filtering/cloud/impl/pass_through_filter.hpp" -#include "noether_filtering/cloud/impl/radius_outlier_filter.hpp" -#include "noether_filtering/cloud/impl/mls_smoothing_filter.hpp" -#include -#include - -namespace noether_filtering -{ -// Explicit template instantiation for all XYZ point types -PCL_INSTANTIATE(VoxelGridFilter, PCL_XYZ_POINT_TYPES); -PCL_INSTANTIATE(StatisticalOutlierFilter, PCL_XYZ_POINT_TYPES); -PCL_INSTANTIATE(CropBoxFilter, PCL_XYZ_POINT_TYPES); -PCL_INSTANTIATE(PassThroughFilter, PCL_XYZ_POINT_TYPES); -PCL_INSTANTIATE(RadiusOutlierFilter, PCL_XYZ_POINT_TYPES); -PCL_INSTANTIATE(MLSSmoothingFilter, PCL_XYZ_POINT_TYPES); - -} // namespace noether_filtering diff --git a/noether_filtering/src/cloud/plugins.cpp b/noether_filtering/src/cloud/plugins.cpp deleted file mode 100644 index c2199b9e..00000000 --- a/noether_filtering/src/cloud/plugins.cpp +++ /dev/null @@ -1,25 +0,0 @@ -#include -#include -#include - -// Cloud Filters -#include "noether_filtering/cloud/voxel_grid_filter.h" -#include "noether_filtering/cloud/statistical_outlier_filter.h" -#include "noether_filtering/cloud/crop_box_filter.h" -#include "noether_filtering/cloud/pass_through_filter.h" -#include "noether_filtering/cloud/radius_outlier_filter.h" -#include "noether_filtering/cloud/mls_smoothing_filter.h" - -#define CREATE_FILTER_PLUGIN_IMPL(r, FILTER_TYPE, POINT_TYPE) \ - PLUGINLIB_EXPORT_CLASS(FILTER_TYPE, noether_filtering::FilterBase>) - -#define CREATE_FILTER_PLUGINS(FILTER_TYPE, POINT_TYPES) \ - BOOST_PP_SEQ_FOR_EACH(CREATE_FILTER_PLUGIN_IMPL, FILTER_TYPE, POINT_TYPES) - -// Point Cloud Filters -CREATE_FILTER_PLUGINS(noether_filtering::cloud::VoxelGridFilter, PCL_XYZ_POINT_TYPES) -CREATE_FILTER_PLUGINS(noether_filtering::cloud::StatisticalOutlierFilter, PCL_XYZ_POINT_TYPES) -CREATE_FILTER_PLUGINS(noether_filtering::cloud::CropBoxFilter, PCL_XYZ_POINT_TYPES) -CREATE_FILTER_PLUGINS(noether_filtering::cloud::PassThroughFilter, PCL_XYZ_POINT_TYPES) -CREATE_FILTER_PLUGINS(noether_filtering::cloud::RadiusOutlierFilter, PCL_XYZ_POINT_TYPES) -CREATE_FILTER_PLUGINS(noether_filtering::cloud::MLSSmoothingFilter, PCL_XYZ_POINT_TYPES) diff --git a/noether_filtering/src/filter_group.cpp b/noether_filtering/src/filter_group.cpp deleted file mode 100644 index 54fc1ef0..00000000 --- a/noether_filtering/src/filter_group.cpp +++ /dev/null @@ -1,17 +0,0 @@ -#include "noether_filtering/impl/filter_group.hpp" -#include -#include -#include -#include - -#define PCL_INSTANTIATE_FilterGroup(T) template class PCL_EXPORTS noether_filtering::FilterGroup>; - -namespace noether_filtering -{ -// Explicit template instantiation -template class FilterGroup; - -// Explicit template instantiation for all XYZ point types -PCL_INSTANTIATE(FilterGroup, PCL_XYZ_POINT_TYPES); - -} // namespace noether_filtering diff --git a/noether_filtering/src/filter_manager.cpp b/noether_filtering/src/filter_manager.cpp deleted file mode 100644 index 46badf91..00000000 --- a/noether_filtering/src/filter_manager.cpp +++ /dev/null @@ -1,18 +0,0 @@ -#include "noether_filtering/impl/filter_manager.hpp" -#include -#include -#include -#include - -#define PCL_INSTANTIATE_FilterManager(T) \ - template class PCL_EXPORTS noether_filtering::FilterManager>; - -namespace noether_filtering -{ -// Explicit template instantiation -template class FilterManager; - -// Explicit template instantiation for all XYZ point types -PCL_INSTANTIATE(FilterManager, PCL_XYZ_POINT_TYPES); - -} // namespace noether_filtering diff --git a/noether_filtering/src/mesh/bspline_reconstruction.cpp b/noether_filtering/src/mesh/bspline_reconstruction.cpp deleted file mode 100644 index 22e020cd..00000000 --- a/noether_filtering/src/mesh/bspline_reconstruction.cpp +++ /dev/null @@ -1,200 +0,0 @@ -/** - * @author Jorge Nicho - * @file bspline_reconstruction.cpp - * @date Oct 10, 2019 - * @copyright Copyright (c) 2019, Southwest Research Institute - * - * @par License - * Software License Agreement (Apache License) - * @par - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * http://www.apache.org/licenses/LICENSE-2.0 - * @par - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include "noether_filtering/mesh/bspline_reconstruction.h" -#include "noether_filtering/utils.h" -#include -#include -#include -#include -#include - -static pcl::on_nurbs::vector_vec3d createNurbData(pcl::PointCloud::ConstPtr cloud) -{ - pcl::on_nurbs::vector_vec3d data; - for (unsigned i = 0; i < cloud->size(); i++) - { - const pcl::PointXYZ& p = cloud->at(i); - if (!std::isnan(p.x) && !std::isnan(p.y) && !std::isnan(p.z)) - data.push_back(Eigen::Vector3d(p.x, p.y, p.z)); - } - return std::move(data); -} - -namespace noether_filtering -{ -namespace mesh -{ -bool BSplineReconstruction::configure(XmlRpc::XmlRpcValue config) -{ - Parameters& p = parameters_; - try - { - p.verbosity_on = static_cast(config["verbosity_on"]); - p.order = static_cast(config["order"]); - p.refinement = static_cast(config["refinement"]); - p.iterations = static_cast(config["iterations"]); - p.mesh_resolution = static_cast(config["mesh_resolution"]); - p.surf_init_method = static_cast(static_cast(config["surf_init_method"])); - - XmlRpc::XmlRpcValue surf_conf = config["fit_surface_parameters"]; - p.surface_params.interior_smoothness = static_cast(surf_conf["interior_smoothness"]); - p.surface_params.interior_weight = static_cast(surf_conf["interior_weight"]); - p.surface_params.boundary_smoothness = static_cast(surf_conf["boundary_smoothness"]); - p.surface_params.boundary_weight = static_cast(surf_conf["boundary_weight"]); - - p.clip_boundary_curve = static_cast(config["clip_boundary_curve"]); - if (!p.clip_boundary_curve) - { - return true; - } - - p.boundary_fit_order = static_cast(config["boundary_fit_order"]); - XmlRpc::XmlRpcValue boundary_conf = config["boundary_curve_parameters"]; - p.boundary_startCPs = static_cast(boundary_conf["startCPs"]); - p.boundary_clipping_required = static_cast(boundary_conf["clipping_required"]); - pcl::on_nurbs::FittingCurve2dAPDM::FitParameter& bp = p.boundary_curve_params; - bp.addCPsAccuracy = static_cast(boundary_conf["addCPsAccuracy"]); - bp.addCPsIteration = static_cast(boundary_conf["addCPsIteration"]); - bp.maxCPs = static_cast(boundary_conf["maxCPs"]); - bp.accuracy = static_cast(boundary_conf["accuracy"]); - bp.iterations = static_cast(boundary_conf["iterations"]); - bp.param.closest_point_resolution = static_cast(boundary_conf["closest_point_resolution"]); - bp.param.closest_point_weight = static_cast(boundary_conf["closest_point_weight"]); - bp.param.closest_point_sigma2 = static_cast(boundary_conf["closest_point_sigma2"]); - bp.param.interior_sigma2 = static_cast(boundary_conf["interior_sigma2"]); - bp.param.smooth_concavity = static_cast(boundary_conf["smooth_concavity"]); - bp.param.smoothness = static_cast(boundary_conf["smoothness"]); - } - catch (XmlRpc::XmlRpcException& e) - { - CONSOLE_BRIDGE_logError("Failed configure %s filter, error: %s", getName().c_str(), e.getMessage().c_str()); - return false; - } - return true; -} - -bool BSplineReconstruction::filter(const pcl::PolygonMesh& mesh_in, pcl::PolygonMesh& mesh_out) -{ - using Cloud = pcl::PointCloud; - - // converting to point cloud - Cloud::Ptr cloud_in = boost::make_shared(); - pcl::fromPCLPointCloud2(mesh_in.cloud, *cloud_in); - - // initializing nurbs_surface surface - pcl::on_nurbs::NurbsDataSurface nurbs_data = pcl::on_nurbs::NurbsDataSurface(); - nurbs_data.interior = createNurbData(cloud_in); - ON_NurbsSurface nurbs_surface; - switch (parameters_.surf_init_method) - { - case SurfInitMethod::PCA: - nurbs_surface = - pcl::on_nurbs::FittingSurface::initNurbsPCA(parameters_.order, &nurbs_data, Eigen::Vector3d::UnitZ()); - break; - case SurfInitMethod::PCA_BB: - nurbs_surface = pcl::on_nurbs::FittingSurface::initNurbsPCABoundingBox( - parameters_.order, &nurbs_data, Eigen::Vector3d::UnitZ()); - break; - case SurfInitMethod::CUSTOM_PLANE: - CONSOLE_BRIDGE_logError("The surface initialization method %i is not yet implemented", - static_cast(parameters_.surf_init_method)); - return false; - default: - CONSOLE_BRIDGE_logError("The surface initialization method %i was not recognized", - static_cast(parameters_.surf_init_method)); - return false; - } - - // fitting surface - pcl::on_nurbs::FittingSurface fit(&nurbs_data, nurbs_surface); - fit.setQuiet(!parameters_.verbosity_on); // enable/disable debug output - pcl::on_nurbs::FittingSurface::Parameter surf_params = parameters_.surface_params; - for (int i = 0; i < parameters_.refinement; i++) - { - CONSOLE_BRIDGE_logDebug("Refinement attempt %i", i + 1); - fit.refine(0); - fit.refine(1); - fit.assemble(surf_params); - fit.solve(); - } - - // improving fit - for (unsigned i = 0; i < parameters_.iterations; i++) - { - CONSOLE_BRIDGE_logDebug("Improvement attempt %i", i + 1); - fit.assemble(surf_params); - fit.solve(); - } - - if (!fit.m_nurbs.IsValid()) - { - CONSOLE_BRIDGE_logError("Surface fitting failed"); - return false; - } - - // fit B-spline boundary curve - std::shared_ptr curve_fit = nullptr; - if (parameters_.clip_boundary_curve) - { - // initialisation (circular) - CONSOLE_BRIDGE_logDebug("Boundary Curve fitting ..."); - pcl::on_nurbs::NurbsDataCurve2d curve_data; - curve_data.interior = nurbs_data.interior_param; - curve_data.interior_weight_function.push_back(true); - ON_NurbsCurve curve_nurbs = pcl::on_nurbs::FittingCurve2dAPDM::initNurbsCurve2D( - parameters_.boundary_fit_order, curve_data.interior, parameters_.boundary_startCPs); - - // curve fitting - curve_fit = std::make_shared(&curve_data, curve_nurbs); - curve_fit->setQuiet(!parameters_.verbosity_on); // enable/disable debug output - curve_fit->fitting(parameters_.boundary_curve_params); - - if (!curve_fit->m_nurbs.IsValid()) - { - if (parameters_.boundary_clipping_required) - { - CONSOLE_BRIDGE_logError("Failed to fit boundary curve"); - return false; - } - else - { - CONSOLE_BRIDGE_logWarn("Failed to fit boundary curve but returning unclipped surface"); - } - } - } - - if (curve_fit && curve_fit->m_nurbs.IsValid()) - { - pcl::on_nurbs::Triangulation::convertTrimmedSurface2PolygonMesh( - fit.m_nurbs, curve_fit->m_nurbs, mesh_out, parameters_.mesh_resolution); - } - else - { - pcl::on_nurbs::Triangulation::convertSurface2PolygonMesh(fit.m_nurbs, mesh_out, parameters_.mesh_resolution); - } - return true; -} - -std::string BSplineReconstruction::getName() const { return utils::getClassName(); } - -} /* namespace mesh */ -} /* namespace noether_filtering */ diff --git a/noether_filtering/src/mesh/clean_data.cpp b/noether_filtering/src/mesh/clean_data.cpp deleted file mode 100644 index a28f5a81..00000000 --- a/noether_filtering/src/mesh/clean_data.cpp +++ /dev/null @@ -1,55 +0,0 @@ -/* - * clean_data.cpp - * - * Created on: Dec 6, 2019 - * Author: jrgnicho - */ - -#include -#include -#include -#include -#include -#include -#include "noether_filtering/mesh/clean_data.h" -#include "noether_filtering/utils.h" - -namespace noether_filtering -{ -namespace mesh -{ -CleanData::CleanData() {} - -CleanData::~CleanData() {} - -bool CleanData::configure(XmlRpc::XmlRpcValue config) -{ - // no configuration needed - return true; -} - -bool CleanData::filter(const pcl::PolygonMesh& mesh_in, pcl::PolygonMesh& mesh_out) -{ - using namespace pcl; - - vtkSmartPointer mesh_data = vtkSmartPointer::New(); - VTKUtils::mesh2vtk(mesh_in, mesh_data); - std::size_t num_start_points = mesh_data->GetNumberOfPoints(); - - mesh_data->BuildCells(); - mesh_data->BuildLinks(); - - vtkSmartPointer cleanPolyData = vtkSmartPointer::New(); - cleanPolyData->SetInputData(mesh_data); - cleanPolyData->Update(); - mesh_data = cleanPolyData->GetOutput(); - std::size_t num_end_points = mesh_data->GetNumberOfPoints(); - CONSOLE_BRIDGE_logInform("Removed duplicate points, retained %lu points from %lu", num_end_points, num_start_points); - VTKUtils::vtk2mesh(mesh_data, mesh_out); - return true; -} - -std::string CleanData::getName() const { return utils::getClassName(); } - -} /* namespace mesh */ -} /* namespace noether_filtering */ diff --git a/noether_filtering/src/mesh/euclidean_clustering.cpp b/noether_filtering/src/mesh/euclidean_clustering.cpp deleted file mode 100644 index 4aaa06a8..00000000 --- a/noether_filtering/src/mesh/euclidean_clustering.cpp +++ /dev/null @@ -1,195 +0,0 @@ -/** - * @author Jorge Nicho - * @file euclidean_clustering.cpp - * @date Nov 26, 2019 - * @copyright Copyright (c) 2019, Southwest Research Institute - * - * @par License - * Software License Agreement (Apache License) - * @par - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * http://www.apache.org/licenses/LICENSE-2.0 - * @par - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include "noether_filtering/mesh/euclidean_clustering.h" -#include "noether_filtering/utils.h" -#include -#include -#include -#include -#include -#include -#include -#include - -namespace noether_filtering -{ -namespace mesh -{ -bool EuclideanClustering::configure(XmlRpc::XmlRpcValue config) -{ - std::vector fields = { "tolerance", "min_cluster_size", "max_cluster_size" }; - if (!std::all_of(fields.begin(), fields.end(), [this, &config](const decltype(fields)::value_type& s) { - if (!config.hasMember(s)) - { - CONSOLE_BRIDGE_logError("%s did not find the %s field in the configuration", getName().c_str(), s.c_str()); - return false; - } - return true; - })) - { - return false; - } - - try - { - int idx = 0; - parameters_.tolerance = static_cast(config[fields[idx++]]); - parameters_.min_cluster_size = static_cast(config[fields[idx++]]); - parameters_.max_cluster_size = static_cast(config[fields[idx++]]); - } - catch (XmlRpc::XmlRpcException& e) - { - CONSOLE_BRIDGE_logError("%s Failed to cast a configuration field, %s", getName().c_str(), e.getMessage().c_str()); - return false; - } - - return true; -} - -bool EuclideanClustering::filter(const pcl::PolygonMesh& mesh_in, pcl::PolygonMesh& mesh_out) -{ - using namespace pcl; - using Cloud = pcl::PointCloud; - - // converting to point cloud - Cloud::Ptr mesh_points = boost::make_shared(); - pcl::fromPCLPointCloud2(mesh_in.cloud, *mesh_points); - - // computing clusters - search::KdTree::Ptr tree = boost::make_shared>(); - tree->setInputCloud(mesh_points); - std::vector cluster_indices; - EuclideanClusterExtraction ec; - ec.setClusterTolerance(parameters_.tolerance); - ec.setMinClusterSize(parameters_.min_cluster_size); - ec.setMaxClusterSize(parameters_.max_cluster_size <= 0 ? mesh_points->size() : parameters_.max_cluster_size); - ec.setSearchMethod(tree); - ec.setInputCloud(mesh_points); - ec.extract(cluster_indices); - - if (cluster_indices.empty()) - { - CONSOLE_BRIDGE_logError("%s found no clusters", getName().c_str()); - return false; - } - - CONSOLE_BRIDGE_logInform("%s found %lu clusters", getName().c_str(), cluster_indices.size()); - std::for_each( - cluster_indices.begin(), cluster_indices.end(), [idx = 0](decltype(cluster_indices)::value_type& c) mutable { - CONSOLE_BRIDGE_logInform("\t cluster[%i] -> %lu points ", idx++, c.indices.size()); - }); - - // accumulate all cluster indices into one - std::vector combined_indices; - std::for_each( - cluster_indices.begin(), cluster_indices.end(), [&combined_indices](decltype(cluster_indices)::value_type& c) { - combined_indices.insert(combined_indices.end(), c.indices.begin(), c.indices.end()); - }); - - // sorting - std::sort(combined_indices.begin(), combined_indices.end(), std::less()); - decltype(combined_indices)::iterator last = std::unique(combined_indices.begin(), combined_indices.end()); - combined_indices.erase(last, combined_indices.end()); - CONSOLE_BRIDGE_logInform("%s clusters contains %lu unique points from the original %lu", - getName().c_str(), - combined_indices.size(), - mesh_points->size()); - - // obtaining ignored indices - std::vector removed_indices; - removed_indices.reserve(mesh_points->size() - combined_indices.size()); - for (std::size_t i = 1; i < combined_indices.size(); i++) - { - const int& v1 = combined_indices[i - 1]; - const int& v2 = combined_indices[i]; - int diff = v2 - v1; - if (std::abs(diff) > 1) - { - std::vector vals(diff - 1); - std::iota(vals.begin(), vals.end(), v1 + 1); - removed_indices.insert(removed_indices.end(), vals.begin(), vals.end()); - } - } - - auto is_valid_polygon = [&points = *mesh_points](const Vertices& polygon, double tol = 1e-6) -> bool { - Eigen::Vector3f a, b, c, dir; - const decltype(polygon.vertices)& vert = polygon.vertices; - a = points[vert[0]].getVector3fMap(); - b = points[vert[1]].getVector3fMap(); - c = points[vert[2]].getVector3fMap(); - - dir = ((b - a).cross((c - a))).normalized(); - float norm = dir.norm(); - return !(std::isnan(norm) || std::isinf(norm)); - }; - - // iterating over the polygons and keeping those in the clusters - decltype(mesh_in.polygons) remaining_polygons; - for (const auto& plgn : mesh_in.polygons) - { - if (!is_valid_polygon(plgn)) - { - continue; - } - - decltype(removed_indices)::iterator pos; - bool add_poly = true; - for (const auto& v : plgn.vertices) - { - pos = std::find(removed_indices.begin(), removed_indices.end(), v); - if (pos != removed_indices.end()) - { - // add the polygon and exit - add_poly = false; - break; - } - } - - if (add_poly) - { - remaining_polygons.push_back(plgn); - } - } - - if (remaining_polygons.empty()) - { - CONSOLE_BRIDGE_logError("%s found no remaining polygons", getName().c_str()); - return false; - } - - CONSOLE_BRIDGE_logInform("New mesh contains %lu polygons from %lu in the original one", - remaining_polygons.size(), - mesh_in.polygons.size()); - - // creating new polygon mesh - pcl::PolygonMesh reduced_mesh; - reduced_mesh = mesh_in; - reduced_mesh.polygons = remaining_polygons; - surface::SimplificationRemoveUnusedVertices mesh_simplification; - mesh_simplification.simplify(reduced_mesh, mesh_out); - return true; -} - -std::string EuclideanClustering::getName() const { return utils::getClassName(); } - -} /* namespace mesh */ -} /* namespace noether_filtering */ diff --git a/noether_filtering/src/mesh/fill_holes.cpp b/noether_filtering/src/mesh/fill_holes.cpp deleted file mode 100644 index e22fee26..00000000 --- a/noether_filtering/src/mesh/fill_holes.cpp +++ /dev/null @@ -1,98 +0,0 @@ -/** - * @author Jorge Nicho - * @file fill_holes.pp - * @date Dec 16, 2019 - * @copyright Copyright (c) 2019, Southwest Research Institute - * - * @par License - * Software License Agreement (Apache License) - * @par - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * http://www.apache.org/licenses/LICENSE-2.0 - * @par - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include -#include -#include -#include -#include -#include -#include -#include "noether_filtering/mesh/fill_holes.h" -#include "noether_filtering/utils.h" - -namespace noether_filtering -{ -namespace mesh -{ -FillHoles::FillHoles() {} - -FillHoles::~FillHoles() {} - -bool FillHoles::configure(XmlRpc::XmlRpcValue config) -{ - std::vector fields = { "hole_size" }; - - if (!std::all_of(fields.begin(), fields.end(), [&config, this](const std::string& f) { - if (!config.hasMember(f)) - { - CONSOLE_BRIDGE_logError("The %s config field %s was not found", getName().c_str(), f.c_str()); - return false; - } - return true; - })) - { - return false; - } - - try - { - std::size_t idx = 0; - hole_size_ = static_cast(config[fields[idx++]]); - } - catch (XmlRpc::XmlRpcException& e) - { - CONSOLE_BRIDGE_logError(e.getMessage().c_str()); - return false; - } - return true; -} - -bool FillHoles::filter(const pcl::PolygonMesh& mesh_in, pcl::PolygonMesh& mesh_out) -{ - using namespace pcl; - - vtkSmartPointer mesh_data = vtkSmartPointer::New(); - VTKUtils::mesh2vtk(mesh_in, mesh_data); - std::size_t start_num_polys = mesh_data->GetNumberOfPolys(); - - vtkSmartPointer fill_holes_filter = vtkSmartPointer::New(); - fill_holes_filter->SetInputData(mesh_data); - fill_holes_filter->SetHoleSize(hole_size_); - fill_holes_filter->Update(); - - vtkSmartPointer normals_rectifier = vtkSmartPointer::New(); - normals_rectifier->SetInputData(fill_holes_filter->GetOutput()); - normals_rectifier->ConsistencyOn(); - normals_rectifier->SplittingOff(); - normals_rectifier->Update(); - - mesh_data = normals_rectifier->GetOutput(); - std::size_t end_num_polys = mesh_data->GetNumberOfPolys(); - CONSOLE_BRIDGE_logInform("Filled %lu polygons", start_num_polys - end_num_polys); - VTKUtils::vtk2mesh(mesh_data, mesh_out); - return true; -} - -std::string FillHoles::getName() const { return utils::getClassName(); } - -} /* namespace mesh */ -} /* namespace noether_filtering */ diff --git a/noether_filtering/src/mesh/plugins.cpp b/noether_filtering/src/mesh/plugins.cpp deleted file mode 100644 index d82e59f0..00000000 --- a/noether_filtering/src/mesh/plugins.cpp +++ /dev/null @@ -1,17 +0,0 @@ -#include - -// Mesh filters -#include "noether_filtering/mesh/euclidean_clustering.h" -#include "noether_filtering/mesh/clean_data.h" -#include "noether_filtering/mesh/windowed_sinc_smoothing.h" -#include "noether_filtering/mesh/fill_holes.h" - -PLUGINLIB_EXPORT_CLASS(noether_filtering::mesh::EuclideanClustering, noether_filtering::mesh::MeshFilterBase) -PLUGINLIB_EXPORT_CLASS(noether_filtering::mesh::CleanData, noether_filtering::mesh::MeshFilterBase) -PLUGINLIB_EXPORT_CLASS(noether_filtering::mesh::WindowedSincSmoothing, noether_filtering::mesh::MeshFilterBase) -PLUGINLIB_EXPORT_CLASS(noether_filtering::mesh::FillHoles, noether_filtering::mesh::MeshFilterBase) - -#ifdef ENABLE_NURBS -#include "noether_filtering/mesh/bspline_reconstruction.h" -PLUGINLIB_EXPORT_CLASS(noether_filtering::mesh::BSplineReconstruction, noether_filtering::mesh::MeshFilterBase) -#endif diff --git a/noether_filtering/src/mesh/windowed_sinc_smoothing.cpp b/noether_filtering/src/mesh/windowed_sinc_smoothing.cpp deleted file mode 100644 index f7a43704..00000000 --- a/noether_filtering/src/mesh/windowed_sinc_smoothing.cpp +++ /dev/null @@ -1,101 +0,0 @@ -/* - * window_sinc_smoothing.cpp - * - * Created on: Dec 6, 2019 - * Author: jrgnicho - */ - -#include -#include -#include -#include -#include -#include -#include "noether_filtering/utils.h" -#include "noether_filtering/mesh/windowed_sinc_smoothing.h" - -namespace noether_filtering -{ -namespace mesh -{ -WindowedSincSmoothing::WindowedSincSmoothing() {} - -WindowedSincSmoothing::~WindowedSincSmoothing() {} - -bool WindowedSincSmoothing::configure(XmlRpc::XmlRpcValue config) -{ - std::vector fields = { - "num_iter", - "enable_boundary_smoothing", - "enable_feature_edge_smoothing", - "enable_non_manifold_smoothing", - "enable_normalize_coordinates", - "feature_angle", - "edge_angle", - "pass_band", - }; - - if (!std::all_of(fields.begin(), fields.end(), [&config, this](const std::string& f) { - if (!config.hasMember(f)) - { - CONSOLE_BRIDGE_logError("The %s config field %s was not found", getName().c_str(), f.c_str()); - return false; - } - return true; - })) - { - return false; - } - - try - { - std::size_t idx = 0; - config_.num_iter = static_cast(config[fields[idx++]]); - config_.enable_boundary_smoothing = static_cast(config[fields[idx++]]); - config_.enable_feature_edge_smoothing = static_cast(config[fields[idx++]]); - config_.enable_non_manifold_smoothing = static_cast(config[fields[idx++]]); - config_.enable_normalize_coordinates = static_cast(config[fields[idx++]]); - config_.feature_angle = static_cast(config[fields[idx++]]); - config_.edge_angle = static_cast(config[fields[idx++]]); - config_.pass_band = static_cast(config[fields[idx++]]); - } - catch (XmlRpc::XmlRpcException& e) - { - CONSOLE_BRIDGE_logError(e.getMessage().c_str()); - return false; - } - return true; -} - -bool WindowedSincSmoothing::filter(const pcl::PolygonMesh& mesh_in, pcl::PolygonMesh& mesh_out) -{ - using namespace pcl; - - vtkSmartPointer mesh_data = vtkSmartPointer::New(); - VTKUtils::mesh2vtk(mesh_in, mesh_data); - std::size_t num_start_points = mesh_data->GetNumberOfPoints(); - - mesh_data->BuildCells(); - mesh_data->BuildLinks(); - - vtkSmartPointer smoother = vtkSmartPointer::New(); - smoother->SetInputData(mesh_data); - smoother->SetNumberOfIterations(config_.num_iter); - smoother->SetFeatureAngle(config_.feature_angle); - smoother->SetEdgeAngle(config_.edge_angle); - smoother->SetPassBand(config_.pass_band); - smoother->SetBoundarySmoothing(config_.enable_boundary_smoothing); - smoother->SetFeatureEdgeSmoothing(config_.enable_feature_edge_smoothing); - smoother->SetNonManifoldSmoothing(config_.enable_non_manifold_smoothing); - smoother->SetNormalizeCoordinates(config_.enable_normalize_coordinates); - smoother->Update(); - - mesh_data = smoother->GetOutput(); - VTKUtils::vtk2mesh(mesh_data, mesh_out); - return true; -} - -std::string WindowedSincSmoothing::getName() const { return utils::getClassName(); } - -} /* namespace mesh */ -} /* namespace noether_filtering */ diff --git a/noether_filtering/src/test/cloud_utest.cpp b/noether_filtering/src/test/cloud_utest.cpp deleted file mode 100644 index 86bbb606..00000000 --- a/noether_filtering/src/test/cloud_utest.cpp +++ /dev/null @@ -1,224 +0,0 @@ -#include -#include -#include "noether_filtering/filter_manager.h" -#include "noether_filtering/utils.h" -#include -#include - -#include "noether_filtering/cloud/crop_box_filter.h" -#include "noether_filtering/cloud/pass_through_filter.h" -#include "noether_filtering/cloud/radius_outlier_filter.h" -#include "noether_filtering/cloud/statistical_outlier_filter.h" -#include "noether_filtering/cloud/voxel_grid_filter.h" -#include "noether_filtering/cloud/mls_smoothing_filter.h" - -template -XmlRpc::XmlRpcValue createVoxelGridConfig() -{ - using namespace noether_filtering; - using namespace noether_filtering::cloud; - using namespace noether_filtering::config_fields::filter; - XmlRpc::XmlRpcValue f; - f[NAME] = "voxel_grid"; - f[TYPE_NAME] = utils::getClassName>(); - - XmlRpc::XmlRpcValue vg; - vg[VoxelGridFilter::LEAF_SIZE] = 0.1; - f[CONFIG] = std::move(vg); - - return f; -} - -template -XmlRpc::XmlRpcValue createStatisticalOutlierConfig() -{ - using namespace noether_filtering; - using namespace noether_filtering::cloud; - using namespace noether_filtering::config_fields::filter; - XmlRpc::XmlRpcValue f; - f[NAME] = "statistical_outlier"; - f[TYPE_NAME] = utils::getClassName>(); - - XmlRpc::XmlRpcValue so; - so[StatisticalOutlierFilter::MEAN_K] = 10; - so[StatisticalOutlierFilter::STD_DEV_MULT] = 1.0; - f[CONFIG] = std::move(so); - - return f; -} - -template -XmlRpc::XmlRpcValue createCropBoxConfig() -{ - using namespace noether_filtering; - using namespace noether_filtering::cloud; - using namespace noether_filtering::config_fields::filter; - XmlRpc::XmlRpcValue f; - f[NAME] = "crop_box"; - f[TYPE_NAME] = utils::getClassName>(); - - XmlRpc::XmlRpcValue min; - min["x"] = -1.0; - min["y"] = -1.0; - min["z"] = -1.0; - - XmlRpc::XmlRpcValue max; - max["x"] = 1.0; - max["y"] = 1.0; - max["z"] = 1.0; - - XmlRpc::XmlRpcValue t; - t["x"] = 0.5; - t["y"] = 0.5; - t["z"] = 0.5; - t["rx"] = 0.1; - t["ry"] = 0.1; - t["rz"] = 0.1; - - XmlRpc::XmlRpcValue cb; - cb[CropBoxFilter::MIN] = min; - cb[CropBoxFilter::MAX] = max; - cb[CropBoxFilter::TRANSFORM] = t; - cb[CropBoxFilter::CROP_OUTSIDE] = false; - - f[CONFIG] = std::move(cb); - - return f; -} - -template -XmlRpc::XmlRpcValue createPassThroughConfig() -{ - using namespace noether_filtering; - using namespace noether_filtering::cloud; - using namespace noether_filtering::config_fields::filter; - XmlRpc::XmlRpcValue f; - f[NAME] = "pass_through"; - f[TYPE_NAME] = utils::getClassName>(); - - XmlRpc::XmlRpcValue pt; - pt[PassThroughFilter::FILTER_FIELD_NAME] = "y"; - pt[PassThroughFilter::MIN_LIMIT] = -1.0; - pt[PassThroughFilter::MAX_LIMIT] = 1.0; - pt[PassThroughFilter::NEGATIVE] = false; - f[CONFIG] = std::move(pt); - - return f; -} - -template -XmlRpc::XmlRpcValue createRadiusOutlierConfig() -{ - using namespace noether_filtering; - using namespace noether_filtering::cloud; - using namespace noether_filtering::config_fields::filter; - XmlRpc::XmlRpcValue f; - f[NAME] = "radius_outlier_filter"; - f[TYPE_NAME] = utils::getClassName>(); - - XmlRpc::XmlRpcValue ro; - ro[RadiusOutlierFilter::RADIUS] = 1.0; - ro[RadiusOutlierFilter::MIN_PTS] = 5; - f[CONFIG] = std::move(ro); - - return f; -} - -template -XmlRpc::XmlRpcValue createMLSSmoothingConfig() -{ - using namespace noether_filtering; - using namespace noether_filtering::cloud; - using namespace noether_filtering::config_fields::filter; - XmlRpc::XmlRpcValue f; - f[NAME] = "mls_smoothing_filter"; - f[TYPE_NAME] = utils::getClassName>(); - - XmlRpc::XmlRpcValue mls; - mls[MLSSmoothingFilter::SEARCH_RADIUS] = 0.1; - mls[MLSSmoothingFilter::POLYNOMIAL_ORDER] = 2; - f[CONFIG] = std::move(mls); - - return f; -} - -template -XmlRpc::XmlRpcValue createManagerConfig(std::string group_name) -{ - using namespace noether_filtering::config_fields; - - // Create filter config(s) - XmlRpc::XmlRpcValue filters; - filters.setSize(6); - filters[0] = createVoxelGridConfig(); - filters[1] = createStatisticalOutlierConfig(); - filters[2] = createCropBoxConfig(); - filters[3] = createPassThroughConfig(); - filters[4] = createRadiusOutlierConfig(); - filters[5] = createMLSSmoothingConfig(); - - // Create a single filter group - XmlRpc::XmlRpcValue g; - g[group::GROUP_NAME] = std::move(group_name); - g[group::CONTINUE_ON_FAILURE] = false; - g[group::VERBOSITY_ON] = true; - g[group::FILTERS] = std::move(filters); - - // Create a collection of filter groups - XmlRpc::XmlRpcValue groups; - groups.setSize(1); - groups[0] = std::move(g); - - // Create the manager configuration - XmlRpc::XmlRpcValue manager; - manager[manager::FILTER_GROUPS] = std::move(groups); - - return manager; -} - -template -class FilterManagerFixture : public testing::Test -{ -public: - using testing::Test::Test; - std::shared_ptr>> manager; -}; - -typedef ::testing::Types Implementations; - -TYPED_TEST_SUITE(FilterManagerFixture, Implementations); - -TYPED_TEST(FilterManagerFixture, FilterManagerTest) -{ - using namespace noether_filtering; - using namespace noether_filtering::config_fields::filter; - using FilterT = noether_filtering::FilterBase>; - using Cloud = pcl::PointCloud; - - const std::string group_name = "test_group"; - XmlRpc::XmlRpcValue config = createManagerConfig(group_name); - - this->manager = std::make_shared>(utils::getClassName()); - ASSERT_TRUE(this->manager->init(config)); - - using Group = FilterGroup; - std::shared_ptr group = this->manager->getFilterGroup(group_name); - - ASSERT_TRUE(group != nullptr); - - pcl::PointCloud input_cloud; - input_cloud.points.resize(100); - pcl::PointCloud output_cloud; - std::string error; - - ASSERT_TRUE(group->applyFilters(input_cloud, output_cloud, error)); - - ASSERT_LE(output_cloud.points.size(), input_cloud.points.size()); -} - -int main(int argc, char** argv) -{ - console_bridge::setLogLevel(console_bridge::LogLevel::CONSOLE_BRIDGE_LOG_INFO); - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/noether_gui/CMakeLists.txt b/noether_gui/CMakeLists.txt index 954a1023..7dc6e65a 100644 --- a/noether_gui/CMakeLists.txt +++ b/noether_gui/CMakeLists.txt @@ -4,7 +4,7 @@ cmake_minimum_required(VERSION 3.5.1) find_package(ros_industrial_cmake_boilerplate REQUIRED) extract_package_metadata(pkg) -project(${pkg_extracted_name} VERSION ${pkg_extracted_version} LANGUAGES CXX) +project(${pkg_extracted_name} VERSION ${pkg_extracted_version} LANGUAGES C CXX) find_package(Eigen3 REQUIRED) find_package(Boost REQUIRED COMPONENTS program_options) @@ -54,8 +54,6 @@ qt5_wrap_ui(${PROJECT_NAME}_widget_ui_mocs ui/tpp_pipeline_widget.ui ui/tpp_widget.ui) -include_directories(include ${CMAKE_CURRENT_BINARY_DIR}) - # Core Library add_library(${PROJECT_NAME} SHARED src/plugin_interface.cpp @@ -95,6 +93,7 @@ add_library(${PROJECT_NAME} SHARED target_include_directories(${PROJECT_NAME} PUBLIC "$" "$") +target_include_directories(${PROJECT_NAME} PRIVATE "$") target_link_libraries(${PROJECT_NAME} PUBLIC noether::noether_tpp Qt5::Widgets ${PCL_LIBRARIES} boost_plugin_loader::boost_plugin_loader Eigen3::Eigen yaml-cpp) target_compile_definitions(${PROJECT_NAME} PUBLIC diff --git a/noether_gui/include/noether_gui/widgets/tpp_widget.h b/noether_gui/include/noether_gui/widgets/tpp_widget.h index 613a91ab..9b6864b3 100644 --- a/noether_gui/include/noether_gui/widgets/tpp_widget.h +++ b/noether_gui/include/noether_gui/widgets/tpp_widget.h @@ -4,7 +4,18 @@ #include #include +#ifndef VTK_MAJOR_VERSION +#include +#endif + +#if VTK_MAJOR_VERSION > 7 +class QVTKOpenGLNativeWidget; +using RenderWidget = QVTKOpenGLNativeWidget; +#else class QVTKWidget; +using RenderWidget = QVTKWidget; +#endif + class vtkActor; class vtkPolyDataMapper; class vtkProp; @@ -63,7 +74,7 @@ class TPPWidget : public QWidget TPPPipelineWidget* pipeline_widget_; // Viewer rendering - QVTKWidget* render_widget_; + RenderWidget* render_widget_; vtkSmartPointer renderer_; vtkSmartPointer mesh_mapper_; vtkSmartPointer mesh_actor_; diff --git a/noether_gui/src/widgets/tpp_widget.cpp b/noether_gui/src/widgets/tpp_widget.cpp index c2b12386..7a798352 100644 --- a/noether_gui/src/widgets/tpp_widget.cpp +++ b/noether_gui/src/widgets/tpp_widget.cpp @@ -13,7 +13,14 @@ #include // Rendering includes +#ifndef VTK_MAJOR_VERSION +#include +#endif +#if VTK_MAJOR_VERSION > 7 +#include +#else #include +#endif #include #include #include @@ -35,7 +42,7 @@ TPPWidget::TPPWidget(boost_plugin_loader::PluginLoader loader, QWidget* parent) : QWidget(parent) , ui_(new Ui::TPP()) , pipeline_widget_(new TPPPipelineWidget(std::move(loader), this)) - , render_widget_(new QVTKWidget(this)) + , render_widget_(new RenderWidget(this)) , renderer_(vtkSmartPointer::New()) , mesh_mapper_(vtkSmartPointer::New()) , mesh_actor_(vtkSmartPointer::New()) diff --git a/noether_tpp/CMakeLists.txt b/noether_tpp/CMakeLists.txt index 3051d5c4..86774bb5 100644 --- a/noether_tpp/CMakeLists.txt +++ b/noether_tpp/CMakeLists.txt @@ -4,10 +4,10 @@ cmake_minimum_required(VERSION 3.5.1) find_package(ros_industrial_cmake_boilerplate REQUIRED) extract_package_metadata(pkg) -project(${pkg_extracted_name} VERSION ${pkg_extracted_version} LANGUAGES CXX) +project(${pkg_extracted_name} VERSION ${pkg_extracted_version} LANGUAGES C CXX) find_package(Eigen3 REQUIRED) -find_package(PCL REQUIRED COMPONENTS common io surface) +find_package(PCL REQUIRED COMPONENTS common io surface segmentation) find_package(VTK REQUIRED NO_MODULE) if(VTK_FOUND AND ("${VTK_VERSION}" VERSION_LESS 7.1)) @@ -30,7 +30,13 @@ add_library(${PROJECT_NAME} SHARED src/core/tool_path_planner_pipeline.cpp src/utils.cpp # Mesh Modifiers + src/mesh_modifiers/subset_extraction/subset_extractor.cpp + src/mesh_modifiers/subset_extraction/extruded_polygon_subset_extractor.cpp src/mesh_modifiers/compound_modifier.cpp + src/mesh_modifiers/clean_data_modifier.cpp + src/mesh_modifiers/euclidean_clustering_modifier.cpp + src/mesh_modifiers/fill_holes_modifier.cpp + src/mesh_modifiers/windowed_sinc_smoothing.cpp # Tool Path Modifiers src/tool_path_modifiers/circular_lead_in_modifier.cpp src/tool_path_modifiers/circular_lead_out_modifier.cpp @@ -56,6 +62,22 @@ add_library(${PROJECT_NAME} SHARED src/tool_path_planners/raster/direction_generators/principal_axis_direction_generator.cpp src/tool_path_planners/raster/plane_slicer_raster_planner.cpp ) + +if(${CMAKE_VERSION} VERSION_GREATER 3.15) + try_compile( + NURBS_FOUND + ${CMAKE_CURRENT_BINARY_DIR}/pcl_nurbs_try_compile + ${CMAKE_CURRENT_SOURCE_DIR}/src/mesh_modifiers/test/check_pcl_nurbs.cpp + CMAKE_FLAGS ${CMAKE_FLAGS} -DINCLUDE_DIRECTORIES=${PCL_INCLUDE_DIRS} + LINK_LIBRARIES ${PCL_LIBRARIES} + ) + if (${NURBS_FOUND}) + target_sources(${PROJECT_NAME} src/mesh_modifiers/bspline_reconstruction_modifier.cpp) + else() + message("PCL NURBS not found; skipping compilation of BSpline reconstruction mesh modifier") + endif() +endif() + target_include_directories(${PROJECT_NAME} PUBLIC "$" "$") @@ -79,4 +101,4 @@ if(${NOETHER_ENABLE_TESTING}) endif() # Package configuration -configure_package(NAMESPACE noether DEPENDENCIES Eigen3 "PCL REQUIRED COMPONENTS common io" VTK TARGETS ${PROJECT_NAME}) +configure_package(NAMESPACE noether DEPENDENCIES Eigen3 "PCL REQUIRED COMPONENTS common io surface segmentation" VTK TARGETS ${PROJECT_NAME}) diff --git a/noether_tpp/include/noether_tpp/mesh_modifiers/bspline_reconstruction_modifier.h b/noether_tpp/include/noether_tpp/mesh_modifiers/bspline_reconstruction_modifier.h new file mode 100644 index 00000000..42a5e8d1 --- /dev/null +++ b/noether_tpp/include/noether_tpp/mesh_modifiers/bspline_reconstruction_modifier.h @@ -0,0 +1,79 @@ +/** + * @author Jorge Nicho + * @file bspline_reconstruction.cpp + * @date Oct 10, 2019 + * @copyright Copyright (c) 2019, Southwest Research Institute + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#pragma once + +#include +#include +#include + +namespace noether +{ +/** + * @class noether_filtering::filters::BSplineReconstruction + * @details: Smoothes a mesh by fitting a bspline surface to it. The details of this implementation' + * can be in found in http://pointclouds.org/documentation/tutorials/bspline_fitting.php + */ +class BSplineReconstruction : public MeshModifier +{ +public: + enum class SurfInitMethod : int + { + PCA = 1, + PCA_BB, + CUSTOM_PLANE, // not implemented yet but will use pcl::on_nurbs::FittingSurface::initNurbs4Corners + }; + + struct Parameters + { + /** @brief print more info */ + bool verbosity_on = false; + /** @brief is the polynomial order of the B-spline surface. */ + int order = 3; + /** @brief refinement is the number of refinement iterations, where for each iteration control-points are inserted + */ + int refinement = 3; + /** @brief is the number of iterations that are performed after refinement is completed */ + unsigned iterations = 1; + /** @brief the number of vertices in each parametric direction, used for triangulation of the B-spline surface.*/ + unsigned mesh_resolution = 50; + /** @brief method for creating the initial surface to be fit */ + SurfInitMethod surf_init_method = SurfInitMethod::PCA_BB; + /** @brief parameters to fit the surface**/ + pcl::on_nurbs::FittingSurface::Parameter surface_params; + /** @brief whether to fit the boundary curve and clip every that extends past it*/ + bool clip_boundary_curve = true; + /** @brief applicable only when clip_boundary_curve = true */ + int boundary_fit_order = 2; + /** @brief initial number of control points */ + int boundary_startCPs = 0; + /** @brief if True then algorithm will fail if the boundary could not be clipped */ + bool boundary_clipping_required = false; + pcl::on_nurbs::FittingCurve2dAPDM::FitParameter boundary_curve_params; + }; + + std::vector modify(const pcl::PolygonMesh& mesh) const override; + +protected: + Parameters parameters_; +}; + +} // namespace noether diff --git a/noether_filtering/include/noether_filtering/mesh/clean_data.h b/noether_tpp/include/noether_tpp/mesh_modifiers/clean_data_modifier.h similarity index 53% rename from noether_filtering/include/noether_filtering/mesh/clean_data.h rename to noether_tpp/include/noether_tpp/mesh_modifiers/clean_data_modifier.h index 43abdd03..f7b3f5e7 100644 --- a/noether_filtering/include/noether_filtering/mesh/clean_data.h +++ b/noether_tpp/include/noether_tpp/mesh_modifiers/clean_data_modifier.h @@ -19,14 +19,11 @@ * limitations under the License. */ -#ifndef INCLUDE_NOETHER_FILTERING_MESH_CLEAN_DATA_H_ -#define INCLUDE_NOETHER_FILTERING_MESH_CLEAN_DATA_H_ +#pragma once -#include "noether_filtering/mesh/mesh_filter_base.h" +#include -namespace noether_filtering -{ -namespace mesh +namespace noether { /** * @class noether_filtering::mesh::CleanData @@ -34,30 +31,10 @@ namespace mesh * https://lorensen.github.io/VTKExamples/site/Cxx/PolyData/CleanPolyData/ Merges duplicate points, and/or remove unused * points and/or remove degenerate cells */ -class CleanData : public MeshFilterBase +class CleanData : public MeshModifier { public: - CleanData(); - virtual ~CleanData(); - - /** - * @brief does not require configuration - * @param config Here to conform to interface but can be an emtpy object - * @return True always - */ - bool configure(XmlRpc::XmlRpcValue config) override final; - - /** - * @brief Merges duplicate points, and/or remove unused points and/or remove degenerate cells - * @param mesh_in - * @param mesh_out - * @return True always - */ - bool filter(const pcl::PolygonMesh& mesh_in, pcl::PolygonMesh& mesh_out) override final; - std::string getName() const override final; + std::vector modify(const pcl::PolygonMesh& mesh) const override; }; -} /* namespace mesh */ -} /* namespace noether_filtering */ - -#endif /* INCLUDE_NOETHER_FILTERING_MESH_CLEAN_DATA_H_ */ +} // namespace noether diff --git a/noether_filtering/include/noether_filtering/mesh/euclidean_clustering.h b/noether_tpp/include/noether_tpp/mesh_modifiers/euclidean_clustering_modifier.h similarity index 61% rename from noether_filtering/include/noether_filtering/mesh/euclidean_clustering.h rename to noether_tpp/include/noether_tpp/mesh_modifiers/euclidean_clustering_modifier.h index e757b663..b1b90b79 100644 --- a/noether_filtering/include/noether_filtering/mesh/euclidean_clustering.h +++ b/noether_tpp/include/noether_tpp/mesh_modifiers/euclidean_clustering_modifier.h @@ -19,18 +19,13 @@ * limitations under the License. */ -#ifndef INCLUDE_NOETHER_FILTERING_MESH_EUCLIDEAN_CLUSTERING_H_ -#define INCLUDE_NOETHER_FILTERING_MESH_EUCLIDEAN_CLUSTERING_H_ +#pragma once -#include -#include -#include "noether_filtering/mesh/mesh_filter_base.h" +#include -namespace noether_filtering +namespace noether { -namespace mesh -{ -class EuclideanClustering : public MeshFilterBase +class EuclideanClustering : public MeshModifier { public: struct Parameters @@ -40,15 +35,10 @@ class EuclideanClustering : public MeshFilterBase int max_cluster_size = -1; // will use input point cloud size when negative }; - bool configure(XmlRpc::XmlRpcValue config) override final; - bool filter(const pcl::PolygonMesh& mesh_in, pcl::PolygonMesh& mesh_out) override final; - std::string getName() const override final; + std::vector modify(const pcl::PolygonMesh& mesh_in) const override; protected: Parameters parameters_; }; -} /* namespace mesh */ -} /* namespace noether_filtering */ - -#endif /* INCLUDE_NOETHER_FILTERING_MESH_EUCLIDEAN_CLUSTERING_H_ */ +} // namespace noether diff --git a/noether_filtering/include/noether_filtering/mesh/fill_holes.h b/noether_tpp/include/noether_tpp/mesh_modifiers/fill_holes_modifier.h similarity index 50% rename from noether_filtering/include/noether_filtering/mesh/fill_holes.h rename to noether_tpp/include/noether_tpp/mesh_modifiers/fill_holes_modifier.h index 216b8e04..f4bf48da 100644 --- a/noether_filtering/include/noether_filtering/mesh/fill_holes.h +++ b/noether_tpp/include/noether_tpp/mesh_modifiers/fill_holes_modifier.h @@ -18,51 +18,24 @@ * See the License for the specific language governing permissions and * limitations under the License. */ +#pragma once -#ifndef INCLUDE_NOETHER_FILTERING_MESH_FILL_HOLES_H_ -#define INCLUDE_NOETHER_FILTERING_MESH_FILL_HOLES_H_ +#include -#include "noether_filtering/mesh/mesh_filter_base.h" - -namespace noether_filtering -{ -namespace mesh +namespace noether { /** * @class noether_filtering::mesh::FillHoles * @brief Applies the vtkFillHoles filter, more details can be found on * https://vtk.org/doc/nightly/html/classvtkFillHolesFilter.html#details */ -class FillHoles : public MeshFilterBase +class FillHoles : public MeshModifier { public: - FillHoles(); - virtual ~FillHoles(); - - /** - * @brief configures the fill hole algorithm from a yaml struct with the following form - * - hole_size: 1.0 # Specify the maximum hole size to fill. Represented as a radius to the bounding circumsphere - * containing the hole. - * - * @param config the configuration - * @return True on success, false otherwise - */ - bool configure(XmlRpc::XmlRpcValue config) override final; - - /** - * @brief Fills holes - * @param mesh_in - * @param mesh_out - * @return True always - */ - bool filter(const pcl::PolygonMesh& mesh_in, pcl::PolygonMesh& mesh_out) override final; - std::string getName() const override final; + std::vector modify(const pcl::PolygonMesh& mesh) const override; protected: double hole_size_ = 1.0; }; -} /* namespace mesh */ -} /* namespace noether_filtering */ - -#endif /* INCLUDE_NOETHER_FILTERING_MESH_FILL_HOLES_H_ */ +} // namespace noether diff --git a/noether_filtering/include/noether_filtering/subset_extraction/extruded_polygon_subset_extractor.h b/noether_tpp/include/noether_tpp/mesh_modifiers/subset_extraction/extruded_polygon_subset_extractor.h similarity index 96% rename from noether_filtering/include/noether_filtering/subset_extraction/extruded_polygon_subset_extractor.h rename to noether_tpp/include/noether_tpp/mesh_modifiers/subset_extraction/extruded_polygon_subset_extractor.h index 210403cb..896a1d8e 100644 --- a/noether_filtering/include/noether_filtering/subset_extraction/extruded_polygon_subset_extractor.h +++ b/noether_tpp/include/noether_tpp/mesh_modifiers/subset_extraction/extruded_polygon_subset_extractor.h @@ -15,7 +15,7 @@ */ #pragma once -#include +#include namespace noether { diff --git a/noether_filtering/include/noether_filtering/subset_extraction/subset_extractor.h b/noether_tpp/include/noether_tpp/mesh_modifiers/subset_extraction/subset_extractor.h similarity index 100% rename from noether_filtering/include/noether_filtering/subset_extraction/subset_extractor.h rename to noether_tpp/include/noether_tpp/mesh_modifiers/subset_extraction/subset_extractor.h diff --git a/noether_tpp/include/noether_tpp/mesh_modifiers/windowed_sinc_smoothing_modifier.h b/noether_tpp/include/noether_tpp/mesh_modifiers/windowed_sinc_smoothing_modifier.h new file mode 100644 index 00000000..b63de2fc --- /dev/null +++ b/noether_tpp/include/noether_tpp/mesh_modifiers/windowed_sinc_smoothing_modifier.h @@ -0,0 +1,48 @@ +/* + * window_sinc_smoothing.h + * + * Created on: Dec 6, 2019 + * Author: jrgnicho + */ + +#pragma once + +#include + +namespace noether +{ +/** + * @class noether_filtering:;mesh::WindowedSincSmoothing + * @details uses the vtkWindowedSincPolyDataFilter to smooth out the mesh, details of the implementation + * and the configuration parameters can be found here + * https://vtk.org/doc/nightly/html/classvtkWindowedSincPolyDataFilter.html + */ +class WindowedSincSmoothing : public MeshModifier +{ +public: + struct Config + { + std::size_t num_iter = 100; + /**@brief Set to true to enable */ + bool enable_boundary_smoothing = true; + /**@brief Set to true to enable */ + bool enable_feature_edge_smoothing = false; + /**@brief Set to true to enable */ + bool enable_non_manifold_smoothing = true; + /**@brief Set to true to enable */ + bool enable_normalize_coordinates = true; + /**@brief degrees, only applicable when feature_edge_smoothing = true */ + double feature_angle = 10.0; + /**@brief degrees, only applicable when feature_edge_smoothing = true */ + double edge_angle = 150.0; + /**@brief PassBand for the windowed sinc filter, see explanation in reference link */ + double pass_band = 0.01; + }; + + std::vector modify(const pcl::PolygonMesh& mesh) const override; + +protected: + Config config_; +}; + +} // namespace noether diff --git a/noether_tpp/src/mesh_modifiers/bspline_reconstruction_modifier.cpp b/noether_tpp/src/mesh_modifiers/bspline_reconstruction_modifier.cpp new file mode 100644 index 00000000..332ce564 --- /dev/null +++ b/noether_tpp/src/mesh_modifiers/bspline_reconstruction_modifier.cpp @@ -0,0 +1,124 @@ +/** + * @author Jorge Nicho + * @file bspline_reconstruction.cpp + * @date Oct 10, 2019 + * @copyright Copyright (c) 2019, Southwest Research Institute + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include +#include + +static pcl::on_nurbs::vector_vec3d createNurbData(pcl::PointCloud::ConstPtr cloud) +{ + pcl::on_nurbs::vector_vec3d data; + for (unsigned i = 0; i < cloud->size(); i++) + { + const pcl::PointXYZ& p = cloud->at(i); + if (!std::isnan(p.x) && !std::isnan(p.y) && !std::isnan(p.z)) + data.push_back(Eigen::Vector3d(p.x, p.y, p.z)); + } + return std::move(data); +} + +namespace noether +{ +pcl::PolygonMesh BSplineReconstruction::modify(const pcl::PolygonMesh& mesh_in) const +{ + using Cloud = pcl::PointCloud; + + // converting to point cloud + Cloud::Ptr cloud_in = boost::make_shared(); + pcl::fromPCLPointCloud2(mesh_in.cloud, *cloud_in); + + // initializing nurbs_surface surface + pcl::on_nurbs::NurbsDataSurface nurbs_data = pcl::on_nurbs::NurbsDataSurface(); + nurbs_data.interior = createNurbData(cloud_in); + ON_NurbsSurface nurbs_surface; + switch (parameters_.surf_init_method) + { + case SurfInitMethod::PCA: + nurbs_surface = + pcl::on_nurbs::FittingSurface::initNurbsPCA(parameters_.order, &nurbs_data, Eigen::Vector3d::UnitZ()); + break; + case SurfInitMethod::PCA_BB: + nurbs_surface = pcl::on_nurbs::FittingSurface::initNurbsPCABoundingBox( + parameters_.order, &nurbs_data, Eigen::Vector3d::UnitZ()); + break; + case SurfInitMethod::CUSTOM_PLANE: + throw std::runtime_error("The surface initialization method 'CUSTOM_PLANE' is not yet implemented"); + default: + throw std::runtime_error("The surface initialization method was not recognized"); + } + + // fitting surface + pcl::on_nurbs::FittingSurface fit(&nurbs_data, nurbs_surface); + fit.setQuiet(!parameters_.verbosity_on); // enable/disable debug output + pcl::on_nurbs::FittingSurface::Parameter surf_params = parameters_.surface_params; + for (int i = 0; i < parameters_.refinement; i++) + { + fit.refine(0); + fit.refine(1); + fit.assemble(surf_params); + fit.solve(); + } + + // improving fit + for (unsigned i = 0; i < parameters_.iterations; i++) + { + fit.assemble(surf_params); + fit.solve(); + } + + if (!fit.m_nurbs.IsValid()) + throw std::runtime_error("Surface fitting failed"); + + // fit B-spline boundary curve + std::shared_ptr curve_fit = nullptr; + if (parameters_.clip_boundary_curve) + { + // initialisation (circular) + pcl::on_nurbs::NurbsDataCurve2d curve_data; + curve_data.interior = nurbs_data.interior_param; + curve_data.interior_weight_function.push_back(true); + ON_NurbsCurve curve_nurbs = pcl::on_nurbs::FittingCurve2dAPDM::initNurbsCurve2D( + parameters_.boundary_fit_order, curve_data.interior, parameters_.boundary_startCPs); + + // curve fitting + curve_fit = std::make_shared(&curve_data, curve_nurbs); + curve_fit->setQuiet(!parameters_.verbosity_on); // enable/disable debug output + curve_fit->fitting(parameters_.boundary_curve_params); + + if (!curve_fit->m_nurbs.IsValid() && parameters_.boundary_clipping_required) + throw std::runtime_error("Failed to fit boundary curve"); + } + + pcl::PolygonMesh mesh_out; + if (curve_fit && curve_fit->m_nurbs.IsValid()) + { + pcl::on_nurbs::Triangulation::convertTrimmedSurface2PolygonMesh( + fit.m_nurbs, curve_fit->m_nurbs, mesh_out, parameters_.mesh_resolution); + } + else + { + pcl::on_nurbs::Triangulation::convertSurface2PolygonMesh(fit.m_nurbs, mesh_out, parameters_.mesh_resolution); + } + + return { mesh_out }; +} + +} // namespace noether diff --git a/noether_tpp/src/mesh_modifiers/clean_data_modifier.cpp b/noether_tpp/src/mesh_modifiers/clean_data_modifier.cpp new file mode 100644 index 00000000..04091235 --- /dev/null +++ b/noether_tpp/src/mesh_modifiers/clean_data_modifier.cpp @@ -0,0 +1,33 @@ +/* + * clean_data.cpp + * + * Created on: Dec 6, 2019 + * Author: jrgnicho + */ + +#include + +#include +#include + +namespace noether +{ +std::vector CleanData::modify(const pcl::PolygonMesh& mesh_in) const +{ + vtkSmartPointer mesh_data = vtkSmartPointer::New(); + pcl::VTKUtils::mesh2vtk(mesh_in, mesh_data); + + mesh_data->BuildCells(); + mesh_data->BuildLinks(); + + vtkSmartPointer cleanPolyData = vtkSmartPointer::New(); + cleanPolyData->SetInputData(mesh_data); + cleanPolyData->Update(); + mesh_data = cleanPolyData->GetOutput(); + + pcl::PolygonMesh mesh_out; + pcl::VTKUtils::vtk2mesh(mesh_data, mesh_out); + return { mesh_out }; +} + +} // namespace noether diff --git a/noether_tpp/src/mesh_modifiers/euclidean_clustering_modifier.cpp b/noether_tpp/src/mesh_modifiers/euclidean_clustering_modifier.cpp new file mode 100644 index 00000000..f7add2c4 --- /dev/null +++ b/noether_tpp/src/mesh_modifiers/euclidean_clustering_modifier.cpp @@ -0,0 +1,60 @@ +/** + * @author Jorge Nicho + * @file euclidean_clustering.cpp + * @date Nov 26, 2019 + * @copyright Copyright (c) 2019, Southwest Research Institute + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +#include +#include + +#include +#include +#include +#include + +namespace noether +{ +std::vector EuclideanClustering::modify(const pcl::PolygonMesh& mesh_in) const +{ + // converting to point cloud + auto mesh_points = pcl::make_shared>(); + pcl::fromPCLPointCloud2(mesh_in.cloud, *mesh_points); + + // computing clusters + auto tree = pcl::make_shared>(); + tree->setInputCloud(mesh_points); + std::vector cluster_indices; + pcl::EuclideanClusterExtraction ec; + ec.setClusterTolerance(parameters_.tolerance); + ec.setMinClusterSize(parameters_.min_cluster_size); + ec.setMaxClusterSize(parameters_.max_cluster_size <= 0 ? mesh_points->size() : parameters_.max_cluster_size); + ec.setSearchMethod(tree); + ec.setInputCloud(mesh_points); + ec.extract(cluster_indices); + + if (cluster_indices.empty()) + throw std::runtime_error("No clusters found"); + + std::vector meshes; + meshes.reserve(cluster_indices.size()); + for (const pcl::PointIndices& cluster : cluster_indices) + meshes.push_back(extractSubMeshFromInlierVertices(mesh_in, cluster.indices)); + + return meshes; +} + +} // namespace noether diff --git a/noether_tpp/src/mesh_modifiers/fill_holes_modifier.cpp b/noether_tpp/src/mesh_modifiers/fill_holes_modifier.cpp new file mode 100644 index 00000000..2a745431 --- /dev/null +++ b/noether_tpp/src/mesh_modifiers/fill_holes_modifier.cpp @@ -0,0 +1,51 @@ +/** + * @author Jorge Nicho + * @file fill_holes.pp + * @date Dec 16, 2019 + * @copyright Copyright (c) 2019, Southwest Research Institute + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include + +#include +#include +#include + +namespace noether +{ +std::vector FillHoles::modify(const pcl::PolygonMesh& mesh_in) const +{ + vtkSmartPointer mesh_data = vtkSmartPointer::New(); + pcl::VTKUtils::mesh2vtk(mesh_in, mesh_data); + + vtkSmartPointer fill_holes_filter = vtkSmartPointer::New(); + fill_holes_filter->SetInputData(mesh_data); + fill_holes_filter->SetHoleSize(hole_size_); + fill_holes_filter->Update(); + + vtkSmartPointer normals_rectifier = vtkSmartPointer::New(); + normals_rectifier->SetInputData(fill_holes_filter->GetOutput()); + normals_rectifier->ConsistencyOn(); + normals_rectifier->SplittingOff(); + normals_rectifier->Update(); + + pcl::PolygonMesh mesh_out; + pcl::VTKUtils::vtk2mesh(normals_rectifier->GetOutput(), mesh_out); + return { mesh_out }; +} + +} // namespace noether diff --git a/noether_filtering/src/subset_extraction/extruded_polygon_subset_extractor.cpp b/noether_tpp/src/mesh_modifiers/subset_extraction/extruded_polygon_subset_extractor.cpp similarity index 97% rename from noether_filtering/src/subset_extraction/extruded_polygon_subset_extractor.cpp rename to noether_tpp/src/mesh_modifiers/subset_extraction/extruded_polygon_subset_extractor.cpp index 049fefec..65ea5ac8 100644 --- a/noether_filtering/src/subset_extraction/extruded_polygon_subset_extractor.cpp +++ b/noether_tpp/src/mesh_modifiers/subset_extraction/extruded_polygon_subset_extractor.cpp @@ -13,7 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. */ -#include +#include #include #include @@ -29,10 +29,10 @@ namespace { #if PCL_VERSION_COMPARE(<, 1, 10, 0) template -using shared_ptr = boost::shared_ptr; +using shared_ptr = std::shared_ptr; template -auto make_shared = boost::make_shared; +auto make_shared = std::make_shared; #else template using shared_ptr = pcl::shared_ptr; diff --git a/noether_filtering/src/subset_extraction/subset_extractor.cpp b/noether_tpp/src/mesh_modifiers/subset_extraction/subset_extractor.cpp similarity index 96% rename from noether_filtering/src/subset_extraction/subset_extractor.cpp rename to noether_tpp/src/mesh_modifiers/subset_extraction/subset_extractor.cpp index 95b21646..6eb7289e 100644 --- a/noether_filtering/src/subset_extraction/subset_extractor.cpp +++ b/noether_tpp/src/mesh_modifiers/subset_extraction/subset_extractor.cpp @@ -13,7 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. */ -#include +#include #include diff --git a/noether_filtering/cmake/test_scripts/check_pcl_nurbs.cpp b/noether_tpp/src/mesh_modifiers/test/check_pcl_nurbs.cpp similarity index 100% rename from noether_filtering/cmake/test_scripts/check_pcl_nurbs.cpp rename to noether_tpp/src/mesh_modifiers/test/check_pcl_nurbs.cpp diff --git a/noether_tpp/src/mesh_modifiers/windowed_sinc_smoothing.cpp b/noether_tpp/src/mesh_modifiers/windowed_sinc_smoothing.cpp new file mode 100644 index 00000000..e4685afd --- /dev/null +++ b/noether_tpp/src/mesh_modifiers/windowed_sinc_smoothing.cpp @@ -0,0 +1,40 @@ +/* + * window_sinc_smoothing.cpp + * + * Created on: Dec 6, 2019 + * Author: jrgnicho + */ + +#include + +#include +#include + +namespace noether +{ +std::vector WindowedSincSmoothing::modify(const pcl::PolygonMesh& mesh_in) const +{ + vtkSmartPointer mesh_data = vtkSmartPointer::New(); + pcl::VTKUtils::mesh2vtk(mesh_in, mesh_data); + + mesh_data->BuildCells(); + mesh_data->BuildLinks(); + + vtkSmartPointer smoother = vtkSmartPointer::New(); + smoother->SetInputData(mesh_data); + smoother->SetNumberOfIterations(config_.num_iter); + smoother->SetFeatureAngle(config_.feature_angle); + smoother->SetEdgeAngle(config_.edge_angle); + smoother->SetPassBand(config_.pass_band); + smoother->SetBoundarySmoothing(config_.enable_boundary_smoothing); + smoother->SetFeatureEdgeSmoothing(config_.enable_feature_edge_smoothing); + smoother->SetNonManifoldSmoothing(config_.enable_non_manifold_smoothing); + smoother->SetNormalizeCoordinates(config_.enable_normalize_coordinates); + smoother->Update(); + + pcl::PolygonMesh mesh_out; + pcl::VTKUtils::vtk2mesh(smoother->GetOutput(), mesh_out); + return { mesh_out }; +} + +} // namespace noether diff --git a/noether_tpp/src/tool_path_planners/raster/direction_generators/principal_axis_direction_generator.cpp b/noether_tpp/src/tool_path_planners/raster/direction_generators/principal_axis_direction_generator.cpp index 4550f194..eca49924 100644 --- a/noether_tpp/src/tool_path_planners/raster/direction_generators/principal_axis_direction_generator.cpp +++ b/noether_tpp/src/tool_path_planners/raster/direction_generators/principal_axis_direction_generator.cpp @@ -12,7 +12,7 @@ PrincipalAxisDirectionGenerator::PrincipalAxisDirectionGenerator(double rotation Eigen::Vector3d PrincipalAxisDirectionGenerator::generate(const pcl::PolygonMesh& mesh) const { - auto vertices = boost::make_shared>(); + auto vertices = pcl::make_shared>(); pcl::fromPCLPointCloud2(mesh.cloud, *vertices); pcl::PCA pca; pca.setInputCloud(vertices); diff --git a/noether_tpp/src/tool_path_planners/raster/plane_slicer_raster_planner.cpp b/noether_tpp/src/tool_path_planners/raster/plane_slicer_raster_planner.cpp index f2647803..f129de45 100644 --- a/noether_tpp/src/tool_path_planners/raster/plane_slicer_raster_planner.cpp +++ b/noether_tpp/src/tool_path_planners/raster/plane_slicer_raster_planner.cpp @@ -27,6 +27,9 @@ #include #include #include +#ifndef VTK_MAJOR_VERSION +#include +#endif namespace { @@ -537,7 +540,11 @@ ToolPaths PlaneSlicerRasterPlanner::planImpl(const pcl::PolygonMesh& mesh) const // collecting raster segments based on min hole size vtkSmartPointer raster_lines = raster_data->GetInput(i); +#if VTK_MAJOR_VERSION > 7 + const vtkIdType* indices; +#else vtkIdType* indices; +#endif vtkIdType num_points; vtkIdType num_lines = raster_lines->GetNumberOfLines(); vtkCellArray* cells = raster_lines->GetLines();