36 #ifndef PCL_SURFACE_IMPL_MARCHING_CUBES_H_
37 #define PCL_SURFACE_IMPL_MARCHING_CUBES_H_
39 #include <pcl/surface/marching_cubes.h>
40 #include <pcl/common/common.h>
41 #include <pcl/common/vector_average.h>
42 #include <pcl/Vertices.h>
43 #include <pcl/kdtree/kdtree_flann.h>
46 template <
typename Po
intNT>
48 : min_p_ (), max_p_ (), percentage_extend_grid_ (), iso_level_ ()
53 template <
typename Po
intNT>
59 template <
typename Po
intNT>
void
64 min_p_ -= (max_p_ - min_p_) * percentage_extend_grid_/2;
65 max_p_ += (max_p_ - min_p_) * percentage_extend_grid_/2;
67 Eigen::Vector4f bounding_box_size = max_p_ - min_p_;
69 bounding_box_size = max_p_ - min_p_;
70 PCL_DEBUG (
"[pcl::MarchingCubesHoppe::getBoundingBox] Size of Bounding Box is [%f, %f, %f]\n",
71 bounding_box_size.x (), bounding_box_size.y (), bounding_box_size.z ());
73 (std::max) ((std::max)(bounding_box_size.x (), bounding_box_size.y ()),
74 bounding_box_size.z ());
78 PCL_DEBUG (
"[pcl::MarchingCubesHoppe::getBoundingBox] Lower left point is [%f, %f, %f]\n",
79 min_p_.x (), min_p_.y (), min_p_.z ());
80 PCL_DEBUG (
"[pcl::MarchingCubesHoppe::getBoundingBox] Upper left point is [%f, %f, %f]\n",
81 max_p_.x (), max_p_.y (), max_p_.z ());
86 template <
typename Po
intNT>
void
91 Eigen::Vector3f &output)
93 float mu = (iso_level_ - val_p1) / (val_p2-val_p1);
94 output = p1 + mu * (p2 - p1);
99 template <
typename Po
intNT>
void
101 Eigen::Vector3i &index_3d,
105 Eigen::Vector3f vertex_list[12];
106 if (leaf_node[0] < iso_level_) cubeindex |= 1;
107 if (leaf_node[1] < iso_level_) cubeindex |= 2;
108 if (leaf_node[2] < iso_level_) cubeindex |= 4;
109 if (leaf_node[3] < iso_level_) cubeindex |= 8;
110 if (leaf_node[4] < iso_level_) cubeindex |= 16;
111 if (leaf_node[5] < iso_level_) cubeindex |= 32;
112 if (leaf_node[6] < iso_level_) cubeindex |= 64;
113 if (leaf_node[7] < iso_level_) cubeindex |= 128;
120 Eigen::Vector3f center;
121 center[0] = min_p_[0] + (max_p_[0] - min_p_[0]) *
float (index_3d[0]) / float (res_x_);
122 center[1] = min_p_[1] + (max_p_[1] - min_p_[1]) *
float (index_3d[1]) / float (res_y_);
123 center[2] = min_p_[2] + (max_p_[2] - min_p_[2]) *
float (index_3d[2]) / float (res_z_);
125 std::vector<Eigen::Vector3f> p;
127 for (
int i = 0; i < 8; ++i)
129 Eigen::Vector3f point = center;
131 point[1] =
static_cast<float> (center[1] + (max_p_[1] - min_p_[1]) /
float (res_y_));
134 point[2] =
static_cast<float> (center[2] + (max_p_[2] - min_p_[2]) /
float (res_z_));
136 if((i & 0x1) ^ ((i >> 1) & 0x1))
137 point[0] = static_cast<float> (center[0] + (max_p_[0] - min_p_[0]) / float (res_x_));
145 interpolateEdge (p[0], p[1], leaf_node[0], leaf_node[1], vertex_list[0]);
147 interpolateEdge (p[1], p[2], leaf_node[1], leaf_node[2], vertex_list[1]);
149 interpolateEdge (p[2], p[3], leaf_node[2], leaf_node[3], vertex_list[2]);
151 interpolateEdge (p[3], p[0], leaf_node[3], leaf_node[0], vertex_list[3]);
153 interpolateEdge (p[4], p[5], leaf_node[4], leaf_node[5], vertex_list[4]);
155 interpolateEdge (p[5], p[6], leaf_node[5], leaf_node[6], vertex_list[5]);
157 interpolateEdge (p[6], p[7], leaf_node[6], leaf_node[7], vertex_list[6]);
159 interpolateEdge (p[7], p[4], leaf_node[7], leaf_node[4], vertex_list[7]);
161 interpolateEdge (p[0], p[4], leaf_node[0], leaf_node[4], vertex_list[8]);
163 interpolateEdge (p[1], p[5], leaf_node[1], leaf_node[5], vertex_list[9]);
165 interpolateEdge (p[2], p[6], leaf_node[2], leaf_node[6], vertex_list[10]);
167 interpolateEdge (p[3], p[7], leaf_node[3], leaf_node[7], vertex_list[11]);
170 for (
int i = 0;
triTable[cubeindex][i] != -1; i+=3)
173 p1.x = vertex_list[
triTable[cubeindex][i ]][0];
174 p1.y = vertex_list[triTable[cubeindex][i ]][1];
175 p1.z = vertex_list[triTable[cubeindex][i ]][2];
177 p2.x = vertex_list[triTable[cubeindex][i+1]][0];
178 p2.y = vertex_list[triTable[cubeindex][i+1]][1];
179 p2.z = vertex_list[triTable[cubeindex][i+1]][2];
181 p3.x = vertex_list[triTable[cubeindex][i+2]][0];
182 p3.y = vertex_list[triTable[cubeindex][i+2]][1];
183 p3.z = vertex_list[triTable[cubeindex][i+2]][2];
190 template <
typename Po
intNT>
void
192 Eigen::Vector3i &index3d)
194 leaf = std::vector<float> (8, 0.0f);
196 leaf[0] = getGridValue (index3d);
197 leaf[1] = getGridValue (index3d + Eigen::Vector3i (1, 0, 0));
198 leaf[2] = getGridValue (index3d + Eigen::Vector3i (1, 0, 1));
199 leaf[3] = getGridValue (index3d + Eigen::Vector3i (0, 0, 1));
200 leaf[4] = getGridValue (index3d + Eigen::Vector3i (0, 1, 0));
201 leaf[5] = getGridValue (index3d + Eigen::Vector3i (1, 1, 0));
202 leaf[6] = getGridValue (index3d + Eigen::Vector3i (1, 1, 1));
203 leaf[7] = getGridValue (index3d + Eigen::Vector3i (0, 1, 1));
208 template <
typename Po
intNT>
float
212 if (pos[0] < 0 || pos[0] >= res_x_)
214 if (pos[1] < 0 || pos[1] >= res_y_)
216 if (pos[2] < 0 || pos[2] >= res_z_)
219 return grid_[pos[0]*res_y_*res_z_ + pos[1]*res_z_ + pos[2]];
224 template <
typename Po
intNT>
void
227 if (!(iso_level_ >= 0 && iso_level_ < 1))
229 PCL_ERROR (
"[pcl::%s::performReconstruction] Invalid iso level %f! Please use a number between 0 and 1.\n", getClassName ().c_str (), iso_level_);
237 grid_ = std::vector<float> (res_x_*res_y_*res_z_, 0.0f);
240 tree_->setInputCloud (input_);
254 for (
int x = 1; x < res_x_-1; ++x)
255 for (
int y = 1; y < res_y_-1; ++y)
256 for (
int z = 1; z < res_z_-1; ++z)
258 Eigen::Vector3i index_3d (x, y, z);
259 std::vector<float> leaf_node;
260 getNeighborList1D (leaf_node, index_3d);
261 createSurface (leaf_node, index_3d, cloud);
266 for (
size_t i = 0; i < output.
polygons.size (); ++i)
270 for (
int j = 0; j < 3; ++j)
271 v.
vertices[j] = static_cast<int> (i) * 3 + j;
278 template <
typename Po
intNT>
void
280 std::vector<pcl::Vertices> &polygons)
282 if (!(iso_level_ >= 0 && iso_level_ < 1))
284 PCL_ERROR (
"[pcl::%s::performReconstruction] Invalid iso level %f! Please use a number between 0 and 1.\n", getClassName ().c_str (), iso_level_);
292 grid_ = std::vector<float> (res_x_*res_y_*res_z_, 0.0f);
295 tree_->setInputCloud (input_);
306 for (
int x = 1; x < res_x_-1; ++x)
307 for (
int y = 1; y < res_y_-1; ++y)
308 for (
int z = 1; z < res_z_-1; ++z)
310 Eigen::Vector3i index_3d (x, y, z);
311 std::vector<float> leaf_node;
312 getNeighborList1D (leaf_node, index_3d);
313 createSurface (leaf_node, index_3d, points);
316 polygons.resize (points.
size () / 3);
317 for (
size_t i = 0; i < polygons.size (); ++i)
321 for (
int j = 0; j < 3; ++j)
322 v.
vertices[j] = static_cast<int> (i) * 3 + j;
329 #define PCL_INSTANTIATE_MarchingCubes(T) template class PCL_EXPORTS pcl::MarchingCubes<T>;
331 #endif // PCL_SURFACE_IMPL_MARCHING_CUBES_H_
std::vector< ::pcl::Vertices > polygons
std::vector< uint32_t > vertices
void getMinMax3D(const pcl::PointCloud< PointT > &cloud, PointT &min_pt, PointT &max_pt)
Get the minimum and maximum values on each of the 3 (x-y-z) dimensions in a given pointcloud...
void toPCLPointCloud2(const pcl::PointCloud< PointT > &cloud, pcl::PCLPointCloud2 &msg)
Convert a pcl::PointCloud object to a PCLPointCloud2 binary data blob.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
void getNeighborList1D(std::vector< float > &leaf, Eigen::Vector3i &index3d)
Method that returns the scalar values of the neighbors of a given 3D position in the grid...
const int triTable[256][16]
virtual void performReconstruction(pcl::PolygonMesh &output)
Extract the surface.
uint32_t width
The point cloud width (if organized as an image-structure).
void createSurface(std::vector< float > &leaf_node, Eigen::Vector3i &index_3d, pcl::PointCloud< PointNT > &cloud)
Calculate out the corresponding polygons in the leaf node.
virtual ~MarchingCubes()
Destructor.
std::vector< pcl::uint8_t > data
const unsigned int edgeTable[256]
void clear()
Removes all points in a cloud and sets the width and height to 0.
void getBoundingBox()
Get the bounding box for the input data points.
uint32_t height
The point cloud height (if organized as an image-structure).
Describes a set of vertices in a polygon mesh, by basically storing an array of indices.
void interpolateEdge(Eigen::Vector3f &p1, Eigen::Vector3f &p2, float val_p1, float val_p2, Eigen::Vector3f &output)
Interpolate along the voxel edge.
virtual float getGridValue(Eigen::Vector3i pos)
Method that returns the scalar value at the given grid position.
MarchingCubes()
Constructor.
::pcl::PCLPointCloud2 cloud
void push_back(const PointT &pt)
Insert a new point in the cloud, at the end of the container.