From fb240908fed9f9e1448bc10db021f6b19adf063e Mon Sep 17 00:00:00 2001 From: Nico van Duijn Date: Tue, 10 Jul 2018 10:57:10 +0200 Subject: [PATCH] Refactor unit tests for generateBVHModel Includes response to PR review, with minor comment fixes as well as a major refactor in the unit test function. --- .../geometric_shape_to_BVH_model-inl.h | 42 +-- .../geometry/geometric_shape_to_BVH_model.h | 20 +- test/CMakeLists.txt | 3 +- ...l_generate_bvh_model_deferred_finalize.cpp | 333 ++++++++++++++++++ ...est_fcl_generate_bvh_model_primitives.cpp} | 70 ++-- 5 files changed, 391 insertions(+), 77 deletions(-) create mode 100644 test/test_fcl_generate_bvh_model_deferred_finalize.cpp rename test/{test_fcl_generate_bvh_model.cpp => test_fcl_generate_bvh_model_primitives.cpp} (77%) diff --git a/include/fcl/geometry/geometric_shape_to_BVH_model-inl.h b/include/fcl/geometry/geometric_shape_to_BVH_model-inl.h index 528f0e3cd..7fe43860c 100644 --- a/include/fcl/geometry/geometric_shape_to_BVH_model-inl.h +++ b/include/fcl/geometry/geometric_shape_to_BVH_model-inl.h @@ -43,6 +43,27 @@ namespace fcl { +//============================================================================== +// Local helper function to ease conditional adding of triangles to a BVHModel +template +int addTriangles(BVHModel& model, const std::vector>& points, const std::vector& tri_indices, FinalizeModel finalize_model) +{ + int retval = BVH_OK; + if(model.build_state == BVH_BUILD_STATE_EMPTY){ + retval = model.beginModel(); + } + + if(retval == BVH_OK){ + retval = model.addSubModel(points, tri_indices); + } + + if(retval == BVH_OK && finalize_model == FinalizeModel::DO){ + retval = model.endModel(); + model.computeLocalAABB(); + } + return retval; +} + //============================================================================== template int generateBVHModel(BVHModel& model, const Box& shape, const Transform3& pose, FinalizeModel finalize_model) @@ -421,27 +442,6 @@ int generateBVHModel(BVHModel& model, const Cone& shape, con return generateBVHModel(model, shape, pose, circle_split_tot, h_num, finalize_model); } -//============================================================================== -template -int addTriangles(BVHModel& model, const std::vector>& points, const std::vector& tri_indices, FinalizeModel finalize_model) -{ - int retval = BVH_OK; - if(model.build_state == BVH_BUILD_STATE_EMPTY){ - retval = model.beginModel(); - } - - if(retval == BVH_OK){ - retval = model.addSubModel(points, tri_indices); - } - - if(retval == BVH_OK && finalize_model == FinalizeModel::DO){ - retval = model.endModel(); - model.computeLocalAABB(); - } - return retval; -} - - } // namespace fcl #endif \ No newline at end of file diff --git a/include/fcl/geometry/geometric_shape_to_BVH_model.h b/include/fcl/geometry/geometric_shape_to_BVH_model.h index dcea0ce82..294fd7eb4 100644 --- a/include/fcl/geometry/geometric_shape_to_BVH_model.h +++ b/include/fcl/geometry/geometric_shape_to_BVH_model.h @@ -65,13 +65,19 @@ enum class FinalizeModel{ /** @defgroup generateBVHModel @brief Create a BVHModel using geometric primitives -@details The functions in this group can be used to add geometric primitives (Box, Sphere, Ellipsoid, Cylinder, Cone) +@details The functions in this group can be used to add geometric primitives (Box, Sphere, Ellipsoid, Cylinder, Cone) to a BVHModel. It can either close off the model or leave it unfinalized in order to add more primitives later. - +@note All functions in this group have a common sub-set of parameters (listed below). In addition, each has unique + parameters related to the geometry type being added and how it should be tessellated. These additional parameters + are documented with their corresponding function +@warning If this function is used to create a BVHModel containing multiple geometric primitives, the BVHModel inherently + represents the *union* of those primitives. The BVHModel structure does not retain any notion of the original + geometric primitive. @param[out] model The BVHModel to be generated or added to @param[in] shape The geometric object to be added to the BVHModel @param[in] pose The pose of the geometric object @param[in] finalize_model an enum indicating whether the model is final or more submodels can be added later +@return BVHReturnCode indicating the success of the operation @{ */ @@ -149,16 +155,6 @@ int generateBVHModel(BVHModel& model, const Cone& shape, con template int generateBVHModel(BVHModel& model, const Cone& shape, const Transform3& pose, unsigned int circle_split_tot_for_unit_cone, FinalizeModel finalize_model = FinalizeModel::DO); -/** -@brief AddTriangles to a BVHModel -@param[in, out] model The BVHModel -@param[in] points The points to add -@param[in] tri_indices The triangles to add -@param[in] finalize_model An enum indicating whether to close off the model afterwards -**/ -template -int addTriangles(BVHModel& model, const std::vector>& points, const std::vector& tri_indices, FinalizeModel finalize_model); - /**@} */ // end of doxygen group generateBVHModel } // namespace fcl diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 549ab2a4a..2b6c18171 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -52,7 +52,8 @@ set(tests test_fcl_distance.cpp test_fcl_frontlist.cpp test_fcl_general.cpp - test_fcl_generate_bvh_model.cpp + test_fcl_generate_bvh_model_deferred_finalize.cpp + test_fcl_generate_bvh_model_primitives.cpp test_fcl_geometric_shapes.cpp test_fcl_math.cpp test_fcl_profiler.cpp diff --git a/test/test_fcl_generate_bvh_model_deferred_finalize.cpp b/test/test_fcl_generate_bvh_model_deferred_finalize.cpp new file mode 100644 index 000000000..bd5569fbe --- /dev/null +++ b/test/test_fcl_generate_bvh_model_deferred_finalize.cpp @@ -0,0 +1,333 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2016, Open Source Robotics Foundation + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Open Source Robotics Foundation nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/** @author Nico van Duijn */ + +#include + +#include "fcl/config.h" +#include "fcl/geometry/geometric_shape_to_BVH_model.h" +#include "test_fcl_utility.h" +#include + +using namespace fcl; + +/** +@description This file tests functionality in generateBVHModel(). Specifically, + it tests that a BVHModel can be either finalized after adding a + geometric primitive, or left "open" in order to add further + shapes at a later time. This functionality is tested without any + regard to proper functionality or special cases in the conversion from + geometric primitive to BVHModel. +**/ + + +/** +@details This function tests adding geometric primitives to an empty model. + It checks proper functionality of those simply by + verifying the return value, the number of vertices, triangles and the state of the model. + In the process, the provided model will always be BVH_BUILD_STATE_BEGUN afterwards +**/ +template +void checkAddToEmptyModel(BVHModel& model, const ShapeType& shape) +{ + using S = typename BV::S; + uint8_t n = 32; // Hard-coded mesh-resolution. Not testing corner cases where n=0 or such + int ret; + + // Make sure we are given an empty model + GTEST_ASSERT_EQ(model.build_state, BVH_BUILD_STATE_EMPTY); + uint8_t num_vertices = model.num_vertices; + uint8_t num_tris = model.num_tris; + GTEST_ASSERT_EQ(num_vertices, 0); + GTEST_ASSERT_EQ(num_tris, 0); + + // Add the shape to the model and count vertices and triangles to make sure it has been created + ret = generateBVHModel(model, shape, Transform3::Identity(), n, FinalizeModel::DONT); + GTEST_ASSERT_EQ(ret, BVH_OK); + EXPECT_GT(model.num_vertices, num_vertices); + EXPECT_GT(model.num_tris, num_tris); + EXPECT_EQ(model.build_state, BVH_BUILD_STATE_BEGUN); +} + +// Specialization for boxes +template +void checkAddToEmptyModel(BVHModel& model, const Box& shape) +{ + using S = typename BV::S; + int ret; + + // Make sure we are given an empty model + GTEST_ASSERT_EQ(model.build_state, BVH_BUILD_STATE_EMPTY); + uint8_t num_vertices = model.num_vertices; + uint8_t num_tris = model.num_tris; + GTEST_ASSERT_EQ(num_vertices, 0); + GTEST_ASSERT_EQ(num_tris, 0); + + // Add the shape to the model and count vertices and triangles to make sure it has been created + ret = generateBVHModel(model, shape, Transform3::Identity(), FinalizeModel::DONT); + GTEST_ASSERT_EQ(ret, BVH_OK); + EXPECT_GT(model.num_vertices, num_vertices); + EXPECT_GT(model.num_tris, num_tris); + EXPECT_EQ(model.build_state, BVH_BUILD_STATE_BEGUN); +} + + +/** +@details This function tests adding geometric primitives to an unfinalized model. + It checks proper functionality by verifying the return value, + the number of vertices, triangles and the state of the model. + After execution, the provided model will always be BVH_BUILD_STATE_BEGUN. +**/ +template +void checkAddToUnfinalizedModel(BVHModel& model, const ShapeType& shape) +{ + using S = typename BV::S; + const uint8_t n = 32; + int ret; + + // Make sure we are given a model that is already begun + GTEST_ASSERT_EQ(model.build_state, BVH_BUILD_STATE_BEGUN); + uint8_t num_vertices = model.num_vertices; + uint8_t num_tris = model.num_tris; + + // Add the shape to the model and count vertices and triangles to make sure it has been created + ret = generateBVHModel(model, shape, Transform3(Translation3(Vector3(2.0, 2.0, 2.0))), n, FinalizeModel::DONT); + GTEST_ASSERT_EQ(ret, BVH_OK); + EXPECT_GT(model.num_vertices, num_vertices); + EXPECT_GT(model.num_tris, num_tris); + EXPECT_EQ(model.build_state, BVH_BUILD_STATE_BEGUN); +} + +// Specialization for boxes +template +void checkAddToUnfinalizedModel(BVHModel& model, const Box& shape) +{ + using S = typename BV::S; + int ret; + + // Make sure we are given a model that is already begun + GTEST_ASSERT_EQ(model.build_state, BVH_BUILD_STATE_BEGUN); + uint8_t num_vertices = model.num_vertices; + uint8_t num_tris = model.num_tris; + + // Add the shape to the model and count vertices and triangles to make sure it has been created + ret = generateBVHModel(model, shape, Transform3(Translation3(Vector3(2.0, 2.0, 2.0))), FinalizeModel::DONT); + GTEST_ASSERT_EQ(ret, BVH_OK); + EXPECT_GT(model.num_vertices, num_vertices); + EXPECT_GT(model.num_tris, num_tris); + EXPECT_EQ(model.build_state, BVH_BUILD_STATE_BEGUN); +} + +/** +@details This function tests adding primitives to a previously begun model + It checks proper functionality by checking the return value, + the number of vertices and triangles and the state of the model + after execution. After this call, the model is finalized. + +**/ +template +void checkAddAndFinalizeModel(BVHModel& model, const ShapeType& shape){ + using S = typename BV::S; + const uint8_t n = 32; + int ret; + + // Make sure we are given a model that is already begun + GTEST_ASSERT_EQ(model.build_state, BVH_BUILD_STATE_BEGUN); + uint8_t num_vertices = model.num_vertices; + uint8_t num_tris = model.num_tris; + + // Add another instance of the shape and make sure it was added to the model by counting vertices and tris + ret = generateBVHModel(model, shape, Transform3(Translation3(Vector3(2.0, 2.0, 2.0))), n, FinalizeModel::DO); + GTEST_ASSERT_EQ(ret, BVH_OK); + EXPECT_GT(model.num_vertices, num_vertices); + EXPECT_GT(model.num_tris, num_tris); + EXPECT_EQ(model.build_state, BVH_BUILD_STATE_PROCESSED); +} + +// Specialization for boxes +template +void checkAddAndFinalizeModel(BVHModel& model, const Box& shape){ + using S = typename BV::S; + int ret; + + // Make sure we are given a model that is already begun + GTEST_ASSERT_EQ(model.build_state, BVH_BUILD_STATE_BEGUN); + uint8_t num_vertices = model.num_vertices; + uint8_t num_tris = model.num_tris; + + // Add another instance of the shape and make sure it was added to the model by counting vertices and tris + ret = generateBVHModel(model, shape, Transform3(Translation3(Vector3(3.0, 3.0, 3.0))), FinalizeModel::DO); + GTEST_ASSERT_EQ(ret, BVH_OK); + EXPECT_GT(model.num_vertices, num_vertices); + EXPECT_GT(model.num_tris, num_tris); + EXPECT_EQ(model.build_state, BVH_BUILD_STATE_PROCESSED); +} + + +/** +@details This function tests that adding geometric primitives to a finalized model indeed + returns the BVH error we would expect. +**/ +template +void checkAddToFinalizedModel(BVHModel& model, const ShapeType& shape) +{ + using S = typename BV::S; + const uint8_t n = 32; + + GTEST_ASSERT_EQ(model.build_state, BVH_BUILD_STATE_PROCESSED); + auto ret = generateBVHModel(model, shape, Transform3::Identity(), n, FinalizeModel::DONT); + EXPECT_EQ(ret, BVH_ERR_BUILD_OUT_OF_SEQUENCE); +} + +// Specialization for boxes +template +void checkAddToFinalizedModel(BVHModel& model, const Box& shape) +{ + using S = typename BV::S; + + GTEST_ASSERT_EQ(model.build_state, BVH_BUILD_STATE_PROCESSED); + auto ret = generateBVHModel(model, shape, Transform3::Identity(), FinalizeModel::DONT); + EXPECT_EQ(ret, BVH_ERR_BUILD_OUT_OF_SEQUENCE); +} + +template +void testBVHModelFromBox() +{ + using S = typename BV::S; + const S a = 1.0; + const S b = 1.0; + const S c = 1.0; + + std::shared_ptr> model(new BVHModel); + Box box(a, b, c); + + checkAddToEmptyModel(*model, box); + checkAddToUnfinalizedModel(*model, box); + checkAddAndFinalizeModel(*model, box); + checkAddToFinalizedModel(*model, box); + +} + +template +void testBVHModelFromSphere() +{ + using S = typename BV::S; + const S r = 1.0; + + Sphere sphere(r); + std::shared_ptr> model(new BVHModel); + checkAddToEmptyModel(*model, sphere); + checkAddToUnfinalizedModel(*model, sphere); + checkAddAndFinalizeModel(*model, sphere); + checkAddToFinalizedModel(*model, sphere); +} + +template +void testBVHModelFromEllipsoid() +{ + using S = typename BV::S; + const S a = 1.0; + const S b = 1.0; + const S c = 1.0; + + Ellipsoid ellipsoid(a, b, c); + std::shared_ptr> model(new BVHModel); + + checkAddToEmptyModel(*model, ellipsoid); + checkAddToUnfinalizedModel(*model, ellipsoid); + checkAddAndFinalizeModel(*model, ellipsoid); + checkAddToFinalizedModel(*model, ellipsoid); +} + +template +void testBVHModelFromCylinder() +{ + using S = typename BV::S; + const S r = 1.0; + const S h = 1.0; + + Cylinder cylinder(r, h); + std::shared_ptr> model(new BVHModel); + + checkAddToEmptyModel(*model, cylinder); + checkAddToUnfinalizedModel(*model, cylinder); + checkAddAndFinalizeModel(*model, cylinder); + checkAddToFinalizedModel(*model, cylinder); +} + +template +void testBVHModelFromCone() +{ + using S = typename BV::S; + const S r = 1.0; + const S h = 1.0; + + Cone cone(r, h); + std::shared_ptr> model(new BVHModel); + + checkAddToEmptyModel(*model, cone); + checkAddToUnfinalizedModel(*model, cone); + checkAddAndFinalizeModel(*model, cone); + checkAddToFinalizedModel(*model, cone); +} + +template +void testBVHModelFromPrimitives() +{ + testBVHModelFromBox(); + testBVHModelFromSphere(); + testBVHModelFromEllipsoid(); + testBVHModelFromCylinder(); + testBVHModelFromCone(); +} + +GTEST_TEST(FCL_GENERATE_BVH_MODELS, generating_bvh_models_from_primitives) +{ + testBVHModelFromPrimitives>(); + testBVHModelFromPrimitives>(); + testBVHModelFromPrimitives>(); + testBVHModelFromPrimitives>(); + testBVHModelFromPrimitives>(); + testBVHModelFromPrimitives >(); + testBVHModelFromPrimitives >(); + testBVHModelFromPrimitives >(); +} + +//============================================================================== +int main(int argc, char* argv[]) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/test/test_fcl_generate_bvh_model.cpp b/test/test_fcl_generate_bvh_model_primitives.cpp similarity index 77% rename from test/test_fcl_generate_bvh_model.cpp rename to test/test_fcl_generate_bvh_model_primitives.cpp index 397915a0d..783baec45 100644 --- a/test/test_fcl_generate_bvh_model.cpp +++ b/test/test_fcl_generate_bvh_model_primitives.cpp @@ -44,25 +44,35 @@ using namespace fcl; /** -@details This function tests adding geometric primitives to a model, by first creating one - and then appending to it. It checks proper functionality of those simply by - verifying the return value, the number of vertices, triangles and the state of the model. +@brief This file tests creation of BVHModels from geometric primitives. + +@details It checks proper functionality of those simply by verifying the return value, + the number of vertices, triangles and the state of the model. + +@note In the process, the provided model will always be finalized. + +@warning Currently, there are no checks that the geometric primitives created are + actually correct in terms of tesselation resolution, dimensions, radii etc. + +@todo These logic checks should be implemented by someone knowledgeable enough about + the inner workings of the generateBVHModel() functions. **/ + + template void checkNumVerticesAndTris(BVHModel& model, const ShapeType& shape, uint8_t n, int vertices, int tris) { using S = typename BV::S; + GTEST_ASSERT_EQ(model.build_state, BVH_BUILD_STATE_EMPTY); + GTEST_ASSERT_EQ(model.num_vertices, 0); + GTEST_ASSERT_EQ(model.num_tris, 0); + // Add the shape to the model and count vertices and triangles to make sure it has been created - generateBVHModel(model, shape, Transform3::Identity(), n, FinalizeModel::DONT); + auto ret = generateBVHModel(model, shape, Transform3::Identity(), n, FinalizeModel::DO); + EXPECT_EQ(ret, BVH_OK); EXPECT_EQ(model.num_vertices, vertices); EXPECT_EQ(model.num_tris, tris); - EXPECT_EQ(model.build_state, BVH_BUILD_STATE_BEGUN); - - // Add another instance of the shape and make sure it was added to the model by counting vertices and tris - generateBVHModel(model, shape, Transform3(Translation3(Vector3(2.0, 2.0, 2.0))), n); - EXPECT_EQ(model.num_vertices, 2*vertices); - EXPECT_EQ(model.num_tris, 2*tris); EXPECT_EQ(model.build_state, BVH_BUILD_STATE_PROCESSED); } @@ -71,38 +81,17 @@ template void checkNumVerticesAndTris(BVHModel& model, const Box& shape, int vertices, int tris) { using S = typename BV::S; - auto ret = generateBVHModel(model, shape, Transform3::Identity(), FinalizeModel::DONT); + + GTEST_ASSERT_EQ(model.build_state, BVH_BUILD_STATE_EMPTY); + GTEST_ASSERT_EQ(model.num_vertices, 0); + GTEST_ASSERT_EQ(model.num_tris, 0); + + // Add the shape to the model and count vertices and triangles to make sure it has been created + auto ret = generateBVHModel(model, shape, Transform3::Identity(), FinalizeModel::DO); EXPECT_EQ(ret, BVH_OK); EXPECT_EQ(model.num_vertices, vertices); EXPECT_EQ(model.num_tris, tris); - EXPECT_EQ(model.build_state, BVH_BUILD_STATE_BEGUN); - ret = generateBVHModel(model, shape, Transform3(Translation3(Vector3(2.0, 2.0, 2.0)))); - EXPECT_EQ(ret, BVH_OK); - EXPECT_EQ(model.num_vertices, 2*vertices); - EXPECT_EQ(model.num_tris, 2*tris); - EXPECT_EQ(model.build_state, BVH_BUILD_STATE_PROCESSED); -} - -/** -@details This function tests that adding geometric primitives to a finalized model indeed - returns the BVH error we would expect. -**/ -template -void checkAddToFinishedModel(BVHModel& model, const ShapeType& shape, uint8_t n) -{ - using S = typename BV::S; - EXPECT_EQ(model.build_state, BVH_BUILD_STATE_PROCESSED); - auto ret = generateBVHModel(model, shape, Transform3::Identity(), n, FinalizeModel::DONT); - EXPECT_EQ(ret, BVH_ERR_BUILD_OUT_OF_SEQUENCE); -} - -template -void checkAddToFinishedModel(BVHModel& model, const Box& shape) -{ - using S = typename BV::S; EXPECT_EQ(model.build_state, BVH_BUILD_STATE_PROCESSED); - auto ret = generateBVHModel(model, shape, Transform3::Identity(), FinalizeModel::DONT); - EXPECT_EQ(ret, BVH_ERR_BUILD_OUT_OF_SEQUENCE); } template @@ -119,7 +108,6 @@ void testBVHModelFromBox() Box box(a, b, c); checkNumVerticesAndTris(*model, box, 8, 12); - checkAddToFinishedModel(*model, box); } } } @@ -142,7 +130,6 @@ void testBVHModelFromSphere() std::shared_ptr> model(new BVHModel); checkNumVerticesAndTris(*model, sphere, n, static_cast(2 + ring * ring), static_cast(2 * ring * ring)); - checkAddToFinishedModel(*model, sphere, n); } } } @@ -170,7 +157,6 @@ void testBVHModelFromEllipsoid() std::shared_ptr> model(new BVHModel); checkNumVerticesAndTris(*model, ellipsoid, n, static_cast(2 + ring * ring), static_cast(2 * ring * ring)); - checkAddToFinishedModel(*model, ellipsoid, n); } } } @@ -195,7 +181,6 @@ void testBVHModelFromCylinder() std::shared_ptr> model(new BVHModel); checkNumVerticesAndTris(*model, cylinder, n, static_cast(2 + n_tot * (h_num + 1)), static_cast((2 * h_num + 2) * n_tot)); - checkAddToFinishedModel(*model, cylinder, n); } } } @@ -219,7 +204,6 @@ void testBVHModelFromCone() std::shared_ptr> model(new BVHModel); checkNumVerticesAndTris(*model, cone, n, static_cast(2 + n_tot * h_num), static_cast(2 * n_tot * h_num)); - checkAddToFinishedModel(*model, cone, n); } } }