template <typename PointInT, typename PointOutT> void
pcl::MovingLeastSquares<PointInT, PointOutT>::MLSVoxelGrid::dilate ()
{
HashMap new_voxel_grid = voxel_grid_;
for (typename MLSVoxelGrid::HashMap::iterator m_it = voxel_grid_.begin (); m_it != voxel_grid_.end (); ++m_it)
{
Eigen::Vector3i index;
getIndexIn3D (m_it->first, index);
// Now dilate all of its voxels
for (int x = -1; x <= 1; ++x)
for (int y = -1; y <= 1; ++y)
for (int z = -1; z <= 1; ++z)
if (x != 0 || y != 0 || z != 0)
{
Eigen::Vector3i new_index;
new_index = index + Eigen::Vector3i (x, y, z);
uint64_t index_1d;
getIndexIn1D (new_index, index_1d);
Leaf leaf;
new_voxel_grid[index_1d] = leaf;
}
}
voxel_grid_ = new_voxel_grid;
}
template <typename PointNT> void
pcl::GridProjection<PointNT>::fillPad (const Eigen::Vector3i &index)
{
for (int i = index[0] - padding_size_; i < index[0] + padding_size_; ++i)
{
for (int j = index[1] - padding_size_; j < index[1] + padding_size_; ++j)
{
for (int k = index[2] - padding_size_; k < index[2] + padding_size_; ++k)
{
Eigen::Vector3i cell_index_3d (i, j, k);
unsigned int cell_index_1d = getIndexIn1D (cell_index_3d);
if (cell_hash_map_.find (cell_index_1d) == cell_hash_map_.end ())
{
cell_hash_map_[cell_index_1d].data_indices.resize (0);
getCellCenterFromIndex (cell_index_3d, cell_hash_map_[cell_index_1d].pt_on_surface);
}
}
}
}
}
pcl::MovingLeastSquares<PointInT, PointOutT>::MLSVoxelGrid::MLSVoxelGrid (PointCloudInConstPtr& cloud,
IndicesPtr &indices,
float voxel_size) :
voxel_grid_ (), bounding_min_ (), bounding_max_ (), data_size_ (), voxel_size_ (voxel_size)
{
pcl::getMinMax3D (*cloud, *indices, bounding_min_, bounding_max_);
Eigen::Vector4f bounding_box_size = bounding_max_ - bounding_min_;
double max_size = (std::max) ((std::max)(bounding_box_size.x (), bounding_box_size.y ()), bounding_box_size.z ());
// Put initial cloud in voxel grid
data_size_ = static_cast<uint64_t> (1.5 * max_size / voxel_size_);
for (unsigned int i = 0; i < indices->size (); ++i)
if (pcl_isfinite (cloud->points[(*indices)[i]].x))
{
Eigen::Vector3i pos;
getCellIndex (cloud->points[(*indices)[i]].getVector3fMap (), pos);
uint64_t index_1d;
getIndexIn1D (pos, index_1d);
Leaf leaf;
voxel_grid_[index_1d] = leaf;
}
}
template <typename PointNT> void
pcl::GridProjection<PointNT>::getDataPtsUnion (const Eigen::Vector3i &index,
std::vector <int> &pt_union_indices)
{
for (int i = index[0] - padding_size_; i <= index[0] + padding_size_; ++i)
{
for (int j = index[1] - padding_size_; j <= index[1] + padding_size_; ++j)
{
for (int k = index[2] - padding_size_; k <= index[2] + padding_size_; ++k)
{
Eigen::Vector3i cell_index_3d (i, j, k);
int cell_index_1d = getIndexIn1D (cell_index_3d);
if (cell_hash_map_.find (cell_index_1d) != cell_hash_map_.end ())
{
// Get the indices of the input points within the cell(i,j,k), which
// is stored in the hash map
pt_union_indices.insert (pt_union_indices.end (),
cell_hash_map_.at (cell_index_1d).data_indices.begin (),
cell_hash_map_.at (cell_index_1d).data_indices.end ());
}
}
}
}
}
template <typename PointNT> bool
pcl::GridProjection<PointNT>::reconstructPolygons (std::vector<pcl::Vertices> &polygons)
{
data_.reset (new pcl::PointCloud<PointNT> (*input_));
getBoundingBox ();
// Store the point cloud data into the voxel grid, and at the same time
// create a hash map to store the information for each cell
cell_hash_map_.max_load_factor (2.0);
cell_hash_map_.rehash (data_->points.size () / static_cast<long unsigned int> (cell_hash_map_.max_load_factor ()));
// Go over all points and insert them into the right leaf
for (int cp = 0; cp < static_cast<int> (data_->points.size ()); ++cp)
{
// Check if the point is invalid
if (!pcl_isfinite (data_->points[cp].x) ||
!pcl_isfinite (data_->points[cp].y) ||
!pcl_isfinite (data_->points[cp].z))
continue;
Eigen::Vector3i index_3d;
getCellIndex (data_->points[cp].getVector4fMap (), index_3d);
int index_1d = getIndexIn1D (index_3d);
if (cell_hash_map_.find (index_1d) == cell_hash_map_.end ())
{
Leaf cell_data;
cell_data.data_indices.push_back (cp);
getCellCenterFromIndex (index_3d, cell_data.pt_on_surface);
cell_hash_map_[index_1d] = cell_data;
occupied_cell_list_[index_1d] = 1;
}
else
{
Leaf cell_data = cell_hash_map_.at (index_1d);
cell_data.data_indices.push_back (cp);
cell_hash_map_[index_1d] = cell_data;
}
}
Eigen::Vector3i index;
int numOfFilledPad = 0;
for (int i = 0; i < data_size_; ++i)
{
for (int j = 0; j < data_size_; ++j)
{
for (int k = 0; k < data_size_; ++k)
{
index[0] = i;
index[1] = j;
index[2] = k;
if (occupied_cell_list_[getIndexIn1D (index)])
{
fillPad (index);
numOfFilledPad++;
}
}
}
}
// Update the hashtable and store the vector and point
BOOST_FOREACH (typename HashMap::value_type entry, cell_hash_map_)
{
getIndexIn3D (entry.first, index);
std::vector <int> pt_union_indices;
getDataPtsUnion (index, pt_union_indices);
// Needs at least 10 points (?)
// NOTE: set as parameter later
if (pt_union_indices.size () > 10)
{
storeVectAndSurfacePoint (entry.first, index, pt_union_indices, entry.second);
//storeVectAndSurfacePointKNN(entry.first, index, entry.second);
occupied_cell_list_[entry.first] = 1;
}
}
template <typename PointNT> void
pcl::GridProjection<PointNT>::createSurfaceForCell (const Eigen::Vector3i &index,
std::vector <int> &pt_union_indices)
{
// 8 vertices of the cell
std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > vertices (8);
// 4 end points that shared by 3 edges connecting the upper left front points
Eigen::Vector4f pts[4];
Eigen::Vector3f vector_at_pts[4];
// Given the index of cell, caluate the coordinates of the eight vertices of the cell
// index the index of the cell in (x,y,z) 3d format
Eigen::Vector4f cell_center = Eigen::Vector4f::Zero ();
getCellCenterFromIndex (index, cell_center);
getVertexFromCellCenter (cell_center, vertices);
// Get the indices of the cells which stores the 4 end points.
Eigen::Vector3i indices[4];
indices[0] = Eigen::Vector3i (index[0], index[1], index[2] - 1);
indices[1] = Eigen::Vector3i (index[0], index[1], index[2]);
indices[2] = Eigen::Vector3i (index[0], index[1] - 1, index[2]);
indices[3] = Eigen::Vector3i (index[0] + 1, index[1], index[2]);
// Get the coordinate of the 4 end points, and the corresponding vectors
for (size_t i = 0; i < 4; ++i)
{
pts[i] = vertices[I_SHIFT_PT[i]];
unsigned int index_1d = getIndexIn1D (indices[i]);
if (cell_hash_map_.find (index_1d) == cell_hash_map_.end () ||
occupied_cell_list_[index_1d] == 0)
return;
else
vector_at_pts[i] = cell_hash_map_.at (index_1d).vect_at_grid_pt;
}
// Go through the 3 edges, test whether they are intersected by the surface
for (size_t i = 0; i < 3; ++i)
{
std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > end_pts (2);
std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > vect_at_end_pts (2);
for (size_t j = 0; j < 2; ++j)
{
end_pts[j] = pts[I_SHIFT_EDGE[i][j]];
vect_at_end_pts[j] = vector_at_pts[I_SHIFT_EDGE[i][j]];
}
if (isIntersected (end_pts, vect_at_end_pts, pt_union_indices))
{
// Indices of cells that contains points which will be connected to
// create a polygon
Eigen::Vector3i polygon[4];
Eigen::Vector4f polygon_pts[4];
int polygon_indices_1d[4];
bool is_all_in_hash_map = true;
switch (i)
{
case 0:
polygon[0] = Eigen::Vector3i (index[0] - 1, index[1] + 1, index[2]);
polygon[1] = Eigen::Vector3i (index[0] - 1, index[1], index[2]);
polygon[2] = Eigen::Vector3i (index[0], index[1], index[2]);
polygon[3] = Eigen::Vector3i (index[0], index[1] + 1, index[2]);
break;
case 1:
polygon[0] = Eigen::Vector3i (index[0], index[1] + 1, index[2] + 1);
polygon[1] = Eigen::Vector3i (index[0], index[1] + 1, index[2]);
polygon[2] = Eigen::Vector3i (index[0], index[1], index[2]);
polygon[3] = Eigen::Vector3i (index[0], index[1], index[2] + 1);
break;
case 2:
polygon[0] = Eigen::Vector3i (index[0] - 1, index[1], index[2] + 1);
polygon[1] = Eigen::Vector3i (index[0] - 1, index[1], index[2]);
polygon[2] = Eigen::Vector3i (index[0], index[1], index[2]);
polygon[3] = Eigen::Vector3i (index[0], index[1], index[2] + 1);
break;
default:
break;
}
for (size_t k = 0; k < 4; k++)
{
polygon_indices_1d[k] = getIndexIn1D (polygon[k]);
if (!occupied_cell_list_[polygon_indices_1d[k]])
{
is_all_in_hash_map = false;
break;
}
}
if (is_all_in_hash_map)
{
for (size_t k = 0; k < 4; k++)
{
polygon_pts[k] = cell_hash_map_.at (polygon_indices_1d[k]).pt_on_surface;
surface_.push_back (polygon_pts[k]);
}
}
}
}
}
