PCLPointCLoud2 needs functionality to drop fields marked as skip (name == "_")

[kunaltyagi]

PR #3320 is removing functionality of dropping the skip fields on concatenation IFF the two point clouds are the same. There needs to be a replacement for the same

Expected Behavior

A function to remove unneded fields

Possible Solution

A decent starting point is

bool
pcl::PCLPointCloud2::squeeze ()
{
  const auto size = width * height;
  struct offset
  {
    std::size_t from = 0, to = 0, size = 0;
    explicit offset(std::size_t f = 0, std::size_t t = 0, std::size_t s = 0): from(f), to(t), size(s) {}
  };
  std::vector<offset> memcpy_offsets;
  // worst case: every second field needs to be moved
  memcpy_offsets.reserve (fields.size ()/2 + 1);

  offset running_offset;
  bool skipping = false;
  for (const auto& field: fields)
  {
    const auto& size = field.count * pcl::getFieldSize (field.datatype);

    if (field.name != "_")
    {
      skipping = false;
      running_offset.size += size;
      continue;
    }
    if (skipping)
    {
      running_offset.from += size;
      continue;
    }
    skipping = true;
    memcpy_offsets.emplace_back(running_offset);
    running_offset.from += running_offset.size;
    running_offset.to += running_offset.size;
    running_offset.size = 0;
  }
  if (!skipping)
  {
    memcpy_offsets.emplace_back(running_offset);
  }

  for (std::size_t cp = 0; cp < size; ++cp)
  {
    bool modified = false;
    for (const auto& memloc: memcpy_offsets)
    {
      if (memloc.to == memloc.from)
      {
        continue;
      }
      modified = true;
      memcpy (reinterpret_cast<char*> (&data[cp * point_step + memloc.to]),
              reinterpret_cast<const char*> (&data[cp * point_step + memloc.from]),
              memloc.size);
    }
    if (!modified)
    {
      break;
    }
  }

  const auto data_size = std::accumulate(memcpy_offsets.begin (), memcpy_offsets.end (), 0,
    [](const auto& sum, const auto& offset)
    {
      return sum + offset.size;
    });
  data.resize (data_size);

  // Removal by indices would only save 1 comparison and require memory for index vector
  fields.erase (std::remove_if(fields.begin (), fields.end (), [](const auto& field)
                {
                  return field.name == "_";
                }),
                fields.end ());
  return true;
}

This has error in the memcpy region of code

[stale]

Marking this as stale due to 30 days of inactivity. It will be closed in 7 days if no further activity occurs.