If here when if(use_global_height_) we do the ++iter_height, then we can remove it from the for loop, which also allows us not to initialize it with z when not set. This will improve performance for users without the height element since we're not double iterating through the Z channel. The else then can also be removed.

I had originally tried implementing as you described, but had two problems.

1. I couldn't find a way to initilize iter_height inside the if statement that didn't give an undeclared error for iter_height inside the for loop. This is possibly a skill issue on my part, please advise if there is a method I am unaware of.
2. If removing the ++iter_height from the for loop and adding it to the loop body then it also needs to be incremented at the < min_range check to keep the iterators synchronized. I saw it as a potential foot gun in future development if other checks are implemented but miss incrementing the height iterator. I will defer to your judgement on that implementation.

In the last commit I pushed I duplicated the for loop with the different iterator implementations as an additional option. I'm not a fan of the code duplication but it worked and would address performance concerns.

Perhaps something like:

  bool height_present = false;
  for(const auto & field : data_->fields) {
    if (field.name == "height") {
      height_present = true;
    }
  }

  // Reference height field
  std::string height_field{""};
  if (use_global_height_ && height_present) {
    height_field = "height";
  } else if (use_global_height_) {
    // Log error here TODO
    return false;
  }

  sensor_msgs::PointCloud2ConstIterator<float> iter_height(*data_, height_field);

  // Refill data array with PointCloud points in base frame
  for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) {
    // Transform point coordinates from source frame -> to base frame
    tf2::Vector3 p_v3_s(*iter_x, *iter_y, *iter_z);

    double data_height = *iter_z;
    if (use_global_height_) {
      data_height = *iter_height;
      iter_height++;
    }

    // Check range from sensor origin before transformation
    double range = p_v3_s.length();
    if (range < min_range_) {
      continue;
    }
    tf2::Vector3 p_v3_b = tf_transform * p_v3_s;
    // Refill data array
    if (data_height >= min_height_ && data_height <= max_height_) {
      data.push_back({p_v3_b.x(), p_v3_b.y()});
    }
  }

This doesn't increment the iter_height unless use_global_height_ = true, which resolves the computation issue since the iterator doesn't do a copy of the PC data. It also removes duplication of the loop and in the loop by just replacing a locally stored version of data_height that is set as z when not using the height. Then height is incremented before any processing happens to keep it aligned.

What do you think?

That works except the iter_height object gives a run time error that the field is not found since height_field is initialized as an empty string. Since you said creating the iterator doesn't copy the PC data that shouldn't be a performance impact. And in that case I will just initialize it as the "z" field and then overwrite it to "height" for the global height case?

Yeah that works!