280 lines
10 KiB
C++
280 lines
10 KiB
C++
|
/*
|
||
|
|
* Software License Agreement (BSD License)
|
||
|
|
*
|
||
|
|
* Copyright (c) 2010, 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_SURFACE_IMPL_MARCHING_CUBES_H_
|
||
|
|
#define PCL_SURFACE_IMPL_MARCHING_CUBES_H_
|
||
|
|
|
||
|
|
#include <pcl/surface/marching_cubes.h>
|
||
|
|
#include <pcl/common/common.h>
|
||
|
|
#include <pcl/common/vector_average.h>
|
||
|
|
#include <pcl/Vertices.h>
|
||
|
|
|
||
|
|
//////////////////////////////////////////////////////////////////////////////////////////////
|
||
|
|
template <typename PointNT>
|
||
|
|
pcl::MarchingCubes<PointNT>::~MarchingCubes ()
|
||
|
|
{
|
||
|
|
}
|
||
|
|
|
||
|
|
//////////////////////////////////////////////////////////////////////////////////////////////
|
||
|
|
template <typename PointNT> void
|
||
|
|
pcl::MarchingCubes<PointNT>::getBoundingBox ()
|
||
|
|
{
|
||
|
|
PointNT max_pt, min_pt;
|
||
|
|
pcl::getMinMax3D (*input_, min_pt, max_pt);
|
||
|
|
|
||
|
|
lower_boundary_ = min_pt.getArray3fMap ();
|
||
|
|
upper_boundary_ = max_pt.getArray3fMap ();
|
||
|
|
|
||
|
|
const Eigen::Array3f size3_extend = 0.5f * percentage_extend_grid_
|
||
|
|
* (upper_boundary_ - lower_boundary_);
|
||
|
|
|
||
|
|
lower_boundary_ -= size3_extend;
|
||
|
|
upper_boundary_ += size3_extend;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
//////////////////////////////////////////////////////////////////////////////////////////////
|
||
|
|
template <typename PointNT> void
|
||
|
|
pcl::MarchingCubes<PointNT>::interpolateEdge (Eigen::Vector3f &p1,
|
||
|
|
Eigen::Vector3f &p2,
|
||
|
|
float val_p1,
|
||
|
|
float val_p2,
|
||
|
|
Eigen::Vector3f &output)
|
||
|
|
{
|
||
|
|
const float mu = (iso_level_ - val_p1) / (val_p2 - val_p1);
|
||
|
|
output = p1 + mu * (p2 - p1);
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
//////////////////////////////////////////////////////////////////////////////////////////////
|
||
|
|
template <typename PointNT> void
|
||
|
|
pcl::MarchingCubes<PointNT>::createSurface (const std::vector<float> &leaf_node,
|
||
|
|
const Eigen::Vector3i &index_3d,
|
||
|
|
pcl::PointCloud<PointNT> &cloud)
|
||
|
|
{
|
||
|
|
int cubeindex = 0;
|
||
|
|
if (leaf_node[0] < iso_level_) cubeindex |= 1;
|
||
|
|
if (leaf_node[1] < iso_level_) cubeindex |= 2;
|
||
|
|
if (leaf_node[2] < iso_level_) cubeindex |= 4;
|
||
|
|
if (leaf_node[3] < iso_level_) cubeindex |= 8;
|
||
|
|
if (leaf_node[4] < iso_level_) cubeindex |= 16;
|
||
|
|
if (leaf_node[5] < iso_level_) cubeindex |= 32;
|
||
|
|
if (leaf_node[6] < iso_level_) cubeindex |= 64;
|
||
|
|
if (leaf_node[7] < iso_level_) cubeindex |= 128;
|
||
|
|
|
||
|
|
// Cube is entirely in/out of the surface
|
||
|
|
if (edgeTable[cubeindex] == 0)
|
||
|
|
return;
|
||
|
|
|
||
|
|
const Eigen::Vector3f center = lower_boundary_
|
||
|
|
+ size_voxel_ * index_3d.cast<float> ().array ();
|
||
|
|
|
||
|
|
std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > p;
|
||
|
|
p.resize (8);
|
||
|
|
for (int i = 0; i < 8; ++i)
|
||
|
|
{
|
||
|
|
Eigen::Vector3f point = center;
|
||
|
|
if (i & 0x4)
|
||
|
|
point[1] = static_cast<float> (center[1] + size_voxel_[1]);
|
||
|
|
|
||
|
|
if (i & 0x2)
|
||
|
|
point[2] = static_cast<float> (center[2] + size_voxel_[2]);
|
||
|
|
|
||
|
|
if ((i & 0x1) ^ ((i >> 1) & 0x1))
|
||
|
|
point[0] = static_cast<float> (center[0] + size_voxel_[0]);
|
||
|
|
|
||
|
|
p[i] = point;
|
||
|
|
}
|
||
|
|
|
||
|
|
// Find the vertices where the surface intersects the cube
|
||
|
|
std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > vertex_list;
|
||
|
|
vertex_list.resize (12);
|
||
|
|
if (edgeTable[cubeindex] & 1)
|
||
|
|
interpolateEdge (p[0], p[1], leaf_node[0], leaf_node[1], vertex_list[0]);
|
||
|
|
if (edgeTable[cubeindex] & 2)
|
||
|
|
interpolateEdge (p[1], p[2], leaf_node[1], leaf_node[2], vertex_list[1]);
|
||
|
|
if (edgeTable[cubeindex] & 4)
|
||
|
|
interpolateEdge (p[2], p[3], leaf_node[2], leaf_node[3], vertex_list[2]);
|
||
|
|
if (edgeTable[cubeindex] & 8)
|
||
|
|
interpolateEdge (p[3], p[0], leaf_node[3], leaf_node[0], vertex_list[3]);
|
||
|
|
if (edgeTable[cubeindex] & 16)
|
||
|
|
interpolateEdge (p[4], p[5], leaf_node[4], leaf_node[5], vertex_list[4]);
|
||
|
|
if (edgeTable[cubeindex] & 32)
|
||
|
|
interpolateEdge (p[5], p[6], leaf_node[5], leaf_node[6], vertex_list[5]);
|
||
|
|
if (edgeTable[cubeindex] & 64)
|
||
|
|
interpolateEdge (p[6], p[7], leaf_node[6], leaf_node[7], vertex_list[6]);
|
||
|
|
if (edgeTable[cubeindex] & 128)
|
||
|
|
interpolateEdge (p[7], p[4], leaf_node[7], leaf_node[4], vertex_list[7]);
|
||
|
|
if (edgeTable[cubeindex] & 256)
|
||
|
|
interpolateEdge (p[0], p[4], leaf_node[0], leaf_node[4], vertex_list[8]);
|
||
|
|
if (edgeTable[cubeindex] & 512)
|
||
|
|
interpolateEdge (p[1], p[5], leaf_node[1], leaf_node[5], vertex_list[9]);
|
||
|
|
if (edgeTable[cubeindex] & 1024)
|
||
|
|
interpolateEdge (p[2], p[6], leaf_node[2], leaf_node[6], vertex_list[10]);
|
||
|
|
if (edgeTable[cubeindex] & 2048)
|
||
|
|
interpolateEdge (p[3], p[7], leaf_node[3], leaf_node[7], vertex_list[11]);
|
||
|
|
|
||
|
|
// Create the triangle
|
||
|
|
for (int i = 0; triTable[cubeindex][i] != -1; i += 3)
|
||
|
|
{
|
||
|
|
PointNT p1, p2, p3;
|
||
|
|
p1.getVector3fMap () = vertex_list[triTable[cubeindex][i]];
|
||
|
|
cloud.push_back (p1);
|
||
|
|
p2.getVector3fMap () = vertex_list[triTable[cubeindex][i+1]];
|
||
|
|
cloud.push_back (p2);
|
||
|
|
p3.getVector3fMap () = vertex_list[triTable[cubeindex][i+2]];
|
||
|
|
cloud.push_back (p3);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
//////////////////////////////////////////////////////////////////////////////////////////////
|
||
|
|
template <typename PointNT> void
|
||
|
|
pcl::MarchingCubes<PointNT>::getNeighborList1D (std::vector<float> &leaf,
|
||
|
|
Eigen::Vector3i &index3d)
|
||
|
|
{
|
||
|
|
leaf.resize (8);
|
||
|
|
|
||
|
|
leaf[0] = getGridValue (index3d);
|
||
|
|
leaf[1] = getGridValue (index3d + Eigen::Vector3i (1, 0, 0));
|
||
|
|
leaf[2] = getGridValue (index3d + Eigen::Vector3i (1, 0, 1));
|
||
|
|
leaf[3] = getGridValue (index3d + Eigen::Vector3i (0, 0, 1));
|
||
|
|
leaf[4] = getGridValue (index3d + Eigen::Vector3i (0, 1, 0));
|
||
|
|
leaf[5] = getGridValue (index3d + Eigen::Vector3i (1, 1, 0));
|
||
|
|
leaf[6] = getGridValue (index3d + Eigen::Vector3i (1, 1, 1));
|
||
|
|
leaf[7] = getGridValue (index3d + Eigen::Vector3i (0, 1, 1));
|
||
|
|
|
||
|
|
for (int i = 0; i < 8; ++i)
|
||
|
|
{
|
||
|
|
if (std::isnan (leaf[i]))
|
||
|
|
{
|
||
|
|
leaf.clear ();
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
//////////////////////////////////////////////////////////////////////////////////////////////
|
||
|
|
template <typename PointNT> float
|
||
|
|
pcl::MarchingCubes<PointNT>::getGridValue (Eigen::Vector3i pos)
|
||
|
|
{
|
||
|
|
/// TODO what to return?
|
||
|
|
if (pos[0] < 0 || pos[0] >= res_x_)
|
||
|
|
return -1.0f;
|
||
|
|
if (pos[1] < 0 || pos[1] >= res_y_)
|
||
|
|
return -1.0f;
|
||
|
|
if (pos[2] < 0 || pos[2] >= res_z_)
|
||
|
|
return -1.0f;
|
||
|
|
|
||
|
|
return grid_[pos[0]*res_y_*res_z_ + pos[1]*res_z_ + pos[2]];
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
//////////////////////////////////////////////////////////////////////////////////////////////
|
||
|
|
template <typename PointNT> void
|
||
|
|
pcl::MarchingCubes<PointNT>::performReconstruction (pcl::PolygonMesh &output)
|
||
|
|
{
|
||
|
|
pcl::PointCloud<PointNT> points;
|
||
|
|
|
||
|
|
performReconstruction (points, output.polygons);
|
||
|
|
|
||
|
|
pcl::toPCLPointCloud2 (points, output.cloud);
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
//////////////////////////////////////////////////////////////////////////////////////////////
|
||
|
|
template <typename PointNT> void
|
||
|
|
pcl::MarchingCubes<PointNT>::performReconstruction (pcl::PointCloud<PointNT> &points,
|
||
|
|
std::vector<pcl::Vertices> &polygons)
|
||
|
|
{
|
||
|
|
if (!(iso_level_ >= 0 && iso_level_ < 1))
|
||
|
|
{
|
||
|
|
PCL_ERROR ("[pcl::%s::performReconstruction] Invalid iso level %f! Please use a number between 0 and 1.\n",
|
||
|
|
getClassName ().c_str (), iso_level_);
|
||
|
|
points.clear ();
|
||
|
|
polygons.clear ();
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
|
||
|
|
// the point cloud really generated from Marching Cubes, prev intermediate_cloud_
|
||
|
|
pcl::PointCloud<PointNT> intermediate_cloud;
|
||
|
|
|
||
|
|
// Create grid
|
||
|
|
grid_ = std::vector<float> (res_x_*res_y_*res_z_, NAN);
|
||
|
|
|
||
|
|
// Compute bounding box and voxel size
|
||
|
|
getBoundingBox ();
|
||
|
|
size_voxel_ = (upper_boundary_ - lower_boundary_)
|
||
|
|
* Eigen::Array3f (res_x_, res_y_, res_z_).inverse ();
|
||
|
|
|
||
|
|
// Transform the point cloud into a voxel grid
|
||
|
|
// This needs to be implemented in a child class
|
||
|
|
voxelizeData ();
|
||
|
|
|
||
|
|
// preallocate memory assuming a hull. suppose 6 point per voxel
|
||
|
|
double size_reserve = std::min((double) intermediate_cloud.points.max_size (),
|
||
|
|
2.0 * 6.0 * (double) (res_y_*res_z_ + res_x_*res_z_ + res_x_*res_y_));
|
||
|
|
intermediate_cloud.reserve ((std::size_t) size_reserve);
|
||
|
|
|
||
|
|
for (int x = 1; x < res_x_-1; ++x)
|
||
|
|
for (int y = 1; y < res_y_-1; ++y)
|
||
|
|
for (int z = 1; z < res_z_-1; ++z)
|
||
|
|
{
|
||
|
|
Eigen::Vector3i index_3d (x, y, z);
|
||
|
|
std::vector<float> leaf_node;
|
||
|
|
getNeighborList1D (leaf_node, index_3d);
|
||
|
|
if (!leaf_node.empty ())
|
||
|
|
createSurface (leaf_node, index_3d, intermediate_cloud);
|
||
|
|
}
|
||
|
|
|
||
|
|
points.swap (intermediate_cloud);
|
||
|
|
|
||
|
|
polygons.resize (points.size () / 3);
|
||
|
|
for (std::size_t i = 0; i < polygons.size (); ++i)
|
||
|
|
{
|
||
|
|
pcl::Vertices v;
|
||
|
|
v.vertices.resize (3);
|
||
|
|
for (int j = 0; j < 3; ++j)
|
||
|
|
v.vertices[j] = static_cast<int> (i) * 3 + j;
|
||
|
|
polygons[i] = v;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
#define PCL_INSTANTIATE_MarchingCubes(T) template class PCL_EXPORTS pcl::MarchingCubes<T>;
|
||
|
|
|
||
|
|
#endif // PCL_SURFACE_IMPL_MARCHING_CUBES_H_
|
||
|
|
|