diff --git a/include/spark_dsg/mesh.h b/include/spark_dsg/mesh.h index 74a705b..8f21e23 100644 --- a/include/spark_dsg/mesh.h +++ b/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; diff --git a/python/bindings/src/mesh.cpp b/python/bindings/src/mesh.cpp index 201ce2d..112fa82 100644 --- a/python/bindings/src/mesh.cpp +++ b/python/bindings/src/mesh.cpp @@ -35,6 +35,7 @@ #include "spark_dsg/python/mesh.h" #include +#include #include #include #include @@ -117,11 +118,13 @@ void setEigenFaces(Mesh& mesh, const Eigen::MatrixXi& indices) { void addBindings(pybind11::module_& module) { py::class_>(module, "Mesh") - .def(py::init(), + .def(py::init(), "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(&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 buffer; mesh.serializeToBinary(buffer); return py::bytes(reinterpret_cast(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(contents); return Mesh::deserializeFromBinary( reinterpret_cast(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()); + }, + "rotation"_a = Eigen::Matrix3d::Identity(), + "translation"_a = Eigen::Vector3d::Zero()) + .def("append", &Mesh::append) + .def(py::self += py::self); } } // namespace spark_dsg::python::mesh diff --git a/src/mesh.cpp b/src/mesh.cpp index b8b12e8..bad7206 100644 --- a/src/mesh.cpp +++ b/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 && diff --git a/tests/utest_mesh.cpp b/tests/utest_mesh.cpp index a26c3dc..1de19b8 100644 --- a/tests/utest_mesh.cpp +++ b/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