Esempio n. 1
0
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;
}
Esempio n. 2
0
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);
      }
}
Esempio n. 3
0
    /** \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])));
    };
Esempio n. 4
0
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;
}
Esempio n. 5
0
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;
}
Esempio n. 6
0
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]);
}
Esempio n. 7
0
// 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);
			}
		}
	}
}
Esempio n. 8
0
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();
}
Esempio n. 9
0
//==============================================================================
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();
  }
}
Esempio n. 11
0
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;
}