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; }
MUintArray * SpatialGrid::getVoxelContents( const gridPoint3<int>& index ) // // Description: // // Returns list of triangles for given voxel. Allocates the array // if it doesn't exist already // { int linearIndex = getLinearVoxelIndex( index ); if( fVoxels[linearIndex] == NULL ) { fVoxels[linearIndex] = new MUintArray; } return fVoxels[linearIndex]; }