38 #ifndef PCL_STANDALONE_MARCHING_CUBES_IMPL_HPP_
39 #define PCL_STANDALONE_MARCHING_CUBES_IMPL_HPP_
41 #include <pcl/gpu/kinfu_large_scale/standalone_marching_cubes.h>
44 template <
typename Po
intT>
47 voxels_x_ = new_voxels_x;
48 voxels_y_ = new_voxels_y;
49 voxels_z_ = new_voxels_z;
50 volume_size_ = new_volume_size;
53 const Eigen::Vector3f volume_size = Eigen::Vector3f::Constant (volume_size_);
55 const Eigen::Vector3i volume_resolution (voxels_x_, voxels_y_, voxels_z_);
57 tsdf_volume_gpu_->setSize (volume_size);
60 int tsdf_total_size = voxels_x_ * voxels_y_ * voxels_z_;
61 tsdf_volume_cpu_= std::vector<int> (tsdf_total_size,0);
73 const Eigen::Vector3f volume_size = Eigen::Vector3f::Constant (volume_size_);
74 std::cout <<
"VOLUME SIZE IS " << volume_size_ << std::endl;
75 const Eigen::Vector3i volume_resolution (voxels_x_, voxels_y_, voxels_z_);
78 tsdf_volume_gpu_->reset ();
81 fill (tsdf_volume_cpu_.begin (), tsdf_volume_cpu_.end (), 0);
84 loadTsdfCloudToGPU (cloud);
87 return ( runMarchingCubes () );
94 template <
typename Po
intT>
void
97 std::vector< MeshPtr > meshes_vector;
99 int max_iterations = std::min( tsdf_clouds.size (), tsdf_offsets.size () );
100 PCL_INFO (
"There are %d cubes to be processed \n", max_iterations);
101 float cell_size = volume_size_ / voxels_x_;
103 int mesh_counter = 0;
105 for(
int i = 0; i < max_iterations; ++i)
107 PCL_INFO (
"Processing cube number %d\n", i);
110 Eigen::Affine3f cloud_transform;
112 float originX = (tsdf_offsets[i]).x();
113 float originY = (tsdf_offsets[i]).y();
114 float originZ = (tsdf_offsets[i]).z();
116 cloud_transform.linear ().setIdentity ();
117 cloud_transform.translation ()[0] = -originX;
118 cloud_transform.translation ()[1] = -originY;
119 cloud_transform.translation ()[2] = -originZ;
124 MeshPtr tmp = getMeshFromTSDFCloud (*tsdf_clouds[i]);
128 meshes_vector.push_back (tmp);
133 PCL_INFO (
"This cloud returned no faces, we skip it!\n");
138 cloud_transform.translation ()[0] = originX * cell_size;
139 cloud_transform.translation ()[1] = originY * cell_size;
140 cloud_transform.translation ()[2] = originZ * cell_size;
149 std::stringstream name;
150 name <<
"mesh_" << mesh_counter <<
".ply";
151 PCL_INFO (
"Saving mesh...%d \n", mesh_counter);
163 return (tsdf_volume_gpu_);
168 template <
typename Po
intT> std::vector<int>&
171 return (tsdf_volume_cpu_);
176 template <
typename Po
intT>
void
180 convertTsdfVectors (cloud, tsdf_volume_cpu_);
183 int cubeColumns = voxels_x_;
184 tsdf_volume_gpu_->data ().upload (tsdf_volume_cpu_, cubeColumns);
189 template <
typename Po
intT>
void
192 const int DIVISOR = 32767;
195 #pragma omp parallel for
197 for(
int i = 0; i < (int) cloud.
points.size (); ++i)
199 int x = cloud.
points[i].x;
200 int y = cloud.
points[i].y;
201 int z = cloud.
points[i].z;
203 if(x > 0 && x < voxels_x_ && y > 0 && y < voxels_y_ && z > 0 && z < voxels_z_)
206 int dst_index = x + voxels_x_ * y + voxels_y_ * voxels_x_ * z;
208 short2& elem = *
reinterpret_cast<short2*
> (&output[dst_index]);
209 elem.x =
static_cast<short> (cloud.
points[i].intensity * DIVISOR);
210 elem.y =
static_cast<short> (1);
220 if (triangles.
empty () )
234 mesh_ptr->polygons.resize (triangles.
size () / 3);
235 for (
size_t i = 0; i < mesh_ptr->polygons.size (); ++i)
241 mesh_ptr->polygons[i] = v;
262 boost::shared_ptr<pcl::PolygonMesh> mesh_ptr_ = convertTrianglesToMesh (triangles_device);
274 #endif // PCL_STANDALONE_MARCHING_CUBES_IMPL_HPP_
void convertTsdfVectors(const PointCloud &cloud, std::vector< int > &output)
Read the data in the point cloud.
void fromPCLPointCloud2(const pcl::PCLPointCloud2 &msg, pcl::PointCloud< PointT > &cloud, const MsgFieldMap &field_map)
Convert a PCLPointCloud2 binary data blob into a pcl::PointCloud object using a field_map...
boost::shared_ptr< PointCloud< PointT > > Ptr
uint32_t width
The point cloud width (if organized as an image-structure).
std::vector< uint32_t > vertices
int savePLYFile(const std::string &file_name, const pcl::PCLPointCloud2 &cloud, const Eigen::Vector4f &origin=Eigen::Vector4f::Zero(), const Eigen::Quaternionf &orientation=Eigen::Quaternionf::Identity(), bool binary_mode=false, bool use_camera=true)
Save point cloud data to a PLY file containing n-D points.
std::vector< int > & tsdfVolumeCPU()
Returns the associated Tsdf Volume buffer in CPU.
void getMeshesFromTSDFVector(const std::vector< PointCloudPtr > &tsdf_clouds, const std::vector< Eigen::Vector3f, Eigen::aligned_allocator< Eigen::Vector3f > > &tsdf_offsets)
Runs marching cubes on every pointcloud in the vector.
void download(T *host_ptr) const
Downloads data from internal buffer to CPU memory.
boost::shared_ptr< pcl::PolygonMesh > MeshPtr
Describes a set of vertices in a polygon mesh, by basically storing an array of indices.
MarchingCubes implements MarchingCubes functionality for TSDF volume on GPU.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
void transformPointCloud(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const Eigen::Transform< Scalar, 3, Eigen::Affine > &transform, bool copy_all_fields=true)
Apply an affine transform defined by an Eigen Transform.
boost::shared_ptr< MarchingCubes > Ptr
Smart pointer.
boost::shared_ptr< TsdfVolume > Ptr
uint32_t height
The point cloud height (if organized as an image-structure).
PointCloud represents the base class in PCL for storing collections of 3D points. ...
MeshPtr getMeshFromTSDFCloud(const PointCloud &cloud)
Run marching cubes in a TSDF cloud and returns a PolygonMesh.
void toPCLPointCloud2(const pcl::PointCloud< PointT > &cloud, pcl::PCLPointCloud2 &msg)
Convert a pcl::PointCloud object to a PCLPointCloud2 binary data blob.
MeshPtr convertTrianglesToMesh(const pcl::gpu::DeviceArray< pcl::PointXYZ > &triangles)
Converts the triangles buffer device to a PolygonMesh.
TsdfVolume::Ptr tsdfVolumeGPU()
Returns the associated Tsdf Volume buffer in GPU.
size_t size() const
Returns size in elements.
bool empty() const
Returns true if unallocated otherwise false.
MeshPtr runMarchingCubes()
Runs marching cubes on the data that is contained in the TSDF Volume in GPU.
void loadTsdfCloudToGPU(const PointCloud &cloud)
Loads a TSDF Cloud to the TSDF Volume in GPU.
StandaloneMarchingCubes(int voxels_x=512, int voxels_y=512, int voxels_z=512, float volume_size=3.0f)
Constructor.