pcl_mesh_to_cgal_v1

pcl_mesh_to_cgal_v1.cpp

// https://stackoverflow.com/questions/61318623/how-can-i-convert-cgal-points-into-pcl-point-cloud
template<class Pt>
static int convert_mesh_from_PCL_to_CGAL(pcl::PolygonMesh::Ptr PCL_mesh, CGAL::Surface_mesh<Pt>& CGAL_mesh, bool flipNormal = false)
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr mesh_cloud (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::fromPCLPointCloud2( PCL_mesh->cloud, *mesh_cloud );
    // clear and reserve the
    CGAL_mesh.clear();
    int n = mesh_cloud->size();
    int f = PCL_mesh->polygons.size();
    int e = 0;
    CGAL_mesh.reserve(n, 2*f, e);
    //copy the vertices
    double x, y, z;
    for (int i=0; i<mesh_cloud->size(); i++)
    {
        Point p;
        x = mesh_cloud->points[i].x;
        y = mesh_cloud->points[i].y;
        z = mesh_cloud->points[i].z;
        p = Point(x, y, z);
        CGAL_mesh.add_vertex(p);
    }
    // copy the faces
    std::vector<int> vertices;
    std::vector<array<int, 3>> polygons_add_later;
    std::vector<array<int, 3>> polygons_add_finally;
    for(int i=0; i<PCL_mesh->polygons.size(); i++)
    {
        vertices.resize(3);
        if (flipNormal)
        {
          vertices[0] = PCL_mesh->polygons[i].vertices[2];
          vertices[1] = PCL_mesh->polygons[i].vertices[1];
          vertices[2] = PCL_mesh->polygons[i].vertices[0];
        }
        else
        {
          vertices[0] = PCL_mesh->polygons[i].vertices[0];
          vertices[1] = PCL_mesh->polygons[i].vertices[1];
          vertices[2] = PCL_mesh->polygons[i].vertices[2];
        }
        
        auto result = CGAL_mesh.add_face(CGAL_Mesh::Vertex_index (vertices[0]), 
                           CGAL_Mesh::Vertex_index (vertices[1]),
                           CGAL_Mesh::Vertex_index (vertices[2]));
        if (result == CGAL_Mesh::null_face())
        {
          //result = CGAL_mesh.add_face(CGAL_Mesh::Vertex_index (vertices[2]), 
          //                  CGAL_Mesh::Vertex_index (vertices[1]),
          //                  CGAL_Mesh::Vertex_index (vertices[0]));
          array<int, 3> polygon = {vertices[0], vertices[1], vertices[2]};
          polygons_add_later.push_back(polygon);
        }
    }

    // split mesh by connectivity and try to flip smaller components 
    // https://stackoverflow.com/questions/54609105/cgal-how-to-use-cgalpolygon-mesh-processingconnected-components-to-convert
    /*
    CGAL_Mesh::Property_map<face_descriptor, std::size_t> fccmap = CGAL_mesh.add_property_map<face_descriptor, std::size_t>("f:CC").first;
    auto num = CGAL::Polygon_mesh_processing::connected_components(CGAL_mesh,fccmap);
    //std::vector<CGAL_Mesh> meshes(num);
    std::vector<Filtered_graph> mesh_parts;
    std::vector<std::pair<int, int>> idx_and_face_count;
    for(int i=0; i< num; ++i)
    {
      Filtered_graph ffg(CGAL_mesh, i, fccmap);
      mesh_parts.push_back(ffg);
      std::pair<int, int> pair(i, ffg.number_of_faces());
      idx_and_face_count.push_back(pair);
      //CGAL::copy_face_graph(ffg, meshes[i]);
    }
    std::sort(idx_and_face_count.begin(), idx_and_face_count.end(), [](auto &left, auto &right) {
      return left.second > right.second;
    });
    std::vector<std::array<CGAL_Mesh::Vertex_index, 3>> triangles_flipped;
    for(int i=1; i< num; ++i)
    {
      CGAL::Polygon_mesh_processing::reverse_face_orientations(CGAL::faces(mesh_parts[idx_and_face_count[i].first]), CGAL_mesh);
    }
    */

    for(int i=0; i<polygons_add_later.size(); i++)
    {
        auto result = CGAL_mesh.add_face(CGAL_Mesh::Vertex_index (polygons_add_later[i][0]), 
                           CGAL_Mesh::Vertex_index (polygons_add_later[i][1]),
                           CGAL_Mesh::Vertex_index (polygons_add_later[i][2]));
        if (result == CGAL_Mesh::null_face())
        {
          result = CGAL_mesh.add_face(CGAL_Mesh::Vertex_index (polygons_add_later[i][2]), 
                            CGAL_Mesh::Vertex_index (polygons_add_later[i][1]),
                            CGAL_Mesh::Vertex_index (polygons_add_later[i][0]));
          if (result == CGAL_Mesh::null_face())
          {
            polygons_add_finally.push_back(polygons_add_later[i]);
          }
        }
    }
    
    cout << "copy " << PCL_mesh->polygons.size() << " faces from pcl to cgal" << endl;
    return 0;
}


template<class Pt>
static int convert_mesh_from_PCL_to_CGAL(pcl::PolygonMesh::Ptr PCL_mesh, CGAL::Surface_mesh<Pt>& CGAL_mesh, bool flipNormal = false)
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr mesh_cloud (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::fromPCLPointCloud2( PCL_mesh->cloud, *mesh_cloud );
    // clear and reserve the
    CGAL_mesh.clear();
    int n = mesh_cloud->size();
    int f = PCL_mesh->polygons.size();
    int e = 0;
    std::vector<Point> points;
    std::vector< std::vector<std::size_t> > polygons;
    //copy the vertices
    double x, y, z;
    for (int i=0; i<mesh_cloud->size(); i++)
    {
        Point p;
        x = mesh_cloud->points[i].x;
        y = mesh_cloud->points[i].y;
        z = mesh_cloud->points[i].z;
        p = Point(x, y, z);
        points.push_back(p);
    }
    // copy the faces
    std::vector<std::size_t> vertices;
    for(int i=0; i<PCL_mesh->polygons.size(); i++)
    {
        vertices.resize(3);
        if (flipNormal)
        {
          vertices[0] = PCL_mesh->polygons[i].vertices[2];
          vertices[1] = PCL_mesh->polygons[i].vertices[1];
          vertices[2] = PCL_mesh->polygons[i].vertices[0];
        }
        else
        {
          vertices[0] = PCL_mesh->polygons[i].vertices[0];
          vertices[1] = PCL_mesh->polygons[i].vertices[1];
          vertices[2] = PCL_mesh->polygons[i].vertices[2];
        }
        
        polygons.push_back(vertices);
    }
    CGAL::Polygon_mesh_processing::orient_polygon_soup(points, polygons);
    CGAL::Polygon_mesh_processing::polygon_soup_to_polygon_mesh(points, polygons, CGAL_mesh);

    if (CGAL::is_closed(CGAL_mesh) && (!CGAL::Polygon_mesh_processing::is_outward_oriented(CGAL_mesh)))
      CGAL::Polygon_mesh_processing::reverse_face_orientations(CGAL_mesh);

    cout << "copy " << PCL_mesh->polygons.size() << " faces from pcl to cgal, get " << CGAL_mesh.number_of_faces() << " faces" << endl;
    return 0;
}