Feature/mesh append

include/spark_dsg/mesh.h

@@ -248,6 +248,22 @@ class Mesh {
    */
   void transform(const Eigen::Isometry3f& transform);
 
+  /**
+   * @brief Append the other mesh to this one
+   * @param other Mesh to append to the current mesh
+   * @note Fails when the available fields don't match
+   * @returns Whether the append was possible
+   */
+  bool append(const Mesh& other);
+
+  /**
+   * @brief append the mesh on the right hand side to this one
+   * @param other Mesh to append to the current mesh
+   * @note Silently fails to append the other mesh if the available fields don't match
+   * @returns The current mesh
+   */
+  Mesh& operator+=(const Mesh& other);
+
  public:
   const bool has_colors;
   const bool has_timestamps;

python/bindings/src/mesh.cpp

@@ -35,6 +35,7 @@
 #include "spark_dsg/python/mesh.h"
 
 #include <pybind11/eigen.h>
+#include <pybind11/operators.h>
 #include <pybind11/pybind11.h>
 #include <pybind11/stl/filesystem.h>
 #include <spark_dsg/mesh.h>
@@ -117,11 +118,13 @@ void setEigenFaces(Mesh& mesh, const Eigen::MatrixXi& indices) {
 
 void addBindings(pybind11::module_& module) {
   py::class_<Mesh, std::shared_ptr<Mesh>>(module, "Mesh")
-      .def(py::init<bool, bool, bool>(),
+      .def(py::init<bool, bool, bool, bool>(),
            "has_colors"_a = true,
            "has_timestamps"_a = true,
-           "has_labels"_a = "true")
+           "has_labels"_a = true,
+           "has_first_seen_stamps"_a = true)
       .def("empty", &Mesh::empty)
+      .def("clear", &Mesh::clear)
       .def("num_vertices", &Mesh::numVertices)
       .def("num_faces", &Mesh::numFaces)
       .def("resize_vertices", &Mesh::resizeVertices)
@@ -133,6 +136,8 @@ void addBindings(pybind11::module_& module) {
       .def("set_color", &Mesh::setColor)
       .def("timestamp", &Mesh::timestamp)
       .def("set_timestamp", &Mesh::setTimestamp)
+      .def("first_seen_timestamp", &Mesh::firstSeenTimestamp)
+      .def("set_first_seen_timestamp", &Mesh::setFirstSeenTimestamp)
       .def("label", &Mesh::label)
       .def("set_label", &Mesh::setLabel)
       .def("face", py::overload_cast<size_t>(&Mesh::face, py::const_))
@@ -141,22 +146,22 @@ void addBindings(pybind11::module_& module) {
              mesh.face(index) = face;
            })
       .def("to_json", &Mesh::serializeToJson)
+      .def_static("from_json", &Mesh::deserializeFromJson)
       .def("to_binary",
            [](const Mesh& mesh) {
              std::vector<uint8_t> buffer;
              mesh.serializeToBinary(buffer);
              return py::bytes(reinterpret_cast<char*>(buffer.data()), buffer.size());
            })
-      .def("save", &Mesh::save)
-      .def("save",
-           [](const Mesh& mesh, const std::filesystem::path& path) { mesh.save(path); })
-      .def_static("from_json", &Mesh::deserializeFromJson)
       .def_static("from_binary",
                   [](const py::bytes& contents) {
                     const auto& view = static_cast<const std::string_view&>(contents);
                     return Mesh::deserializeFromBinary(
                         reinterpret_cast<const uint8_t*>(view.data()), view.size());
                   })
+      .def("save", &Mesh::save)
+      .def("save",
+           [](const Mesh& mesh, const std::filesystem::path& path) { mesh.save(path); })
       .def_static("load", &Mesh::load)
       .def_static("load",
                   [](const std::filesystem::path& path) { return Mesh::load(path); })
@@ -167,9 +172,21 @@ void addBindings(pybind11::module_& module) {
            [](Mesh& mesh, const Eigen::MatrixXd& points) {
              setEigenVertices(mesh, points);
            })
-      .def("set_faces", [](Mesh& mesh, const Eigen::MatrixXi& faces) {
-        setEigenFaces(mesh, faces);
-      });
+      .def("set_faces",
+           [](Mesh& mesh, const Eigen::MatrixXi& faces) { setEigenFaces(mesh, faces); })
+      .def(
+          "transform",
+          [](Mesh& mesh,
+             const Eigen::Matrix3d& rotation,
+             const Eigen::Vector3d& translation) {
+            const Eigen::Isometry3d transform =
+                Eigen::Translation3d(translation) * Eigen::Quaterniond(rotation);
+            mesh.transform(transform.cast<float>());
+          },
+          "rotation"_a = Eigen::Matrix3d::Identity(),
+          "translation"_a = Eigen::Vector3d::Zero())
+      .def("append", &Mesh::append)
+      .def(py::self += py::self);
 }
 
 }  // namespace spark_dsg::python::mesh

src/mesh.cpp

@@ -248,6 +248,34 @@ void Mesh::transform(const Eigen::Isometry3f& transform) {
   }
 }
 
+bool Mesh::append(const Mesh& other) {
+  if (has_colors != other.has_colors && has_timestamps != other.has_timestamps &&
+      has_labels != other.has_labels &&
+      has_first_seen_stamps != other.has_first_seen_stamps) {
+    return false;
+  }
+
+  size_t offset = points.size();
+  points.insert(points.end(), other.points.begin(), other.points.end());
+  colors.insert(colors.end(), other.colors.begin(), other.colors.end());
+  stamps.insert(stamps.end(), other.stamps.begin(), other.stamps.end());
+  first_seen_stamps.insert(first_seen_stamps.end(),
+                           other.first_seen_stamps.begin(),
+                           other.first_seen_stamps.end());
+  labels.insert(labels.end(), other.labels.begin(), other.labels.end());
+
+  for (const auto& face : other.faces) {
+    faces.push_back({face[0] + offset, face[1] + offset, face[2] + offset});
+  }
+
+  return true;
+}
+
+Mesh& Mesh::operator+=(const Mesh& other) {
+  append(other);
+  return *this;
+}
+
 bool operator==(const Mesh& lhs, const Mesh& rhs) {
   return lhs.has_colors == rhs.has_colors && lhs.has_timestamps == rhs.has_timestamps &&
          lhs.has_labels == rhs.has_labels &&

tests/utest_mesh.cpp

@@ -121,4 +121,29 @@ TEST(MeshTests, transform) {
   }
 }
 
+TEST(MeshTests, append) {
+  Mesh mesh = createDummyMesh();
+  const auto original_vertices = mesh.numVertices();
+  const auto original_faces = mesh.numFaces();
+  mesh.append(createDummyMesh());
+  ASSERT_EQ(mesh.numVertices(), 2 * original_vertices);
+  ASSERT_EQ(mesh.numFaces(), 2 * original_faces);
+  for (size_t i = 0; i < original_vertices; ++i) {
+    EXPECT_EQ(mesh.pos(i), mesh.pos(i + original_vertices));
+    EXPECT_EQ(mesh.color(i), mesh.color(i + original_vertices));
+    EXPECT_EQ(mesh.timestamp(i), mesh.timestamp(i + original_vertices));
+  }
+
+  for (size_t i = 0; i < original_faces; ++i) {
+    const auto& orig = mesh.face(i);
+    const auto& appended = mesh.face(i + original_faces);
+    Mesh::Face expected{
+        orig[0] + original_vertices, orig[1] + original_vertices, orig[2] + original_vertices};
+    EXPECT_EQ(expected, appended);
+    EXPECT_LT(appended[0], mesh.numVertices());
+    EXPECT_LT(appended[1], mesh.numVertices());
+    EXPECT_LT(appended[2], mesh.numVertices());
+  }
+}
+
 }  // namespace spark_dsg