Point Cloud Library (PCL)  1.9.1
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Modules Pages
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  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_);
76 
77  //Clear values in TSDF Volume GPU
78  tsdf_volume_gpu_->reset (); // This one uses the same tsdf volume but clears it before loading new values. This one is our friend.
79 
80  //Clear values in TSDF Volume CPU
81  fill (tsdf_volume_cpu_.begin (), tsdf_volume_cpu_.end (), 0);
82 
83  //Loading values to GPU
84  loadTsdfCloudToGPU (cloud);
85 
86  //Creating and returning mesh
87  return ( runMarchingCubes () );
88 
89 }
90 
91 ///////////////////////////////////////////////////////////////////////////////
92 
93 //template <typename PointT> std::vector< typename pcl::gpu::StandaloneMarchingCubes<PointT>::MeshPtr >
94 template <typename PointT> void
95 pcl::gpu::kinfuLS::StandaloneMarchingCubes<PointT>::getMeshesFromTSDFVector (const std::vector<PointCloudPtr> &tsdf_clouds, const std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > &tsdf_offsets)
96 {
97  std::vector< MeshPtr > meshes_vector;
98 
99  int max_iterations = std::min( tsdf_clouds.size (), tsdf_offsets.size () ); //Safety check
100  PCL_INFO ("There are %d cubes to be processed \n", max_iterations);
101  float cell_size = volume_size_ / voxels_x_;
102 
103  int mesh_counter = 0;
104 
105  for(int i = 0; i < max_iterations; ++i)
106  {
107  PCL_INFO ("Processing cube number %d\n", i);
108 
109  //Making cloud local
110  Eigen::Affine3f cloud_transform;
111 
112  float originX = (tsdf_offsets[i]).x();
113  float originY = (tsdf_offsets[i]).y();
114  float originZ = (tsdf_offsets[i]).z();
115 
116  cloud_transform.linear ().setIdentity ();
117  cloud_transform.translation ()[0] = -originX;
118  cloud_transform.translation ()[1] = -originY;
119  cloud_transform.translation ()[2] = -originZ;
120 
121  transformPointCloud (*tsdf_clouds[i], *tsdf_clouds[i], cloud_transform);
122 
123  //Get mesh
124  MeshPtr tmp = getMeshFromTSDFCloud (*tsdf_clouds[i]);
125 
126  if(tmp != 0)
127  {
128  meshes_vector.push_back (tmp);
129  mesh_counter++;
130  }
131  else
132  {
133  PCL_INFO ("This cloud returned no faces, we skip it!\n");
134  continue;
135  }
136 
137  //Making cloud global
138  cloud_transform.translation ()[0] = originX * cell_size;
139  cloud_transform.translation ()[1] = originY * cell_size;
140  cloud_transform.translation ()[2] = originZ * cell_size;
141 
143  fromPCLPointCloud2 ( (meshes_vector.back () )->cloud, *cloud_tmp_ptr);
144 
145  transformPointCloud (*cloud_tmp_ptr, *cloud_tmp_ptr, cloud_transform);
146 
147  toPCLPointCloud2 (*cloud_tmp_ptr, (meshes_vector.back () )->cloud);
148 
149  std::stringstream name;
150  name << "mesh_" << mesh_counter << ".ply";
151  PCL_INFO ("Saving mesh...%d \n", mesh_counter);
152  pcl::io::savePLYFile (name.str (), *(meshes_vector.back ()));
153 
154  }
155  return;
156 }
157 
158 ///////////////////////////////////////////////////////////////////////////////
159 
160 template <typename PointT> pcl::gpu::kinfuLS::TsdfVolume::Ptr
162 {
163  return (tsdf_volume_gpu_);
164 }
165 
166 ///////////////////////////////////////////////////////////////////////////////
167 
168 template <typename PointT> std::vector<int>& //todo
170 {
171  return (tsdf_volume_cpu_);
172 }
173 
174 ///////////////////////////////////////////////////////////////////////////////
175 
176 template <typename PointT> void
178 {
179  //Converting Values
180  convertTsdfVectors (cloud, tsdf_volume_cpu_);
181 
182  //Uploading data to GPU
183  int cubeColumns = voxels_x_;
184  tsdf_volume_gpu_->data ().upload (tsdf_volume_cpu_, cubeColumns);
185 }
186 
187 ///////////////////////////////////////////////////////////////////////////////
188 
189 template <typename PointT> void
191 {
192  const int DIVISOR = 32767; // SHRT_MAX;
193 
194  ///For every point in the cloud
195 #pragma omp parallel for
196 
197  for(int i = 0; i < (int) cloud.points.size (); ++i)
198  {
199  int x = cloud.points[i].x;
200  int y = cloud.points[i].y;
201  int z = cloud.points[i].z;
202 
203  if(x > 0 && x < voxels_x_ && y > 0 && y < voxels_y_ && z > 0 && z < voxels_z_)
204  {
205  ///Calculate the index to write to
206  int dst_index = x + voxels_x_ * y + voxels_y_ * voxels_x_ * z;
207 
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);
211  }
212  }
213 }
214 
215 ///////////////////////////////////////////////////////////////////////////////
216 
217 template <typename PointT> typename pcl::gpu::kinfuLS::StandaloneMarchingCubes<PointT>::MeshPtr
219 {
220  if (triangles.empty () )
221  {
222  return MeshPtr ();
223  }
224 
226  cloud.width = (int)triangles.size ();
227  cloud.height = 1;
228  triangles.download (cloud.points);
229 
230  boost::shared_ptr<pcl::PolygonMesh> mesh_ptr ( new pcl::PolygonMesh () );
231 
232  pcl::toPCLPointCloud2 (cloud, mesh_ptr->cloud);
233 
234  mesh_ptr->polygons.resize (triangles.size () / 3);
235  for (size_t i = 0; i < mesh_ptr->polygons.size (); ++i)
236  {
237  pcl::Vertices v;
238  v.vertices.push_back (i*3+0);
239  v.vertices.push_back (i*3+2);
240  v.vertices.push_back (i*3+1);
241  mesh_ptr->polygons[i] = v;
242  }
243  return (mesh_ptr);
244 }
245 
246 ///////////////////////////////////////////////////////////////////////////////
247 
248 template <typename PointT> typename pcl::gpu::kinfuLS::StandaloneMarchingCubes<PointT>::MeshPtr
250 {
251  //Preparing the pointers and variables
252  const TsdfVolume::Ptr tsdf_volume_const_ = tsdf_volume_gpu_;
253  pcl::gpu::DeviceArray<pcl::PointXYZ> triangles_buffer_device_;
254 
255  //Creating Marching cubes instance
256  MarchingCubes::Ptr marching_cubes_ = MarchingCubes::Ptr ( new MarchingCubes() );
257 
258  //Running marching cubes
259  pcl::gpu::DeviceArray<pcl::PointXYZ> triangles_device = marching_cubes_->run (*tsdf_volume_const_, triangles_buffer_device_);
260 
261  //Creating mesh
262  boost::shared_ptr<pcl::PolygonMesh> mesh_ptr_ = convertTrianglesToMesh (triangles_device);
263 
264  if(mesh_ptr_ != 0)
265  {
267  fromPCLPointCloud2 ( mesh_ptr_->cloud, *cloud_tmp_ptr);
268  }
269  return (mesh_ptr_);
270 }
271 
272 ///////////////////////////////////////////////////////////////////////////////
273 
274 #endif // PCL_STANDALONE_MARCHING_CUBES_IMPL_HPP_
275 
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...
Definition: conversions.h:169
boost::shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:428
uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:413
std::vector< uint32_t > vertices
Definition: Vertices.h:19
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:814
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.
Definition: Vertices.h:14
MarchingCubes implements MarchingCubes functionality for TSDF volume on GPU.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:410
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:62
boost::shared_ptr< MarchingCubes > Ptr
Smart pointer.
boost::shared_ptr< TsdfVolume > Ptr
Definition: tsdf_volume.h:65
uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:415
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.
Definition: conversions.h:242
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.