diff --git a/src/Open3D/Geometry/Geometry.h b/src/Open3D/Geometry/Geometry.h index 79d4b7272ab..67c58fe8cf7 100644 --- a/src/Open3D/Geometry/Geometry.h +++ b/src/Open3D/Geometry/Geometry.h @@ -41,6 +41,7 @@ class Geometry { HalfEdgeTriangleMesh = 6, Image = 7, RGBDImage = 8, + TetraMesh = 9, }; public: diff --git a/src/Open3D/Geometry/LineSet.h b/src/Open3D/Geometry/LineSet.h index 9a48380e234..d13ee2669ad 100644 --- a/src/Open3D/Geometry/LineSet.h +++ b/src/Open3D/Geometry/LineSet.h @@ -37,6 +37,7 @@ namespace geometry { class PointCloud; class TriangleMesh; +class TetraMesh; class LineSet : public Geometry3D { public: @@ -94,6 +95,10 @@ class LineSet : public Geometry3D { static std::shared_ptr CreateFromTriangleMesh( const TriangleMesh &mesh); + /// Factory function to create a LineSet from edges of a tetra mesh + /// \param mesh. + static std::shared_ptr CreateFromTetraMesh(const TetraMesh &mesh); + public: std::vector points_; std::vector lines_; diff --git a/src/Open3D/Geometry/LineSetFactory.cpp b/src/Open3D/Geometry/LineSetFactory.cpp index ff45bd92850..2e530501f5b 100644 --- a/src/Open3D/Geometry/LineSetFactory.cpp +++ b/src/Open3D/Geometry/LineSetFactory.cpp @@ -28,6 +28,7 @@ #include "Open3D/Geometry/LineSet.h" #include "Open3D/Geometry/PointCloud.h" +#include "Open3D/Geometry/TetraMesh.h" #include "Open3D/Geometry/TriangleMesh.h" namespace open3d { @@ -79,5 +80,31 @@ std::shared_ptr LineSet::CreateFromTriangleMesh( return line_set; } +std::shared_ptr LineSet::CreateFromTetraMesh(const TetraMesh &mesh) { + auto line_set = std::make_shared(); + line_set->points_ = mesh.vertices_; + + std::unordered_set> + inserted_edges; + auto InsertEdge = [&](int vidx0, int vidx1) { + Eigen::Vector2i edge(std::min(vidx0, vidx1), std::max(vidx0, vidx1)); + if (inserted_edges.count(edge) == 0) { + inserted_edges.insert(edge); + line_set->lines_.push_back(Eigen::Vector2i(vidx0, vidx1)); + } + }; + for (const auto &tetra : mesh.tetras_) { + InsertEdge(tetra(0), tetra(1)); + InsertEdge(tetra(1), tetra(2)); + InsertEdge(tetra(2), tetra(0)); + InsertEdge(tetra(3), tetra(0)); + InsertEdge(tetra(3), tetra(1)); + InsertEdge(tetra(3), tetra(2)); + } + + return line_set; +} + } // namespace geometry } // namespace open3d diff --git a/src/Open3D/Geometry/Qhull.cpp b/src/Open3D/Geometry/Qhull.cpp index 4187f8700c7..eab65d9e60f 100644 --- a/src/Open3D/Geometry/Qhull.cpp +++ b/src/Open3D/Geometry/Qhull.cpp @@ -25,7 +25,9 @@ // ---------------------------------------------------------------------------- #include "Open3D/Geometry/Qhull.h" +#include "Open3D/Geometry/TetraMesh.h" #include "Open3D/Geometry/TriangleMesh.h" +#include "Open3D/Utility/Console.h" #include "libqhullcpp/PointCoordinates.h" #include "libqhullcpp/Qhull.h" @@ -97,5 +99,83 @@ std::shared_ptr Qhull::ComputeConvexHull( return convex_hull; } +std::shared_ptr Qhull::ComputeDelaunayTetrahedralization( + const std::vector& points) { + typedef decltype(TetraMesh::tetras_)::value_type Vector4i; + auto delaunay_triangulation = std::make_shared(); + + if (points.size() < 4) { + utility::LogWarning( + "[ComputeDelaunayTriangulation3D] not enough points to create " + "a tetrahedral mesh.\n"); + return delaunay_triangulation; + } + + // qhull cannot deal with this case + if (points.size() == 4) { + delaunay_triangulation->vertices_ = points; + delaunay_triangulation->tetras_.push_back(Vector4i(0, 1, 2, 3)); + return delaunay_triangulation; + } + + std::vector qhull_points_data(points.size() * 3); + for (size_t pidx = 0; pidx < points.size(); ++pidx) { + const auto& pt = points[pidx]; + qhull_points_data[pidx * 3 + 0] = pt(0); + qhull_points_data[pidx * 3 + 1] = pt(1); + qhull_points_data[pidx * 3 + 2] = pt(2); + } + + orgQhull::PointCoordinates qhull_points(3, ""); + qhull_points.append(qhull_points_data); + + orgQhull::Qhull qhull; + qhull.runQhull(qhull_points.comment().c_str(), qhull_points.dimension(), + qhull_points.count(), qhull_points.coordinates(), + "d Qbb Qt"); + + orgQhull::QhullFacetList facets = qhull.facetList(); + delaunay_triangulation->tetras_.resize(facets.count()); + std::unordered_map vert_map; + std::unordered_set inserted_vertices; + int tidx = 0; + for (orgQhull::QhullFacetList::iterator it = facets.begin(); + it != facets.end(); ++it) { + if (!(*it).isGood()) continue; + + orgQhull::QhullFacet f = *it; + orgQhull::QhullVertexSet vSet = f.vertices(); + int tetra_subscript = 0; + for (orgQhull::QhullVertexSet::iterator vIt = vSet.begin(); + vIt != vSet.end(); ++vIt) { + orgQhull::QhullVertex v = *vIt; + orgQhull::QhullPoint p = v.point(); + + int vidx = p.id(); + delaunay_triangulation->tetras_[tidx](tetra_subscript) = vidx; + tetra_subscript++; + + if (inserted_vertices.count(vidx) == 0) { + inserted_vertices.insert(vidx); + vert_map[vidx] = int(delaunay_triangulation->vertices_.size()); + double* coords = p.coordinates(); + delaunay_triangulation->vertices_.push_back( + Eigen::Vector3d(coords[0], coords[1], coords[2])); + } + } + + tidx++; + } + + for (auto& tetra : delaunay_triangulation->tetras_) { + tetra(0) = vert_map[tetra(0)]; + tetra(1) = vert_map[tetra(1)]; + tetra(2) = vert_map[tetra(2)]; + tetra(3) = vert_map[tetra(3)]; + } + + return delaunay_triangulation; +} + } // namespace geometry } // namespace open3d diff --git a/src/Open3D/Geometry/Qhull.h b/src/Open3D/Geometry/Qhull.h index 5daa964cd9a..40d6ad8eff0 100644 --- a/src/Open3D/Geometry/Qhull.h +++ b/src/Open3D/Geometry/Qhull.h @@ -34,11 +34,15 @@ namespace open3d { namespace geometry { class TriangleMesh; +class TetraMesh; class Qhull { public: static std::shared_ptr ComputeConvexHull( const std::vector& points); + + static std::shared_ptr ComputeDelaunayTetrahedralization( + const std::vector& points); }; } // namespace geometry diff --git a/src/Open3D/Geometry/TetraMesh.cpp b/src/Open3D/Geometry/TetraMesh.cpp new file mode 100644 index 00000000000..d0acfda30c8 --- /dev/null +++ b/src/Open3D/Geometry/TetraMesh.cpp @@ -0,0 +1,433 @@ +// ---------------------------------------------------------------------------- +// - Open3D: www.open3d.org - +// ---------------------------------------------------------------------------- +// The MIT License (MIT) +// +// Copyright (c) 2019 www.open3d.org +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS +// IN THE SOFTWARE. +// ---------------------------------------------------------------------------- + +#include "Open3D/Geometry/TetraMesh.h" +#include "Open3D/Geometry/PointCloud.h" +#include "Open3D/Geometry/TriangleMesh.h" + +#include +#include +#include +#include + +#include "Open3D/Utility/Console.h" + +namespace open3d { +namespace geometry { + +TetraMesh &TetraMesh::Clear() { + vertices_.clear(); + tetras_.clear(); + return *this; +} + +bool TetraMesh::IsEmpty() const { return !HasVertices(); } + +Eigen::Vector3d TetraMesh::GetMinBound() const { + if (!HasVertices()) { + return Eigen::Vector3d(0.0, 0.0, 0.0); + } + return std::accumulate( + vertices_.begin(), vertices_.end(), vertices_[0], + [](const Eigen::Vector3d &a, const Eigen::Vector3d &b) { + return a.array().min(b.array()).matrix(); + }); +} + +Eigen::Vector3d TetraMesh::GetMaxBound() const { + if (!HasVertices()) { + return Eigen::Vector3d(0.0, 0.0, 0.0); + } + return std::accumulate( + vertices_.begin(), vertices_.end(), vertices_[0], + [](const Eigen::Vector3d &a, const Eigen::Vector3d &b) { + return a.array().max(b.array()).matrix(); + }); +} + +TetraMesh &TetraMesh::Transform(const Eigen::Matrix4d &transformation) { + for (auto &vertex : vertices_) { + Eigen::Vector4d new_point = + transformation * + Eigen::Vector4d(vertex(0), vertex(1), vertex(2), 1.0); + vertex = new_point.block<3, 1>(0, 0); + } + return *this; +} + +TetraMesh &TetraMesh::Translate(const Eigen::Vector3d &translation) { + for (auto &vertex : vertices_) { + vertex += translation; + } + return *this; +} + +TetraMesh &TetraMesh::Scale(const double scale, bool center) { + Eigen::Vector3d vertex_center(0, 0, 0); + if (center && !vertices_.empty()) { + vertex_center = std::accumulate(vertices_.begin(), vertices_.end(), + vertex_center); + vertex_center /= double(vertices_.size()); + } + for (auto &vertex : vertices_) { + vertex = (vertex - vertex_center) * scale + vertex_center; + } + return *this; +} + +TetraMesh &TetraMesh::Rotate(const Eigen::Vector3d &rotation, + bool center, + RotationType type) { + Eigen::Vector3d vertex_center(0, 0, 0); + if (center && !vertices_.empty()) { + vertex_center = std::accumulate(vertices_.begin(), vertices_.end(), + vertex_center); + vertex_center /= double(vertices_.size()); + } + const Eigen::Matrix3d R = GetRotationMatrix(rotation, type); + for (auto &vertex : vertices_) { + vertex = R * (vertex - vertex_center) + vertex_center; + } + return *this; +} + +TetraMesh &TetraMesh::operator+=(const TetraMesh &mesh) { + typedef decltype(tetras_)::value_type Vector4i; + if (mesh.IsEmpty()) return (*this); + size_t old_vert_num = vertices_.size(); + size_t add_vert_num = mesh.vertices_.size(); + size_t new_vert_num = old_vert_num + add_vert_num; + size_t old_tetra_num = tetras_.size(); + size_t add_tetra_num = mesh.tetras_.size(); + vertices_.resize(new_vert_num); + for (size_t i = 0; i < add_vert_num; i++) + vertices_[old_vert_num + i] = mesh.vertices_[i]; + + tetras_.resize(tetras_.size() + mesh.tetras_.size()); + Vector4i index_shift = Vector4i::Constant((int64_t)old_vert_num); + for (size_t i = 0; i < add_tetra_num; i++) { + tetras_[old_tetra_num + i] = mesh.tetras_[i] + index_shift; + } + return (*this); +} + +TetraMesh TetraMesh::operator+(const TetraMesh &mesh) const { + return (TetraMesh(*this) += mesh); +} + +TetraMesh &TetraMesh::RemoveDuplicatedVertices() { + typedef decltype(tetras_)::value_type::Scalar Index; + typedef std::tuple Coordinate3; + std::unordered_map> + point_to_old_index; + std::vector index_old_to_new(vertices_.size()); + size_t old_vertex_num = vertices_.size(); + size_t k = 0; // new index + for (size_t i = 0; i < old_vertex_num; i++) { // old index + Coordinate3 coord = std::make_tuple(vertices_[i](0), vertices_[i](1), + vertices_[i](2)); + if (point_to_old_index.find(coord) == point_to_old_index.end()) { + point_to_old_index[coord] = i; + vertices_[k] = vertices_[i]; + index_old_to_new[i] = (Index)k; + k++; + } else { + index_old_to_new[i] = index_old_to_new[point_to_old_index[coord]]; + } + } + vertices_.resize(k); + if (k < old_vertex_num) { + for (auto &tetra : tetras_) { + tetra(0) = index_old_to_new[tetra(0)]; + tetra(1) = index_old_to_new[tetra(1)]; + tetra(2) = index_old_to_new[tetra(2)]; + tetra(3) = index_old_to_new[tetra(3)]; + } + } + utility::LogDebug( + "[RemoveDuplicatedVertices] {:d} vertices have been removed.\n", + (int)(old_vertex_num - k)); + + return *this; +} + +TetraMesh &TetraMesh::RemoveDuplicatedTetras() { + typedef decltype(tetras_)::value_type::Scalar Index; + typedef std::tuple Index4; + std::unordered_map> + tetra_to_old_index; + size_t old_tetra_num = tetras_.size(); + size_t k = 0; + for (size_t i = 0; i < old_tetra_num; i++) { + Index4 index; + std::array t{tetras_[i](0), tetras_[i](1), tetras_[i](2), + tetras_[i](3)}; + + // We sort the indices to find duplicates, because tetra (0-1-2-3) + // and tetra (2-0-3-1) are the same. + std::sort(t.begin(), t.end()); + index = std::make_tuple(t[0], t[1], t[2], t[3]); + + if (tetra_to_old_index.find(index) == tetra_to_old_index.end()) { + tetra_to_old_index[index] = i; + tetras_[k] = tetras_[i]; + k++; + } + } + tetras_.resize(k); + utility::LogDebug( + "[RemoveDuplicatedTetras] {:d} tetras have been removed.\n", + (int)(old_tetra_num - k)); + + return *this; +} + +TetraMesh &TetraMesh::RemoveUnreferencedVertices() { + typedef decltype(tetras_)::value_type::Scalar Index; + std::vector vertex_has_reference(vertices_.size(), false); + for (const auto &tetra : tetras_) { + vertex_has_reference[tetra(0)] = true; + vertex_has_reference[tetra(1)] = true; + vertex_has_reference[tetra(2)] = true; + vertex_has_reference[tetra(3)] = true; + } + std::vector index_old_to_new(vertices_.size()); + size_t old_vertex_num = vertices_.size(); + size_t k = 0; // new index + for (size_t i = 0; i < old_vertex_num; i++) { // old index + if (vertex_has_reference[i]) { + vertices_[k] = vertices_[i]; + index_old_to_new[i] = (Index)k; + k++; + } else { + index_old_to_new[i] = -1; + } + } + vertices_.resize(k); + if (k < old_vertex_num) { + for (auto &tetra : tetras_) { + tetra(0) = index_old_to_new[tetra(0)]; + tetra(1) = index_old_to_new[tetra(1)]; + tetra(2) = index_old_to_new[tetra(2)]; + tetra(3) = index_old_to_new[tetra(3)]; + } + } + utility::LogDebug( + "[RemoveUnreferencedVertices] {:d} vertices have been removed.\n", + (int)(old_vertex_num - k)); + + return *this; +} + +TetraMesh &TetraMesh::RemoveDegenerateTetras() { + size_t old_tetra_num = tetras_.size(); + size_t k = 0; + for (size_t i = 0; i < old_tetra_num; i++) { + const auto &tetra = tetras_[i]; + if (tetra(0) != tetra(1) && tetra(0) != tetra(2) && + tetra(0) != tetra(3) && tetra(1) != tetra(2) && + tetra(1) != tetra(3) && tetra(2) != tetra(3)) { + tetras_[k] = tetras_[i]; + k++; + } + } + tetras_.resize(k); + utility::LogDebug( + "[RemoveDegenerateTetras] {:d} tetras have been " + "removed.\n", + (int)(old_tetra_num - k)); + return *this; +} + +std::shared_ptr TetraMesh::ExtractTriangleMesh( + const std::vector &values, double level) { + typedef decltype(tetras_)::value_type::Scalar Index; + static_assert(std::is_signed(), "Index type must be signed"); + typedef std::tuple Index2; + + auto triangle_mesh = std::make_shared(); + + if (values.size() != vertices_.size()) { + utility::LogWarning( + "[ExtractTriangleMesh] number of values does not match the " + "number of vertices.\n"); + return triangle_mesh; + } + + auto SurfaceIntersectionTest = [](double v0, double v1, double level) { + return (v0 < level && v1 >= level) || (v0 >= level && v1 < level); + }; + + auto ComputeEdgeVertex = [&values, level, this](Index idx1, Index idx2) { + double v1 = values[idx1]; + double v2 = values[idx2]; + + double t = (level - v2) / (v1 - v2); + if (std::isnan(t) || t < 0 || t > 1) { + t = 0.5; + } + Eigen::Vector3d intersection = + t * vertices_[idx1] + (1 - t) * vertices_[idx2]; + + return intersection; + }; + + auto ComputeTriangleNormal = [](const Eigen::Vector3d &a, + const Eigen::Vector3d &b, + const Eigen::Vector3d &c) { + Eigen::Vector3d ab = b - a; + Eigen::Vector3d ac = c - a; + return ab.cross(ac); + }; + + auto MakeSortedTuple2 = [](Index a, Index b) { + if (a < b) + return std::make_tuple(a, b); + else + return std::make_tuple(b, a); + }; + + auto HasCommonVertexIndex = [](Index2 a, Index2 b) { + return std::get<0>(b) == std::get<0>(a) || + std::get<1>(b) == std::get<0>(a) || + std::get<0>(b) == std::get<1>(a) || + std::get<1>(b) == std::get<1>(a); + }; + + std::unordered_map> + intersecting_edges; + + const int tetra_edges[][2] = {{0, 1}, {0, 2}, {0, 3}, + {1, 2}, {1, 3}, {2, 3}}; + + for (size_t tetra_i = 0; tetra_i < tetras_.size(); ++tetra_i) { + const auto &tetra = tetras_[tetra_i]; + + std::array verts; + std::array keys; + std::array verts_indices; + std::array edge_dirs; + int num_verts = 0; + + for (int tet_edge_i = 0; tet_edge_i < 6; ++tet_edge_i) { + Index edge_vert1 = tetra[tetra_edges[tet_edge_i][0]]; + Index edge_vert2 = tetra[tetra_edges[tet_edge_i][1]]; + double vert_value1 = values[edge_vert1]; + double vert_value2 = values[edge_vert2]; + if (SurfaceIntersectionTest(vert_value1, vert_value2, level)) { + Index2 index = MakeSortedTuple2(edge_vert1, edge_vert2); + verts[num_verts] = ComputeEdgeVertex(edge_vert1, edge_vert2); + keys[num_verts] = index; + + // make edge_vert1 be the vertex that is smaller than level + // (inside) + if (values[edge_vert1] > values[edge_vert2]) + std::swap(edge_vert1, edge_vert2); + edge_dirs[num_verts] = + vertices_[edge_vert2] - vertices_[edge_vert1]; + ++num_verts; + } + } + + // add vertices and get the vertex indices + for (int i = 0; i < num_verts; ++i) { + if (intersecting_edges.count(keys[i]) == 0) { + Index idx = intersecting_edges.size(); + verts_indices[i] = idx; + intersecting_edges[keys[i]] = idx; + triangle_mesh->vertices_.push_back(verts[i]); + } else { + verts_indices[i] = intersecting_edges[keys[i]]; + } + } + + // create triangles for this tetrahedron + if (3 == num_verts) { + Eigen::Vector3i tri(verts_indices[0], verts_indices[1], + verts_indices[2]); + + Eigen::Vector3d tri_normal = + ComputeTriangleNormal(verts[0], verts[1], verts[2]); + + // accumulate to improve robustness of the triangle orientation test + double dot = 0; + for (int i = 0; i < 3; ++i) dot += tri_normal.dot(edge_dirs[i]); + if (dot < 0) std::swap(tri.x(), tri.y()); + + triangle_mesh->triangles_.push_back(tri); + } else if (4 == num_verts) { + std::array order; + if (HasCommonVertexIndex(keys[0], keys[1]) && + HasCommonVertexIndex(keys[0], keys[2])) { + order = {1, 0, 2, 3}; + } else if (HasCommonVertexIndex(keys[0], keys[1]) && + HasCommonVertexIndex(keys[0], keys[3])) { + order = {1, 0, 3, 2}; + } else if (HasCommonVertexIndex(keys[0], keys[2]) && + HasCommonVertexIndex(keys[0], keys[3])) { + order = {2, 0, 3, 1}; + } + + // accumulate to improve robustness of the triangle orientation test + double dot = 0; + for (int i = 0; i < 4; ++i) { + Eigen::Vector3d tri_normal = ComputeTriangleNormal( + verts[order[(4 + i - 1) % 4]], verts[order[i]], + verts[order[(i + 1) % 4]]); + dot += tri_normal.dot(edge_dirs[order[i]]); + } + if (dot < 0) std::reverse(order.begin(), order.end()); + + std::array tris; + if ((verts[order[0]] - verts[order[2]]).squaredNorm() < + (verts[order[1]] - verts[order[3]]).squaredNorm()) { + tris[0] << verts_indices[order[0]], verts_indices[order[1]], + verts_indices[order[2]]; + tris[1] << verts_indices[order[2]], verts_indices[order[3]], + verts_indices[order[0]]; + } else { + tris[0] << verts_indices[order[0]], verts_indices[order[1]], + verts_indices[order[3]]; + tris[1] << verts_indices[order[1]], verts_indices[order[2]], + verts_indices[order[3]]; + } + + triangle_mesh->triangles_.insert(triangle_mesh->triangles_.end(), + {tris[0], tris[1]}); + } else if (0 != num_verts) { + utility::LogWarning( + "[ExtractTriangleMesh] failed to create triangles for " + "tetrahedron {:d}\n", + int(tetra_i)); + } + } + + return triangle_mesh; +} + +} // namespace geometry +} // namespace open3d diff --git a/src/Open3D/Geometry/TetraMesh.h b/src/Open3D/Geometry/TetraMesh.h new file mode 100644 index 00000000000..aa1f917b1af --- /dev/null +++ b/src/Open3D/Geometry/TetraMesh.h @@ -0,0 +1,111 @@ +// ---------------------------------------------------------------------------- +// - Open3D: www.open3d.org - +// ---------------------------------------------------------------------------- +// The MIT License (MIT) +// +// Copyright (c) 2019 www.open3d.org +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS +// IN THE SOFTWARE. +// ---------------------------------------------------------------------------- + +#pragma once + +#include +#include +#include +#include + +#include "Open3D/Geometry/Geometry3D.h" +#include "Open3D/Utility/Eigen.h" +#include "Open3D/Utility/Helper.h" + +namespace open3d { +namespace geometry { + +class PointCloud; +class TriangleMesh; + +class TetraMesh : public Geometry3D { +public: + TetraMesh() : Geometry3D(Geometry::GeometryType::TetraMesh) {} + ~TetraMesh() override {} + +public: + TetraMesh &Clear() override; + bool IsEmpty() const override; + Eigen::Vector3d GetMinBound() const override; + Eigen::Vector3d GetMaxBound() const override; + TetraMesh &Transform(const Eigen::Matrix4d &transformation) override; + TetraMesh &Translate(const Eigen::Vector3d &translation) override; + TetraMesh &Scale(const double scale, bool center = true) override; + TetraMesh &Rotate(const Eigen::Vector3d &rotation, + bool center = true, + RotationType type = RotationType::XYZ) override; + +public: + TetraMesh &operator+=(const TetraMesh &mesh); + TetraMesh operator+(const TetraMesh &mesh) const; + + /// Function that removes duplicated verties, i.e., vertices that have + /// identical coordinates. + TetraMesh &RemoveDuplicatedVertices(); + + /// Function that removes duplicated tetrahedra, i.e., removes tetrahedra + /// that reference the same four vertices, independent of their order. + TetraMesh &RemoveDuplicatedTetras(); + + /// This function removes vertices from the tetra mesh that are not + /// referenced in any tetrahedron of the mesh. + TetraMesh &RemoveUnreferencedVertices(); + + /// Function that removes degenerate tetrahedra, i.e., tetrahedra that + /// reference a single vertex multiple times in a single tetrahedron. + /// They are usually the product of removing duplicated vertices. + TetraMesh &RemoveDegenerateTetras(); + + bool HasVertices() const { return vertices_.size() > 0; } + + bool HasTetras() const { + return vertices_.size() > 0 && tetras_.size() > 0; + } + + /// Function to extract a triangle mesh of the specified iso-surface at + /// \param level. \param values are values per-vertex. + /// This method applies primal contouring and generates triangles for each + /// tetrahedron. + std::shared_ptr ExtractTriangleMesh( + const std::vector &values, double level); + + /// Function that creates a tetrahedral mesh (TetraMeshFactory.cpp). + /// from a point cloud. The method creates the Delaunay triangulation + /// using the implementation from Qhull. + static std::shared_ptr CreateFromPointCloud( + const PointCloud &point_cloud); + +protected: + // Forward child class type to avoid indirect nonvirtual base + TetraMesh(Geometry::GeometryType type) : Geometry3D(type) {} + +public: + std::vector vertices_; + std::vector tetras_; +}; + +} // namespace geometry +} // namespace open3d diff --git a/src/Open3D/Geometry/TetraMeshFactory.cpp b/src/Open3D/Geometry/TetraMeshFactory.cpp new file mode 100644 index 00000000000..6fbb106763b --- /dev/null +++ b/src/Open3D/Geometry/TetraMeshFactory.cpp @@ -0,0 +1,47 @@ +// ---------------------------------------------------------------------------- +// - Open3D: www.open3d.org - +// ---------------------------------------------------------------------------- +// The MIT License (MIT) +// +// Copyright (c) 2018 www.open3d.org +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS +// IN THE SOFTWARE. +// ---------------------------------------------------------------------------- + +#include "Open3D/Geometry/PointCloud.h" +#include "Open3D/Geometry/Qhull.h" +#include "Open3D/Geometry/TetraMesh.h" +#include "Open3D/Utility/Console.h" + +namespace open3d { +namespace geometry { + +std::shared_ptr TetraMesh::CreateFromPointCloud( + const PointCloud& point_cloud) { + if (point_cloud.points_.size() < 4) { + utility::LogWarning( + "[CreateFromPointCloud] not enough points to create a " + "tetrahedral mesh.\n"); + return std::make_shared(); + } + return Qhull::ComputeDelaunayTetrahedralization(point_cloud.points_); +} + +} // namespace geometry +} // namespace open3d diff --git a/src/Open3D/Visualization/Shader/GeometryRenderer.cpp b/src/Open3D/Visualization/Shader/GeometryRenderer.cpp index c981c529969..2b963304dd4 100644 --- a/src/Open3D/Visualization/Shader/GeometryRenderer.cpp +++ b/src/Open3D/Visualization/Shader/GeometryRenderer.cpp @@ -181,6 +181,27 @@ bool LineSetRenderer::UpdateGeometry() { return true; } +bool TetraMeshRenderer::Render(const RenderOption &option, + const ViewControl &view) { + if (is_visible_ == false || geometry_ptr_->IsEmpty()) return true; + return simple_tetramesh_shader_.Render(*geometry_ptr_, option, view); +} + +bool TetraMeshRenderer::AddGeometry( + std::shared_ptr geometry_ptr) { + if (geometry_ptr->GetGeometryType() != + geometry::Geometry::GeometryType::TetraMesh) { + return false; + } + geometry_ptr_ = geometry_ptr; + return UpdateGeometry(); +} + +bool TetraMeshRenderer::UpdateGeometry() { + simple_tetramesh_shader_.InvalidateGeometry(); + return true; +} + bool TriangleMeshRenderer::Render(const RenderOption &option, const ViewControl &view) { if (is_visible_ == false || geometry_ptr_->IsEmpty()) return true; diff --git a/src/Open3D/Visualization/Shader/GeometryRenderer.h b/src/Open3D/Visualization/Shader/GeometryRenderer.h index b4d49ecea71..3a437be0106 100644 --- a/src/Open3D/Visualization/Shader/GeometryRenderer.h +++ b/src/Open3D/Visualization/Shader/GeometryRenderer.h @@ -121,6 +121,20 @@ class LineSetRenderer : public GeometryRenderer { SimpleShaderForLineSet simple_lineset_shader_; }; +class TetraMeshRenderer : public GeometryRenderer { +public: + ~TetraMeshRenderer() override {} + +public: + bool Render(const RenderOption &option, const ViewControl &view) override; + bool AddGeometry( + std::shared_ptr geometry_ptr) override; + bool UpdateGeometry() override; + +protected: + SimpleShaderForTetraMesh simple_tetramesh_shader_; +}; + class TriangleMeshRenderer : public GeometryRenderer { public: ~TriangleMeshRenderer() override {} diff --git a/src/Open3D/Visualization/Shader/SimpleShader.cpp b/src/Open3D/Visualization/Shader/SimpleShader.cpp index 44836a7164c..b0ef9d96959 100644 --- a/src/Open3D/Visualization/Shader/SimpleShader.cpp +++ b/src/Open3D/Visualization/Shader/SimpleShader.cpp @@ -29,6 +29,7 @@ #include "Open3D/Geometry/LineSet.h" #include "Open3D/Geometry/Octree.h" #include "Open3D/Geometry/PointCloud.h" +#include "Open3D/Geometry/TetraMesh.h" #include "Open3D/Geometry/TriangleMesh.h" #include "Open3D/Geometry/VoxelGrid.h" #include "Open3D/Visualization/Shader/Shader.h" @@ -262,6 +263,71 @@ bool SimpleShaderForLineSet::PrepareBinding( return true; } +bool SimpleShaderForTetraMesh::PrepareRendering( + const geometry::Geometry &geometry, + const RenderOption &option, + const ViewControl &view) { + if (geometry.GetGeometryType() != + geometry::Geometry::GeometryType::TetraMesh) { + PrintShaderWarning("Rendering type is not geometry::TetraMesh."); + return false; + } + glLineWidth(GLfloat(option.line_width_)); + glEnable(GL_DEPTH_TEST); + glDepthFunc(GL_LESS); + return true; +} + +bool SimpleShaderForTetraMesh::PrepareBinding( + const geometry::Geometry &geometry, + const RenderOption &option, + const ViewControl &view, + std::vector &points, + std::vector &colors) { + typedef decltype(geometry::TetraMesh::tetras_)::value_type TetraIndices; + typedef decltype(geometry::TetraMesh::tetras_)::value_type::Scalar Index; + typedef std::tuple Index2; + + if (geometry.GetGeometryType() != + geometry::Geometry::GeometryType::TetraMesh) { + PrintShaderWarning("Rendering type is not geometry::TetraMesh."); + return false; + } + const geometry::TetraMesh &tetramesh = + (const geometry::TetraMesh &)geometry; + if (tetramesh.HasTetras() == false) { + PrintShaderWarning("Binding failed with empty geometry::TetraMesh."); + return false; + } + + std::unordered_set> + inserted_edges; + auto InsertEdge = [&](Index vidx0, Index vidx1) { + Index2 edge(std::min(vidx0, vidx1), std::max(vidx0, vidx1)); + if (inserted_edges.count(edge) == 0) { + inserted_edges.insert(edge); + Eigen::Vector3f p0 = tetramesh.vertices_[vidx0].cast(); + Eigen::Vector3f p1 = tetramesh.vertices_[vidx1].cast(); + points.insert(points.end(), {p0, p1}); + Eigen::Vector3f color(0, 0, 0); + colors.insert(colors.end(), {color, color}); + } + }; + + for (size_t i = 0; i < tetramesh.tetras_.size(); i++) { + const TetraIndices tetra = tetramesh.tetras_[i]; + InsertEdge(tetra(0), tetra(1)); + InsertEdge(tetra(1), tetra(2)); + InsertEdge(tetra(2), tetra(0)); + InsertEdge(tetra(3), tetra(0)); + InsertEdge(tetra(3), tetra(1)); + InsertEdge(tetra(3), tetra(2)); + } + draw_arrays_mode_ = GL_LINES; + draw_arrays_size_ = GLsizei(points.size()); + return true; +} + bool SimpleShaderForTriangleMesh::PrepareRendering( const geometry::Geometry &geometry, const RenderOption &option, diff --git a/src/Open3D/Visualization/Shader/SimpleShader.h b/src/Open3D/Visualization/Shader/SimpleShader.h index bec382a378f..b4cb5b21ffa 100644 --- a/src/Open3D/Visualization/Shader/SimpleShader.h +++ b/src/Open3D/Visualization/Shader/SimpleShader.h @@ -102,6 +102,21 @@ class SimpleShaderForLineSet : public SimpleShader { std::vector &colors) final; }; +class SimpleShaderForTetraMesh : public SimpleShader { +public: + SimpleShaderForTetraMesh() : SimpleShader("SimpleShaderForTetraMesh") {} + +protected: + bool PrepareRendering(const geometry::Geometry &geometry, + const RenderOption &option, + const ViewControl &view) final; + bool PrepareBinding(const geometry::Geometry &geometry, + const RenderOption &option, + const ViewControl &view, + std::vector &points, + std::vector &colors) final; +}; + class SimpleShaderForTriangleMesh : public SimpleShader { public: SimpleShaderForTriangleMesh() diff --git a/src/Open3D/Visualization/Visualizer/Visualizer.cpp b/src/Open3D/Visualization/Visualizer/Visualizer.cpp index b3e22cda37f..7ad415ac3ff 100644 --- a/src/Open3D/Visualization/Visualizer/Visualizer.cpp +++ b/src/Open3D/Visualization/Visualizer/Visualizer.cpp @@ -347,6 +347,12 @@ bool Visualizer::AddGeometry( if (renderer_ptr->AddGeometry(geometry_ptr) == false) { return false; } + } else if (geometry_ptr->GetGeometryType() == + geometry::Geometry::GeometryType::TetraMesh) { + renderer_ptr = std::make_shared(); + if (renderer_ptr->AddGeometry(geometry_ptr) == false) { + return false; + } } else { return false; } diff --git a/src/Python/geometry/geometry.cpp b/src/Python/geometry/geometry.cpp index a7f1e86d6c1..0ff6e2a6471 100644 --- a/src/Python/geometry/geometry.cpp +++ b/src/Python/geometry/geometry.cpp @@ -69,6 +69,7 @@ void pybind_geometry_classes(py::module &m) { .value("HalfEdgeTriangleMesh", geometry::Geometry::GeometryType::HalfEdgeTriangleMesh) .value("Image", geometry::Geometry::GeometryType::Image) + .value("TetraMesh", geometry::Geometry::GeometryType::TetraMesh) .export_values(); // open3d.geometry.Geometry3D @@ -151,6 +152,7 @@ void pybind_geometry(py::module &m) { pybind_trianglemesh(m_submodule); pybind_halfedgetrianglemesh(m_submodule); pybind_image(m_submodule); + pybind_tetramesh(m_submodule); pybind_pointcloud_methods(m_submodule); pybind_voxelgrid_methods(m_submodule); pybind_trianglemesh_methods(m_submodule); diff --git a/src/Python/geometry/geometry.h b/src/Python/geometry/geometry.h index 40ca02e2848..916a787bca7 100644 --- a/src/Python/geometry/geometry.h +++ b/src/Python/geometry/geometry.h @@ -36,6 +36,7 @@ void pybind_lineset(py::module &m); void pybind_trianglemesh(py::module &m); void pybind_halfedgetrianglemesh(py::module &m); void pybind_image(py::module &m); +void pybind_tetramesh(py::module &m); void pybind_kdtreeflann(py::module &m); void pybind_pointcloud_methods(py::module &m); void pybind_voxelgrid_methods(py::module &m); diff --git a/src/Python/geometry/geometry_trampoline.h b/src/Python/geometry/geometry_trampoline.h index 1aa74f0e554..227ffa9c687 100644 --- a/src/Python/geometry/geometry_trampoline.h +++ b/src/Python/geometry/geometry_trampoline.h @@ -31,6 +31,7 @@ #include "Open3D/Geometry/Geometry.h" #include "Open3D/Geometry/Geometry2D.h" #include "Open3D/Geometry/Geometry3D.h" +#include "Open3D/Geometry/TetraMesh.h" #include "Open3D/Geometry/TriangleMesh.h" #include "Python/geometry/geometry.h" diff --git a/src/Python/geometry/lineset.cpp b/src/Python/geometry/lineset.cpp index 7cea7ed5381..60240456294 100644 --- a/src/Python/geometry/lineset.cpp +++ b/src/Python/geometry/lineset.cpp @@ -69,6 +69,11 @@ void pybind_lineset(py::module &m) { "Factory function to create a LineSet from edges of a " "triangle mesh.", "mesh"_a) + .def_static("create_from_tetra_mesh", + &geometry::LineSet::CreateFromTetraMesh, + "Factory function to create a LineSet from edges of a " + "tetra mesh.", + "mesh"_a) .def_readwrite("points", &geometry::LineSet::points_, "``float64`` array of shape ``(num_points, 3)``, " "use ``numpy.asarray()`` to access data: Points " @@ -96,6 +101,8 @@ void pybind_lineset(py::module &m) { {"correspondences", "Set of correspondences."}}); docstring::ClassMethodDocInject(m, "LineSet", "create_from_triangle_mesh", {{"mesh", "The input triangle mesh."}}); + docstring::ClassMethodDocInject(m, "LineSet", "create_from_tetra_mesh", + {{"mesh", "The input tetra mesh."}}); } void pybind_lineset_methods(py::module &m) {} diff --git a/src/Python/geometry/tetramesh.cpp b/src/Python/geometry/tetramesh.cpp new file mode 100644 index 00000000000..44fdba8acae --- /dev/null +++ b/src/Python/geometry/tetramesh.cpp @@ -0,0 +1,114 @@ +// ---------------------------------------------------------------------------- +// - Open3D: www.open3d.org - +// ---------------------------------------------------------------------------- +// The MIT License (MIT) +// +// Copyright (c) 2018 www.open3d.org +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS +// IN THE SOFTWARE. +// ---------------------------------------------------------------------------- + +#include "Open3D/Geometry/TetraMesh.h" +#include "Open3D/Geometry/PointCloud.h" +#include "Python/docstring.h" +#include "Python/geometry/geometry.h" +#include "Python/geometry/geometry_trampoline.h" + +using namespace open3d; + +void pybind_tetramesh(py::module &m) { + py::class_, + std::shared_ptr, geometry::Geometry3D> + trianglemesh(m, "TetraMesh", + "TetraMesh class. Tetra mesh contains vertices " + "and tetrahedra represented by the indices to the " + "vertices."); + py::detail::bind_default_constructor(trianglemesh); + py::detail::bind_copy_functions(trianglemesh); + trianglemesh + .def("__repr__", + [](const geometry::TetraMesh &mesh) { + return std::string("geometry::TetraMesh with ") + + std::to_string(mesh.vertices_.size()) + + " points and " + + std::to_string(mesh.tetras_.size()) + + " tetrahedra."; + }) + .def(py::self + py::self) + .def(py::self += py::self) + .def("remove_duplicated_vertices", + &geometry::TetraMesh::RemoveDuplicatedVertices, + "Function that removes duplicated vertices, i.e., vertices " + "that have identical coordinates.") + .def("remove_duplicated_tetras", + &geometry::TetraMesh::RemoveDuplicatedTetras, + "Function that removes duplicated tetras, i.e., removes " + "tetras that reference the same four vertices, " + "independent of their order.") + .def("remove_unreferenced_vertices", + &geometry::TetraMesh::RemoveUnreferencedVertices, + "This function removes vertices from the tetra mesh that " + "are not referenced in any tetra of the mesh.") + .def("remove_degenerate_tetras", + &geometry::TetraMesh::RemoveDegenerateTetras, + "Function that removes degenerate tetras, i.e., tetras " + "that references a single vertex multiple times in a single " + "tetra. They are usually the product of removing " + "duplicated vertices.") + .def("has_vertices", &geometry::TetraMesh::HasVertices, + "Returns ``True`` if the mesh contains vertices.") + .def("has_tetras", &geometry::TetraMesh::HasTetras, + "Returns ``True`` if the mesh contains tetras.") + .def("extract_triangle_mesh", + &geometry::TetraMesh::ExtractTriangleMesh, + "Function that generates a triangle mesh of the specified " + "iso-surface.", + "values"_a, "level"_a) + .def_static( + "create_from_point_cloud", + &geometry::TetraMesh::CreateFromPointCloud, + "Function to create a tetrahedral mesh from a point cloud.", + "point_cloud"_a) + .def_readwrite("vertices", &geometry::TetraMesh::vertices_, + "``float64`` array of shape ``(num_vertices, 3)``, " + "use ``numpy.asarray()`` to access data: Vertex " + "coordinates.") + .def_readwrite("tetras", &geometry::TetraMesh::tetras_, + "``int64`` array of shape ``(num_tetras, 4)``, use " + "``numpy.asarray()`` to access data: List of " + "tetras denoted by the index of points forming " + "the tetra."); + docstring::ClassMethodDocInject(m, "TetraMesh", "has_tetras"); + docstring::ClassMethodDocInject(m, "TetraMesh", "has_vertices"); + docstring::ClassMethodDocInject(m, "TetraMesh", + "remove_duplicated_vertices"); + docstring::ClassMethodDocInject(m, "TetraMesh", "remove_duplicated_tetras"); + docstring::ClassMethodDocInject(m, "TetraMesh", + "remove_unreferenced_vertices"); + docstring::ClassMethodDocInject(m, "TetraMesh", "remove_degenerate_tetras"); + docstring::ClassMethodDocInject( + m, "TetraMesh", "extract_triangle_mesh", + {{"values", + "Vector with a scalar value for each vertex in the tetra mesh"}, + {"level", "A scalar which defines the level-set to extract"}}); + docstring::ClassMethodDocInject(m, "TetraMesh", "create_from_point_cloud", + {{"point_cloud", "A PointCloud."}}); +} + +void pybind_tetramesh_methods(py::module &m) {} diff --git a/src/Python/open3d_pybind.h b/src/Python/open3d_pybind.h index a2f81878981..2b34dfdaf97 100644 --- a/src/Python/open3d_pybind.h +++ b/src/Python/open3d_pybind.h @@ -43,6 +43,8 @@ using namespace py::literals; typedef std::vector temp_eigen_matrix4d; +typedef std::vector + temp_eigen_vector4i; PYBIND11_MAKE_OPAQUE(std::vector); PYBIND11_MAKE_OPAQUE(std::vector); @@ -50,6 +52,7 @@ PYBIND11_MAKE_OPAQUE(std::vector); PYBIND11_MAKE_OPAQUE(std::vector); PYBIND11_MAKE_OPAQUE(std::vector); PYBIND11_MAKE_OPAQUE(temp_eigen_matrix4d); +PYBIND11_MAKE_OPAQUE(temp_eigen_vector4i); PYBIND11_MAKE_OPAQUE(std::vector); PYBIND11_MAKE_OPAQUE(std::vector); diff --git a/src/Python/utility/eigen.cpp b/src/Python/utility/eigen.cpp index dab64ad79fd..f5c7a7d3e3b 100644 --- a/src/Python/utility/eigen.cpp +++ b/src/Python/utility/eigen.cpp @@ -87,6 +87,40 @@ std::vector py_array_to_vectors_int( return eigen_vectors; } +template > +std::vector +py_array_to_vectors_int_eigen_allocator( + py::array_t array) { + size_t eigen_vector_size = EigenVector::SizeAtCompileTime; + if (array.ndim() != 2 || array.shape(1) != eigen_vector_size) { + throw py::cast_error(); + } + std::vector eigen_vectors(array.shape(0)); + auto array_unchecked = array.mutable_unchecked<2>(); + for (auto i = 0; i < array_unchecked.shape(0); ++i) { + eigen_vectors[i] = Eigen::Map(&array_unchecked(i, 0)); + } + return eigen_vectors; +} + +template > +std::vector +py_array_to_vectors_int64_eigen_allocator( + py::array_t array) { + size_t eigen_vector_size = EigenVector::SizeAtCompileTime; + if (array.ndim() != 2 || array.shape(1) != eigen_vector_size) { + throw py::cast_error(); + } + std::vector eigen_vectors(array.shape(0)); + auto array_unchecked = array.mutable_unchecked<2>(); + for (auto i = 0; i < array_unchecked.shape(0); ++i) { + eigen_vectors[i] = Eigen::Map(&array_unchecked(i, 0)); + } + return eigen_vectors; +} + } // namespace pybind11 namespace { @@ -192,6 +226,53 @@ py::class_ pybind_eigen_vector_of_vector( //}); } +template , + typename Vector = std::vector, + typename holder_type = std::unique_ptr, + typename InitFunc> +py::class_ pybind_eigen_vector_of_vector_eigen_allocator( + py::module &m, + const std::string &bind_name, + const std::string &repr_name, + InitFunc init_func) { + typedef typename EigenVector::Scalar Scalar; + auto vec = py::bind_vector_without_repr< + std::vector>(m, bind_name, + py::buffer_protocol()); + vec.def(py::init(init_func)); + vec.def_buffer( + [](std::vector &v) -> py::buffer_info { + size_t rows = EigenVector::RowsAtCompileTime; + return py::buffer_info(v.data(), sizeof(Scalar), + py::format_descriptor::format(), + 2, {v.size(), rows}, + {sizeof(EigenVector), sizeof(Scalar)}); + }); + vec.def("__repr__", + [repr_name](const std::vector &v) { + return repr_name + std::string(" with ") + + std::to_string(v.size()) + std::string(" elements.\n") + + std::string("Use numpy.asarray() to access data."); + }); + vec.def("__copy__", [](std::vector &v) { + return std::vector(v); + }); + vec.def("__deepcopy__", + [](std::vector &v, py::dict &memo) { + return std::vector(v); + }); + + // py::detail must be after custom constructor + using Class_ = py::class_>; + py::detail::vector_if_copy_constructible(vec); + py::detail::vector_if_equal_operator(vec); + py::detail::vector_modifiers(vec); + py::detail::vector_accessor(vec); + + return vec; +} + template , typename Vector = std::vector, @@ -345,4 +426,15 @@ Example usage "Open3D format."; }), py::none(), py::none(), ""); + + auto vector4ivector = pybind_eigen_vector_of_vector_eigen_allocator< + Eigen::Vector4i>( + m, "Vector4iVector", "std::vector", + py::py_array_to_vectors_int_eigen_allocator); + vector4ivector.attr("__doc__") = docstring::static_property( + py::cpp_function([](py::handle arg) -> std::string { + return "Convert int numpy array of shape ``(n, 4)`` to " + "Open3D format."; + }), + py::none(), py::none(), ""); } diff --git a/src/UnitTest/Geometry/TetraMesh.cpp b/src/UnitTest/Geometry/TetraMesh.cpp new file mode 100644 index 00000000000..600b48a9bf1 --- /dev/null +++ b/src/UnitTest/Geometry/TetraMesh.cpp @@ -0,0 +1,524 @@ +// ---------------------------------------------------------------------------- +// - Open3D: www.open3d.org - +// ---------------------------------------------------------------------------- +// The MIT License (MIT) +// +// Copyright (c) 2018 www.open3d.org +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS +// IN THE SOFTWARE. +// ---------------------------------------------------------------------------- + +#include "Open3D/Geometry/TetraMesh.h" +#include +#include "Open3D/Geometry/PointCloud.h" +#include "Open3D/Geometry/TriangleMesh.h" +#include "TestUtility/UnitTest.h" + +using namespace Eigen; +using namespace open3d; +using namespace std; +using namespace unit_test; + +// ---------------------------------------------------------------------------- +// +// ---------------------------------------------------------------------------- +TEST(TetraMesh, Constructor) { + geometry::TetraMesh tm; + + // inherited from Geometry2D + EXPECT_EQ(geometry::Geometry::GeometryType::TetraMesh, + tm.GetGeometryType()); + EXPECT_EQ(3, tm.Dimension()); + + // public member variables + EXPECT_EQ(0u, tm.vertices_.size()); + EXPECT_EQ(0u, tm.tetras_.size()); + + // public members + EXPECT_TRUE(tm.IsEmpty()); + + ExpectEQ(Zero3d, tm.GetMinBound()); + ExpectEQ(Zero3d, tm.GetMaxBound()); + + EXPECT_FALSE(tm.HasVertices()); + EXPECT_FALSE(tm.HasTetras()); +} + +// ---------------------------------------------------------------------------- +// +// ---------------------------------------------------------------------------- +TEST(TetraMesh, DISABLED_MemberData) { unit_test::NotImplemented(); } + +// ---------------------------------------------------------------------------- +// +// ---------------------------------------------------------------------------- +TEST(TetraMesh, Clear) { + int size = 100; + + Vector3d dmin(0.0, 0.0, 0.0); + Vector3d dmax(1000.0, 1000.0, 1000.0); + + Vector4i imin(0, 0, 0, 0); + Vector4i imax(size - 1, size - 1, size - 1, size - 1); + + geometry::TetraMesh tm; + + tm.vertices_.resize(size); + tm.tetras_.resize(size); + + Rand(tm.vertices_, dmin, dmax, 0); + Rand(tm.tetras_, imin, imax, 0); + + EXPECT_FALSE(tm.IsEmpty()); + + ExpectEQ(Vector3d(19.607843, 0.0, 0.0), tm.GetMinBound()); + ExpectEQ(Vector3d(996.078431, 996.078431, 996.078431), tm.GetMaxBound()); + + EXPECT_TRUE(tm.HasVertices()); + EXPECT_TRUE(tm.HasTetras()); + + tm.Clear(); + + // public members + EXPECT_TRUE(tm.IsEmpty()); + + ExpectEQ(Zero3d, tm.GetMinBound()); + ExpectEQ(Zero3d, tm.GetMaxBound()); + + EXPECT_FALSE(tm.HasVertices()); + EXPECT_FALSE(tm.HasTetras()); +} + +// ---------------------------------------------------------------------------- +// +// ---------------------------------------------------------------------------- +TEST(TetraMesh, IsEmpty) { + int size = 100; + + geometry::TetraMesh tm; + + EXPECT_TRUE(tm.IsEmpty()); + + tm.vertices_.resize(size); + + EXPECT_FALSE(tm.IsEmpty()); +} + +// ---------------------------------------------------------------------------- +// +// ---------------------------------------------------------------------------- +TEST(TetraMesh, GetMinBound) { + int size = 100; + + Vector3d dmin(0.0, 0.0, 0.0); + Vector3d dmax(1000.0, 1000.0, 1000.0); + + geometry::TetraMesh tm; + + tm.vertices_.resize(size); + Rand(tm.vertices_, dmin, dmax, 0); + + ExpectEQ(Vector3d(19.607843, 0.0, 0.0), tm.GetMinBound()); +} + +// ---------------------------------------------------------------------------- +// +// ---------------------------------------------------------------------------- +TEST(TetraMesh, GetMaxBound) { + int size = 100; + + Vector3d dmin(0.0, 0.0, 0.0); + Vector3d dmax(1000.0, 1000.0, 1000.0); + + geometry::TetraMesh tm; + + tm.vertices_.resize(size); + Rand(tm.vertices_, dmin, dmax, 0); + + ExpectEQ(Vector3d(996.078431, 996.078431, 996.078431), tm.GetMaxBound()); +} + +// ---------------------------------------------------------------------------- +// +// ---------------------------------------------------------------------------- +TEST(TetraMesh, Transform) { + vector ref_vertices = {{396.870588, 1201.976471, 880.472941}, + {320.792157, 1081.976471, 829.139608}, + {269.027451, 818.447059, 406.786667}, + {338.831373, 1001.192157, 614.237647}, + {423.537255, 1153.349020, 483.727843}, + {432.949020, 1338.447059, 964.512157}, + {140.007843, 444.721569, 189.296471}, + {292.164706, 763.152941, 317.178824}, + {134.517647, 407.858824, 192.002353}, + {274.909804, 802.368627, 218.747451}}; + + int size = 10; + + Vector3d dmin(0.0, 0.0, 0.0); + Vector3d dmax(1000.0, 1000.0, 1000.0); + + geometry::TetraMesh tm; + + tm.vertices_.resize(size); + + Rand(tm.vertices_, dmin, dmax, 0); + + Matrix4d transformation; + transformation << 0.10, 0.20, 0.30, 0.40, 0.50, 0.60, 0.70, 0.80, 0.90, + 0.10, 0.11, 0.12, 0.13, 0.14, 0.15, 0.16; + + tm.Transform(transformation); + + ExpectEQ(ref_vertices, tm.vertices_); +} + +// ---------------------------------------------------------------------------- +// +// ---------------------------------------------------------------------------- +TEST(TetraMesh, OperatorAppend) { + size_t size = 100; + + Vector3d dmin(0.0, 0.0, 0.0); + Vector3d dmax(1000.0, 1000.0, 1000.0); + + Vector4i imin(0, 0, 0, 0); + Vector4i imax(size - 1, size - 1, size - 1, size - 1); + + geometry::TetraMesh tm0; + geometry::TetraMesh tm1; + + tm0.vertices_.resize(size); + tm0.tetras_.resize(size); + + tm1.vertices_.resize(size); + tm1.tetras_.resize(size); + + Rand(tm0.vertices_, dmin, dmax, 0); + Rand(tm0.tetras_, imin, imax, 0); + + Rand(tm1.vertices_, dmin, dmax, 0); + Rand(tm1.tetras_, imin, imax, 0); + + geometry::TetraMesh tm(tm0); + tm += tm1; + + EXPECT_EQ(2 * size, tm.vertices_.size()); + for (size_t i = 0; i < size; i++) { + ExpectEQ(tm0.vertices_[i], tm.vertices_[i + 0]); + ExpectEQ(tm1.vertices_[i], tm.vertices_[i + size]); + } + + EXPECT_EQ(2 * size, tm.tetras_.size()); + for (size_t i = 0; i < size; i++) { + ExpectEQ(tm0.tetras_[i], tm.tetras_[i + 0]); + ExpectEQ(Vector4i(tm1.tetras_[i](0, 0) + size, + tm1.tetras_[i](1, 0) + size, + tm1.tetras_[i](2, 0) + size, + tm1.tetras_[i](3, 0) + size), + tm.tetras_[i + size]); + } +} + +// ---------------------------------------------------------------------------- +// +// ---------------------------------------------------------------------------- +TEST(TetraMesh, OperatorADD) { + size_t size = 100; + + Vector3d dmin(0.0, 0.0, 0.0); + Vector3d dmax(1000.0, 1000.0, 1000.0); + + Vector4i imin(0, 0, 0, 0); + Vector4i imax(size - 1, size - 1, size - 1, size - 1); + + geometry::TetraMesh tm0; + geometry::TetraMesh tm1; + + tm0.vertices_.resize(size); + tm0.tetras_.resize(size); + + tm1.vertices_.resize(size); + tm1.tetras_.resize(size); + + Rand(tm0.vertices_, dmin, dmax, 0); + Rand(tm0.tetras_, imin, imax, 0); + + Rand(tm1.vertices_, dmin, dmax, 0); + Rand(tm1.tetras_, imin, imax, 0); + + geometry::TetraMesh tm = tm0 + tm1; + + EXPECT_EQ(2 * size, tm.vertices_.size()); + for (size_t i = 0; i < size; i++) { + ExpectEQ(tm0.vertices_[i], tm.vertices_[i + 0]); + ExpectEQ(tm1.vertices_[i], tm.vertices_[i + size]); + } + + EXPECT_EQ(2 * size, tm.tetras_.size()); + for (size_t i = 0; i < size; i++) { + ExpectEQ(tm0.tetras_[i], tm.tetras_[i + 0]); + ExpectEQ(Vector4i(tm1.tetras_[i](0, 0) + size, + tm1.tetras_[i](1, 0) + size, + tm1.tetras_[i](2, 0) + size, + tm1.tetras_[i](3, 0) + size), + tm.tetras_[i + size]); + } +} + +// ---------------------------------------------------------------------------- +// +// ---------------------------------------------------------------------------- +TEST(TetraMesh, Purge) { + typedef vector> vector_Vector4i; + + vector vertices = {// duplicate + {796.078431, 909.803922, 196.078431}, + {796.078431, 909.803922, 196.078431}, + {333.333333, 764.705882, 274.509804}, + {552.941176, 474.509804, 627.450980}, + {364.705882, 509.803922, 949.019608}, + {913.725490, 635.294118, 713.725490}, + {141.176471, 603.921569, 15.686275}, + // unreferenced + {239.215686, 133.333333, 803.921569}}; + + vector_Vector4i tetras = {{2, 6, 3, 4}, + // same tetra after vertex duplicate is removed + {0, 2, 3, 4}, + {1, 2, 3, 4}, + // duplicates + {3, 4, 5, 6}, + {4, 3, 5, 6}, + {3, 4, 6, 5}, + {5, 3, 4, 6}, + // degenerate + {4, 5, 6, 6}}; + + geometry::TetraMesh tm; + tm.vertices_ = vertices; + tm.tetras_ = tetras; + + tm.RemoveDuplicatedTetras(); + + vector_Vector4i ref_tetras_after_tetra_duplicate_removal = { + {2, 6, 3, 4}, {0, 2, 3, 4}, {1, 2, 3, 4}, {3, 4, 5, 6}, {4, 5, 6, 6} + + }; + ExpectEQ(ref_tetras_after_tetra_duplicate_removal, tm.tetras_); + + tm.RemoveDuplicatedVertices(); + + vector ref_vertices_after_duplicate_removal = { + {796.078431, 909.803922, 196.078431}, + {333.333333, 764.705882, 274.509804}, + {552.941176, 474.509804, 627.450980}, + {364.705882, 509.803922, 949.019608}, + {913.725490, 635.294118, 713.725490}, + {141.176471, 603.921569, 15.686275}, + {239.215686, 133.333333, 803.921569}}; + vector_Vector4i ref_tetras_after_vertex_duplicate_removal = {{1, 5, 2, 3}, + {0, 1, 2, 3}, + {0, 1, 2, 3}, + {2, 3, 4, 5}, + {3, 4, 5, 5}}; + ExpectEQ(ref_vertices_after_duplicate_removal, tm.vertices_); + ExpectEQ(ref_tetras_after_vertex_duplicate_removal, tm.tetras_); + + tm.RemoveUnreferencedVertices(); + + vector ref_vertices_after_unreferenced_removal = { + {796.078431, 909.803922, 196.078431}, + {333.333333, 764.705882, 274.509804}, + {552.941176, 474.509804, 627.450980}, + {364.705882, 509.803922, 949.019608}, + {913.725490, 635.294118, 713.725490}, + {141.176471, 603.921569, 15.686275}}; + ExpectEQ(ref_vertices_after_unreferenced_removal, tm.vertices_); + + tm.RemoveDegenerateTetras(); + + vector_Vector4i ref_tetras_after_degenerate_removal = { + {1, 5, 2, 3}, {0, 1, 2, 3}, {0, 1, 2, 3}, {2, 3, 4, 5} + + }; + ExpectEQ(ref_tetras_after_degenerate_removal, tm.tetras_); +} + +// ---------------------------------------------------------------------------- +// +// ---------------------------------------------------------------------------- +TEST(TetraMesh, HasVertices) { + int size = 100; + + geometry::TetraMesh tm; + + EXPECT_FALSE(tm.HasVertices()); + + tm.vertices_.resize(size); + + EXPECT_TRUE(tm.HasVertices()); +} + +// ---------------------------------------------------------------------------- +// +// ---------------------------------------------------------------------------- +TEST(TetraMesh, HasTetras) { + int size = 100; + + geometry::TetraMesh tm; + + EXPECT_FALSE(tm.HasTetras()); + + tm.vertices_.resize(size); + tm.tetras_.resize(size); + + EXPECT_TRUE(tm.HasTetras()); +} + +// ---------------------------------------------------------------------------- +// +// ---------------------------------------------------------------------------- +TEST(TetraMesh, CreateFromPointCloud) { + // create a random point cloud + geometry::PointCloud pc; + + int size = 100; + + Vector3d dmin(0.0, 0.0, 0.0); + Vector3d dmax(1000.0, 1000.0, 1000.0); + + pc.points_.resize(size); + + Rand(pc.points_, dmin, dmax, 0); + + auto tm = geometry::TetraMesh::CreateFromPointCloud(pc); + + EXPECT_EQ(pc.points_.size(), tm->vertices_.size()); + + // check if the delaunay property holds for all tetras. + // There should be no vertex inside the circumsphere of a tetrahedron. + // This is more a check to see if we have used the right Qhull parameters. + auto tetrahedron_circumsphere = [&](const geometry::TetraMesh& tm, + size_t tetra_idx) { + Vector4i tetra = tm.tetras_[tetra_idx]; + Matrix4d homogeneous_points; + for (int i = 0; i < 4; ++i) { + homogeneous_points.row(i) = tm.vertices_[tetra[i]].homogeneous(); + } + double det = homogeneous_points.determinant(); + Vector4d b; + for (int i = 0; i < 4; ++i) { + b[i] = -tm.vertices_[tetra[i]].squaredNorm(); + } + + Matrix4d tmp; + double x[3]; + for (int i = 0; i < 3; ++i) { + tmp = homogeneous_points; + tmp.col(i) = b; + x[i] = tmp.determinant() / det; + } + Vector3d center(-x[0] / 2, -x[1] / 2, -x[2] / 2); + double sqr_radius = (tm.vertices_[tetra[0]] - center).squaredNorm(); + + return std::make_tuple(center, sqr_radius); + }; + + bool circumsphere_property = true; + for (size_t i = 0; i < tm->tetras_.size() && circumsphere_property; ++i) { + Vector3d sphere_center; + double sqr_radius; + std::tie(sphere_center, sqr_radius) = tetrahedron_circumsphere(*tm, i); + + const Vector4i tetra = tm->tetras_[i]; + + for (int j = 0; j < (int)tm->vertices_.size(); ++j) { + if (tetra[0] == j || tetra[1] == j || tetra[2] == j || + tetra[3] == j) { + continue; + } + Vector3d v = tm->vertices_[j]; + double sqr_dist_center_v = (v - sphere_center).squaredNorm(); + if (sqr_dist_center_v <= sqr_radius) { + circumsphere_property = false; + break; + } + } + } + EXPECT_TRUE(circumsphere_property); +} + +// ---------------------------------------------------------------------------- +// +// ---------------------------------------------------------------------------- +TEST(TetraMesh, ExtractTriangleMesh) { + // create a random point cloud + geometry::PointCloud pc; + + int size = 250; + + Vector3d dmin(0.0, 0.0, 0.0); + Vector3d dmax(1.0, 1.0, 1.0); + + pc.points_.resize(size); + + Rand(pc.points_, dmin, dmax, 0); + + auto tm = geometry::TetraMesh::CreateFromPointCloud(pc); + + vector values(tm->vertices_.size()); + + // distance to center as values + Vector3d center(0.5 * (dmin + dmax)); + for (size_t i = 0; i < tm->vertices_.size(); ++i) { + double dist = (tm->vertices_[i] - center).norm(); + values[i] = dist; + } + + // all values are below the level + { + auto triangle_mesh = tm->ExtractTriangleMesh(values, 123.4); + EXPECT_EQ(triangle_mesh->vertices_.size(), size_t(0)); + EXPECT_EQ(triangle_mesh->triangles_.size(), size_t(0)); + } + + // all values are above the level + { + auto triangle_mesh = tm->ExtractTriangleMesh(values, -123.4); + EXPECT_EQ(triangle_mesh->vertices_.size(), size_t(0)); + EXPECT_EQ(triangle_mesh->triangles_.size(), size_t(0)); + } + + // values below and above the level + { + auto triangle_mesh = tm->ExtractTriangleMesh(values, 0.25); + EXPECT_EQ(triangle_mesh->vertices_.size(), size_t(125)); + EXPECT_EQ(triangle_mesh->triangles_.size(), size_t(246)); + + // the following should have no effect + triangle_mesh->RemoveDuplicatedVertices(); + triangle_mesh->RemoveDuplicatedTriangles(); + triangle_mesh->RemoveUnreferencedVertices(); + triangle_mesh->RemoveDegenerateTriangles(); + + EXPECT_EQ(triangle_mesh->vertices_.size(), size_t(125)); + EXPECT_EQ(triangle_mesh->triangles_.size(), size_t(246)); + } +} diff --git a/src/UnitTest/TestUtility/Print.h b/src/UnitTest/TestUtility/Print.h index f3642edace6..b67eb909afe 100644 --- a/src/UnitTest/TestUtility/Print.h +++ b/src/UnitTest/TestUtility/Print.h @@ -58,7 +58,7 @@ void Print(const Eigen::Matrix& matrix, std::cout << std::setw((tabs + 1) * TAB_SIZE) << ""; for (int n = 0; n < N; n++) { - std::cout << std::setw(width) << matrix(n, m); + std::cout << std::setw(width) << matrix(m, n); if (m != (M - 1) || n != (N - 1)) std::cout << ","; } } @@ -93,6 +93,30 @@ void Print(const std::vector>& v, if (',' != terminator) std::cout << std::endl; } +// Print a vector of Matrix that uses the Eigen::aligned_allocator. +template +void Print( + const std::vector, + Eigen::aligned_allocator>>& v, + const int& tabs = 1, + const char& terminator = ';') { + std::cout << std::setw(tabs * TAB_SIZE) << "{"; + std::cout << std::endl; + for (size_t i = 0; i < v.size(); i++) { + if (i == (v.size() - 1)) + Print(v[i], tabs + 1, '0'); + else + Print(v[i], tabs + 1, ','); + + std::cout << std::endl; + } + std::cout << std::setw(tabs * TAB_SIZE) << "}"; + + if (';' == terminator || ',' == terminator) std::cout << terminator; + + if (',' != terminator) std::cout << std::endl; +} + // Print an array. template void Print(const T* const v, const size_t& size, const int& width = 12) { diff --git a/src/UnitTest/TestUtility/Rand.cpp b/src/UnitTest/TestUtility/Rand.cpp index 0189d1e9e00..a27c0699dc3 100644 --- a/src/UnitTest/TestUtility/Rand.cpp +++ b/src/UnitTest/TestUtility/Rand.cpp @@ -176,6 +176,30 @@ void unit_test::Rand(vector &v, } } +// ---------------------------------------------------------------------------- +// Initialize an Vector4i vector. +// Output range: [vmin:vmax]. +// ---------------------------------------------------------------------------- +void unit_test::Rand(vector &v, + const Vector4i &vmin, + const Vector4i &vmax, + const int &seed) { + Raw raw(seed); + + Vector4d factor; + factor(0, 0) = (double)(vmax(0, 0) - vmin(0, 0)) / Raw::VMAX; + factor(1, 0) = (double)(vmax(1, 0) - vmin(1, 0)) / Raw::VMAX; + factor(2, 0) = (double)(vmax(2, 0) - vmin(2, 0)) / Raw::VMAX; + factor(3, 0) = (double)(vmax(3, 0) - vmin(3, 0)) / Raw::VMAX; + + for (size_t i = 0; i < v.size(); i++) { + v[i](0, 0) = vmin(0, 0) + (int)(raw.Next() * factor(0, 0)); + v[i](1, 0) = vmin(1, 0) + (int)(raw.Next() * factor(1, 0)); + v[i](2, 0) = vmin(2, 0) + (int)(raw.Next() * factor(2, 0)); + v[i](3, 0) = vmin(3, 0) + (int)(raw.Next() * factor(2, 0)); + } +} + // ---------------------------------------------------------------------------- // Initialize a uint8_t vector. // Output range: [vmin:vmax]. diff --git a/src/UnitTest/TestUtility/Rand.h b/src/UnitTest/TestUtility/Rand.h index b199bbd3f64..e0594750c46 100644 --- a/src/UnitTest/TestUtility/Rand.h +++ b/src/UnitTest/TestUtility/Rand.h @@ -81,6 +81,13 @@ void Rand(std::vector& v, const int& vmax, const int& seed); +// Initialize an Eigen::Vector4i vector. +// Output range: [vmin:vmax]. +void Rand(std::vector& v, + const Eigen::Vector4i& vmin, + const Eigen::Vector4i& vmax, + const int& seed); + // Initialize a uint8_t vector. // Output range: [vmin:vmax]. void Rand(std::vector& v, diff --git a/src/UnitTest/TestUtility/UnitTest.h b/src/UnitTest/TestUtility/UnitTest.h index 730595dc9a0..f0b3f73d624 100644 --- a/src/UnitTest/TestUtility/UnitTest.h +++ b/src/UnitTest/TestUtility/UnitTest.h @@ -69,6 +69,18 @@ void ExpectEQ(const std::vector>& v0, EXPECT_EQ(v0.size(), v1.size()); for (size_t i = 0; i < v0.size(); i++) ExpectEQ(v0[i], v1[i], threshold); } +template +void ExpectEQ( + const std::vector, + Eigen::aligned_allocator>>& + v0, + const std::vector, + Eigen::aligned_allocator>>& + v1, + double threshold = THRESHOLD_1E_6) { + EXPECT_EQ(v0.size(), v1.size()); + for (size_t i = 0; i < v0.size(); i++) ExpectEQ(v0[i], v1[i], threshold); +} // Less than or Equal test. template