HashedId::HashedId(const Eigen::Vector3i&pos,const int&edgeLen,const int&no) { _id[0]=edgeLen; _id[1]=pos.x(); _id[2]=pos.y(); _id[3]=pos.z(); _no=no; }
void WorldDownloadManager::findExtraCubesForBoundingBox(const Eigen::Vector3f& current_cube_min, const Eigen::Vector3f& current_cube_max,const Eigen::Vector3f& bbox_min,const Eigen::Vector3f& bbox_max,Vector3fVector& cubes_centers, bool& extract_current) { const Eigen::Vector3f & cube_size = current_cube_max - current_cube_min; cubes_centers.clear(); extract_current = false; const Eigen::Vector3f relative_act_bbox_min = bbox_min - current_cube_min; const Eigen::Vector3f relative_act_bbox_max = bbox_max - current_cube_min; const Eigen::Vector3i num_cubes_plus = Eigen::Vector3f(floor3f(Eigen::Vector3f(relative_act_bbox_max.array() / cube_size.array()) - (Eigen::Vector3f::Ones() * 0.0001))).cast<int>(); const Eigen::Vector3i num_cubes_minus = Eigen::Vector3f(floor3f(Eigen::Vector3f(relative_act_bbox_min.array() / cube_size.array()) + (Eigen::Vector3f::Ones() * 0.0001))).cast<int>(); for (int z = num_cubes_minus.z(); z <= num_cubes_plus.z(); z++) for (int y = num_cubes_minus.y(); y <= num_cubes_plus.y(); y++) for (int x = num_cubes_minus.x(); x <= num_cubes_plus.x(); x++) { const Eigen::Vector3i cube_index(x,y,z); if ((cube_index.array() == Eigen::Vector3i::Zero().array()).all()) { extract_current = true; continue; } const Eigen::Vector3f relative_cube_origin = cube_index.cast<float>().array() * cube_size.array(); const Eigen::Vector3f cube_center = relative_cube_origin + current_cube_min + (cube_size * 0.5); cubes_centers.push_back(cube_center); } }
/** \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]))); };
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; }
IntBoundingBox3D::IntBoundingBox3D(const Eigen::Vector3i& a, const Eigen::Vector3i& b) { Eigen::Vector3i bl, tr; for (unsigned i = 0; i < a.size(); ++i) { bl[i] = std::min(a[i], b[i]); tr[i] = std::max(a[i], b[i]); } elems[0] = static_cast<int> (bl[0]); elems[1] = static_cast<int> (tr[0]); elems[2] = static_cast<int> (bl[1]); elems[3] = static_cast<int> (tr[1]); elems[4] = static_cast<int> (bl[2]); elems[5] = static_cast<int> (tr[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); } } } }
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(); }
//============================================================================== std::string toString(const Eigen::Vector3i& _v) { return boost::lexical_cast<std::string>(_v.transpose()); }
void CollisionSpace::propagatePositive( std::vector<std::vector<CollisionSpaceCell*> >& bucket_queue) { double max_distance_sq = getMaxtDistSq(); // now process the queue: cout << "process the Queue : " << bucket_queue.size() << endl; for (unsigned int i = 0; i < bucket_queue.size(); ++i) { std::vector<CollisionSpaceCell*>::iterator list_it = bucket_queue[i].begin(); while (list_it != bucket_queue[i].end()) { CollisionSpaceCell* vptr = *list_it; // TODO: no idea why this happens if (vptr == NULL) { continue; } // Get the cell location in grid Eigen::Vector3i loc = getCellCoord(vptr); int D = i; if (D > 1) D = 1; // avoid a possible segfault situation: if (vptr->m_UpdateDirection < 0 || vptr->m_UpdateDirection > 26) { cout << "Invalid update direction detected: " << vptr->m_UpdateDirection << endl; ++list_it; continue; } // select the neighborhood list based on the update direction: std::vector<std::vector<int> >& neighborhood = m_Neighborhoods[D][vptr->m_UpdateDirection]; // Look in the neighbouring cells and update distance for (unsigned int n = 0; n < neighborhood.size(); n++) { Eigen::Vector3i direction; direction.x() = neighborhood[n][0]; direction.y() = neighborhood[n][1]; direction.z() = neighborhood[n][2]; Eigen::Vector3i neigh_loc = loc + direction; CollisionSpaceCell* neighbour = getCellSpaceCell(neigh_loc); if (!neighbour) continue; double new_distance_sq_float = (vptr->m_ClosestPoint - neigh_loc).squaredNorm(); int new_distance_sq = new_distance_sq_float; if (new_distance_sq > max_distance_sq) { cout << "new_distance_sq : " << new_distance_sq << endl; continue; } if (new_distance_sq < neighbour->m_DistanceSquare) { neighbour->m_DistanceSquare = new_distance_sq; neighbour->m_ClosestPoint = vptr->m_ClosestPoint; neighbour->m_UpdateDirection = getDirectionNumber(direction[0], direction[1], direction[2]); // and put it in the queue bucket_queue[new_distance_sq].push_back(neighbour); } } ++list_it; } bucket_queue[i].clear(); } }
vtkVolume * VTKDialog::cubeVolume(Cube *cube) { qDebug() << "Cube dimensions: " << cube->dimensions().x() << cube->dimensions().y() << cube->dimensions().z(); qDebug() << "min/max:" << cube->minValue() << cube->maxValue(); qDebug() << cube->data()->size(); vtkNew<vtkImageData> data; data->SetNumberOfScalarComponents(1); Eigen::Vector3i dim = cube->dimensions(); data->SetExtent(0, dim.x()-1, 0, dim.y()-1, 0, dim.z()-1); data->SetOrigin(cube->min().x(), cube->min().y(), cube->min().z()); data->SetSpacing(cube->spacing().data()); data->SetScalarTypeToDouble(); data->AllocateScalars(); data->Update(); double *dataPtr = static_cast<double *>(data->GetScalarPointer()); std::vector<double> *cubePtr = cube->data(); for (int i = 0; i < dim.x(); ++i) for (int j = 0; j < dim.y(); ++j) for (int k = 0; k < dim.z(); ++k) { dataPtr[(k * dim.y() + j) * dim.x() + i] = (*cubePtr)[(i * dim.y() + j) * dim.z() + k]; } double range[2]; data->Update(); range[0] = data->GetScalarRange()[0]; range[1] = data->GetScalarRange()[1]; // a->GetRange(range); qDebug() << "ImageData range: " << range[0] << range[1]; vtkNew<vtkImageShiftScale> t; t->SetInput(data.GetPointer()); t->SetShift(-range[0]); double magnitude = range[1] - range[0]; if(magnitude == 0.0) { magnitude = 1.0; } t->SetScale(255.0/magnitude); t->SetOutputScalarTypeToDouble(); qDebug() << "magnitude: " << magnitude; t->Update(); vtkNew<vtkSmartVolumeMapper> volumeMapper; vtkNew<vtkVolumeProperty> volumeProperty; vtkVolume *volume = vtkVolume::New(); volumeMapper->SetBlendModeToComposite(); // volumeMapper->SetBlendModeToComposite(); // composite first volumeMapper->SetInputConnection(t->GetOutputPort()); volumeProperty->ShadeOff(); volumeProperty->SetInterpolationTypeToLinear(); vtkNew<vtkPiecewiseFunction> compositeOpacity; vtkNew<vtkColorTransferFunction> color; if (cube->cubeType() == Cube::MO) { compositeOpacity->AddPoint( 0.00, 0.0); compositeOpacity->AddPoint( 63.75, 0.5); compositeOpacity->AddPoint(127.50, 0.0); compositeOpacity->AddPoint(192.25, 0.5); compositeOpacity->AddPoint(255.00, 0.0); color->AddRGBPoint( 0.00, 0.0, 0.0, 0.0); color->AddRGBPoint( 63.75, 1.0, 0.0, 0.0); color->AddRGBPoint(127.50, 0.0, 0.2, 0.0); color->AddRGBPoint(191.25, 0.0, 0.0, 1.0); color->AddRGBPoint(255.00, 0.0, 0.0, 0.0); } else { compositeOpacity->AddPoint( 0.00, 0.00); compositeOpacity->AddPoint( 1.75, 0.30); compositeOpacity->AddPoint( 2.50, 0.50); compositeOpacity->AddPoint(192.25, 0.85); compositeOpacity->AddPoint(255.00, 0.90); color->AddRGBPoint( 0.00, 0.0, 0.0, 1.0); color->AddRGBPoint( 63.75, 0.0, 0.0, 0.8); color->AddRGBPoint(127.50, 0.0, 0.0, 0.5); color->AddRGBPoint(191.25, 0.0, 0.0, 0.2); color->AddRGBPoint(255.00, 0.0, 0.0, 0.0); } volumeProperty->SetScalarOpacity(compositeOpacity.GetPointer()); // composite first. volumeProperty->SetColor(color.GetPointer()); volume->SetMapper(volumeMapper.GetPointer()); volume->SetProperty(volumeProperty.GetPointer()); return volume; }