diff --git a/ci b/ci new file mode 120000 index 00000000..ecd623e5 --- /dev/null +++ b/ci @@ -0,0 +1 @@ +.github/workflows/ \ No newline at end of file diff --git a/noether_tpp/CMakeLists.txt b/noether_tpp/CMakeLists.txt index 51dabbbf..f81c8584 100644 --- a/noether_tpp/CMakeLists.txt +++ b/noether_tpp/CMakeLists.txt @@ -9,6 +9,14 @@ project(${pkg_extracted_name} VERSION ${pkg_extracted_version} LANGUAGES CXX) find_package(Eigen3 REQUIRED) find_package(PCL REQUIRED COMPONENTS common io) +find_package(VTK REQUIRED NO_MODULE) +if(VTK_FOUND AND ("${VTK_VERSION}" VERSION_LESS 7.1)) + message(FATAL_ERROR "The minimum required version of VTK is 7.1, but found ${VTK_VERSION}") + set(VTK_FOUND FALSE) +else() + include(${VTK_USE_FILE}) +endif() + # Create targets if necessary if(NOT TARGET Eigen3::Eigen) find_package(Threads REQUIRED) @@ -29,6 +37,7 @@ add_library(${PROJECT_NAME} SHARED # Tool Path Planners src/tool_path_planners/edge_planner.cpp src/tool_path_planners/raster_planner.cpp + src/tool_path_planners/plane_slicer_raster_planner.cpp src/tool_path_planners/direction_generators.cpp src/tool_path_planners/origin_generators.cpp ) @@ -36,7 +45,7 @@ target_include_directories(${PROJECT_NAME} PUBLIC "$" "$") target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC ${PCL_INCLUDE_DIRS}) -target_link_libraries(${PROJECT_NAME} PUBLIC Eigen3::Eigen ${PCL_LIBRARIES}) +target_link_libraries(${PROJECT_NAME} PUBLIC Eigen3::Eigen ${PCL_LIBRARIES} ${VTK_LIBRARIES}) target_cxx_version(${PROJECT_NAME} PUBLIC VERSION 14) target_clang_tidy(${PROJECT_NAME} ENABLE ${NOETHER_ENABLE_CLANG_TIDY} @@ -55,4 +64,4 @@ if(${NOETHER_ENABLE_TESTING}) endif() # Package configuration -configure_package(NAMESPACE noether TARGETS ${PROJECT_NAME}) +configure_package(NAMESPACE noether DEPENDENCIES Eigen3 "PCL REQUIRED COMPONENTS common io" VTK TARGETS ${PROJECT_NAME}) diff --git a/noether_tpp/include/noether_tpp/tool_path_planners/raster/plane_slicer_raster_planner.h b/noether_tpp/include/noether_tpp/tool_path_planners/raster/plane_slicer_raster_planner.h new file mode 100644 index 00000000..90049af4 --- /dev/null +++ b/noether_tpp/include/noether_tpp/tool_path_planners/raster/plane_slicer_raster_planner.h @@ -0,0 +1,52 @@ +/** + * @file plane_slicer_raster_planner.h + * @copyright Copyright (c) 2021, 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 + +namespace noether +{ +/** + * @brief An implementation of the Raster Planner using a series of parallel cutting planes. + * This implementation works best on approximately planar parts. + */ +class PlaneSlicerRasterPlanner : public RasterPlanner +{ +public: + PlaneSlicerRasterPlanner(DirectionGenerator::ConstPtr dir_gen, OriginGenerator::ConstPtr origin_gen); + + void setSearchRadius(const double search_radius); + void setMinSegmentSize(const double min_segment_size); + +protected: + /** + * @brief Implementation of the tool path planning capability + * @param mesh + * @return + */ + ToolPaths planImpl(const pcl::PolygonMesh& mesh) const; + +private: + /** @brief Minimum length of valid segment (m) */ + double min_segment_size_; + /** @brief Search radius for calculating normals (m) */ + double search_radius_; +}; + +} // namespace noether diff --git a/noether_tpp/include/noether_tpp/tool_path_planners/raster/raster_planner.h b/noether_tpp/include/noether_tpp/tool_path_planners/raster/raster_planner.h index 19ec24bb..b3b6ee6a 100644 --- a/noether_tpp/include/noether_tpp/tool_path_planners/raster/raster_planner.h +++ b/noether_tpp/include/noether_tpp/tool_path_planners/raster/raster_planner.h @@ -78,7 +78,6 @@ class RasterPlanner : public ToolPathPlanner DirectionGenerator::ConstPtr dir_gen_; OriginGenerator::ConstPtr origin_gen_; -private: /** @brief Distance between waypoints on the same raster line (m) */ double point_spacing_; /** @brief Distance between raster lines */ diff --git a/noether_tpp/src/tool_path_planners/plane_slicer_raster_planner.cpp b/noether_tpp/src/tool_path_planners/plane_slicer_raster_planner.cpp new file mode 100644 index 00000000..01b4be66 --- /dev/null +++ b/noether_tpp/src/tool_path_planners/plane_slicer_raster_planner.cpp @@ -0,0 +1,638 @@ +#include + +#include // std::find(), std::reverse(), std::unique() +#include // std::iota() +#include // std::runtime_error +#include // std::to_string() +#include // std::move() +#include // std::vector + +#include // pcl::getMinMax3d() +#include // pcl::PCA +#include // pcl::fromPCLPointCloud2 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace +{ +static const double EPSILON = 1e-6; + +struct RasterConstructData +{ + std::vector> raster_segments; + std::vector segment_lengths; +}; + +Eigen::Matrix3d computeRotation(const Eigen::Vector3d& vx, const Eigen::Vector3d& vy, const Eigen::Vector3d& vz) +{ + Eigen::Matrix3d m; + m.setIdentity(); + m.col(0) = vx.normalized(); + m.col(1) = vy.normalized(); + m.col(2) = vz.normalized(); + return m; +} + +double computeLength(const vtkSmartPointer& points) +{ + const vtkIdType num_points = points->GetNumberOfPoints(); + double total_length = 0.0; + if (num_points < 2) + { + return total_length; + } + + Eigen::Vector3d p0, pf; + for (vtkIdType i = 1; i < num_points; i++) + { + points->GetPoint(i - 1, p0.data()); + points->GetPoint(i, pf.data()); + + total_length += (pf - p0).norm(); + } + return total_length; +} + +vtkSmartPointer applyParametricSpline(const vtkSmartPointer& points, + double total_length, + double point_spacing) +{ + vtkSmartPointer new_points = vtkSmartPointer::New(); + + // create spline + vtkSmartPointer spline = vtkSmartPointer::New(); + spline->SetPoints(points); + spline->SetParameterizeByLength(true); + spline->ClosedOff(); + + // adding first point + Eigen::Vector3d pt_prev; + points->GetPoint(0, pt_prev.data()); + new_points->InsertNextPoint(pt_prev.data()); + + // adding remaining points by evaluating spline + std::size_t num_points = static_cast(std::ceil(total_length / point_spacing) + 1); + double du[9]; + Eigen::Vector3d u, pt; + for (unsigned i = 1; i < num_points; i++) + { + double interv = static_cast(i) / static_cast(num_points - 1); + interv = interv > 1.0 ? 1.0 : interv; + if (std::abs(interv - 1.0) < EPSILON) + { + break; // reach end + } + + u = interv * Eigen::Vector3d::Ones(); + std::tie(u[0], u[1], u[2]) = std::make_tuple(interv, interv, interv); + spline->Evaluate(u.data(), pt.data(), du); + + // check distance + if ((pt - pt_prev).norm() >= point_spacing) + { + new_points->InsertNextPoint(pt.data()); + pt_prev = pt; + } + } + + // add last point + points->GetPoint(points->GetNumberOfPoints() - 1, pt_prev.data()); + new_points->InsertNextPoint(pt_prev.data()); + + return new_points; +} + +/** + * @brief removes points that appear in multiple lists such that only one instance of that point + * index remains + * @param points_lists + */ +void removeRedundant(std::vector>& points_lists) +{ + using IdList = std::vector; + if (points_lists.size() < 2) + { + return; + } + + std::vector> new_points_lists; + new_points_lists.push_back(points_lists.front()); + for (std::size_t i = 1; i < points_lists.size(); i++) + { + IdList& current_list = points_lists[i]; + IdList new_list; + IdList all_ids; + + // create list of all ids + for (auto& ref_list : new_points_lists) + { + all_ids.insert(all_ids.end(), ref_list.begin(), ref_list.end()); + } + + for (auto& id : current_list) + { + // add if not found in any of the previous lists + if (std::find(all_ids.begin(), all_ids.end(), id) == all_ids.end()) + { + new_list.push_back(id); + } + } + + // add if it has enough points + if (new_list.size() > 0) + { + new_points_lists.push_back(new_list); + } + } + + points_lists.clear(); + points_lists.assign(new_points_lists.begin(), new_points_lists.end()); +} + +void mergeRasterSegments(const vtkSmartPointer& points, + double merge_dist, + std::vector>& points_lists) +{ + using namespace Eigen; + using IdList = std::vector; + if (points_lists.size() < 2) + { + return; + } + + std::vector new_points_lists; + IdList merged_list_ids; + IdList merged_list; + + auto do_merge = + [&points](const IdList& current_list, const IdList& next_list, double merge_dist, IdList& merged_list) { + Vector3d cl_point, nl_point; + + // checking front and back end points respectively + points->GetPoint(current_list.front(), cl_point.data()); + points->GetPoint(next_list.back(), nl_point.data()); + double d = (cl_point - nl_point).norm(); + if (d < merge_dist) + { + merged_list.assign(next_list.begin(), next_list.end()); + merged_list.insert(merged_list.end(), current_list.begin(), current_list.end()); + return true; + } + + // checking back and front end points respectively + points->GetPoint(current_list.back(), cl_point.data()); + points->GetPoint(next_list.front(), nl_point.data()); + d = (cl_point - nl_point).norm(); + if (d < merge_dist) + { + merged_list.assign(current_list.begin(), current_list.end()); + merged_list.insert(merged_list.end(), next_list.begin(), next_list.end()); + return true; + } + return false; + }; + + for (std::size_t i = 0; i < points_lists.size(); i++) + { + if (std::find(merged_list_ids.begin(), merged_list_ids.end(), i) != merged_list_ids.end()) + { + // already merged + continue; + } + + IdList current_list = points_lists[i]; + Vector3d cl_point, nl_point; + bool seek_adjacent = true; + while (seek_adjacent) + { + seek_adjacent = false; + for (std::size_t j = i + 1; j < points_lists.size(); j++) + { + if (std::find(merged_list_ids.begin(), merged_list_ids.end(), j) != merged_list_ids.end()) + { + // already merged + continue; + } + + merged_list.clear(); + IdList next_list = points_lists[j]; + if (do_merge(current_list, next_list, merge_dist, merged_list)) + { + current_list = merged_list; + merged_list_ids.push_back(static_cast(j)); + seek_adjacent = true; + continue; + } + + std::reverse(next_list.begin(), next_list.end()); + if (do_merge(current_list, next_list, merge_dist, merged_list)) + { + current_list = merged_list; + merged_list_ids.push_back(static_cast(j)); + seek_adjacent = true; + continue; + } + } + } + new_points_lists.push_back(current_list); + } + points_lists.clear(); + std::copy_if(new_points_lists.begin(), new_points_lists.end(), std::back_inserter(points_lists), [](const IdList& l) { + return l.size() > 1; + }); +} + +void rectifyDirection(const vtkSmartPointer& points, + const Eigen::Vector3d& ref_point, + std::vector>& points_lists) +{ + Eigen::Vector3d p0, pf; + if (points_lists.size() < 2) + return; + + // getting first and last points + points->GetPoint(points_lists.front().front(), p0.data()); + points->GetPoint(points_lists.back().back(), pf.data()); + + bool reverse = (ref_point - p0).norm() > (ref_point - pf).norm(); + if (reverse) + { + for (auto& s : points_lists) + { + std::reverse(s.begin(), s.end()); + } + std::reverse(points_lists.begin(), points_lists.end()); + } +} + +noether::ToolPaths convertToPoses(const std::vector& rasters_data) +{ + noether::ToolPaths rasters_array; + bool reverse = true; + for (const RasterConstructData& rd : rasters_data) + { + reverse = !reverse; + noether::ToolPath raster_path; + std::vector> raster_segments; + raster_segments.assign(rd.raster_segments.begin(), rd.raster_segments.end()); + if (reverse) + { + std::reverse(raster_segments.begin(), raster_segments.end()); + } + + for (const vtkSmartPointer& polydata : raster_segments) + { + noether::ToolPathSegment raster_path_segment; + std::size_t num_points = polydata->GetNumberOfPoints(); + Eigen::Vector3d p, p_next, vx, vy, vz; + Eigen::Isometry3d pose; + std::vector indices(num_points); + std::iota(indices.begin(), indices.end(), 0); + if (reverse) + { + std::reverse(indices.begin(), indices.end()); + } + for (std::size_t i = 0; i < indices.size() - 1; i++) + { + int idx = indices[i]; + int idx_next = indices[i + 1]; + polydata->GetPoint(idx, p.data()); + polydata->GetPoint(idx_next, p_next.data()); + polydata->GetPointData()->GetNormals()->GetTuple(idx, vz.data()); + vx = (p_next - p).normalized(); + vy = vz.cross(vx).normalized(); + vz = vx.cross(vy).normalized(); + pose = Eigen::Translation3d(p) * Eigen::AngleAxisd(computeRotation(vx, vy, vz)); + raster_path_segment.push_back(pose); + } + + // adding last pose + pose.translation() = p_next; // orientation stays the same as previous + raster_path_segment.push_back(pose); + + raster_path.push_back(raster_path_segment); + } + + if (!raster_path.empty()) + rasters_array.push_back(raster_path); + } + + return rasters_array; +} + +bool insertNormals(const double search_radius, + vtkSmartPointer& mesh_data_, + vtkSmartPointer& kd_tree_, + vtkSmartPointer& data) +{ + // Find closest cell to each point and uses its normal vector + vtkSmartPointer new_normals = vtkSmartPointer::New(); + new_normals->SetNumberOfComponents(3); + new_normals->SetNumberOfTuples(data->GetPoints()->GetNumberOfPoints()); + + // get normal data + vtkSmartPointer normal_data = mesh_data_->GetPointData()->GetNormals(); + + if (!normal_data) + { + return false; + } + + Eigen::Vector3d normal_vect = Eigen::Vector3d::UnitZ(); + for (int i = 0; i < data->GetPoints()->GetNumberOfPoints(); ++i) + { + // locate closest cell + Eigen::Vector3d query_point; + vtkSmartPointer id_list = vtkSmartPointer::New(); + data->GetPoints()->GetPoint(i, query_point.data()); + kd_tree_->FindPointsWithinRadius(search_radius, query_point.data(), id_list); + if (id_list->GetNumberOfIds() < 1) + { + kd_tree_->FindClosestNPoints(1, query_point.data(), id_list); + + if (id_list->GetNumberOfIds() < 1) + { + return false; + } + } + + // compute normal average + normal_vect = Eigen::Vector3d::Zero(); + std::size_t num_normals = 0; + for (auto p = 0; p < id_list->GetNumberOfIds(); p++) + { + Eigen::Vector3d temp_normal, query_point, closest_point; + vtkIdType p_id = id_list->GetId(p); + + if (p_id < 0) + { + continue; + } + + // get normal and add it to average + normal_data->GetTuple(p_id, temp_normal.data()); + normal_vect += temp_normal.normalized(); + num_normals++; + } + + normal_vect /= num_normals; + normal_vect.normalize(); + + // save normal + new_normals->SetTuple3(i, normal_vect(0), normal_vect(1), normal_vect(2)); + } + data->GetPointData()->SetNormals(new_normals); + return true; +} +} // namespace + +namespace noether +{ +PlaneSlicerRasterPlanner::PlaneSlicerRasterPlanner(DirectionGenerator::ConstPtr dir_gen, + OriginGenerator::ConstPtr origin_gen) + : RasterPlanner(std::move(dir_gen), std::move(origin_gen)) +{ +} + +void PlaneSlicerRasterPlanner::setMinSegmentSize(const double min_segment_size) +{ + min_segment_size_ = min_segment_size; +} +void PlaneSlicerRasterPlanner::setSearchRadius(const double search_radius) { search_radius_ = search_radius; } + +ToolPaths PlaneSlicerRasterPlanner::planImpl(const pcl::PolygonMesh& mesh) const +{ + // Convert input mesh to VTK type & calculate normals if necessary + vtkSmartPointer mesh_data_ = vtkSmartPointer::New(); + pcl::VTKUtils::mesh2vtk(mesh, mesh_data_); + mesh_data_->BuildLinks(); + mesh_data_->BuildCells(); + if (!mesh_data_->GetPointData()->GetNormals() || !mesh_data_->GetCellData()->GetNormals()) + { + vtkSmartPointer normal_generator = vtkSmartPointer::New(); + normal_generator->SetInputData(mesh_data_); + normal_generator->ComputePointNormalsOn(); + normal_generator->SetComputeCellNormals(!mesh_data_->GetCellData()->GetNormals()); + normal_generator->SetFeatureAngle(M_PI_2); + normal_generator->SetSplitting(true); + normal_generator->SetConsistency(true); + normal_generator->SetAutoOrientNormals(false); + normal_generator->SetFlipNormals(false); + normal_generator->SetNonManifoldTraversal(false); + normal_generator->Update(); + + if (!mesh_data_->GetPointData()->GetNormals()) + { + mesh_data_->GetPointData()->SetNormals(normal_generator->GetOutput()->GetPointData()->GetNormals()); + } + + if (!mesh_data_->GetCellData()->GetNormals()) + { + mesh_data_->GetCellData()->SetNormals(normal_generator->GetOutput()->GetCellData()->GetNormals()); + } + } + + // Use principal component analysis (PCA) to determine the principal axes of the mesh + Eigen::Matrix3d pca_vecs; // Principal axes, scaled to the size of the mesh in each direction + Eigen::Vector3d centroid; // Mesh centroid + { + // Use Original PCL point cloud + pcl::PointCloud::Ptr cloud(new pcl::PointCloud()); + pcl::fromPCLPointCloud2(mesh.cloud, *cloud); + + // Perform PCA analysis + pcl::PCA pca; + pca.setInputCloud(cloud); + + // Get the extents of the point cloud relative to its mean and principal axes + pcl::PointCloud proj; + pca.project(*cloud, proj); + pcl::PointXYZ min, max; + pcl::getMinMax3D(proj, min, max); + Eigen::Array3f scales = max.getArray3fMap() - min.getArray3fMap(); + + centroid = pca.getMean().head<3>().cast(); + pca_vecs = (pca.getEigenVectors().array().rowwise() * scales.transpose()).cast(); + } + + // Get the initial cutting plane + Eigen::Vector3d cut_direction = dir_gen_->generate(mesh); + Eigen::Vector3d cut_normal = (cut_direction.normalized().cross(pca_vecs.col(2).normalized())).normalized(); + + // Calculate the number of planes needed to cover the mesh according to the length of the principle axes + double max_extent = + std::sqrt(pca_vecs.col(0).squaredNorm() + pca_vecs.col(1).squaredNorm() + pca_vecs.col(2).squaredNorm()); + std::size_t num_planes = static_cast(max_extent / line_spacing_); + + // Calculate the start location as the centroid less half the full diagonal of the bounding box + Eigen::Vector3d start_loc = centroid - (max_extent / 2.0) * cut_normal; + + // Generate each plane and collect the intersection points + vtkSmartPointer raster_data = vtkSmartPointer::New(); + for (std::size_t i = 0; i < num_planes + 1; i++) + { + vtkSmartPointer plane = vtkSmartPointer::New(); + vtkSmartPointer cutter = vtkSmartPointer::New(); + vtkSmartPointer stripper = vtkSmartPointer::New(); + + Eigen::Vector3d current_loc = start_loc + i * line_spacing_ * cut_normal; + plane->SetOrigin(current_loc.x(), current_loc.y(), current_loc.z()); + plane->SetNormal(cut_normal.x(), cut_normal.y(), cut_normal.z()); + + cutter->SetCutFunction(plane); + cutter->SetInputData(mesh_data_); + cutter->SetSortBy(1); + cutter->SetGenerateTriangles(false); + cutter->Update(); + + stripper->SetInputConnection(cutter->GetOutputPort()); + stripper->JoinContiguousSegmentsOn(); + stripper->SetMaximumLength(mesh_data_->GetNumberOfPoints()); + stripper->Update(); + + if (stripper->GetErrorCode() != vtkErrorCode::NoError) + { + continue; + } + + for (int r = 0; r < stripper->GetNumberOfOutputPorts(); r++) + { + raster_data->AddInputData(stripper->GetOutput(r)); + } + } + + // build cell locator and kd_tree to recover normals later on + vtkSmartPointer kd_tree_ = vtkSmartPointer::New(); + kd_tree_->SetDataSet(mesh_data_); + kd_tree_->BuildLocator(); + vtkSmartPointer cell_locator_ = vtkSmartPointer::New(); + cell_locator_->SetDataSet(mesh_data_); + cell_locator_->BuildLocator(); + + // collect rasters and set direction + raster_data->Update(); + vtkIdType num_slices = raster_data->GetTotalNumberOfInputConnections(); + std::vector rasters_data_vec; + std::vector> raster_ids; + for (std::size_t i = 0; i < num_slices; i++) + { + RasterConstructData r; + + // collecting raster segments based on min hole size + vtkSmartPointer raster_lines = raster_data->GetInput(i); + vtkIdType* indices; + vtkIdType num_points; + vtkIdType num_lines = raster_lines->GetNumberOfLines(); + vtkCellArray* cells = raster_lines->GetLines(); + + if (num_lines == 0) + { + continue; + } + + raster_ids.clear(); + + unsigned int lineCount = 0; + for (cells->InitTraversal(); cells->GetNextCell(num_points, indices); lineCount++) + { + std::vector point_ids; + for (vtkIdType i = 0; i < num_points; i++) + { + if (std::find(point_ids.begin(), point_ids.end(), indices[i]) != point_ids.end()) + { + continue; + } + point_ids.push_back(indices[i]); + } + if (point_ids.empty()) + { + continue; + } + + // removing duplicates + auto iter = std::unique(point_ids.begin(), point_ids.end()); + point_ids.erase(iter, point_ids.end()); + + // adding + raster_ids.push_back(point_ids); + } + + if (raster_ids.empty()) + { + continue; + } + + // remove redundant indices + removeRedundant(raster_ids); + + // merging segments + mergeRasterSegments(raster_lines->GetPoints(), min_hole_size_, raster_ids); + + // rectifying + if (rasters_data_vec.size() > 1) + { + Eigen::Vector3d ref_point; + rasters_data_vec.back().raster_segments.front()->GetPoint(0, ref_point.data()); // first point in previous raster + rectifyDirection(raster_lines->GetPoints(), ref_point, raster_ids); + } + + for (auto& rpoint_ids : raster_ids) + { + // Populating with points + vtkSmartPointer points = vtkSmartPointer::New(); + std::for_each(rpoint_ids.begin(), rpoint_ids.end(), [&points, &raster_lines](vtkIdType& id) { + std::array p; + raster_lines->GetPoint(id, p.data()); + points->InsertNextPoint(p.data()); + }); + + // compute length and add points if segment length is greater than threshold + double line_length = computeLength(points); + if (line_length > min_segment_size_ && points->GetNumberOfPoints() > 1) + { + // enforce point spacing + vtkSmartPointer new_points = applyParametricSpline(points, line_length, point_spacing_); + + // add points to segment now + vtkSmartPointer segment_data = vtkSmartPointer::New(); + segment_data->SetPoints(new_points); + + // inserting normals + if (!insertNormals(search_radius_, mesh_data_, kd_tree_, segment_data)) + { + throw std::runtime_error("Could not insert normals for segment " + std::to_string(r.raster_segments.size()) + + " of raster " + std::to_string(i)); + } + + // saving into raster + r.raster_segments.push_back(segment_data); + r.segment_lengths.push_back(line_length); + } + } + + // Save raster + if (!r.raster_segments.empty()) + rasters_data_vec.push_back(r); + } + + // converting to poses msg now + ToolPaths tool_paths = convertToPoses(rasters_data_vec); + return tool_paths; +} + +} // namespace noether diff --git a/tool_path_planner/CMakeLists.txt b/tool_path_planner/CMakeLists.txt index ca38f2c5..b9a2e36e 100644 --- a/tool_path_planner/CMakeLists.txt +++ b/tool_path_planner/CMakeLists.txt @@ -19,6 +19,7 @@ find_package(catkin REQUIRED COMPONENTS find_package(jsoncpp REQUIRED) find_package(vtk_viewer REQUIRED) +find_package(noether_tpp REQUIRED) find_package(VTK REQUIRED NO_MODULE) if(VTK_FOUND AND ("${VTK_VERSION}" VERSION_LESS 7.1)) @@ -35,7 +36,14 @@ if(PCL_FOUND AND ("${PCL_VERSION}" VERSION_LESS 1.9)) else() add_definitions(${PCL_DEFINITIONS}) endif() + find_package(Eigen3 REQUIRED) +if(NOT TARGET Eigen3::Eigen) + find_package(Threads REQUIRED) + add_library(Eigen3::Eigen IMPORTED INTERFACE) + set_property(TARGET Eigen3::Eigen PROPERTY INTERFACE_COMPILE_DEFINITIONS ${EIGEN3_DEFINITIONS}) + set_property(TARGET Eigen3::Eigen PROPERTY INTERFACE_INCLUDE_DIRECTORIES ${EIGEN3_INCLUDE_DIRS}) +endif() catkin_package( INCLUDE_DIRS @@ -75,6 +83,7 @@ target_link_libraries(${PROJECT_NAME} ${PCL_LIBRARIES} ${VTK_LIBRARIES} noether::vtk_viewer + noether::noether_tpp jsoncpp_lib ) add_dependencies(${PROJECT_NAME} diff --git a/tool_path_planner/include/tool_path_planner/plane_slicer_raster_generator.h b/tool_path_planner/include/tool_path_planner/plane_slicer_raster_generator.h index 14e22573..6c2a537c 100644 --- a/tool_path_planner/include/tool_path_planner/plane_slicer_raster_generator.h +++ b/tool_path_planner/include/tool_path_planner/plane_slicer_raster_generator.h @@ -22,22 +22,16 @@ #ifndef INCLUDE_PLANE_SLICER_RASTER_GENERATOR_H_ #define INCLUDE_PLANE_SLICER_RASTER_GENERATOR_H_ -#include -#include -#include +#include + #include -#include -#include -#include -#include #include -#include - -#include +#include namespace tool_path_planner { -class PlaneSlicerRasterGenerator : public PathGenerator +class [[deprecated("Replaced by PlaneSliceRasterPlanner in noether_tpp")]] PlaneSlicerRasterGenerator + : public PathGenerator { static constexpr double DEFAULT_RASTER_SPACING = 0.04; static constexpr double DEFAULT_POINT_SPACING = 0.01; @@ -56,73 +50,10 @@ class PlaneSlicerRasterGenerator : public PathGenerator double search_radius{ DEFAULT_SEARCH_RADIUS }; double min_hole_size{ DEFAULT_MIN_HOLE_SIZE }; - Json::Value toJson() const - { - Json::Value jv(Json::ValueType::objectValue); - jv["raster_spacing"] = raster_spacing; - jv["point_spacing"] = point_spacing; - jv["raster_rot_offset"] = raster_rot_offset; - jv["min_segment_size"] = min_segment_size; - jv["search_radius"] = search_radius; - jv["min_hole_size"] = min_hole_size; - - return jv; - } - - bool fromJson(const Json::Value& jv) - { - if (jv.isNull()) - { - ROS_ERROR("Json value is null"); - return false; - } - if (jv.type() != Json::ValueType::objectValue) - { - ROS_ERROR("Json type %i is invalid, only '%i' is allowed", - static_cast(jv.type()), - static_cast(Json::ValueType::objectValue)); - return false; - } - auto validate = [](const Json::Value& jv, const std::string& name_, const Json::ValueType& type_) -> bool { - return jv.isMember(name_) && jv[name_].type() == type_; - }; - raster_spacing = validate(jv, "raster_spacing", Json::ValueType::realValue) ? jv["raster_spacing"].asDouble() : - DEFAULT_RASTER_SPACING; - point_spacing = validate(jv, "point_spacing", Json::ValueType::realValue) ? jv["point_spacing"].asDouble() : - DEFAULT_POINT_SPACING; - raster_rot_offset = validate(jv, "raster_rot_offset", Json::ValueType::realValue) ? - jv["raster_rot_offset"].asDouble() : - DEFAULT_RASTER_ROT_OFFSET; - min_segment_size = validate(jv, "min_segment_size", Json::ValueType::realValue) ? - jv["min_segment_size"].asDouble() : - DEFAULT_MIN_SEGMENT_SIZE; - search_radius = validate(jv, "search_radius", Json::ValueType::realValue) ? jv["search_radius"].asDouble() : - DEFAULT_SEARCH_RADIUS; - min_hole_size = validate(jv, "min_hole_size", Json::ValueType::realValue) ? jv["min_hole_size"].asDouble() : - DEFAULT_MIN_HOLE_SIZE; - return true; - } - - bool fromJson(const std::string& jv_string) - { - Json::Value jv; - Json::Reader r; - if (!r.parse(jv_string, jv)) - return false; - - return fromJson(jv); - } - - std::string str() - { - std::stringstream ss; - ss << "raster_spacing: " << raster_spacing << std::endl; - ss << "point_spacing: " << point_spacing << std::endl; - ss << "raster_rot_offset: " << raster_rot_offset << std::endl; - ss << "min_segment_size: " << min_segment_size << std::endl; - ss << "search_radius: " << search_radius << std::endl; - return ss.str(); - } + Json::Value toJson() const; + bool fromJson(const Json::Value& jv); + bool fromJson(const std::string& jv_string); + std::string str(); }; PlaneSlicerRasterGenerator() = default; @@ -147,11 +78,7 @@ class PlaneSlicerRasterGenerator : public PathGenerator std::string getName() const override; private: - bool insertNormals(const double search_radius, vtkSmartPointer& data); - - vtkSmartPointer mesh_data_; - vtkSmartPointer kd_tree_; - vtkSmartPointer cell_locator_; + pcl::PolygonMesh mesh_data_; Config config_; }; diff --git a/tool_path_planner/package.xml b/tool_path_planner/package.xml index 310f39ef..0ecaf9e2 100644 --- a/tool_path_planner/package.xml +++ b/tool_path_planner/package.xml @@ -13,6 +13,7 @@ eigen_conversions eigen_stl_containers noether_conversions + noether_tpp geometry_msgs noether_msgs rosconsole diff --git a/tool_path_planner/src/plane_slicer_raster_generator.cpp b/tool_path_planner/src/plane_slicer_raster_generator.cpp index ab5583bb..c16694fa 100644 --- a/tool_path_planner/src/plane_slicer_raster_generator.cpp +++ b/tool_path_planner/src/plane_slicer_raster_generator.cpp @@ -19,389 +19,110 @@ * limitations under the License. */ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include #include -#include -#include -#include -#include -#include -#include #include #include -#include -#include - -static const double EPSILON = 1e-6; - -using PolyDataPtr = vtkSmartPointer; -struct RasterConstructData -{ - std::vector raster_segments; - std::vector segment_lengths; -}; - -static Eigen::Matrix3d computeRotation(const Eigen::Vector3d& vx, const Eigen::Vector3d& vy, const Eigen::Vector3d& vz) -{ - Eigen::Matrix3d m; - m.setIdentity(); - m.col(0) = vx.normalized(); - m.col(1) = vy.normalized(); - m.col(2) = vz.normalized(); - return m; -} - -double computeLength(const vtkSmartPointer& points) -{ - const vtkIdType num_points = points->GetNumberOfPoints(); - double total_length = 0.0; - if (num_points < 2) - { - return total_length; - } - - Eigen::Vector3d p0, pf; - for (vtkIdType i = 1; i < num_points; i++) - { - points->GetPoint(i - 1, p0.data()); - points->GetPoint(i, pf.data()); - - total_length += (pf - p0).norm(); - } - return total_length; -} +#include +#include +#include +#include +#include +#include +#include +#include +#include -static vtkSmartPointer applyParametricSpline(const vtkSmartPointer& points, - double total_length, - double point_spacing) +namespace tool_path_planner { - vtkSmartPointer new_points = vtkSmartPointer::New(); - - // create spline - vtkSmartPointer spline = vtkSmartPointer::New(); - spline->SetPoints(points); - spline->SetParameterizeByLength(true); - spline->ClosedOff(); - - // adding first point - Eigen::Vector3d pt_prev; - points->GetPoint(0, pt_prev.data()); - new_points->InsertNextPoint(pt_prev.data()); - - // adding remaining points by evaluating spline - std::size_t num_points = static_cast(std::ceil(total_length / point_spacing) + 1); - double du[9]; - Eigen::Vector3d u, pt; - for (unsigned i = 1; i < num_points; i++) - { - double interv = static_cast(i) / static_cast(num_points - 1); - interv = interv > 1.0 ? 1.0 : interv; - if (std::abs(interv - 1.0) < EPSILON) - { - break; // reach end - } - - u = interv * Eigen::Vector3d::Ones(); - std::tie(u[0], u[1], u[2]) = std::make_tuple(interv, interv, interv); - spline->Evaluate(u.data(), pt.data(), du); - - // check distance - if ((pt - pt_prev).norm() >= point_spacing) - { - new_points->InsertNextPoint(pt.data()); - pt_prev = pt; - } - } - - // add last point - points->GetPoint(points->GetNumberOfPoints() - 1, pt_prev.data()); - new_points->InsertNextPoint(pt_prev.data()); - - return new_points; -} - -/** - * @brief removes points that appear in multiple lists such that only one instance of that point - * index remains - * @param points_lists - */ -static void removeRedundant(std::vector>& points_lists) +Json::Value PlaneSlicerRasterGenerator::Config::toJson() const { - using IdList = std::vector; - if (points_lists.size() < 2) - { - return; - } - - std::vector> new_points_lists; - new_points_lists.push_back(points_lists.front()); - for (std::size_t i = 1; i < points_lists.size(); i++) - { - IdList& current_list = points_lists[i]; - IdList new_list; - IdList all_ids; - - // create list of all ids - for (auto& ref_list : new_points_lists) - { - all_ids.insert(all_ids.end(), ref_list.begin(), ref_list.end()); - } - - for (auto& id : current_list) - { - // add if not found in any of the previous lists - if (std::find(all_ids.begin(), all_ids.end(), id) == all_ids.end()) - { - new_list.push_back(id); - } - } - - // add if it has enough points - if (new_list.size() > 0) - { - new_points_lists.push_back(new_list); - } - } - - points_lists.clear(); - points_lists.assign(new_points_lists.begin(), new_points_lists.end()); + Json::Value jv(Json::ValueType::objectValue); + jv["raster_spacing"] = raster_spacing; + jv["point_spacing"] = point_spacing; + jv["raster_rot_offset"] = raster_rot_offset; + jv["min_segment_size"] = min_segment_size; + jv["search_radius"] = search_radius; + jv["min_hole_size"] = min_hole_size; + + return jv; } -static void mergeRasterSegments(const vtkSmartPointer& points, - double merge_dist, - std::vector>& points_lists) +bool PlaneSlicerRasterGenerator::Config::fromJson(const Json::Value& jv) { - using namespace Eigen; - using IdList = std::vector; - if (points_lists.size() < 2) + if (jv.isNull()) { - return; + CONSOLE_BRIDGE_logError("Json value is null"); + return false; } - - std::vector new_points_lists; - IdList merged_list_ids; - IdList merged_list; - - auto do_merge = - [&points](const IdList& current_list, const IdList& next_list, double merge_dist, IdList& merged_list) { - Vector3d cl_point, nl_point; - - // checking front and back end points respectively - points->GetPoint(current_list.front(), cl_point.data()); - points->GetPoint(next_list.back(), nl_point.data()); - double d = (cl_point - nl_point).norm(); - if (d < merge_dist) - { - merged_list.assign(next_list.begin(), next_list.end()); - merged_list.insert(merged_list.end(), current_list.begin(), current_list.end()); - return true; - } - - // checking back and front end points respectively - points->GetPoint(current_list.back(), cl_point.data()); - points->GetPoint(next_list.front(), nl_point.data()); - d = (cl_point - nl_point).norm(); - if (d < merge_dist) - { - merged_list.assign(current_list.begin(), current_list.end()); - merged_list.insert(merged_list.end(), next_list.begin(), next_list.end()); - return true; - } - return false; - }; - - for (std::size_t i = 0; i < points_lists.size(); i++) + if (jv.type() != Json::ValueType::objectValue) { - if (std::find(merged_list_ids.begin(), merged_list_ids.end(), i) != merged_list_ids.end()) - { - // already merged - CONSOLE_BRIDGE_logDebug("Segment %i has already been merged, skipping", i); - continue; - } - - IdList current_list = points_lists[i]; - Vector3d cl_point, nl_point; - bool seek_adjacent = true; - while (seek_adjacent) - { - seek_adjacent = false; - for (std::size_t j = i + 1; j < points_lists.size(); j++) - { - if (std::find(merged_list_ids.begin(), merged_list_ids.end(), j) != merged_list_ids.end()) - { - // already merged - CONSOLE_BRIDGE_logDebug("Segment %i has already been merged, skipping", j); - continue; - } - - merged_list.clear(); - IdList next_list = points_lists[j]; - if (do_merge(current_list, next_list, merge_dist, merged_list)) - { - CONSOLE_BRIDGE_logDebug("Merged segment %lu onto segment %lu", j, i); - current_list = merged_list; - merged_list_ids.push_back(static_cast(j)); - seek_adjacent = true; - continue; - } - - std::reverse(next_list.begin(), next_list.end()); - if (do_merge(current_list, next_list, merge_dist, merged_list)) - { - CONSOLE_BRIDGE_logDebug("Merged segment %lu onto segment %lu", j, i); - current_list = merged_list; - merged_list_ids.push_back(static_cast(j)); - seek_adjacent = true; - continue; - } - } - } - new_points_lists.push_back(current_list); + CONSOLE_BRIDGE_logError("Json type %i is invalid, only '%i' is allowed", + static_cast(jv.type()), + static_cast(Json::ValueType::objectValue)); + return false; } - points_lists.clear(); - std::copy_if(new_points_lists.begin(), new_points_lists.end(), std::back_inserter(points_lists), [](const IdList& l) { - return l.size() > 1; - }); - CONSOLE_BRIDGE_logDebug("Final raster contains %lu segments", points_lists.size()); + auto validate = [](const Json::Value& jv, const std::string& name_, const Json::ValueType& type_) -> bool { + return jv.isMember(name_) && jv[name_].type() == type_; + }; + raster_spacing = validate(jv, "raster_spacing", Json::ValueType::realValue) ? jv["raster_spacing"].asDouble() : + DEFAULT_RASTER_SPACING; + point_spacing = validate(jv, "point_spacing", Json::ValueType::realValue) ? jv["point_spacing"].asDouble() : + DEFAULT_POINT_SPACING; + raster_rot_offset = validate(jv, "raster_rot_offset", Json::ValueType::realValue) ? + jv["raster_rot_offset"].asDouble() : + DEFAULT_RASTER_ROT_OFFSET; + min_segment_size = validate(jv, "min_segment_size", Json::ValueType::realValue) ? jv["min_segment_size"].asDouble() : + DEFAULT_MIN_SEGMENT_SIZE; + search_radius = validate(jv, "search_radius", Json::ValueType::realValue) ? jv["search_radius"].asDouble() : + DEFAULT_SEARCH_RADIUS; + min_hole_size = validate(jv, "min_hole_size", Json::ValueType::realValue) ? jv["min_hole_size"].asDouble() : + DEFAULT_MIN_HOLE_SIZE; + return true; } -static void rectifyDirection(const vtkSmartPointer& points, - const Eigen::Vector3d& ref_point, - std::vector>& points_lists) +bool PlaneSlicerRasterGenerator::Config::fromJson(const std::string& jv_string) { - using namespace Eigen; - Vector3d p0, pf; - if (points_lists.size() < 2) - return; - - // getting first and last points - points->GetPoint(points_lists.front().front(), p0.data()); - points->GetPoint(points_lists.back().back(), pf.data()); + Json::Value jv; + Json::Reader r; + if (!r.parse(jv_string, jv)) + return false; - bool reverse = (ref_point - p0).norm() > (ref_point - pf).norm(); - if (reverse) - { - for (auto& s : points_lists) - { - std::reverse(s.begin(), s.end()); - } - std::reverse(points_lists.begin(), points_lists.end()); - } + return fromJson(jv); } -static tool_path_planner::ToolPaths convertToPoses(const std::vector& rasters_data) +std::string PlaneSlicerRasterGenerator::Config::str() { - using namespace Eigen; - tool_path_planner::ToolPaths rasters_array; - bool reverse = true; - for (const RasterConstructData& rd : rasters_data) - { - reverse = !reverse; - tool_path_planner::ToolPath raster_path; - std::vector raster_segments; - raster_segments.assign(rd.raster_segments.begin(), rd.raster_segments.end()); - if (reverse) - { - std::reverse(raster_segments.begin(), raster_segments.end()); - } - - for (const PolyDataPtr& polydata : raster_segments) - { - tool_path_planner::ToolPathSegment raster_path_segment; - std::size_t num_points = polydata->GetNumberOfPoints(); - Vector3d p, p_next, vx, vy, vz; - Isometry3d pose; - std::vector indices(num_points); - std::iota(indices.begin(), indices.end(), 0); - if (reverse) - { - std::reverse(indices.begin(), indices.end()); - } - for (std::size_t i = 0; i < indices.size() - 1; i++) - { - int idx = indices[i]; - int idx_next = indices[i + 1]; - polydata->GetPoint(idx, p.data()); - polydata->GetPoint(idx_next, p_next.data()); - polydata->GetPointData()->GetNormals()->GetTuple(idx, vz.data()); - vx = (p_next - p).normalized(); - vy = vz.cross(vx).normalized(); - vz = vx.cross(vy).normalized(); - pose = Translation3d(p) * AngleAxisd(computeRotation(vx, vy, vz)); - raster_path_segment.push_back(pose); - } - - // adding last pose - pose.translation() = p_next; // orientation stays the same as previous - raster_path_segment.push_back(pose); - - raster_path.push_back(raster_path_segment); - } - - if (!raster_path.empty()) - rasters_array.push_back(raster_path); - } - - return rasters_array; + std::stringstream ss; + ss << "raster_spacing: " << raster_spacing << std::endl; + ss << "point_spacing: " << point_spacing << std::endl; + ss << "raster_rot_offset: " << raster_rot_offset << std::endl; + ss << "min_segment_size: " << min_segment_size << std::endl; + ss << "search_radius: " << search_radius << std::endl; + return ss.str(); } -namespace tool_path_planner -{ void PlaneSlicerRasterGenerator::setConfiguration(const PlaneSlicerRasterGenerator::Config& config) { config_ = config; } -void PlaneSlicerRasterGenerator::setInput(pcl::PolygonMesh::ConstPtr mesh) -{ - auto mesh_data = vtkSmartPointer::New(); - pcl::VTKUtils::mesh2vtk(*mesh, mesh_data); - mesh_data->BuildLinks(); - mesh_data->BuildCells(); - setInput(mesh_data); -} +void PlaneSlicerRasterGenerator::setInput(pcl::PolygonMesh::ConstPtr mesh) { mesh_data_ = *mesh; } void PlaneSlicerRasterGenerator::setInput(vtkSmartPointer mesh) { - if (!mesh_data_) - mesh_data_ = vtkSmartPointer::New(); - - mesh_data_->DeepCopy(mesh); + if (!mesh) + throw std::runtime_error("Input mesh is null"); - if (mesh_data_->GetPointData()->GetNormals() && mesh_data_->GetCellData()->GetNormals()) + // Compute normals if none exist + if (!mesh->GetPointData()->GetNormals() || !mesh->GetCellData()->GetNormals()) { - CONSOLE_BRIDGE_logInform("Normal data is available", getName().c_str()); - } - else - { - CONSOLE_BRIDGE_logWarn("%s generating normal data", getName().c_str()); vtkSmartPointer normal_generator = vtkSmartPointer::New(); - normal_generator->SetInputData(mesh_data_); + normal_generator->SetInputData(mesh); normal_generator->ComputePointNormalsOn(); - normal_generator->SetComputeCellNormals(!mesh_data_->GetCellData()->GetNormals()); + normal_generator->SetComputeCellNormals(!mesh->GetCellData()->GetNormals()); normal_generator->SetFeatureAngle(M_PI_2); normal_generator->SetSplitting(true); normal_generator->SetConsistency(true); @@ -410,16 +131,18 @@ void PlaneSlicerRasterGenerator::setInput(vtkSmartPointer mesh) normal_generator->SetNonManifoldTraversal(false); normal_generator->Update(); - if (!mesh_data_->GetPointData()->GetNormals()) + if (!mesh->GetPointData()->GetNormals()) { - mesh_data_->GetPointData()->SetNormals(normal_generator->GetOutput()->GetPointData()->GetNormals()); + mesh->GetPointData()->SetNormals(normal_generator->GetOutput()->GetPointData()->GetNormals()); } - if (!mesh_data_->GetCellData()->GetNormals()) + if (!mesh->GetCellData()->GetNormals()) { - mesh_data_->GetCellData()->SetNormals(normal_generator->GetOutput()->GetCellData()->GetNormals()); + mesh->GetCellData()->SetNormals(normal_generator->GetOutput()->GetCellData()->GetNormals()); } } + + pcl::VTKUtils::vtk2mesh(mesh, mesh_data_); } void PlaneSlicerRasterGenerator::setInput(const shape_msgs::Mesh& mesh) @@ -429,282 +152,32 @@ void PlaneSlicerRasterGenerator::setInput(const shape_msgs::Mesh& mesh) setInput(pcl_mesh); } -vtkSmartPointer PlaneSlicerRasterGenerator::getInput() { return mesh_data_; } - -boost::optional PlaneSlicerRasterGenerator::generate() +vtkSmartPointer PlaneSlicerRasterGenerator::getInput() { - using namespace Eigen; - using IDVec = std::vector; - - boost::optional rasters = boost::none; - if (!mesh_data_) - { - CONSOLE_BRIDGE_logDebug("%s No mesh data has been provided", getName().c_str()); - } - - // Use principal component analysis (PCA) to determine the principal axes of the mesh - Eigen::Matrix3d pca_vecs; // Principal axes, scaled to the size of the mesh in each direction - Eigen::Vector3d centroid; // Mesh centroid - { - // Convert back to PCL point cloud - pcl::PolygonMesh mesh; - pcl::VTKUtils::vtk2mesh(mesh_data_, mesh); - auto cloud = boost::make_shared>(); - pcl::fromPCLPointCloud2(mesh.cloud, *cloud); - - // Perform PCA analysis - pcl::PCA pca; - pca.setInputCloud(cloud); - - // Get the extents of the point cloud relative to its mean and principal axes - pcl::PointCloud proj; - pca.project(*cloud, proj); - pcl::PointXYZ min, max; - pcl::getMinMax3D(proj, min, max); - Eigen::Array3f scales = max.getArray3fMap() - min.getArray3fMap(); - - centroid = pca.getMean().head<3>().cast(); - pca_vecs = (pca.getEigenVectors().array().rowwise() * scales.transpose()).cast(); - } - - // The cutting plane should cut along the largest principal axis (arbitrary decision). - // Therefore the cutting plane is defined by the direction of the second largest principal axis - // We then choose to rotate this plane about the smallest principal axis by a configurable angle - Vector3d raster_dir = - AngleAxisd(config_.raster_rot_offset, pca_vecs.col(2).normalized()) * pca_vecs.col(1).normalized(); - - // Calculate the number of planes needed to cover the mesh according to the largest principal dimension - auto num_planes = static_cast(pca_vecs.col(0).norm() / config_.raster_spacing); - - // Calculate the start location as the centroid less half the largest principal dimension of the mesh - Vector3d start_loc = centroid - (pca_vecs.col(0).norm() / 2.0) * raster_dir; - - vtkSmartPointer raster_data = vtkSmartPointer::New(); - for (std::size_t i = 0; i < num_planes + 1; i++) - { - vtkSmartPointer plane = vtkSmartPointer::New(); - vtkSmartPointer cutter = vtkSmartPointer::New(); - vtkSmartPointer stripper = vtkSmartPointer::New(); - - Vector3d current_loc = start_loc + i * config_.raster_spacing * raster_dir; - plane->SetOrigin(current_loc.x(), current_loc.y(), current_loc.z()); - plane->SetNormal(raster_dir.x(), raster_dir.y(), raster_dir.z()); - - cutter->SetCutFunction(plane); - cutter->SetInputData(mesh_data_); - cutter->SetSortBy(1); - cutter->SetGenerateTriangles(false); - cutter->Update(); - - stripper->SetInputConnection(cutter->GetOutputPort()); - stripper->JoinContiguousSegmentsOn(); - stripper->SetMaximumLength(mesh_data_->GetNumberOfPoints()); - stripper->Update(); - - if (stripper->GetErrorCode() != vtkErrorCode::NoError) - { - continue; - } - - for (int r = 0; r < stripper->GetNumberOfOutputPorts(); r++) - { - raster_data->AddInputData(stripper->GetOutput(r)); - } - } - - // build cell locator and kd_tree to recover normals later on - kd_tree_ = vtkSmartPointer::New(); - kd_tree_->SetDataSet(mesh_data_); - kd_tree_->BuildLocator(); - - cell_locator_ = vtkSmartPointer::New(); - cell_locator_->SetDataSet(mesh_data_); - cell_locator_->BuildLocator(); - - // collect rasters and set direction - raster_data->Update(); - vtkIdType num_slices = raster_data->GetTotalNumberOfInputConnections(); - std::vector rasters_data_vec; - std::vector raster_ids; - boost::optional ref_dir; - for (std::size_t i = 0; i < num_slices; i++) - { - RasterConstructData r; - - // collecting raster segments based on min hole size - vtkSmartPointer raster_lines = raster_data->GetInput(i); - vtkIdType* indices; - vtkIdType num_points; - vtkIdType num_lines = raster_lines->GetNumberOfLines(); - vtkCellArray* cells = raster_lines->GetLines(); - - CONSOLE_BRIDGE_logDebug("%s raster %i has %i lines and %i points", - getName().c_str(), - i, - raster_lines->GetNumberOfLines(), - raster_lines->GetNumberOfPoints()); - - if (num_lines == 0) - { - continue; - } - - raster_ids.clear(); - - unsigned int lineCount = 0; - for (cells->InitTraversal(); cells->GetNextCell(num_points, indices); lineCount++) - { - IDVec point_ids; - for (vtkIdType i = 0; i < num_points; i++) - { - if (std::find(point_ids.begin(), point_ids.end(), indices[i]) != point_ids.end()) - { - continue; - } - point_ids.push_back(indices[i]); - } - if (point_ids.empty()) - { - continue; - } - - // removing duplicates - auto iter = std::unique(point_ids.begin(), point_ids.end()); - point_ids.erase(iter, point_ids.end()); - - // adding - raster_ids.push_back(point_ids); - } - - if (raster_ids.empty()) - { - continue; - } - - // remove redundant indices - removeRedundant(raster_ids); - - // merging segments - mergeRasterSegments(raster_lines->GetPoints(), config_.min_hole_size, raster_ids); - - // rectifying - if (rasters_data_vec.size() > 1) - { - Vector3d ref_point; - rasters_data_vec.back().raster_segments.front()->GetPoint(0, ref_point.data()); // first point in previous raster - rectifyDirection(raster_lines->GetPoints(), ref_point, raster_ids); - } - - for (auto& rpoint_ids : raster_ids) - { - // Populating with points - vtkSmartPointer points = vtkSmartPointer::New(); - std::for_each(rpoint_ids.begin(), rpoint_ids.end(), [&points, &raster_lines](vtkIdType& id) { - std::array p; - raster_lines->GetPoint(id, p.data()); - points->InsertNextPoint(p.data()); - }); - - // compute length and add points if segment length is greater than threshold - double line_length = computeLength(points); - if (line_length > config_.min_segment_size && points->GetNumberOfPoints() > 1) - { - // enforce point spacing - decltype(points) new_points = applyParametricSpline(points, line_length, config_.point_spacing); - - // add points to segment now - PolyDataPtr segment_data = PolyDataPtr::New(); - segment_data->SetPoints(new_points); - - // inserting normals - if (!insertNormals(config_.search_radius, segment_data)) - { - CONSOLE_BRIDGE_logError("%s failed to insert normals to segment %lu of raster %lu", - getName().c_str(), - r.raster_segments.size(), - i); - return boost::none; - } - - // saving into raster - r.raster_segments.push_back(segment_data); - r.segment_lengths.push_back(line_length); - } - } - - rasters_data_vec.push_back(r); - } - - // converting to poses msg now - rasters = convertToPoses(rasters_data_vec); - return rasters; + auto vtk_mesh = vtkSmartPointer::New(); + pcl::VTKUtils::mesh2vtk(mesh_data_, vtk_mesh); + return vtk_mesh; } -bool PlaneSlicerRasterGenerator::insertNormals(const double search_radius, vtkSmartPointer& data) +boost::optional PlaneSlicerRasterGenerator::generate() { - // Find closest cell to each point and uses its normal vector - vtkSmartPointer new_normals = vtkSmartPointer::New(); - new_normals->SetNumberOfComponents(3); - new_normals->SetNumberOfTuples(data->GetPoints()->GetNumberOfPoints()); - - // get normal data - vtkSmartPointer normal_data = mesh_data_->GetPointData()->GetNormals(); - - if (!normal_data) + try { - CONSOLE_BRIDGE_logError("%s Normal data is not available", getName().c_str()); - return false; - } + noether::PlaneSlicerRasterPlanner planner( + std::make_unique(config_.raster_rot_offset), + std::make_unique()); + planner.setLineSpacing(config_.raster_spacing); + planner.setPointSpacing(config_.point_spacing); + planner.setMinHoleSize(config_.min_hole_size); + planner.setSearchRadius(config_.search_radius); + planner.setMinSegmentSize(config_.min_segment_size); - Eigen::Vector3d normal_vect = Eigen::Vector3d::UnitZ(); - for (int i = 0; i < data->GetPoints()->GetNumberOfPoints(); ++i) + return planner.plan(mesh_data_); + } + catch (const std::exception& ex) { - // locate closest cell - Eigen::Vector3d query_point; - vtkSmartPointer id_list = vtkSmartPointer::New(); - data->GetPoints()->GetPoint(i, query_point.data()); - kd_tree_->FindPointsWithinRadius(search_radius, query_point.data(), id_list); - if (id_list->GetNumberOfIds() < 1) - { - CONSOLE_BRIDGE_logWarn("%s FindPointsWithinRadius found no points for normal averaging, using closests", - getName().c_str()); - kd_tree_->FindClosestNPoints(1, query_point.data(), id_list); - - if (id_list->GetNumberOfIds() < 1) - { - CONSOLE_BRIDGE_logError("%s failed to find closest for normal computation", getName().c_str()); - return false; - } - } - - // compute normal average - normal_vect = Eigen::Vector3d::Zero(); - std::size_t num_normals = 0; - for (auto p = 0; p < id_list->GetNumberOfIds(); p++) - { - Eigen::Vector3d temp_normal, query_point, closest_point; - vtkIdType p_id = id_list->GetId(p); - - if (p_id < 0) - { - CONSOLE_BRIDGE_logError("%s point id is invalid", getName().c_str()); - continue; - } - - // get normal and add it to average - normal_data->GetTuple(p_id, temp_normal.data()); - normal_vect += temp_normal.normalized(); - num_normals++; - } - - normal_vect /= num_normals; - normal_vect.normalize(); - - // save normal - new_normals->SetTuple3(i, normal_vect(0), normal_vect(1), normal_vect(2)); + return {}; } - data->GetPointData()->SetNormals(new_normals); - return true; } std::string PlaneSlicerRasterGenerator::getName() const { return getClassName(); }