Remove more unnecessary code

Author: marip8

noether_tpp/src/tool_path_planners/raster/plane_slicer_raster_planner.cpp

@@ -30,12 +30,6 @@ namespace
 {
 static const double EPSILON = 1e-6;
 
-struct RasterConstructData
-{
-  std::vector<vtkSmartPointer<vtkPolyData>> raster_segments;
-  std::vector<double> segment_lengths;
-};
-
 Eigen::Matrix3d computeRotation(const Eigen::Vector3d& vx, const Eigen::Vector3d& vy, const Eigen::Vector3d& vz)
 {
   Eigen::Matrix3d m;
@@ -46,154 +40,14 @@ Eigen::Matrix3d computeRotation(const Eigen::Vector3d& vx, const Eigen::Vector3d
   return m;
 }
 
-/**
- * @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<std::vector<vtkIdType>>& points_lists)
-{
-  using IdList = std::vector<vtkIdType>;
-  if (points_lists.size() < 2)
-  {
-    return;
-  }
-
-  std::vector<std::vector<vtkIdType>> 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<vtkPoints>& points,
-                         double merge_dist,
-                         std::vector<std::vector<vtkIdType>>& points_lists)
-{
-  using namespace Eigen;
-  using IdList = std::vector<vtkIdType>;
-  if (points_lists.size() < 2)
-  {
-    return;
-  }
-
-  std::vector<IdList> 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<vtkIdType>(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<vtkIdType>(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;
-  });
-}
-
-noether::ToolPaths convertToPoses(const std::vector<RasterConstructData>& rasters_data)
+noether::ToolPaths convertToPoses(const std::vector<std::vector<vtkSmartPointer<vtkPolyData>>>& rasters_data)
 {
   noether::ToolPaths rasters_array;
-  for (const RasterConstructData& rd : rasters_data)
+  for (const std::vector<vtkSmartPointer<vtkPolyData>>& rd : rasters_data)
   {
     noether::ToolPath raster_path;
     std::vector<vtkSmartPointer<vtkPolyData>> raster_segments;
-    raster_segments.assign(rd.raster_segments.begin(), rd.raster_segments.end());
+    raster_segments.assign(rd.begin(), rd.end());
 
     for (const vtkSmartPointer<vtkPolyData>& polydata : raster_segments)
     {
@@ -383,7 +237,7 @@ ToolPaths PlaneSlicerRasterPlanner::planImpl(const pcl::PolygonMesh& mesh) const
 
     cutter->SetCutFunction(plane);
     cutter->SetInputData(mesh_data_);
-    cutter->SetSortBy(1);
+    cutter->SetSortByToSortByCell();
     cutter->SetGenerateTriangles(false);
     cutter->Update();
 
@@ -406,69 +260,15 @@ ToolPaths PlaneSlicerRasterPlanner::planImpl(const pcl::PolygonMesh& mesh) const
   // collect rasters and set direction
   raster_data->Update();
   vtkIdType num_slices = raster_data->GetTotalNumberOfInputConnections();
-  std::vector<RasterConstructData> rasters_data_vec;
-  std::vector<std::vector<vtkIdType>> raster_ids;
+  std::vector<std::vector<vtkSmartPointer<vtkPolyData>>> rasters_data_vec;
   for (std::size_t i = 0; i < num_slices; i++)
   {
-    RasterConstructData r;
-
-    // collecting raster segments based on min hole size
     vtkSmartPointer<vtkPolyData> 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();
-
-    if (num_lines == 0)
-    {
-      continue;
-    }
-
-    raster_ids.clear();
-
-    unsigned int lineCount = 0;
-    for (cells->InitTraversal(); cells->GetNextCell(num_points, indices); lineCount++)
-    {
-      std::vector<vtkIdType> 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())
-    {
+    if (raster_lines->GetNumberOfLines() == 0)
       continue;
-    }
-
-    // remove redundant indices
-    removeRedundant(raster_ids);
-
-    // merging segments
-    mergeRasterSegments(raster_lines->GetPoints(), min_hole_size_, raster_ids);
 
     // Save raster
-    r.raster_segments.push_back(raster_lines);
-    rasters_data_vec.push_back(r);
+    rasters_data_vec.push_back({raster_lines});
   }
 
   // converting to poses msg now