[perception] PointCloud can morph its fields after construction (Robo…

…tLocomotion#18987)

Prior to this PR, the `Fields` of a PointCloud object is determined
at construction time, and doesn't allow any morphing after that.
An exception will be thrown if one attempts to copy/move-assign a
PointCloud with a different `Fields` to another. However, the same
PointCloud can be used to copy/move-construct another Pointcloud.
This PR tries to make the behavior consistent and avoid confusions
for users.

bindings/pydrake/perception_py.cc

@@ -119,6 +119,8 @@ void init_perception(py::module m) {
               self->SetFrom(other);
             },
             py::arg("other"), cls_doc.SetFrom.doc)
+        .def("SetFields", &Class::SetFields, py::arg("new_fields"),
+            py::arg("skip_initialize") = false, cls_doc.SetFields.doc)
         .def("Crop", &Class::Crop, py::arg("lower_xyz"), py::arg("upper_xyz"),
             cls_doc.Crop.doc)
         .def("FlipNormalsTowardPoint", &Class::FlipNormalsTowardPoint,

bindings/pydrake/test/perception_test.py

@@ -77,6 +77,22 @@ def test_point_cloud_api(self):
         pc.rgbs()
         pc.mutable_rgb(i=0)
         pc.rgb(i=0)
+        # Test morphing fields after PointCloud creation.
+        rgb_pc = mut.PointCloud(new_size=count,
+                                fields=mut.Fields(mut.BaseField.kRGBs))
+        all_fields_pc = mut.PointCloud(new_size=count, fields=all_fields)
+        test_rgbs = [[1, 2, 3]]
+        all_fields_pc.mutable_rgbs().T[:] = test_rgbs
+        self.assertFalse((rgb_pc.rgbs().T == test_rgbs).all())
+        self.assertFalse(rgb_pc.has_xyzs())
+        rgb_pc = all_fields_pc
+        self.assertTrue((rgb_pc.rgbs().T == test_rgbs).all())
+        self.assertTrue(rgb_pc.has_xyzs())
+        self.assertTrue(rgb_pc.has_normals())
+        rgb_pc.SetFields(new_fields=mut.Fields(mut.BaseField.kNormals),
+                         skip_initialize=False)
+        self.assertFalse(rgb_pc.has_rgbs())
+        self.assertTrue(rgb_pc.has_normals())
         # - Check for none.
         with self.assertRaises(RuntimeError) as ex:
             mut.PointCloud(new_size=0, fields=mut.Fields(mut.BaseField.kNone))

bindings/pydrake/visualization/test/meldis_test.py

@@ -25,9 +25,6 @@
 from pydrake.common import (
     FindResourceOrThrow,
 )
-from pydrake.common.value import (
-    Value,
-)
 from pydrake.geometry import (
     DrakeVisualizer,
     DrakeVisualizerParams,
@@ -119,8 +116,7 @@ def _publish_lcm_point_cloud(self, lcm, channel, cloud):
         # Set input and publish the point cloud.
         context = diagram.CreateDefaultContext()
         cloud_context = cloud_to_lcm.GetMyContextFromRoot(context)
-        cloud_to_lcm.get_input_port().FixValue(
-            cloud_context, Value[PointCloud](cloud))
+        cloud_to_lcm.get_input_port().FixValue(cloud_context, cloud)
         diagram.ForcedPublish(context)
 
     def test_viewer_applet(self):

perception/point_cloud.cc

@@ -83,7 +83,10 @@ class PointCloud::Storage {
     normals_.conservativeResize(NoChange,
                                 f.contains(pc_flags::kNormals) ? size_ : 0);
     rgbs_.conservativeResize(NoChange, f.contains(pc_flags::kRGBs) ? size_ : 0);
-    descriptors_.conservativeResize(NoChange, f.has_descriptor() ? size_ : 0);
+    // Note: The row size can change depends on whether 'f' contains a
+    // descriptor field and the type of the descriptor.
+    descriptors_.conservativeResize(f.descriptor_type().size(),
+                                    f.has_descriptor() ? size_ : 0);
     fields_ = f;
     CheckInvariants();
   }
@@ -95,21 +98,32 @@ class PointCloud::Storage {
 
  private:
   void CheckInvariants() const {
+    const int xyz_size = xyzs_.cols();
     if (fields_.contains(pc_flags::kXYZs)) {
-      const int xyz_size = xyzs_.cols();
       DRAKE_DEMAND(xyz_size == size());
+    } else {
+      DRAKE_DEMAND(xyz_size == 0);
     }
+    const int normals_size = normals_.cols();
     if (fields_.contains(pc_flags::kNormals)) {
-      const int normals_size = normals_.cols();
       DRAKE_DEMAND(normals_size == size());
+    } else {
+      DRAKE_DEMAND(normals_size == 0);
     }
+    const int rgbs_size = rgbs_.cols();
     if (fields_.contains(pc_flags::kRGBs)) {
-      const int rgbs_size = rgbs_.cols();
       DRAKE_DEMAND(rgbs_size == size());
+    } else {
+      DRAKE_DEMAND(rgbs_size == 0);
     }
+    const int descriptor_cols = descriptors_.cols();
+    const int descriptor_rows = descriptors_.rows();
     if (fields_.has_descriptor()) {
-      const int descriptor_size = descriptors_.cols();
-      DRAKE_DEMAND(descriptor_size == size());
+      DRAKE_DEMAND(descriptor_cols == size());
+      DRAKE_DEMAND(descriptor_rows == fields_.descriptor_type().size());
+    } else {
+      DRAKE_DEMAND(descriptor_cols == 0);
+      DRAKE_DEMAND(descriptor_rows == 0);
     }
   }
 
@@ -132,23 +146,21 @@ pc_flags::Fields ResolveFields(
   }
 }
 
-// Resolves the fields from a pair of point clouds and desired fields.
-// Implements the resolution rules in `SetFrom`.
-// @pre Valid point clouds `a` and `b`.
-// @returns Fields that both point clouds have.
-pc_flags::Fields ResolvePairFields(
-    const PointCloud& a,
-    const PointCloud& b,
-    pc_flags::Fields fields) {
-  if (fields == pc_flags::kInherit) {
-    // If we do not permit a subset, expect the exact same fields.
-    a.RequireExactFields(b.fields());
-    return a.fields();
-  } else {
-    a.RequireFields(fields);
-    b.RequireFields(fields);
-    return fields;
+// Finds the added fields from `new_fields` w.r.t. `old_fields`.
+pc_flags::Fields FindAddedFields(pc_flags::Fields old_fields,
+                                 pc_flags::Fields new_fields) {
+  pc_flags::Fields added_fields(pc_flags::kNone);
+  for (const auto field :
+       {pc_flags::kXYZs, pc_flags::kNormals, pc_flags::kRGBs}) {
+    if (!old_fields.contains(field) && new_fields.contains(field))
+      added_fields |= field;
   }
+
+  if (new_fields.has_descriptor() &&
+      old_fields.descriptor_type() != new_fields.descriptor_type()) {
+    added_fields |= new_fields.descriptor_type();
+  }
+  return added_fields;
 }
 
 }  // namespace
@@ -172,7 +184,7 @@ PointCloud::PointCloud(const PointCloud& other,
 }
 
 PointCloud::PointCloud(PointCloud&& other)
-    : PointCloud(0, other.fields(), true) {
+    : PointCloud(0, other.storage_->fields(), true) {
   // This has zero size. Directly swap storages.
   storage_.swap(other.storage_);
 }
@@ -183,8 +195,6 @@ PointCloud& PointCloud::operator=(const PointCloud& other) {
 }
 
 PointCloud& PointCloud::operator=(PointCloud&& other) {
-  // We may only take rvalue references if the fields match exactly.
-  RequireExactFields(other.fields());
   // Swap storages.
   storage_.swap(other.storage_);
   // Empty out the other cloud, but let it remain being a valid point cloud
@@ -207,6 +217,8 @@ int PointCloud::size() const {
 void PointCloud::resize(int new_size, bool skip_initialization) {
   DRAKE_DEMAND(new_size >= 0);
   const int old_size = size();
+  if (old_size == new_size)
+    return;
   storage_->resize(new_size);
   DRAKE_DEMAND(storage_->size() == new_size);
   if (new_size > old_size && !skip_initialization) {
@@ -215,6 +227,27 @@ void PointCloud::resize(int new_size, bool skip_initialization) {
   }
 }
 
+void PointCloud::SetFields(pc_flags::Fields new_fields, bool skip_initialize) {
+  const pc_flags::Fields old_fields = storage_->fields();
+  if (old_fields == new_fields)
+    return;
+  storage_->UpdateFields(new_fields);
+
+  if (!skip_initialize) {
+    // Default-initialize containers for newly added fields.
+    const pc_flags::Fields added_fields =
+        FindAddedFields(old_fields, new_fields);
+    if (added_fields.contains(pc_flags::kXYZs))
+      mutable_xyzs().setConstant(kDefaultValue);
+    if (added_fields.contains(pc_flags::kNormals))
+      mutable_normals().setConstant(kDefaultValue);
+    if (added_fields.contains(pc_flags::kRGBs))
+      mutable_rgbs().setConstant(kDefaultColor);
+    if (added_fields.has_descriptor())
+      mutable_descriptors().setConstant(kDefaultValue);
+  }
+}
+
 void PointCloud::SetDefault(int start, int num) {
   auto set = [=](auto ref, auto value) {
     ref.middleCols(start, num).setConstant(value);
@@ -236,6 +269,7 @@ void PointCloud::SetDefault(int start, int num) {
 void PointCloud::SetFrom(const PointCloud& other,
                          pc_flags::Fields fields_in,
                          bool allow_resize) {
+  // Update the size of this point cloud if necessary.
   int old_size = size();
   int new_size = other.size();
   if (allow_resize) {
@@ -244,18 +278,28 @@ void PointCloud::SetFrom(const PointCloud& other,
     throw std::runtime_error(
         fmt::format("SetFrom: {} != {}", new_size, old_size));
   }
-  pc_flags::Fields fields_resolved =
-      ResolvePairFields(*this, other, fields_in);
-  if (fields_resolved.contains(pc_flags::kXYZs)) {
+
+  // Update or check the fields of the point cloud(s) if necessary.
+  pc_flags::Fields fields_to_copy = fields_in;
+  if (fields_in == pc_flags::kInherit) {
+    fields_to_copy = other.storage_->fields();
+    SetFields(other.storage_->fields(), true);
+  } else {
+    this->RequireFields(fields_to_copy);
+    other.RequireFields(fields_to_copy);
+  }
+
+  // Populate data from `other` to this point cloud.
+  if (fields_to_copy.contains(pc_flags::kXYZs)) {
     mutable_xyzs() = other.xyzs();
   }
-  if (fields_resolved.contains(pc_flags::kNormals)) {
+  if (fields_to_copy.contains(pc_flags::kNormals)) {
     mutable_normals() = other.normals();
   }
-  if (fields_resolved.contains(pc_flags::kRGBs)) {
+  if (fields_to_copy.contains(pc_flags::kRGBs)) {
     mutable_rgbs() = other.rgbs();
   }
-  if (fields_resolved.has_descriptor()) {
+  if (fields_to_copy.has_descriptor()) {
     mutable_descriptors() = other.descriptors();
   }
 }

perception/point_cloud.h

@@ -22,10 +22,6 @@ namespace perception {
 /// This is a mix between the philosophy of PCL (templated interface to
 /// provide a compile-time open set, run-time closed set) and VTK (non-templated
 /// interface to provide a very free form run-time open set).
-/// You may construct one PointCloud which will contain different sets of
-/// data, but you cannot change the contained data types after construction.
-/// However, you can mutate the data contained within the structure and resize
-/// the cloud.
 ///
 /// Definitions:
 ///
@@ -42,7 +38,7 @@ namespace perception {
 ///
 /// @note "contiguous" here means contiguous in memory. This was chosen to
 /// avoid ambiguity between PCL and Eigen, where in PCL "dense" implies that
-/// the point cloud corresponds to a cloud with invalid values, and in Eigen
+/// the point cloud corresponds to a cloud with only valid values, and in Eigen
 /// "dense" implies contiguous storage.
 ///
 /// @note The accessors / mutators for the point fields of this class returns
@@ -90,7 +86,7 @@ class PointCloud final {
   /// @param fields
   ///   Fields that the point cloud contains.
   /// @param skip_initialize
-  ///    Do not default-initialize new values.
+  ///   Do not default-initialize new values.
   explicit PointCloud(int new_size = 0,
                       pc_flags::Fields fields = pc_flags::kXYZs,
                       bool skip_initialize = false);
@@ -253,9 +249,9 @@ class PointCloud final {
   /// @param other
   ///    Other point cloud.
   /// @param fields_in
-  ///    Fields to copy. If this is `kInherit`, then both clouds must have the
-  ///    exact same fields. Otherwise, both clouds must support the fields
-  ///    indicated this parameter.
+  ///    Fields to copy. If this is `kInherit`, then `other`s fields will be
+  ///    copied. Otherwise, both clouds must support the fields indicated this
+  ///    parameter.
   /// @param allow_resize
   ///    Permit resizing to the other cloud's size.
   void SetFrom(
@@ -277,6 +273,16 @@ class PointCloud final {
   /// @name Fields
   /// @{
 
+  /// Updates the point cloud to a given set of fields. In the case of
+  /// introducing a new field, its container will be allocated with the current
+  /// size and default initialized. The data for all retained fields will remain
+  /// unchanged.
+  /// @param new_fields
+  ///    New fields to set to.
+  /// @param skip_initialize
+  ///    Do not default-initialize new values.
+  void SetFields(pc_flags::Fields new_fields, bool skip_initialize = false);
+
   /// Returns if a point cloud has a given set of fields.
   bool HasFields(pc_flags::Fields fields_in) const;