Esempio n. 1
0
Vector<uint64_t,3> count_block_wins(const section_t section, const Vector<uint8_t,4> block, RawArray<const Vector<super_t,2>> flat_data) {
  // Prepare
  const auto base = block_size*Vector<int,4>(block);
  const auto shape = block_shape(section.shape(),block);
  const auto rmin0 = safe_rmin_slice(section.counts[0],base[0]+range(shape[0])),
             rmin1 = safe_rmin_slice(section.counts[1],base[1]+range(shape[1])),
             rmin2 = safe_rmin_slice(section.counts[2],base[2]+range(shape[2])),
             rmin3 = safe_rmin_slice(section.counts[3],base[3]+range(shape[3]));
  GEODE_ASSERT(shape.product()==flat_data.size());
  const RawArray<const Vector<super_t,2>,4> block_data(shape,flat_data.data());

  // Count
  Vector<uint64_t,3> counts;
  for (int i0=0;i0<shape[0];i0++)
    for (int i1=0;i1<shape[1];i1++)
      for (int i2=0;i2<shape[2];i2++)
        for (int i3=0;i3<shape[3];i3++)
          counts += Vector<uint64_t,3>(popcounts_over_stabilizers(quadrants(rmin0[i0],rmin1[i1],rmin2[i2],rmin3[i3]),block_data(i0,i1,i2,i3)));
  return counts;
}
Esempio n. 2
0
template<typename PointInT, typename PointNT, typename PointOutT> void
pcl::OURCVFHEstimation<PointInT, PointNT, PointOutT>::computeRFAndShapeDistribution (PointInTPtr & processed, PointCloudOut & output,
                                                                                     std::vector<pcl::PointIndices> & cluster_indices)
{
  PointCloudOut ourcvfh_output;
  for (size_t i = 0; i < centroids_dominant_orientations_.size (); i++)
  {

    std::vector < Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > transformations;
    PointInTPtr grid (new pcl::PointCloud<PointInT>);
    sgurf (centroids_dominant_orientations_[i], dominant_normals_[i], processed, transformations, grid, cluster_indices[i]);

    for (size_t t = 0; t < transformations.size (); t++)
    {

      pcl::transformPointCloud (*processed, *grid, transformations[t]);
      transforms_.push_back (transformations[t]);
      valid_transforms_.push_back (true);

      std::vector < Eigen::VectorXf > quadrants (8);
      int size_hists = 13;
      int num_hists = 8;
      for (int k = 0; k < num_hists; k++)
        quadrants[k].setZero (size_hists);

      Eigen::Vector4f centroid_p;
      centroid_p.setZero ();
      Eigen::Vector4f max_pt;
      pcl::getMaxDistance (*grid, centroid_p, max_pt);
      max_pt[3] = 0;
      double distance_normalization_factor = (centroid_p - max_pt).norm ();

      float hist_incr;
      if (normalize_bins_)
        hist_incr = 100.0f / static_cast<float> (grid->points.size () - 1);
      else
        hist_incr = 1.0f;

      float * weights = new float[num_hists];
      float sigma = 0.01f; //1cm
      float sigma_sq = sigma * sigma;

      for (int k = 0; k < static_cast<int> (grid->points.size ()); k++)
      {
        Eigen::Vector4f p = grid->points[k].getVector4fMap ();
        p[3] = 0.f;
        float d = p.norm ();

        //compute weight for all octants
        float wx = 1.f - std::exp (-((p[0] * p[0]) / (2.f * sigma_sq))); //how is the weight distributed among two semi-cubes
        float wy = 1.f - std::exp (-((p[1] * p[1]) / (2.f * sigma_sq)));
        float wz = 1.f - std::exp (-((p[2] * p[2]) / (2.f * sigma_sq)));

        //distribute the weights using the x-coordinate
        if (p[0] >= 0)
        {
          for (size_t ii = 0; ii <= 3; ii++)
            weights[ii] = 0.5f - wx * 0.5f;

          for (size_t ii = 4; ii <= 7; ii++)
            weights[ii] = 0.5f + wx * 0.5f;
        }
        else
        {
          for (size_t ii = 0; ii <= 3; ii++)
            weights[ii] = 0.5f + wx * 0.5f;

          for (size_t ii = 4; ii <= 7; ii++)
            weights[ii] = 0.5f - wx * 0.5f;
        }

        //distribute the weights using the y-coordinate
        if (p[1] >= 0)
        {
          for (size_t ii = 0; ii <= 1; ii++)
            weights[ii] *= 0.5f - wy * 0.5f;
          for (size_t ii = 4; ii <= 5; ii++)
            weights[ii] *= 0.5f - wy * 0.5f;

          for (size_t ii = 2; ii <= 3; ii++)
            weights[ii] *= 0.5f + wy * 0.5f;

          for (size_t ii = 6; ii <= 7; ii++)
            weights[ii] *= 0.5f + wy * 0.5f;
        }
        else
        {
          for (size_t ii = 0; ii <= 1; ii++)
            weights[ii] *= 0.5f + wy * 0.5f;
          for (size_t ii = 4; ii <= 5; ii++)
            weights[ii] *= 0.5f + wy * 0.5f;

          for (size_t ii = 2; ii <= 3; ii++)
            weights[ii] *= 0.5f - wy * 0.5f;

          for (size_t ii = 6; ii <= 7; ii++)
            weights[ii] *= 0.5f - wy * 0.5f;
        }

        //distribute the weights using the z-coordinate
        if (p[2] >= 0)
        {
          for (size_t ii = 0; ii <= 7; ii += 2)
            weights[ii] *= 0.5f - wz * 0.5f;

          for (size_t ii = 1; ii <= 7; ii += 2)
            weights[ii] *= 0.5f + wz * 0.5f;

        }
        else
        {
          for (size_t ii = 0; ii <= 7; ii += 2)
            weights[ii] *= 0.5f + wz * 0.5f;

          for (size_t ii = 1; ii <= 7; ii += 2)
            weights[ii] *= 0.5f - wz * 0.5f;
        }

        int h_index = static_cast<int> (std::floor (size_hists * (d / distance_normalization_factor)));
        for (int j = 0; j < num_hists; j++)
          quadrants[j][h_index] += hist_incr * weights[j];

      }

      //copy to the cvfh signature
      PointCloudOut vfh_signature;
      vfh_signature.points.resize (1);
      vfh_signature.width = vfh_signature.height = 1;
      for (int d = 0; d < 308; ++d)
        vfh_signature.points[0].histogram[d] = output.points[i].histogram[d];

      int pos = 45 * 3;
      for (int k = 0; k < num_hists; k++)
      {
        for (int ii = 0; ii < size_hists; ii++, pos++)
        {
          vfh_signature.points[0].histogram[pos] = quadrants[k][ii];
        }
      }

      ourcvfh_output.points.push_back (vfh_signature.points[0]);

      delete[] weights;
    }
  }

  output = ourcvfh_output;
}