Skip to content

Commit 836ed98

Browse files
committed
Add tests for MakeObb
1 parent 7b81014 commit 836ed98

File tree

4 files changed

+140
-18
lines changed

4 files changed

+140
-18
lines changed

geometry/proximity/BUILD.bazel

Lines changed: 21 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -44,6 +44,7 @@ drake_cc_package_library(
4444
":make_ellipsoid_mesh",
4545
":make_mesh_field",
4646
":make_mesh_from_vtk",
47+
":make_obb_from_mesh",
4748
":make_sphere_field",
4849
":make_sphere_mesh",
4950
":mesh_field",
@@ -590,17 +591,18 @@ drake_cc_library(
590591
name = "make_obb_from_mesh",
591592
srcs = ["make_obb_from_mesh.cc"],
592593
hdrs = ["make_obb_from_mesh.h"],
593-
internal = True,
594-
visibility = ["//geometry:__subpackages__"],
595594
deps = [
596595
":obb",
597596
":obj_to_surface_mesh",
597+
":polygon_surface_mesh",
598598
":triangle_surface_mesh",
599599
":volume_mesh",
600600
":vtk_to_volume_mesh",
601601
"//common:essential",
602602
"//geometry:mesh_source",
603-
"@fmt",
603+
],
604+
implementation_deps = [
605+
":make_convex_hull_mesh_impl",
604606
],
605607
)
606608

@@ -1490,6 +1492,22 @@ drake_cc_googletest(
14901492
],
14911493
)
14921494

1495+
drake_cc_googletest(
1496+
name = "make_obb_from_mesh_test",
1497+
data = [
1498+
"//geometry:test_gltf_files",
1499+
"//geometry:test_obj_files",
1500+
"//geometry:test_vtk_files",
1501+
],
1502+
deps = [
1503+
":make_obb_from_mesh",
1504+
"//common:find_resource",
1505+
"//common/test_utilities:eigen_matrix_compare",
1506+
"//common/test_utilities:expect_throws_message",
1507+
"//geometry:mesh_source",
1508+
],
1509+
)
1510+
14931511
drake_cc_googletest(
14941512
name = "make_cylinder_field_test",
14951513
deps = [

geometry/proximity/make_obb_from_mesh.cc

Lines changed: 7 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -1,13 +1,14 @@
11
#include "drake/geometry/proximity/make_obb_from_mesh.h"
22

33
#include <set>
4-
#include <string>
54

65
#include <fmt/format.h>
76

87
#include "drake/common/eigen_types.h"
8+
#include "drake/geometry/proximity/make_convex_hull_mesh_impl.h"
99
#include "drake/geometry/proximity/obb.h"
1010
#include "drake/geometry/proximity/obj_to_surface_mesh.h"
11+
#include "drake/geometry/proximity/polygon_surface_mesh.h"
1112
#include "drake/geometry/proximity/triangle_surface_mesh.h"
1213
#include "drake/geometry/proximity/volume_mesh.h"
1314
#include "drake/geometry/proximity/vtk_to_volume_mesh.h"
@@ -23,35 +24,29 @@ Obb MakeObb(const MeshSource& mesh_source, const Vector3d& scale) {
2324
// For OBJ files, create a TriangleSurfaceMesh and use ObbMaker.
2425
TriangleSurfaceMesh<double> surface_mesh =
2526
ReadObjToTriangleSurfaceMesh(mesh_source, scale);
26-
27-
// Create a set containing all vertex indices
27+
// Create a set containing all vertex indices.
2828
std::set<int> all_vertices;
2929
for (int i = 0; i < surface_mesh.num_vertices(); ++i) {
3030
all_vertices.insert(i);
3131
}
32-
33-
// Use ObbMaker to compute the OBB
3432
ObbMaker<TriangleSurfaceMesh<double>> obb_maker(surface_mesh, all_vertices);
3533
return obb_maker.Compute();
3634
} else if (mesh_source.extension() == ".vtk") {
3735
// For VTK files, create a VolumeMesh and use ObbMaker.
3836
const VolumeMesh<double> volume_mesh =
3937
ReadVtkToVolumeMesh(mesh_source, scale);
40-
41-
// Create a set containing all vertex indices
38+
// Create a set containing all vertex indices.
4239
std::set<int> all_vertices;
4340
for (int i = 0; i < volume_mesh.num_vertices(); ++i) {
4441
all_vertices.insert(i);
4542
}
46-
47-
// Use ObbMaker to compute the OBB
4843
ObbMaker<VolumeMesh<double>> obb_maker(volume_mesh, all_vertices);
4944
return obb_maker.Compute();
5045
} else if (mesh_source.extension() == ".gltf") {
5146
// For glTF files, we create the convex hull of the mesh and then compute
52-
// the OBB of the convex hull.
53-
const Mesh mesh(mesh_source, scale);
54-
const PolygonSurfaceMesh<double> polygon_mesh = MakeConvexHull(mesh);
47+
// the OBB of the convex hull so that we can reuse existing functions.
48+
const PolygonSurfaceMesh<double> polygon_mesh =
49+
MakeConvexHull(mesh_source, scale);
5550
std::set<int> all_vertices;
5651
for (int i = 0; i < polygon_mesh.num_vertices(); ++i) {
5752
all_vertices.insert(i);

geometry/proximity/obb.cc

Lines changed: 1 addition & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -3,8 +3,8 @@
33
#include <algorithm>
44
#include <limits>
55

6-
#include "drake/geometry/proximity/triangle_surface_mesh.h"
76
#include "drake/geometry/proximity/polygon_surface_mesh.h"
7+
#include "drake/geometry/proximity/triangle_surface_mesh.h"
88
#include "drake/geometry/proximity/volume_mesh.h"
99
#include "drake/geometry/utilities.h"
1010

@@ -134,7 +134,6 @@ Obb ObbMaker<MeshType>::CalcOrientedBox(const RotationMatrixd& R_MB) const {
134134
// Fo=Mo → → → → → → → Mx
135135
//
136136
const RotationMatrixd& R_MF = R_MB;
137-
138137
// We will find the upper and lower bounding points U and L of the box as
139138
// measured and expressed in frame F: p_FU, p_FL. We cannot do it in frame
140139
// B because we do not know the origin Bo of frame B yet. It is not true in
Lines changed: 111 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1 +1,111 @@
1-
1+
#include "drake/geometry/proximity/make_obb_from_mesh.h"
2+
3+
#include <gtest/gtest.h>
4+
5+
#include "drake/common/find_resource.h"
6+
#include "drake/common/test_utilities/eigen_matrix_compare.h"
7+
#include "drake/common/test_utilities/expect_throws_message.h"
8+
#include "drake/geometry/mesh_source.h"
9+
10+
namespace drake {
11+
namespace geometry {
12+
namespace internal {
13+
namespace {
14+
15+
using Eigen::Vector3d;
16+
constexpr double kTol = 1e-13;
17+
18+
class MakeObbFromMeshTest : public ::testing::Test {
19+
protected:
20+
void SetUp() override {
21+
obj_path_ = FindResourceOrThrow("drake/geometry/test/cube_with_hole.obj");
22+
vtk_path_ = FindResourceOrThrow("drake/geometry/test/cube_as_volume.vtk");
23+
gltf_path_ = FindResourceOrThrow("drake/geometry/test/cube_with_hole.gltf");
24+
}
25+
26+
// Helper function to verify that the OBB has the correct half-widths and
27+
// pose.
28+
void ValidateObb(const Obb& obb, const Vector3<double>& half_width) {
29+
const Matrix3<double>& R_abs = obb.pose().rotation().matrix().cwiseAbs();
30+
for (int i = 0; i < 3; ++i) {
31+
EXPECT_NEAR(R_abs.row(i).sum(), 1.0, kTol);
32+
EXPECT_NEAR(R_abs.col(i).sum(), 1.0, kTol);
33+
}
34+
// Verify that the half-widths are correct. We don't know the exact
35+
// ordering of the half-widths, so we order them and then compare.
36+
Vector3d expected = half_width;
37+
Vector3d computed = obb.half_width();
38+
std::sort(expected.data(), expected.data() + expected.size());
39+
std::sort(computed.data(), computed.data() + computed.size());
40+
EXPECT_TRUE(CompareMatrices(computed, expected, kTol));
41+
}
42+
43+
// Each of the test files is a cube (with or without a hole) that has an
44+
// obb with half-widths (1, 1, 1) and with identity pose.
45+
std::string obj_path_;
46+
std::string vtk_path_;
47+
std::string gltf_path_;
48+
};
49+
50+
TEST_F(MakeObbFromMeshTest, Obj) {
51+
const Eigen::Vector3d scale = Eigen::Vector3d::Ones();
52+
const Obb obj_obb = MakeObb(MeshSource(obj_path_), scale);
53+
ValidateObb(obj_obb, scale);
54+
}
55+
56+
TEST_F(MakeObbFromMeshTest, Vtk) {
57+
const Eigen::Vector3d scale = Eigen::Vector3d::Ones();
58+
const Obb vtk_obb = MakeObb(MeshSource(vtk_path_), scale);
59+
ValidateObb(vtk_obb, scale);
60+
}
61+
62+
// The OBB for the gltf cube suffers from #14067. We just check that the
63+
// half-widths are positive.
64+
TEST_F(MakeObbFromMeshTest, Gltf) {
65+
const Eigen::Vector3d scale = Eigen::Vector3d::Ones();
66+
const Obb gltf_obb = MakeObb(MeshSource(gltf_path_), scale);
67+
EXPECT_GT(gltf_obb.half_width().x(), 0.0);
68+
EXPECT_GT(gltf_obb.half_width().y(), 0.0);
69+
EXPECT_GT(gltf_obb.half_width().z(), 0.0);
70+
}
71+
72+
// Test scaling effects.
73+
TEST_F(MakeObbFromMeshTest, ScalingEffect) {
74+
const MeshSource mesh_source(obj_path_);
75+
const Eigen::Vector3d double_scale = Eigen::Vector3d(2.0, 2.0, 2.0);
76+
const Obb scaled_obb = MakeObb(mesh_source, double_scale);
77+
ValidateObb(scaled_obb, double_scale);
78+
}
79+
80+
// Test non-uniform scaling.
81+
TEST_F(MakeObbFromMeshTest, NonUniformScaling) {
82+
const MeshSource mesh_source(vtk_path_);
83+
const Eigen::Vector3d scale(2.0, 3.0, 4.0);
84+
const Obb obb = MakeObb(mesh_source, scale);
85+
ValidateObb(obb, scale);
86+
}
87+
88+
// Test unsupported file extension.
89+
TEST_F(MakeObbFromMeshTest, UnsupportedExtension) {
90+
// Create a mesh source with an unsupported extension.
91+
const MeshSource mesh_source("fake_file.stl");
92+
const Eigen::Vector3d scale = Eigen::Vector3d::Ones();
93+
94+
DRAKE_EXPECT_THROWS_MESSAGE(
95+
MakeObb(mesh_source, scale),
96+
"MakeObb only applies to .obj, .vtk, and .gltf meshes; "
97+
"unsupported extension '.stl' for geometry data: .*");
98+
}
99+
100+
// Test with zero scale (should still work but produce degenerate OBB).
101+
TEST_F(MakeObbFromMeshTest, ZeroScale) {
102+
const MeshSource mesh_source(obj_path_);
103+
const Eigen::Vector3d zero_scale = Eigen::Vector3d::Zero();
104+
const Obb obb = MakeObb(mesh_source, zero_scale);
105+
ValidateObb(obb, zero_scale);
106+
}
107+
108+
} // namespace
109+
} // namespace internal
110+
} // namespace geometry
111+
} // namespace drake

0 commit comments

Comments
 (0)