From 23be16fab74067d31e8b7e006315d81b6ea7cb7b Mon Sep 17 00:00:00 2001 From: Michael Ripperger Date: Thu, 18 Jan 2024 11:50:08 -0600 Subject: [PATCH] Added application for performing convex decomposition (#968) --- tesseract_collision/vhacd/CMakeLists.txt | 24 +++ .../vhacd/src/create_convex_decomposition.cpp | 182 ++++++++++++++++++ .../include/tesseract_common/macros.h | 3 +- 3 files changed, 208 insertions(+), 1 deletion(-) create mode 100644 tesseract_collision/vhacd/src/create_convex_decomposition.cpp diff --git a/tesseract_collision/vhacd/CMakeLists.txt b/tesseract_collision/vhacd/CMakeLists.txt index f81af3c877d..dd8c14065a9 100644 --- a/tesseract_collision/vhacd/CMakeLists.txt +++ b/tesseract_collision/vhacd/CMakeLists.txt @@ -23,6 +23,30 @@ target_include_directories( ${PROJECT_NAME}_vhacd_convex_decomposition PUBLIC "$" "$") +if(NOT MSVC) + find_package(Threads REQUIRED) + # Create target for creating convex decompositions from meshes + add_executable(create_convex_decomposition src/create_convex_decomposition.cpp) + target_link_libraries( + create_convex_decomposition + PRIVATE ${PROJECT_NAME}_vhacd_convex_decomposition + Boost::boost + Boost::program_options + Eigen3::Eigen + tesseract::tesseract_common + tesseract::tesseract_geometry + console_bridge::console_bridge + Threads::Threads) + target_compile_options(create_convex_decomposition PRIVATE ${TESSERACT_COMPILE_OPTIONS_PRIVATE} + ${TESSERACT_COMPILE_OPTIONS_PUBLIC}) + target_compile_definitions(create_convex_decomposition PRIVATE ${TESSERACT_COMPILE_DEFINITIONS}) + target_cxx_version(create_convex_decomposition PRIVATE VERSION ${TESSERACT_CXX_VERSION}) + + list(APPEND PACKAGE_LIBRARIES create_convex_decomposition) + + install_targets(TARGETS create_convex_decomposition COMPONENT vhacd) +endif() + # Mark cpp header files for installation install( DIRECTORY include/${PROJECT_NAME} diff --git a/tesseract_collision/vhacd/src/create_convex_decomposition.cpp b/tesseract_collision/vhacd/src/create_convex_decomposition.cpp new file mode 100644 index 00000000000..9862e65a92a --- /dev/null +++ b/tesseract_collision/vhacd/src/create_convex_decomposition.cpp @@ -0,0 +1,182 @@ +/** + * @file Create a convex decomposition of a mesh + * @brief This takes an input file and generates a convex decomposition + * + * @author Michael Ripperger + * @date Nov 22, 2023 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2023, Southwest Research Institute + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +#include +TESSERACT_COMMON_IGNORE_WARNINGS_PUSH +#include +#include +#include +#include +TESSERACT_COMMON_IGNORE_WARNINGS_POP + +#include +#include + +namespace +{ +const size_t ERROR_IN_COMMAND_LINE = 1; +const size_t SUCCESS = 0; +const size_t ERROR_UNHANDLED_EXCEPTION = 2; + +} // namespace + +template +void check_range(const T& value, const T& min, const T& max) +{ + if (value < min || value > max) + { + std::stringstream ss; + ss << "Value " << value << " is not in valid range [" << min << ", " << max << "]"; + throw std::runtime_error(ss.str()); + } +} + +int main(int argc, char** argv) +{ + std::string input; + std::string output; + tesseract_collision::VHACDParameters params; + + // clang-format off + namespace po = boost::program_options; + po::options_description desc("Options"); + desc.add_options() + ( + "help,h", + "Print help messages" + ) + ( + "input,i", + po::value(&input)->required(), + "File path to mesh used to create a convex hull." + ) + ( + "output,o", + po::value(&output)->required(), + "File path to save the generated convex hull as a ply." + ) + ( + "max_convex_hulls,n", + po::value(&(params.max_convex_hulls))->notifier([](const unsigned& val){check_range(val, 1u, 32u);}), + "Maximum number of convex hulls" + ) + ( + "resolution,r", + po::value(&(params.resolution))->notifier([](const unsigned& val){check_range(val, 1u, std::numeric_limits::max());}), + "Number of voxels to use to represent the shape" + ) + ( + "min_volume_percent_error,e", + po::value(&(params.minimum_volume_percent_error_allowed))->notifier([](const double& val){check_range(val, 0.001, 10.0);}), + "If the voxels are within this threshold percentage of the volume of the hull, we consider this a close enough approximation" + ) + ( + "max_recursion_depth,d", + po::value(&(params.max_recursion_depth)), + "Maximum recursion depth for convex decomposition improvement" + ) + ( + "shrinkwrap,s", + po::value(&(params.shrinkwrap)), + "Shrinkwrap the voxel positions to the source mesh on output" + ) + ( + "max_num_vertices,v", + po::value(&(params.max_num_vertices_per_ch))->notifier([](const unsigned& val){check_range(val, 4u, std::numeric_limits::max());}), + "Maximum number of vertices per convex hull" + ) + ( + "min_edge_length,l", + po::value(&(params.min_edge_length))->notifier([](const unsigned& val){check_range(val, 1u, std::numeric_limits::max());}), + "Once a voxel patch has an edge length of less than this value in all 3 dimensions, stop recursing" + ) + ( + "find_best_plane,p", + po::value(&(params.find_best_plane)), + "Flag for attempting to split planes along best location (experimental)" + ); + // clang-format on + + po::variables_map vm; + try + { + po::store(po::parse_command_line(argc, argv, desc), vm); // can throw + + /** --help option */ + if (vm.count("help") != 0U) + { + std::cout << "Basic Command Line Parameter App" << std::endl << desc << std::endl; + return SUCCESS; + } + + po::notify(vm); // throws on error, so do after help in case + // there are any problems + } + catch (po::error& e) + { + std::cerr << "ERROR: " << e.what() << std::endl << std::endl; + std::cerr << desc << std::endl; + return ERROR_IN_COMMAND_LINE; + } + + std::ifstream file(input, std::ios::binary | std::ios::ate); + std::streamsize size = file.tellg(); + if (size < 0) + { + CONSOLE_BRIDGE_logError("Failed to locate input file!"); + return ERROR_UNHANDLED_EXCEPTION; + } + + tesseract_common::VectorVector3d mesh_vertices; + Eigen::VectorXi mesh_faces; + int num_faces = tesseract_collision::loadSimplePlyFile(input, mesh_vertices, mesh_faces, true); + if (num_faces < 0) + { + CONSOLE_BRIDGE_logError("Failed to read mesh from file!"); + return ERROR_UNHANDLED_EXCEPTION; + } + + tesseract_collision::ConvexDecompositionVHACD convex_decomp(params); + std::vector convex_hulls = convex_decomp.compute(mesh_vertices, mesh_faces); + + if (convex_hulls.empty()) + { + CONSOLE_BRIDGE_logError("Failed to create convex decomposition!"); + return ERROR_UNHANDLED_EXCEPTION; + } + + for (std::size_t i = 0; i < convex_hulls.size(); ++i) + { + auto ch = convex_hulls[i]; + if (!tesseract_collision::writeSimplePlyFile( + std::to_string(i) + "_" + output, *(ch->getVertices()), *(ch->getFaces()), ch->getFaceCount())) + { + CONSOLE_BRIDGE_logError("Failed to write convex hull to file!"); + return ERROR_UNHANDLED_EXCEPTION; + } + } + + return 0; +} diff --git a/tesseract_common/include/tesseract_common/macros.h b/tesseract_common/include/tesseract_common/macros.h index 7948768a1c1..fd79d718153 100644 --- a/tesseract_common/include/tesseract_common/macros.h +++ b/tesseract_common/include/tesseract_common/macros.h @@ -58,7 +58,8 @@ _Pragma("GCC diagnostic ignored \"-Wdeprecated-declarations\"") #endif -#define TESSERACT_COMMON_IGNORE_WARNINGS_POP _Pragma("GCC diagnostic pop") +#define TESSERACT_COMMON_IGNORE_WARNINGS_POP \ + _Pragma("GCC diagnostic pop") #elif defined(_MSC_VER) #define DEPRECATED(X) __declspec(deprecated(X))