Point Cloud Library (PCL)  1.9.1-dev
standalone_marching_cubes.hpp
1  /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2011, Willow Garage, Inc.
6  *
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of Willow Garage, Inc. nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  */
37 
38 #ifndef PCL_STANDALONE_MARCHING_CUBES_IMPL_HPP_
39 #define PCL_STANDALONE_MARCHING_CUBES_IMPL_HPP_
40 
41 #include <pcl/gpu/kinfu_large_scale/standalone_marching_cubes.h>
42 
43 ///////////////////////////////////////////////////////////////////////////////
44 template <typename PointT>
45 pcl::gpu::kinfuLS::StandaloneMarchingCubes<PointT>::StandaloneMarchingCubes (int new_voxels_x, int new_voxels_y, int new_voxels_z, float new_volume_size)
46 {
47  voxels_x_ = new_voxels_x;
48  voxels_y_ = new_voxels_y;
49  voxels_z_ = new_voxels_z;
50  volume_size_ = new_volume_size;
51 
52  ///Creating GPU TSDF Volume instance
53  const Eigen::Vector3f volume_size = Eigen::Vector3f::Constant (volume_size_);
54  // std::cout << "VOLUME SIZE IS " << volume_size_ << std::endl;
55  const Eigen::Vector3i volume_resolution (voxels_x_, voxels_y_, voxels_z_);
56  tsdf_volume_gpu_ = TsdfVolume::Ptr ( new TsdfVolume (volume_resolution) );
57  tsdf_volume_gpu_->setSize (volume_size);
58 
59  ///Creating CPU TSDF Volume instance
60  int tsdf_total_size = voxels_x_ * voxels_y_ * voxels_z_;
61  tsdf_volume_cpu_= std::vector<int> (tsdf_total_size,0);
62 
63  mesh_counter_ = 0;
64 }
65 
66 ///////////////////////////////////////////////////////////////////////////////
67 
68 template <typename PointT> typename pcl::gpu::kinfuLS::StandaloneMarchingCubes<PointT>::MeshPtr
70 {
71 
72  //Clearing TSDF GPU and cPU
73  std::cout << "VOLUME SIZE IS " << volume_size_ << std::endl;
74 
75  //Clear values in TSDF Volume GPU
76  tsdf_volume_gpu_->reset (); // This one uses the same tsdf volume but clears it before loading new values. This one is our friend.
77 
78  //Clear values in TSDF Volume CPU
79  fill (tsdf_volume_cpu_.begin (), tsdf_volume_cpu_.end (), 0);
80 
81  //Loading values to GPU
82  loadTsdfCloudToGPU (cloud);
83 
84  //Creating and returning mesh
85  return ( runMarchingCubes () );
86 
87 }
88 
89 ///////////////////////////////////////////////////////////////////////////////
90 
91 //template <typename PointT> std::vector< typename pcl::gpu::StandaloneMarchingCubes<PointT>::MeshPtr >
92 template <typename PointT> void
93 pcl::gpu::kinfuLS::StandaloneMarchingCubes<PointT>::getMeshesFromTSDFVector (const std::vector<PointCloudPtr> &tsdf_clouds, const std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > &tsdf_offsets)
94 {
95  std::vector< MeshPtr > meshes_vector;
96 
97  int max_iterations = std::min( tsdf_clouds.size (), tsdf_offsets.size () ); //Safety check
98  PCL_INFO ("There are %d cubes to be processed \n", max_iterations);
99  float cell_size = volume_size_ / voxels_x_;
100 
101  int mesh_counter = 0;
102 
103  for(int i = 0; i < max_iterations; ++i)
104  {
105  PCL_INFO ("Processing cube number %d\n", i);
106 
107  //Making cloud local
108  Eigen::Affine3f cloud_transform;
109 
110  float originX = (tsdf_offsets[i]).x();
111  float originY = (tsdf_offsets[i]).y();
112  float originZ = (tsdf_offsets[i]).z();
113 
114  cloud_transform.linear ().setIdentity ();
115  cloud_transform.translation ()[0] = -originX;
116  cloud_transform.translation ()[1] = -originY;
117  cloud_transform.translation ()[2] = -originZ;
118 
119  transformPointCloud (*tsdf_clouds[i], *tsdf_clouds[i], cloud_transform);
120 
121  //Get mesh
122  MeshPtr tmp = getMeshFromTSDFCloud (*tsdf_clouds[i]);
123 
124  if(tmp != nullptr)
125  {
126  meshes_vector.push_back (tmp);
127  mesh_counter++;
128  }
129  else
130  {
131  PCL_INFO ("This cloud returned no faces, we skip it!\n");
132  continue;
133  }
134 
135  //Making cloud global
136  cloud_transform.translation ()[0] = originX * cell_size;
137  cloud_transform.translation ()[1] = originY * cell_size;
138  cloud_transform.translation ()[2] = originZ * cell_size;
139 
141  fromPCLPointCloud2 ( (meshes_vector.back () )->cloud, *cloud_tmp_ptr);
142 
143  transformPointCloud (*cloud_tmp_ptr, *cloud_tmp_ptr, cloud_transform);
144 
145  toPCLPointCloud2 (*cloud_tmp_ptr, (meshes_vector.back () )->cloud);
146 
147  std::stringstream name;
148  name << "mesh_" << mesh_counter << ".ply";
149  PCL_INFO ("Saving mesh...%d \n", mesh_counter);
150  pcl::io::savePLYFile (name.str (), *(meshes_vector.back ()));
151 
152  }
153  return;
154 }
155 
156 ///////////////////////////////////////////////////////////////////////////////
157 
158 template <typename PointT> pcl::gpu::kinfuLS::TsdfVolume::Ptr
160 {
161  return (tsdf_volume_gpu_);
162 }
163 
164 ///////////////////////////////////////////////////////////////////////////////
165 
166 template <typename PointT> std::vector<int>& //todo
168 {
169  return (tsdf_volume_cpu_);
170 }
171 
172 ///////////////////////////////////////////////////////////////////////////////
173 
174 template <typename PointT> void
176 {
177  //Converting Values
178  convertTsdfVectors (cloud, tsdf_volume_cpu_);
179 
180  //Uploading data to GPU
181  int cubeColumns = voxels_x_;
182  tsdf_volume_gpu_->data ().upload (tsdf_volume_cpu_, cubeColumns);
183 }
184 
185 ///////////////////////////////////////////////////////////////////////////////
186 
187 template <typename PointT> void
189 {
190  const int DIVISOR = 32767; // SHRT_MAX;
191 
192  ///For every point in the cloud
193 #pragma omp parallel for
194 
195  for(int i = 0; i < (int) cloud.points.size (); ++i)
196  {
197  int x = cloud.points[i].x;
198  int y = cloud.points[i].y;
199  int z = cloud.points[i].z;
200 
201  if(x > 0 && x < voxels_x_ && y > 0 && y < voxels_y_ && z > 0 && z < voxels_z_)
202  {
203  ///Calculate the index to write to
204  int dst_index = x + voxels_x_ * y + voxels_y_ * voxels_x_ * z;
205 
206  short2& elem = *reinterpret_cast<short2*> (&output[dst_index]);
207  elem.x = static_cast<short> (cloud.points[i].intensity * DIVISOR);
208  elem.y = static_cast<short> (1);
209  }
210  }
211 }
212 
213 ///////////////////////////////////////////////////////////////////////////////
214 
215 template <typename PointT> typename pcl::gpu::kinfuLS::StandaloneMarchingCubes<PointT>::MeshPtr
217 {
218  if (triangles.empty () )
219  {
220  return MeshPtr ();
221  }
222 
224  cloud.width = (int)triangles.size ();
225  cloud.height = 1;
226  triangles.download (cloud.points);
227 
228  boost::shared_ptr<pcl::PolygonMesh> mesh_ptr ( new pcl::PolygonMesh () );
229 
230  pcl::toPCLPointCloud2 (cloud, mesh_ptr->cloud);
231 
232  mesh_ptr->polygons.resize (triangles.size () / 3);
233  for (size_t i = 0; i < mesh_ptr->polygons.size (); ++i)
234  {
235  pcl::Vertices v;
236  v.vertices.push_back (i*3+0);
237  v.vertices.push_back (i*3+2);
238  v.vertices.push_back (i*3+1);
239  mesh_ptr->polygons[i] = v;
240  }
241  return (mesh_ptr);
242 }
243 
244 ///////////////////////////////////////////////////////////////////////////////
245 
246 template <typename PointT> typename pcl::gpu::kinfuLS::StandaloneMarchingCubes<PointT>::MeshPtr
248 {
249  //Preparing the pointers and variables
250  const TsdfVolume::Ptr tsdf_volume_const_ = tsdf_volume_gpu_;
251  pcl::gpu::DeviceArray<pcl::PointXYZ> triangles_buffer_device_;
252 
253  //Creating Marching cubes instance
254  MarchingCubes::Ptr marching_cubes_ = MarchingCubes::Ptr ( new MarchingCubes() );
255 
256  //Running marching cubes
257  pcl::gpu::DeviceArray<pcl::PointXYZ> triangles_device = marching_cubes_->run (*tsdf_volume_const_, triangles_buffer_device_);
258 
259  //Creating mesh
260  boost::shared_ptr<pcl::PolygonMesh> mesh_ptr_ = convertTrianglesToMesh (triangles_device);
261 
262  if(mesh_ptr_ != nullptr)
263  {
265  fromPCLPointCloud2 ( mesh_ptr_->cloud, *cloud_tmp_ptr);
266  }
267  return (mesh_ptr_);
268 }
269 
270 ///////////////////////////////////////////////////////////////////////////////
271 
272 #endif // PCL_STANDALONE_MARCHING_CUBES_IMPL_HPP_
273 
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<T> object using a field_map...
Definition: conversions.h:168
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:409
std::vector< uint32_t > vertices
Definition: Vertices.h:19
boost::shared_ptr< TsdfVolume > Ptr
Definition: tsdf_volume.h:64
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.
Definition: ply_io.h:809
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
uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:414
Describes a set of vertices in a polygon mesh, by basically storing an array of indices.
Definition: Vertices.h:14
MarchingCubes implements MarchingCubes functionality for TSDF volume on GPU.
uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:412
boost::shared_ptr< MarchingCubes > Ptr
Smart pointer.
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.
Definition: transforms.hpp:215
TsdfVolume class.
Definition: tsdf_volume.h:61
boost::shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:427
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<T> object to a PCLPointCloud2 binary data blob.
Definition: conversions.h:241
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.