36 #ifndef PCL_SURFACE_IMPL_MARCHING_CUBES_H_
37 #define PCL_SURFACE_IMPL_MARCHING_CUBES_H_
39 #include <pcl/surface/marching_cubes.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>
52 template <
typename Po
intNT>
void
55 PointNT max_pt, min_pt;
58 lower_boundary_ = min_pt.getArray3fMap ();
59 upper_boundary_ = max_pt.getArray3fMap ();
61 const Eigen::Array3f size3_extend = 0.5f * percentage_extend_grid_
62 * (upper_boundary_ - lower_boundary_);
64 lower_boundary_ -= size3_extend;
65 upper_boundary_ += size3_extend;
70 template <
typename Po
intNT>
void
75 Eigen::Vector3f &output)
77 const float mu = (iso_level_ - val_p1) / (val_p2 - val_p1);
78 output = p1 + mu * (p2 - p1);
83 template <
typename Po
intNT>
void
85 const Eigen::Vector3i &index_3d,
89 if (leaf_node[0] < iso_level_) cubeindex |= 1;
90 if (leaf_node[1] < iso_level_) cubeindex |= 2;
91 if (leaf_node[2] < iso_level_) cubeindex |= 4;
92 if (leaf_node[3] < iso_level_) cubeindex |= 8;
93 if (leaf_node[4] < iso_level_) cubeindex |= 16;
94 if (leaf_node[5] < iso_level_) cubeindex |= 32;
95 if (leaf_node[6] < iso_level_) cubeindex |= 64;
96 if (leaf_node[7] < iso_level_) cubeindex |= 128;
102 const Eigen::Vector3f center = lower_boundary_
103 + size_voxel_ * index_3d.cast<
float> ().array ();
105 std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > p;
107 for (
int i = 0; i < 8; ++i)
109 Eigen::Vector3f point = center;
111 point[1] =
static_cast<float> (center[1] + size_voxel_[1]);
114 point[2] =
static_cast<float> (center[2] + size_voxel_[2]);
116 if ((i & 0x1) ^ ((i >> 1) & 0x1))
117 point[0] =
static_cast<float> (center[0] + size_voxel_[0]);
123 std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > vertex_list;
124 vertex_list.resize (12);
126 interpolateEdge (p[0], p[1], leaf_node[0], leaf_node[1], vertex_list[0]);
128 interpolateEdge (p[1], p[2], leaf_node[1], leaf_node[2], vertex_list[1]);
130 interpolateEdge (p[2], p[3], leaf_node[2], leaf_node[3], vertex_list[2]);
132 interpolateEdge (p[3], p[0], leaf_node[3], leaf_node[0], vertex_list[3]);
134 interpolateEdge (p[4], p[5], leaf_node[4], leaf_node[5], vertex_list[4]);
136 interpolateEdge (p[5], p[6], leaf_node[5], leaf_node[6], vertex_list[5]);
138 interpolateEdge (p[6], p[7], leaf_node[6], leaf_node[7], vertex_list[6]);
140 interpolateEdge (p[7], p[4], leaf_node[7], leaf_node[4], vertex_list[7]);
142 interpolateEdge (p[0], p[4], leaf_node[0], leaf_node[4], vertex_list[8]);
144 interpolateEdge (p[1], p[5], leaf_node[1], leaf_node[5], vertex_list[9]);
146 interpolateEdge (p[2], p[6], leaf_node[2], leaf_node[6], vertex_list[10]);
148 interpolateEdge (p[3], p[7], leaf_node[3], leaf_node[7], vertex_list[11]);
151 for (
int i = 0;
triTable[cubeindex][i] != -1; i += 3)
154 p1.getVector3fMap () = vertex_list[
triTable[cubeindex][i]];
156 p2.getVector3fMap () = vertex_list[
triTable[cubeindex][i+1]];
158 p3.getVector3fMap () = vertex_list[
triTable[cubeindex][i+2]];
165 template <
typename Po
intNT>
void
167 Eigen::Vector3i &index3d)
171 leaf[0] = getGridValue (index3d);
172 leaf[1] = getGridValue (index3d + Eigen::Vector3i (1, 0, 0));
173 leaf[2] = getGridValue (index3d + Eigen::Vector3i (1, 0, 1));
174 leaf[3] = getGridValue (index3d + Eigen::Vector3i (0, 0, 1));
175 leaf[4] = getGridValue (index3d + Eigen::Vector3i (0, 1, 0));
176 leaf[5] = getGridValue (index3d + Eigen::Vector3i (1, 1, 0));
177 leaf[6] = getGridValue (index3d + Eigen::Vector3i (1, 1, 1));
178 leaf[7] = getGridValue (index3d + Eigen::Vector3i (0, 1, 1));
180 for (
int i = 0; i < 8; ++i)
182 if (std::isnan (leaf[i]))
192 template <
typename Po
intNT>
float
196 if (pos[0] < 0 || pos[0] >= res_x_)
198 if (pos[1] < 0 || pos[1] >= res_y_)
200 if (pos[2] < 0 || pos[2] >= res_z_)
203 return grid_[pos[0]*res_y_*res_z_ + pos[1]*res_z_ + pos[2]];
208 template <
typename Po
intNT>
void
213 performReconstruction (points, output.
polygons);
220 template <
typename Po
intNT>
void
222 std::vector<pcl::Vertices> &polygons)
224 if (!(iso_level_ >= 0 && iso_level_ < 1))
226 PCL_ERROR (
"[pcl::%s::performReconstruction] Invalid iso level %f! Please use a number between 0 and 1.\n",
227 getClassName ().c_str (), iso_level_);
238 grid_ = std::vector<float> (res_x_*res_y_*res_z_, NAN);
241 tree_->setInputCloud (input_);
245 size_voxel_ = (upper_boundary_ - lower_boundary_)
246 * Eigen::Array3f (res_x_, res_y_, res_z_).inverse ();
253 double size_reserve = std::min((
double) intermediate_cloud.
points.max_size (),
254 2.0 * 6.0 * (
double) (res_y_*res_z_ + res_x_*res_z_ + res_x_*res_y_));
255 intermediate_cloud.
reserve ((
size_t) size_reserve);
257 for (
int x = 1; x < res_x_-1; ++x)
258 for (
int y = 1; y < res_y_-1; ++y)
259 for (
int z = 1; z < res_z_-1; ++z)
261 Eigen::Vector3i index_3d (x, y, z);
262 std::vector<float> leaf_node;
263 getNeighborList1D (leaf_node, index_3d);
264 if (!leaf_node.empty ())
265 createSurface (leaf_node, index_3d, intermediate_cloud);
268 points.
swap (intermediate_cloud);
270 polygons.resize (points.
size () / 3);
271 for (
size_t i = 0; i < polygons.size (); ++i)
275 for (
int j = 0; j < 3; ++j)
276 v.
vertices[j] =
static_cast<int> (i) * 3 + j;
281 #define PCL_INSTANTIATE_MarchingCubes(T) template class PCL_EXPORTS pcl::MarchingCubes<T>;
283 #endif // PCL_SURFACE_IMPL_MARCHING_CUBES_H_