/** \brief Constructor
* param[in] volume_size size of the volume in mm
* param[in] volume_res volume grid resolution (typically device::VOLUME_X x device::VOLUME_Y x device::VOLUME_Z)
*/
DeviceVolume (const Eigen::Vector3f &volume_size, const Eigen::Vector3i &volume_res)
: volume_size_ (volume_size)
{
// initialize GPU
device_volume_.create (volume_res[1] * volume_res[2], volume_res[0]); // (device::VOLUME_Y * device::VOLUME_Z, device::VOLUME_X)
pcl::device::initVolume (device_volume_);
// truncation distance
Eigen::Vector3f voxel_size = volume_size.array() / volume_res.array().cast<float>();
trunc_dist_ = max ((float)min_trunc_dist, 2.1f * max (voxel_size[0], max (voxel_size[1], voxel_size[2])));
};
// Create root cells and connect their nodes accordingly
void OctreeGrid::createRootCells() {
// Clear current octree
m_Nodes.clear();
m_Cells.clear();
// Anisotropic grids: we need multiple root cells
int minFineCellSize = m_CellGridSize.minCoeff();
Eigen::Vector3i coarseCellGridSize = m_CellGridSize / minFineCellSize;
Eigen::Vector3i coarseNodeGridSize = coarseCellGridSize.array() + 1;
// Create coarse grid nodes and set up adjacency relations
for (int i = 0; i < coarseNodeGridSize.prod(); ++i) {
Node newNode;
Eigen::Vector3i coarsePos = Layout3D::toGrid(i, coarseNodeGridSize);
newNode.position = (1 << m_MaxDepth) * coarsePos;
for (int axis = 0; axis < 3; ++axis) {
for (int c = 0; c < 2; ++c) {
Eigen::Vector3i q = coarsePos;
q[axis] += (c ? 1 : -1);
q = Layout3D::clamp(q, coarseNodeGridSize);
int j = Layout3D::toIndex(q, coarseNodeGridSize);
newNode.neighNodeId[2*axis+c] = (j != i ? j : -1);
}
}
m_Nodes.emplace_back(newNode);
}
// Create coarse grid cells
for (int e = 0; e < coarseCellGridSize.prod(); ++e) {
Cell newCell;
Eigen::Vector3i lowerCorner = Layout3D::toGrid(e, coarseCellGridSize);
for (int k = 0; k < 8; ++k) {
Eigen::Vector3i currentCorner = lowerCorner + Cube::delta(k);
int i = Layout3D::toIndex(currentCorner, coarseNodeGridSize);
newCell.setCorner(k, i);
}
m_Cells.emplace_back(newCell);
}
m_NumRootCells = (int) m_Cells.size();
// Link adjacent cells together
for (int i = 0; i < coarseCellGridSize.prod(); ++i) {
Eigen::Vector3i coarsePos = Layout3D::toGrid(i, coarseCellGridSize);
for (int axis = 0; axis < 3; ++axis) {
for (int c = 0; c < 2; ++c) {
Eigen::Vector3i q = coarsePos;
q[axis] += (c ? 1 : -1);
q = Layout3D::clamp(q, coarseCellGridSize);
int j = Layout3D::toIndex(q, coarseCellGridSize);
m_Cells[i].neighCellId[2*axis+c] = (j != i ? j : -1);
}
}
}
}
template <typename VoxelT, typename WeightT> bool
pcl::TSDFVolume<VoxelT, WeightT>::extractNeighborhood (const Eigen::Vector3i &voxel_coord, int neighborhood_size,
VoxelTVec &neighborhood) const
{
// point_index is at the center of a cube of scale_ x scale_ x scale_ voxels
int shift = (neighborhood_size - 1) / 2;
Eigen::Vector3i min_index = voxel_coord.array() - shift;
Eigen::Vector3i max_index = voxel_coord.array() + shift;
// check that index is within range
if (getLinearVoxelIndex(min_index) < 0 || getLinearVoxelIndex(max_index) >= (int)size())
{
pcl::console::print_info ("[extractNeighborhood] Skipping voxel with coord (%d, %d, %d).\n", voxel_coord(0), voxel_coord(1), voxel_coord(2));
return false;
}
static const int descriptor_size = neighborhood_size*neighborhood_size*neighborhood_size;
neighborhood.resize (descriptor_size);
const Eigen::RowVector3i offset_vector (1, neighborhood_size, neighborhood_size*neighborhood_size);
// loop over all voxels in 3D neighborhood
#pragma omp parallel for
for (int z = min_index(2); z <= max_index(2); ++z)
{
for (int y = min_index(1); y <= max_index(1); ++y)
{
for (int x = min_index(0); x <= max_index(0); ++x)
{
// linear voxel index in volume and index in descriptor vector
Eigen::Vector3i point (x,y,z);
int volume_idx = getLinearVoxelIndex (point);
int descr_idx = offset_vector * (point - min_index);
/* std::cout << "linear index " << volume_idx << std::endl;
std::cout << "weight " << weights_->at (volume_idx) << std::endl;
std::cout << "volume " << volume_->at (volume_idx) << std::endl;
std::cout << "descr " << neighborhood.rows() << "x" << neighborhood.cols() << ", val = " << neighborhood << std::endl;
std::cout << "descr index = " << descr_idx << std::endl;
*/
// get the TSDF value and store as descriptor entry
if (weights_->at (volume_idx) != 0)
neighborhood (descr_idx) = volume_->at (volume_idx);
else
neighborhood (descr_idx) = -1.0; // if never visited we assume inside of object (outside captured and thus filled with positive values)
}
}
}
return true;
}
template <typename VoxelT, typename WeightT> bool
pcl::TSDFVolume<VoxelT, WeightT>::addNeighborhood (const Eigen::Vector3i &voxel_coord, int neighborhood_size,
const VoxelTVec &neighborhood, WeightT voxel_weight)
{
// point_index is at the center of a cube of scale_ x scale_ x scale_ voxels
int shift = (neighborhood_size - 1) / 2;
Eigen::Vector3i min_index = voxel_coord.array() - shift;
Eigen::Vector3i max_index = voxel_coord.array() + shift;
// check that index is within range
if (getLinearVoxelIndex(min_index) < 0 || getLinearVoxelIndex(max_index) >= (int)size())
{
pcl::console::print_info ("[addNeighborhood] Skipping voxel with coord (%d, %d, %d).\n", voxel_coord(0), voxel_coord(1), voxel_coord(2));
return false;
}
// static const int descriptor_size = neighborhood_size*neighborhood_size*neighborhood_size;
const Eigen::RowVector3i offset_vector (1, neighborhood_size, neighborhood_size*neighborhood_size);
Eigen::Vector3i index = min_index;
// loop over all voxels in 3D neighborhood
#pragma omp parallel for
for (int z = min_index(2); z <= max_index(2); ++z)
{
for (int y = min_index(1); y <= max_index(1); ++y)
{
for (int x = min_index(0); x <= max_index(0); ++x)
{
// linear voxel index in volume and index in descriptor vector
Eigen::Vector3i point (x,y,z);
int volume_idx = getLinearVoxelIndex (point);
int descr_idx = offset_vector * (point - min_index);
// add the descriptor entry to the volume
VoxelT &voxel = volume_->at (volume_idx);
WeightT &weight = weights_->at (volume_idx);
// TODO check that this simple lock works correctly!!
#pragma omp atomic
voxel += neighborhood (descr_idx);
#pragma omp atomic
weight += voxel_weight;
}
}
}
return true;
}
OctreeGrid::OctreeGrid(Eigen::Vector3i fineCellGridSize, int maxNodeGuess, int maxCellGuess)
: m_NodeGridSize(fineCellGridSize.array() + 1)
, m_CellGridSize(fineCellGridSize)
, m_NumRootCells(0)
{
m_Nodes.reserve(maxNodeGuess);
m_Cells.reserve(maxCellGuess);
// Some sanity checks
oct_assert(Math::isPowerOfTwo(fineCellGridSize[0]));
oct_assert(Math::isPowerOfTwo(fineCellGridSize[1]));
oct_assert(Math::isPowerOfTwo(fineCellGridSize[2]));
// Max depth depends on fineCellGridSize
int minFineCellSize = m_CellGridSize.minCoeff();
m_MaxDepth = 0;
while ( m_MaxDepth < 64 && (1 << m_MaxDepth) < minFineCellSize) { ++m_MaxDepth; }
// logger_debug("OctreeGrid", "MaxDepth: %s", m_MaxDepth);
// Create root cells
createRootCells();
}