Skip to content

Commit

Permalink
Remove more unnecessary code
Browse files Browse the repository at this point in the history
  • Loading branch information
@marip8
marip8 committed Oct 1, 2024
1 parent 4435f71 commit 31e3ec7
Showing 1 changed file with 8 additions and 206 deletions.
214 changes: 8 additions & 206 deletions noether_tpp/src/tool_path_planners/raster/plane_slicer_raster_planner.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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)
{
Expand Down Expand Up @@ -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();

Expand All @@ -406,69 +260,17 @@ 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);
std::cout << "Found " << raster_lines->GetNumberOfLines() << " lines" << std::endl;

// 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
Expand Down

0 comments on commit 31e3ec7

Please sign in to comment.