120 lines
3.6 KiB
C
120 lines
3.6 KiB
C
|
#pragma once
|
||
|
|
|
||
|
|
#include <algorithm>
|
||
|
|
#include <vector>
|
||
|
|
#include <ostream>
|
||
|
|
|
||
|
|
// Include the correct Header path here
|
||
|
|
#include <pcl/PCLHeader.h>
|
||
|
|
#include <pcl/PCLPointCloud2.h>
|
||
|
|
#include <pcl/Vertices.h>
|
||
|
|
|
||
|
|
namespace pcl
|
||
|
|
{
|
||
|
|
struct PolygonMesh
|
||
|
|
{
|
||
|
|
PolygonMesh ()
|
||
|
|
{}
|
||
|
|
|
||
|
|
::pcl::PCLHeader header;
|
||
|
|
|
||
|
|
::pcl::PCLPointCloud2 cloud;
|
||
|
|
|
||
|
|
std::vector< ::pcl::Vertices> polygons;
|
||
|
|
|
||
|
|
/** \brief Inplace concatenate two pcl::PolygonMesh
|
||
|
|
* \param[in,out] mesh1 the first input and output mesh
|
||
|
|
* \param[in] mesh2 the second input mesh
|
||
|
|
* \return true if successful, false otherwise (unexpected error)
|
||
|
|
*/
|
||
|
|
static bool
|
||
|
|
concatenate (pcl::PolygonMesh &mesh1, const pcl::PolygonMesh &mesh2)
|
||
|
|
{
|
||
|
|
const auto point_offset = mesh1.cloud.width * mesh1.cloud.height;
|
||
|
|
|
||
|
|
bool success = pcl::PCLPointCloud2::concatenate(mesh1.cloud, mesh2.cloud);
|
||
|
|
if (success == false) {
|
||
|
|
return false;
|
||
|
|
}
|
||
|
|
// Make the resultant polygon mesh take the newest stamp
|
||
|
|
mesh1.header.stamp = std::max(mesh1.header.stamp, mesh2.header.stamp);
|
||
|
|
|
||
|
|
std::transform(mesh2.polygons.begin (),
|
||
|
|
mesh2.polygons.end (),
|
||
|
|
std::back_inserter (mesh1.polygons),
|
||
|
|
[point_offset](auto polygon)
|
||
|
|
{
|
||
|
|
std::transform(polygon.vertices.begin (),
|
||
|
|
polygon.vertices.end (),
|
||
|
|
polygon.vertices.begin (),
|
||
|
|
[point_offset](auto& point_idx)
|
||
|
|
{
|
||
|
|
return point_idx + point_offset;
|
||
|
|
});
|
||
|
|
return polygon;
|
||
|
|
});
|
||
|
|
|
||
|
|
return true;
|
||
|
|
}
|
||
|
|
|
||
|
|
/** \brief Concatenate two pcl::PCLPointCloud2
|
||
|
|
* \param[in] mesh1 the first input mesh
|
||
|
|
* \param[in] mesh2 the second input mesh
|
||
|
|
* \param[out] mesh_out the resultant output mesh
|
||
|
|
* \return true if successful, false otherwise (unexpected error)
|
||
|
|
*/
|
||
|
|
static bool
|
||
|
|
concatenate (const PolygonMesh &mesh1,
|
||
|
|
const PolygonMesh &mesh2,
|
||
|
|
PolygonMesh &mesh_out)
|
||
|
|
{
|
||
|
|
mesh_out = mesh1;
|
||
|
|
return concatenate(mesh_out, mesh2);
|
||
|
|
}
|
||
|
|
|
||
|
|
/** \brief Add another polygon mesh to the current mesh.
|
||
|
|
* \param[in] rhs the mesh to add to the current mesh
|
||
|
|
* \return the new mesh as a concatenation of the current mesh and the new given mesh
|
||
|
|
*/
|
||
|
|
inline PolygonMesh&
|
||
|
|
operator += (const PolygonMesh& rhs)
|
||
|
|
{
|
||
|
|
concatenate((*this), rhs);
|
||
|
|
return (*this);
|
||
|
|
}
|
||
|
|
|
||
|
|
/** \brief Add a polygon mesh to another mesh.
|
||
|
|
* \param[in] rhs the mesh to add to the current mesh
|
||
|
|
* \return the new mesh as a concatenation of the current mesh and the new given mesh
|
||
|
|
*/
|
||
|
|
inline const PolygonMesh
|
||
|
|
operator + (const PolygonMesh& rhs)
|
||
|
|
{
|
||
|
|
return (PolygonMesh (*this) += rhs);
|
||
|
|
}
|
||
|
|
|
||
|
|
public:
|
||
|
|
using Ptr = shared_ptr< ::pcl::PolygonMesh>;
|
||
|
|
using ConstPtr = shared_ptr<const ::pcl::PolygonMesh>;
|
||
|
|
}; // struct PolygonMesh
|
||
|
|
|
||
|
|
using PolygonMeshPtr = PolygonMesh::Ptr;
|
||
|
|
using PolygonMeshConstPtr = PolygonMesh::ConstPtr;
|
||
|
|
|
||
|
|
inline std::ostream& operator<<(std::ostream& s, const ::pcl::PolygonMesh &v)
|
||
|
|
{
|
||
|
|
s << "header: " << std::endl;
|
||
|
|
s << v.header;
|
||
|
|
s << "cloud: " << std::endl;
|
||
|
|
s << v.cloud;
|
||
|
|
s << "polygons[]" << std::endl;
|
||
|
|
for (std::size_t i = 0; i < v.polygons.size (); ++i)
|
||
|
|
{
|
||
|
|
s << " polygons[" << i << "]: " << std::endl;
|
||
|
|
s << v.polygons[i];
|
||
|
|
}
|
||
|
|
return (s);
|
||
|
|
}
|
||
|
|
|
||
|
|
} // namespace pcl
|