trajectory_files_manager.cpp

Go to the documentation of this file.

#include <ram_trajectory_utils/trajectory_files_manager.hpp>

namespace ram_trajectory_utils
{
TrajectoryFilesManager::TrajectoryFilesManager()
{
}

bool TrajectoryFilesManager::yamlFileToPolydata(const std::string yaml_file,
                                                Polygon poly_data)
{
  // Open YAML file
  std::vector<std::vector<vtkVector3d> > polygon_point_list;
  try
  {
    YAML::Node yaml;
    yaml_parser::yamlNodeFromFileName(yaml_file, yaml);

    // Parse "layer" node in YAML file
    std::string node_name("layer");
    const YAML::Node &layer_node = yaml_parser::parseNode(yaml, node_name.c_str());
    if (!yaml[node_name])
    {
      YAML::Exception yaml_exception(YAML::Mark(), "Cannot parse " + node_name + " node");
      throw yaml_exception;
    }

    // First Loop
    // Parse all  nodes in "layer" node
    for (auto iterator_node : layer_node)
    {
      //Parse "polygon" node

      std::string node_name("polygon");
      const YAML::Node &polygon_node = yaml_parser::parseNode(iterator_node, node_name.c_str());
      if (!iterator_node[node_name])
      {
        YAML::Exception iterator_node_exception(YAML::Mark(), "Cannot parse " + node_name + " node");
        throw iterator_node_exception;
      }
      std::vector<vtkVector3d> polygon;

      // Second Loop
      // Parse all "point" tags in "polygon" node
      for (auto point_node : polygon_node)
      {
        std::vector<double> point_values;
        yaml_parser::parseVectorD(point_node, "point", point_values);
        vtkVector3d point(point_values[0], point_values[1], point_values[2]);
        polygon.push_back(point);
      }
      polygon_point_list.push_back(polygon);
    }

  }
  catch (const YAML::Exception &e)
  {
    std::cerr << "Problem occurred during parsing YAML file, problem is:\n" << e.msg << endl <<
                     "File name = " << yaml_file << std::endl;
    return false;
  }

  vtkSmartPointer<vtkCellArray> polygon_array = vtkSmartPointer<vtkCellArray>::New();
  vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New();
  for (auto polygon_iterator : polygon_point_list)
  {
    // Create the polygon
    vtkSmartPointer<vtkPolygon> polygon = vtkSmartPointer<vtkPolygon>::New();
    for (auto point_iterator : polygon_iterator)
    {
      polygon->GetPointIds()->InsertNextId(points->GetNumberOfPoints());
      points->InsertNextPoint(point_iterator.GetData());
    }
    polygon_array->InsertNextCell(polygon);
  }

  poly_data->SetPolys(polygon_array);
  poly_data->SetPoints(points);
  return true;
}

bool TrajectoryFilesManager::polydataToYamlFile(const std::string yaml_file,
                                                const Polygon poly_data)
{
  vtkIdType n_cells = poly_data->GetNumberOfCells();
  if (n_cells == 0)
  {
    std::cerr << "polydataToYamlFile: the polydata is empty" << std::endl;
    return false;
  }

  double p[3];
  std::ofstream yaml_file_ofstream;
  yaml_file_ofstream.open(yaml_file);
  yaml_file_ofstream << "---\n"
      "layer:\n";

  for (vtkIdType cell_id(0); cell_id < n_cells; ++cell_id)
  {
    vtkIdType n_points = poly_data->GetCell(cell_id)->GetNumberOfPoints();
    if (n_points == 0)
    {
      std::cerr << "polydataToYamlFile: the polygon " << cell_id << " in polydata is empty" << std::endl;
      return false;
    }
    yaml_file_ofstream << "  -\n"
        "    polygon:\n";
    for (vtkIdType point_id(0); point_id < n_points; ++point_id)
    {
      poly_data->GetCell(cell_id)->GetPoints()->GetPoint(point_id, p);
      yaml_file_ofstream << "    - point: [" << p[0] << ", " << p[1] << ", " << p[2] << "]\n";
    }
  }

  yaml_file_ofstream << "---\n";
  yaml_file_ofstream << "\n";
  yaml_file_ofstream.close();
  std::cout << "File " << yaml_file << " was written on the disk" << std::endl;

  return true;
}

}