point_cloud: Support Normals and RGBs

bindings/pydrake/perception_py.cc

@@ -25,9 +25,11 @@ void init_pc_flags(py::module m) {
   {
     using Class = BaseField;
     constexpr auto& cls_doc = doc.BaseField;
-    py::enum_<Class>(m, "BaseField", cls_doc.doc)
+    py::enum_<Class>(m, "BaseField", py::arithmetic(), cls_doc.doc)
         .value("kNone", Class::kNone, cls_doc.kNone.doc)
-        .value("kXYZs", Class::kXYZs, cls_doc.kXYZs.doc);
+        .value("kXYZs", Class::kXYZs, cls_doc.kXYZs.doc)
+        .value("kNormals", Class::kNormals, cls_doc.kNormals.doc)
+        .value("kRGBs", Class::kRGBs, cls_doc.kRGBs.doc);
   }
 
   {
@@ -39,8 +41,8 @@ void init_pc_flags(py::module m) {
         .def("base_fields", &Class::base_fields, cls_doc.base_fields.doc)
         .def("has_base_fields", &Class::has_base_fields,
             cls_doc.has_base_fields.doc)
-        // TODO(eric.cousineau): Bind bitwise operator overloads if they're
-        // useful.
+        .def(py::self | py::self)
+        .def(py::self & py::self)
         .def(py::self == py::self)
         .def(py::self != py::self);
   }
@@ -62,6 +64,7 @@ void init_perception(py::module m) {
     constexpr auto& cls_doc = doc.PointCloud;
     py::class_<Class> cls(m, "PointCloud", cls_doc.doc);
     cls.attr("T") = GetPyParam<Class::T>()[0];
+    cls.attr("C") = GetPyParam<Class::C>()[0];
     cls.attr("D") = GetPyParam<Class::D>()[0];
     // N.B. Workaround linking error for `constexpr` bits.
     cls.attr("kDefaultValue") = Class::T{Class::kDefaultValue};
@@ -79,13 +82,32 @@ void init_perception(py::module m) {
         .def("resize",
             [](PointCloud* self, int new_size) { self->resize(new_size); },
             py::arg("new_size"), cls_doc.resize.doc)
+        // XYZs
         .def("has_xyzs", &Class::has_xyzs, cls_doc.has_xyzs.doc)
         .def("xyzs", &Class::xyzs, py_reference_internal, cls_doc.xyzs.doc)
         .def("mutable_xyzs", &Class::mutable_xyzs, py_reference_internal,
             cls_doc.mutable_xyzs.doc)
         .def("xyz", &Class::xyz, py::arg("i"), cls_doc.xyz.doc)
         .def("mutable_xyz", &Class::mutable_xyz, py::arg("i"),
             py_reference_internal, cls_doc.mutable_xyz.doc)
+        // Normals
+        .def("has_normals", &Class::has_normals, cls_doc.has_normals.doc)
+        .def("normals", &Class::normals, py_reference_internal,
+            cls_doc.normals.doc)
+        .def("mutable_normals", &Class::mutable_normals, py_reference_internal,
+            cls_doc.mutable_normals.doc)
+        .def("normal", &Class::normal, py::arg("i"), cls_doc.normal.doc)
+        .def("mutable_normal", &Class::mutable_normal, py::arg("i"),
+            py_reference_internal, cls_doc.mutable_normal.doc)
+        // RGBs
+        .def("has_rgbs", &Class::has_rgbs, cls_doc.has_rgbs.doc)
+        .def("rgbs", &Class::rgbs, py_reference_internal, cls_doc.rgbs.doc)
+        .def("mutable_rgbs", &Class::mutable_rgbs, py_reference_internal,
+            cls_doc.mutable_rgbs.doc)
+        .def("rgb", &Class::rgb, py::arg("i"), cls_doc.rgb.doc)
+        .def("mutable_rgb", &Class::mutable_rgb, py::arg("i"),
+            py_reference_internal, cls_doc.mutable_rgb.doc)
+        // Mutators.
         .def("SetFrom",
             [](PointCloud* self, const PointCloud& other) {
               self->SetFrom(other);

bindings/pydrake/test/perception_test.py

@@ -23,8 +23,14 @@ def test_pc_flags_api(self):
         fields_none = mut.Fields(enum.kNone)
         self.assertNotEqual(fields, fields_none)
 
+    def check_array(self, x, dtype_expected, shape_expected):
+        x = np.asarray(x)
+        self.assertEqual(x.dtype, dtype_expected)
+        self.assertTupleEqual(x.shape, shape_expected)
+
     def test_point_cloud_api(self):
         self.assertEqual(mut.PointCloud.T, np.float32)
+        self.assertEqual(mut.PointCloud.C, np.uint8)
         self.assertEqual(mut.PointCloud.D, np.float32)
         self.assertTrue(mut.PointCloud.IsDefaultValue(
             value=mut.PointCloud.kDefaultValue))
@@ -50,6 +56,23 @@ def test_point_cloud_api(self):
         pc_new = mut.PointCloud()
         pc_new.SetFrom(other=pc)
         np.testing.assert_equal(pc.xyzs(), pc_new.xyzs())
+        # - Additional types are tested via simple existence checks.
+        all_fields = mut.Fields(
+            mut.BaseField.kXYZs | mut.BaseField.kNormals | mut.BaseField.kRGBs)
+        count = 1
+        pc = mut.PointCloud(new_size=count, fields=all_fields)
+        self.check_array(pc.mutable_xyzs(), np.float32, (3, count))
+        self.check_array(pc.mutable_normals(), np.float32, (3, count))
+        pc.normals()
+        pc.mutable_normal(i=0)
+        pc.normal(i=0)
+        self.check_array(pc.mutable_rgbs(), np.uint8, (3, count))
+        pc.rgbs()
+        pc.mutable_rgb(i=0)
+        pc.rgb(i=0)
+        # - Check for none.
+        with self.assertRaises(RuntimeError) as ex:
+            mut.PointCloud(new_size=0, fields=mut.Fields(mut.BaseField.kNone))
         # Test Systems' value registration.
         self.assertIsInstance(AbstractValue.Make(pc), Value[mut.PointCloud])
 

perception/point_cloud.cc

@@ -17,6 +17,7 @@ namespace {
 
 // Convenience aliases.
 typedef PointCloud::T T;
+typedef PointCloud::C C;
 typedef PointCloud::D D;
 
 }  // namespace
@@ -46,12 +47,18 @@ class PointCloud::Storage {
     size_ = new_size;
     if (fields_.contains(pc_flags::kXYZs))
       xyzs_.conservativeResize(NoChange, new_size);
+    if (fields_.contains(pc_flags::kNormals))
+      normals_.conservativeResize(NoChange, new_size);
+    if (fields_.contains(pc_flags::kRGBs))
+      rgbs_.conservativeResize(NoChange, new_size);
     if (fields_.has_descriptor())
       descriptors_.conservativeResize(NoChange, new_size);
     CheckInvariants();
   }
 
   Eigen::Ref<Matrix3X<T>> xyzs() { return xyzs_; }
+  Eigen::Ref<Matrix3X<T>> normals() { return normals_; }
+  Eigen::Ref<Matrix3X<C>> rgbs() { return rgbs_; }
   Eigen::Ref<MatrixX<T>> descriptors() { return descriptors_; }
 
  private:
@@ -60,6 +67,14 @@ class PointCloud::Storage {
       const int xyz_size = xyzs_.cols();
       DRAKE_DEMAND(xyz_size == size());
     }
+    if (fields_.contains(pc_flags::kNormals)) {
+      const int normals_size = normals_.cols();
+      DRAKE_DEMAND(normals_size == size());
+    }
+    if (fields_.contains(pc_flags::kRGBs)) {
+      const int rgbs_size = rgbs_.cols();
+      DRAKE_DEMAND(rgbs_size == size());
+    }
     if (fields_.has_descriptor()) {
       const int descriptor_size = descriptors_.cols();
       DRAKE_DEMAND(descriptor_size == size());
@@ -69,6 +84,8 @@ class PointCloud::Storage {
   const pc_flags::Fields fields_;
   int size_{};
   Matrix3X<T> xyzs_;
+  Matrix3X<T> normals_;
+  Matrix3X<C> rgbs_;
   MatrixX<T> descriptors_;
 };
 
@@ -172,6 +189,12 @@ void PointCloud::SetDefault(int start, int num) {
   if (has_xyzs()) {
     set(mutable_xyzs(), kDefaultValue);
   }
+  if (has_normals()) {
+    set(mutable_normals(), kDefaultValue);
+  }
+  if (has_rgbs()) {
+    set(mutable_rgbs(), kDefaultColor);
+  }
   if (has_descriptors()) {
     set(mutable_descriptors(), kDefaultValue);
   }
@@ -193,6 +216,12 @@ void PointCloud::SetFrom(const PointCloud& other,
   if (fields_resolved.contains(pc_flags::kXYZs)) {
     mutable_xyzs() = other.xyzs();
   }
+  if (fields_resolved.contains(pc_flags::kNormals)) {
+    mutable_normals() = other.normals();
+  }
+  if (fields_resolved.contains(pc_flags::kRGBs)) {
+    mutable_rgbs() = other.rgbs();
+  }
   if (fields_resolved.has_descriptor()) {
     mutable_descriptors() = other.descriptors();
   }
@@ -218,6 +247,30 @@ Eigen::Ref<Matrix3X<T>> PointCloud::mutable_xyzs() {
   return storage_->xyzs();
 }
 
+bool PointCloud::has_normals() const {
+  return fields_.contains(pc_flags::kNormals);
+}
+Eigen::Ref<const Matrix3X<T>> PointCloud::normals() const {
+  DRAKE_DEMAND(has_normals());
+  return storage_->normals();
+}
+Eigen::Ref<Matrix3X<T>> PointCloud::mutable_normals() {
+  DRAKE_DEMAND(has_normals());
+  return storage_->normals();
+}
+
+bool PointCloud::has_rgbs() const {
+  return fields_.contains(pc_flags::kRGBs);
+}
+Eigen::Ref<const Matrix3X<C>> PointCloud::rgbs() const {
+  DRAKE_DEMAND(has_rgbs());
+  return storage_->rgbs();
+}
+Eigen::Ref<Matrix3X<C>> PointCloud::mutable_rgbs() {
+  DRAKE_DEMAND(has_rgbs());
+  return storage_->rgbs();
+}
+
 bool PointCloud::has_descriptors() const {
   return fields_.has_descriptor();
 }

perception/point_cloud.h

@@ -1,6 +1,7 @@
 #pragma once
 
 #include <cmath>
+#include <cstdint>
 #include <limits>
 #include <memory>
 #include <string>
@@ -69,11 +70,15 @@ class PointCloud final {
   /// Geometric scalar type.
   using T = float;
 
+  /// Color channel scalar type.
+  using C = uint8_t;
+
   /// Descriptor scalar type.
   using D = T;
 
   /// Represents an invalid or uninitialized value.
   static constexpr T kDefaultValue = std::numeric_limits<T>::quiet_NaN();
+  static constexpr C kDefaultColor{};
   static inline bool IsDefaultValue(T value) { return std::isnan(value); }
   static inline bool IsInvalidValue(T value) { return !std::isfinite(value); }
 
@@ -131,8 +136,7 @@ class PointCloud final {
   ///    Do not default-initialize new values.
   void resize(int new_size, bool skip_initialize = false);
 
-
-  /// @name Geometric Descriptors
+  /// @name Geometric Descriptors - XYZs
   /// @{
 
   /// Returns if this cloud provides XYZ values.
@@ -146,17 +150,69 @@ class PointCloud final {
   /// @pre `has_xyzs()` must be true.
   Eigen::Ref<Matrix3X<T>> mutable_xyzs();
 
-  /// Returns access to a XYZ values.
+  /// Returns access to an XYZ value.
   /// @pre `has_xyzs()` must be true.
   Vector3<T> xyz(int i) const { return xyzs().col(i); }
 
-  /// Returns mutable access to a XYZ values.
+  /// Returns mutable access to an XYZ value.
   /// @pre `has_xyzs()` must be true.
   Eigen::Ref<Vector3<T>> mutable_xyz(int i) {
     return mutable_xyzs().col(i);
   }
 
-  ///@}
+  /// @}  // Geometric Descriptors - XYZs
+
+  /// @name Geometric Descriptors - Normals
+  /// @{
+
+  /// Returns if this cloud provides normals.
+  bool has_normals() const;
+
+  /// Returns access to normals.
+  /// @pre `has_normals()` must be true.
+  Eigen::Ref<const Matrix3X<T>> normals() const;
+
+  /// Returns mutable access to normals.
+  /// @pre `has_normals()` must be true.
+  Eigen::Ref<Matrix3X<T>> mutable_normals();
+
+  /// Returns access to a normal.
+  /// @pre `has_normals()` must be true.
+  Vector3<T> normal(int i) const { return normals().col(i); }
+
+  /// Returns mutable access to a normal.
+  /// @pre `has_normals()` must be true.
+  Eigen::Ref<Vector3<T>> mutable_normal(int i) {
+    return mutable_normals().col(i);
+  }
+
+  /// @}  // Geometric Descriptors - Normals
+
+  /// @name Geometric Descriptors - RGBs
+  /// @{
+
+  /// Returns if this cloud provides RGB colors.
+  bool has_rgbs() const;
+
+  /// Returns access to RGB colors.
+  /// @pre `has_rgbs()` must be true.
+  Eigen::Ref<const Matrix3X<C>> rgbs() const;
+
+  /// Returns mutable access to RGB colors.
+  /// @pre `has_rgbs()` must be true.
+  Eigen::Ref<Matrix3X<C>> mutable_rgbs();
+
+  /// Returns access to an RGB color.
+  /// @pre `has_rgbs()` must be true.
+  Vector3<C> rgb(int i) const { return rgbs().col(i); }
+
+  /// Returns mutable access to an RGB color.
+  /// @pre `has_rgbs()` must be true.
+  Eigen::Ref<Vector3<C>> mutable_rgb(int i) {
+    return mutable_rgbs().col(i);
+  }
+
+  /// @}  // Geometric Descriptors - RGBs
 
   /// @name Run-Time Descriptors
   /// @{
@@ -180,11 +236,11 @@ class PointCloud final {
   /// @pre `has_descriptors()` must be true.
   Eigen::Ref<MatrixX<D>> mutable_descriptors();
 
-  /// Returns access to a descriptor values.
+  /// Returns access to a descriptor value.
   /// @pre `has_descriptors()` must be true.
   VectorX<T> descriptor(int i) const { return descriptors().col(i); }
 
-  /// Returns mutable access to a descriptor values.
+  /// Returns mutable access to a descriptor value.
   /// @pre `has_descriptors()` must be true.
   Eigen::Ref<VectorX<T>> mutable_descriptor(int i) {
     return mutable_descriptors().col(i);

perception/point_cloud_flags.h

@@ -22,8 +22,18 @@ enum BaseField : int {
   kInherit = 1 << 0,
   /// XYZ point in Cartesian space.
   kXYZs = 1 << 1,
+  /// Normals.
+  kNormals = 1 << 2,
+  /// RGB colors.
+  kRGBs = 1 << 3,
 };
 
+namespace internal {
+
+constexpr BaseField kMaxBitInUse = kRGBs;
+
+}  // namespace internal
+
 /// Describes an descriptor field with a name and the descriptor's size.
 ///
 /// @note This is defined as follows to enable an open set of descriptors, but
@@ -84,7 +94,7 @@ class Fields {
   // NOLINTNEXTLINE(runtime/explicit): This conversion is desirable.
   Fields(BaseFieldT base_fields)
       : base_fields_(base_fields) {
-    if (base_fields < 0 || base_fields >= (kXYZs << 1))
+    if (base_fields < 0 || base_fields >= (internal::kMaxBitInUse << 1))
       throw std::runtime_error("Invalid BaseField specified.");
   }