Feature/mesh bindings (#59)

* add mesh setters/getters via eigen to python bindings

* actually add elements to vertices/faces

* add mesh edge bindings and fix layerPrefix python conversion

Co-authored-by: Nathan Hughes <[email protected]>

python/bindings/spark_dsg_bindings.cpp

@@ -53,6 +53,8 @@ using namespace py::literals;
 
 using namespace spark_dsg;
 
+PYBIND11_MAKE_OPAQUE(std::map<size_t, MeshEdge>);
+
 template <typename Scalar>
 struct Quaternion {
   Quaternion() : w(1.0f), x(0.0f), y(0.0f), z(0.0f) {}
@@ -335,6 +337,11 @@ PYBIND11_MODULE(_dsg_bindings, module) {
                     nullptr,
                     py::return_value_policy::reference_internal);
 
+  py::class_<MeshEdge>(module, "MeshEdge")
+      .def_property_readonly(
+          "node_id", [](const MeshEdge& edge) { return NodeSymbol(edge.source_node); })
+      .def_readonly("mesh_vertex", &MeshEdge::mesh_vertex);
+
 #define MAKE_SPECIALIZED_NODE_ADD(AttributeClass)                   \
   def("add_node",                                                   \
       [](DynamicSceneGraph& graph,                                  \
@@ -368,6 +375,7 @@ PYBIND11_MODULE(_dsg_bindings, module) {
       .def(py::init<>())
       .def(py::init<const DynamicSceneGraph::LayerIds&>())
       .def("clear", &DynamicSceneGraph::clear)
+      .def("create_dynamic_layer", &DynamicSceneGraph::createDynamicLayer)
       .MAKE_SPECIALIZED_NODE_ADD(ObjectNodeAttributes)
       .MAKE_SPECIALIZED_NODE_ADD(RoomNodeAttributes)
       .MAKE_SPECIALIZED_NODE_ADD(PlaceNodeAttributes)
@@ -479,9 +487,98 @@ PYBIND11_MODULE(_dsg_bindings, module) {
                     py::return_value_policy::reference_internal)
       .def("clone", &DynamicSceneGraph::clone)
       .def("__deepcopy__",
-           [](const DynamicSceneGraph& G, py::object) { return G.clone(); });
+           [](const DynamicSceneGraph& G, py::object) { return G.clone(); })
+      .def("get_mesh_vertices",
+           [](const DynamicSceneGraph& G) {
+             auto vertices = G.getMeshVertices();
+             if (!vertices) {
+               return Eigen::MatrixXd();
+             }
+
+             Eigen::MatrixXd to_return(6, vertices->size());
+             for (size_t i = 0; i < vertices->size(); ++i) {
+               const auto& point = vertices->at(i);
+               to_return(0, i) = point.x;
+               to_return(1, i) = point.y;
+               to_return(2, i) = point.z;
+               to_return(3, i) = point.r / 255.0;
+               to_return(4, i) = point.g / 255.0;
+               to_return(5, i) = point.b / 255.0;
+             }
+             return to_return;
+           })
+      .def("get_mesh_faces",
+           [](const DynamicSceneGraph& G) {
+             auto faces = G.getMeshFaces();
+             if (!faces) {
+               return Eigen::MatrixXi();
+             }
+
+             Eigen::MatrixXi to_return(3, faces->size());
+             for (size_t i = 0; i < faces->size(); ++i) {
+               const auto& face = faces->at(i);
+               to_return(0, i) = face.vertices.at(0);
+               to_return(1, i) = face.vertices.at(1);
+               to_return(2, i) = face.vertices.at(2);
+             }
+             return to_return;
+           })
+      .def("set_mesh_vertices",
+           [](DynamicSceneGraph& G, const Eigen::MatrixXd& points) {
+             if (points.rows() != 6) {
+               std::stringstream ss;
+               ss << "point rows do not match expected: " << points.rows() << " != 6";
+               throw std::invalid_argument(ss.str());
+             }
+
+             DynamicSceneGraph::MeshVertices::Ptr vertices(
+                 new DynamicSceneGraph::MeshVertices());
+             for (int i = 0; i < points.cols(); ++i) {
+               pcl::PointXYZRGBA point;
+               point.x = points(0, i);
+               point.y = points(1, i);
+               point.z = points(2, i);
+               point.r = static_cast<uint8_t>(points(3, i) * 255);
+               point.g = static_cast<uint8_t>(points(4, i) * 255);
+               point.b = static_cast<uint8_t>(points(5, i) * 255);
+               point.a = 255;
+               vertices->push_back(point);
+             }
+             G.setMesh(vertices, G.getMeshFaces(), false);
+           })
+      .def("set_mesh_faces",
+           [](DynamicSceneGraph& G, const Eigen::MatrixXd& indices) {
+             if (indices.rows() != 3) {
+               std::stringstream ss;
+               ss << "index rows do not match expected: " << indices.rows() << " != 3";
+               throw std::invalid_argument(ss.str());
+             }
+
+             std::shared_ptr<DynamicSceneGraph::MeshFaces> faces(
+                 new DynamicSceneGraph::MeshFaces());
+             for (int i = 0; i < indices.cols(); ++i) {
+               pcl::Vertices face;
+               face.vertices.push_back(indices(0, i));
+               face.vertices.push_back(indices(1, i));
+               face.vertices.push_back(indices(2, i));
+               faces->push_back(face);
+             }
+             G.setMesh(G.getMeshVertices(), faces, false);
+           })
+      .def("insert_mesh_edge",
+           &DynamicSceneGraph::insertMeshEdge,
+           "source"_a,
+           "mesh_vertex"_a,
+           "allow_invalid_mesh"_a = true)
+      .def("remove_mesh_edge", &DynamicSceneGraph::removeMeshEdge)
+      .def("clear_mesh_edges", &DynamicSceneGraph::clearMeshEdges)
+      .def("invalidate_mesh_vertex", &DynamicSceneGraph::invalidateMeshVertex)
+      .def("get_mesh_connections", &DynamicSceneGraph::getMeshConnectionIndices)
+      .def_property_readonly("mesh_edges", &DynamicSceneGraph::getMeshEdges);
 
 #undef MAKE_SPECIALZIED_NODE_ADD
 
   module.def("compute_ancestor_bounding_box", &computeAncestorBoundingBox);
+
+  py::implicitly_convertible<char, LayerPrefix>();
 }

python/tests/test_bindings.py

@@ -1,3 +1,4 @@
+"""Test that the bindings are working appropriately."""
 import spark_dsg as dsg
 
 
@@ -6,3 +7,10 @@ def test_empty_graph():
     G = dsg.DynamicSceneGraph()
     assert G.num_nodes() == 0
     assert G.num_edges() == 0
+
+
+def test_implicit_prefix():
+    """Test that we got rid of the need for explicit layer prefix construction."""
+    G = dsg.DynamicSceneGraph()
+    G.create_dynamic_layer(dsg.DsgLayers.AGENTS, "a")
+    assert G.has_layer(dsg.DsgLayers.AGENTS, "a")