示例#1
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);
      }
}
示例#2
0
void transformMeans(
    Vector3fVector& means,
    const AffineTransform& transform)
{
    Matrix3f R = transform.rotation();
    Vector3f t = transform.translation();

    unsigned int size = means.size();
    for(unsigned int i = 0; i < size; ++i)
    {
        Vector3f& m = means[i];
        m = R * m + t;
    }
}
示例#3
0
void removeInvalidMeans(
  const Vector3fVector& means,
  const BoolVector& valid,
  Vector3fVector& means_f)
{
  unsigned int size = valid.size(); 
  for(unsigned int i = 0; i < size; ++i)
  {
    if (valid[i])
    {
      const Vector3f& mean = means[i];
      means_f.push_back(mean);
    }
  }
}
示例#4
0
void transformDistributions(
    Vector3fVector& means,
    Matrix3fVector& covariances,
    const AffineTransform& transform)
{
    Matrix3f R = transform.rotation();
    Vector3f t = transform.translation();
    Matrix3f R_T = R.transpose();

    unsigned int size = means.size();
    for(unsigned int i = 0; i < size; ++i)
    {
        Vector3f& m = means[i];
        Matrix3f& c = covariances[i];
        m = R * m + t;
        c = R * c * R_T;
    }
}
示例#5
0
void removeInvalidDistributions(
  const Vector3fVector& means,
  const Matrix3fVector& covariances,
  const BoolVector& valid,
  Vector3fVector& means_f,
  Matrix3fVector& covariances_f)
{
  unsigned int size = valid.size(); 
  for(unsigned int i = 0; i < size; ++i)
  {
    if (valid[i])
    {
      const Vector3f& mean = means[i];
      const Matrix3f& cov  = covariances[i];

      means_f.push_back(mean);
      covariances_f.push_back(cov);
    }
  }
}
示例#6
0
void pointCloudFromMeans(
    const Vector3fVector& means,
    PointCloudFeature& cloud)
{
    unsigned int size = means.size();
    cloud.points.resize(size);
    for(unsigned int i = 0; i < size; ++i)
    {
        const Vector3f& m = means[i];
        PointFeature& p = cloud.points[i];

        p.x = m(0,0);
        p.y = m(1,0);
        p.z = m(2,0);
    }

    cloud.height = 1;
    cloud.width = size;
    cloud.is_dense = true;
}