standalone_marching_cubes.hpp
/*
* Software License Agreement (BSD License)
*
* Point Cloud Library (PCL) - www.pointclouds.org
* Copyright (c) 2011, Willow Garage, Inc.
*
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
*/
#ifndef PCL_STANDALONE_MARCHING_CUBES_IMPL_HPP_
#define PCL_STANDALONE_MARCHING_CUBES_IMPL_HPP_
#include <pcl/gpu/kinfu_large_scale/standalone_marching_cubes.h>
///////////////////////////////////////////////////////////////////////////////
template <typename PointT>
pcl::gpu::kinfuLS::StandaloneMarchingCubes<PointT>::StandaloneMarchingCubes (int new_voxels_x, int new_voxels_y, int new_voxels_z, float new_volume_size)
{
voxels_x_ = new_voxels_x;
voxels_y_ = new_voxels_y;
voxels_z_ = new_voxels_z;
volume_size_ = new_volume_size;
///Creating GPU TSDF Volume instance
const Eigen::Vector3f volume_size = Eigen::Vector3f::Constant (volume_size_);
// std::cout << "VOLUME SIZE IS " << volume_size_ << std::endl;
const Eigen::Vector3i volume_resolution (voxels_x_, voxels_y_, voxels_z_);
tsdf_volume_gpu_ = TsdfVolume::Ptr ( new TsdfVolume (volume_resolution) );
tsdf_volume_gpu_->setSize (volume_size);
///Creating CPU TSDF Volume instance
int tsdf_total_size = voxels_x_ * voxels_y_ * voxels_z_;
tsdf_volume_cpu_= std::vector<int> (tsdf_total_size,0);
mesh_counter_ = 0;
}
///////////////////////////////////////////////////////////////////////////////
template <typename PointT> typename pcl::gpu::kinfuLS::StandaloneMarchingCubes<PointT>::MeshPtr
pcl::gpu::kinfuLS::StandaloneMarchingCubes<PointT>::getMeshFromTSDFCloud (const PointCloud &cloud)
{
//Clearing TSDF GPU and cPU
const Eigen::Vector3f volume_size = Eigen::Vector3f::Constant (volume_size_);
std::cout << "VOLUME SIZE IS " << volume_size_ << std::endl;
const Eigen::Vector3i volume_resolution (voxels_x_, voxels_y_, voxels_z_);
//Clear values in TSDF Volume GPU
tsdf_volume_gpu_->reset (); // This one uses the same tsdf volume but clears it before loading new values. This one is our friend.
//Clear values in TSDF Volume CPU
fill (tsdf_volume_cpu_.begin (), tsdf_volume_cpu_.end (), 0);
//Loading values to GPU
loadTsdfCloudToGPU (cloud);
//Creating and returning mesh
return ( runMarchingCubes () );
}
///////////////////////////////////////////////////////////////////////////////
//template <typename PointT> std::vector< typename pcl::gpu::StandaloneMarchingCubes<PointT>::MeshPtr >
template <typename PointT> void
pcl::gpu::kinfuLS::StandaloneMarchingCubes<PointT>::getMeshesFromTSDFVector (const std::vector<PointCloudPtr> &tsdf_clouds, const std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > &tsdf_offsets)
{
std::vector< MeshPtr > meshes_vector;
int max_iterations = std::min( tsdf_clouds.size (), tsdf_offsets.size () ); //Safety check
PCL_INFO ("There are %d cubes to be processed \n", max_iterations);
float cell_size = volume_size_ / voxels_x_;
int mesh_counter = 0;
for(int i = 0; i < max_iterations; ++i)
{
PCL_INFO ("Processing cube number %d\n", i);
//Making cloud local
Eigen::Affine3f cloud_transform;
float originX = (tsdf_offsets[i]).x();
float originY = (tsdf_offsets[i]).y();
float originZ = (tsdf_offsets[i]).z();
cloud_transform.linear ().setIdentity ();
cloud_transform.translation ()[0] = -originX;
cloud_transform.translation ()[1] = -originY;
cloud_transform.translation ()[2] = -originZ;
transformPointCloud (*tsdf_clouds[i], *tsdf_clouds[i], cloud_transform);
//Get mesh
MeshPtr tmp = getMeshFromTSDFCloud (*tsdf_clouds[i]);
if(tmp != 0)
{
meshes_vector.push_back (tmp);
mesh_counter++;
}
else
{
PCL_INFO ("This cloud returned no faces, we skip it!\n");
continue;
}
//Making cloud global
cloud_transform.translation ()[0] = originX * cell_size;
cloud_transform.translation ()[1] = originY * cell_size;
cloud_transform.translation ()[2] = originZ * cell_size;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_tmp_ptr (new pcl::PointCloud<pcl::PointXYZ>);
fromPCLPointCloud2 ( (meshes_vector.back () )->cloud, *cloud_tmp_ptr);
transformPointCloud (*cloud_tmp_ptr, *cloud_tmp_ptr, cloud_transform);
toPCLPointCloud2 (*cloud_tmp_ptr, (meshes_vector.back () )->cloud);
std::stringstream name;
name << "mesh_" << mesh_counter << ".ply";
PCL_INFO ("Saving mesh...%d \n", mesh_counter);
pcl::io::savePLYFile (name.str (), *(meshes_vector.back ()));
}
return;
}
///////////////////////////////////////////////////////////////////////////////
template <typename PointT> pcl::gpu::kinfuLS::TsdfVolume::Ptr
pcl::gpu::kinfuLS::StandaloneMarchingCubes<PointT>::tsdfVolumeGPU ()
{
return (tsdf_volume_gpu_);
}
///////////////////////////////////////////////////////////////////////////////
template <typename PointT> std::vector<int>& //todo
pcl::gpu::kinfuLS::StandaloneMarchingCubes<PointT>::tsdfVolumeCPU ()
{
return (tsdf_volume_cpu_);
}
///////////////////////////////////////////////////////////////////////////////
template <typename PointT> void
pcl::gpu::kinfuLS::StandaloneMarchingCubes<PointT>::loadTsdfCloudToGPU (const PointCloud &cloud)
{
//Converting Values
convertTsdfVectors (cloud, tsdf_volume_cpu_);
//Uploading data to GPU
int cubeColumns = voxels_x_;
tsdf_volume_gpu_->data ().upload (tsdf_volume_cpu_, cubeColumns);
}
///////////////////////////////////////////////////////////////////////////////
template <typename PointT> void
pcl::gpu::kinfuLS::StandaloneMarchingCubes<PointT>::convertTsdfVectors (const PointCloud &cloud, std::vector<int> &output)
{
const int DIVISOR = 32767; // SHRT_MAX;
///For every point in the cloud
#pragma omp parallel for
for(int i = 0; i < (int) cloud.points.size (); ++i)
{
int x = cloud.points[i].x;
int y = cloud.points[i].y;
int z = cloud.points[i].z;
if(x > 0 && x < voxels_x_ && y > 0 && y < voxels_y_ && z > 0 && z < voxels_z_)
{
///Calculate the index to write to
int dst_index = x + voxels_x_ * y + voxels_y_ * voxels_x_ * z;
short2& elem = *reinterpret_cast<short2*> (&output[dst_index]);
elem.x = static_cast<short> (cloud.points[i].intensity * DIVISOR);
elem.y = static_cast<short> (1);
}
}
}
///////////////////////////////////////////////////////////////////////////////
template <typename PointT> typename pcl::gpu::kinfuLS::StandaloneMarchingCubes<PointT>::MeshPtr
pcl::gpu::kinfuLS::StandaloneMarchingCubes<PointT>::convertTrianglesToMesh (const pcl::gpu::DeviceArray<pcl::PointXYZ>& triangles)
{
if (triangles.empty () )
{
return MeshPtr ();
}
pcl::PointCloud<pcl::PointXYZ> cloud;
cloud.width = (int)triangles.size ();
cloud.height = 1;
triangles.download (cloud.points);
boost::shared_ptr<pcl::PolygonMesh> mesh_ptr ( new pcl::PolygonMesh () );
pcl::toPCLPointCloud2 (cloud, mesh_ptr->cloud);
mesh_ptr->polygons.resize (triangles.size () / 3);
for (size_t i = 0; i < mesh_ptr->polygons.size (); ++i)
{
pcl::Vertices v;
v.vertices.push_back (i*3+0);
v.vertices.push_back (i*3+2);
v.vertices.push_back (i*3+1);
mesh_ptr->polygons[i] = v;
}
return (mesh_ptr);
}
///////////////////////////////////////////////////////////////////////////////
template <typename PointT> typename pcl::gpu::kinfuLS::StandaloneMarchingCubes<PointT>::MeshPtr
pcl::gpu::kinfuLS::StandaloneMarchingCubes<PointT>::runMarchingCubes ()
{
//Preparing the pointers and variables
const TsdfVolume::Ptr tsdf_volume_const_ = tsdf_volume_gpu_;
pcl::gpu::DeviceArray<pcl::PointXYZ> triangles_buffer_device_;
//Creating Marching cubes instance
MarchingCubes::Ptr marching_cubes_ = MarchingCubes::Ptr ( new MarchingCubes() );
//Running marching cubes
pcl::gpu::DeviceArray<pcl::PointXYZ> triangles_device = marching_cubes_->run (*tsdf_volume_const_, triangles_buffer_device_);
//Creating mesh
boost::shared_ptr<pcl::PolygonMesh> mesh_ptr_ = convertTrianglesToMesh (triangles_device);
if(mesh_ptr_ != 0)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_tmp_ptr (new pcl::PointCloud<pcl::PointXYZ>);
fromPCLPointCloud2 ( mesh_ptr_->cloud, *cloud_tmp_ptr);
}
return (mesh_ptr_);
}
///////////////////////////////////////////////////////////////////////////////
#endif // PCL_STANDALONE_MARCHING_CUBES_IMPL_HPP_
pcl::gpu::kinfuLS::StandaloneMarchingCubes::convertTsdfVectors
void convertTsdfVectors(const PointCloud &cloud, std::vector< int > &output)
Read the data in the point cloud.
Definition: standalone_marching_cubes.hpp:190
pcl::fromPCLPointCloud2
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:169
pcl::gpu::DeviceMemory::empty
bool empty() const
Returns true if unallocated otherwise false.
pcl::PointCloud::points
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:410
pcl::Vertices::vertices
std::vector< uint32_t > vertices
Definition: Vertices.h:19
pcl::io::savePLYFile
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:804
pcl::gpu::kinfuLS::StandaloneMarchingCubes::tsdfVolumeCPU
std::vector< int > & tsdfVolumeCPU()
Returns the associated Tsdf Volume buffer in CPU.
Definition: standalone_marching_cubes.hpp:169
pcl::gpu::kinfuLS::StandaloneMarchingCubes::getMeshesFromTSDFVector
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.
Definition: standalone_marching_cubes.hpp:95
pcl::gpu::kinfuLS::StandaloneMarchingCubes::MeshPtr
boost::shared_ptr< pcl::PolygonMesh > MeshPtr
Definition: standalone_marching_cubes.h:83
pcl::PointCloud::height
uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:415
pcl::Vertices
Describes a set of vertices in a polygon mesh, by basically storing an array of indices.
Definition: Vertices.h:14
pcl::gpu::kinfuLS::MarchingCubes
MarchingCubes implements MarchingCubes functionality for TSDF volume on GPU.
Definition: marching_cubes.h:58
pcl::PointCloud::Ptr
boost::shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:428
pcl::gpu::DeviceArray< pcl::PointXYZ >
pcl::PointCloud::width
uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:413
pcl::transformPointCloud
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:42
pcl::gpu::kinfuLS::TsdfVolume
TsdfVolume class.
Definition: tsdf_volume.h:62
pcl::gpu::kinfuLS::MarchingCubes::Ptr
boost::shared_ptr< MarchingCubes > Ptr
Smart pointer.
Definition: marching_cubes.h:73
pcl::gpu::kinfuLS::TsdfVolume::Ptr
boost::shared_ptr< TsdfVolume > Ptr
Definition: tsdf_volume.h:65
pcl::gpu::DeviceArray::download
void download(T *host_ptr) const
Downloads data from internal buffer to CPU memory.
Definition: device_array.hpp:59
pcl::PointCloud
PointCloud represents the base class in PCL for storing collections of 3D points. ...
Definition: projection_matrix.h:53
pcl::gpu::kinfuLS::StandaloneMarchingCubes::getMeshFromTSDFCloud
MeshPtr getMeshFromTSDFCloud(const PointCloud &cloud)
Run marching cubes in a TSDF cloud and returns a PolygonMesh.
Definition: standalone_marching_cubes.hpp:69
pcl::toPCLPointCloud2
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:242
pcl::PolygonMesh
Definition: PolygonMesh.h:15
pcl::gpu::DeviceArray::size
size_t size() const
Returns size in elements.
Definition: device_array.hpp:66
pcl::gpu::kinfuLS::StandaloneMarchingCubes::convertTrianglesToMesh
MeshPtr convertTrianglesToMesh(const pcl::gpu::DeviceArray< pcl::PointXYZ > &triangles)
Converts the triangles buffer device to a PolygonMesh.
Definition: standalone_marching_cubes.hpp:218
pcl::gpu::kinfuLS::StandaloneMarchingCubes::tsdfVolumeGPU
TsdfVolume::Ptr tsdfVolumeGPU()
Returns the associated Tsdf Volume buffer in GPU.
Definition: standalone_marching_cubes.hpp:161
pcl::gpu::kinfuLS::StandaloneMarchingCubes::runMarchingCubes
MeshPtr runMarchingCubes()
Runs marching cubes on the data that is contained in the TSDF Volume in GPU.
Definition: standalone_marching_cubes.hpp:249
pcl::gpu::kinfuLS::StandaloneMarchingCubes::loadTsdfCloudToGPU
void loadTsdfCloudToGPU(const PointCloud &cloud)
Loads a TSDF Cloud to the TSDF Volume in GPU.
Definition: standalone_marching_cubes.hpp:177
pcl::gpu::kinfuLS::StandaloneMarchingCubes::StandaloneMarchingCubes
StandaloneMarchingCubes(int voxels_x=512, int voxels_y=512, int voxels_z=512, float volume_size=3.0f)
Constructor.
Definition: standalone_marching_cubes.hpp:45