Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Added application for performing convex decomposition #968

Merged
24 changes: 24 additions & 0 deletions tesseract_collision/vhacd/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -23,6 +23,30 @@ target_include_directories(
${PROJECT_NAME}_vhacd_convex_decomposition PUBLIC "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
"$<INSTALL_INTERFACE:include>")

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}
182 changes: 182 additions & 0 deletions tesseract_collision/vhacd/src/create_convex_decomposition.cpp
Original file line number Diff line number Diff line change
@@ -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/macros.h>
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
#include <console_bridge/console.h>
#include <boost/program_options.hpp>
#include <iostream>
#include <fstream>
TESSERACT_COMMON_IGNORE_WARNINGS_POP

#include <tesseract_collision/core/common.h>
#include <tesseract_collision/vhacd/convex_decomposition_vhacd.h>

namespace
{
const size_t ERROR_IN_COMMAND_LINE = 1;
const size_t SUCCESS = 0;
const size_t ERROR_UNHANDLED_EXCEPTION = 2;

} // namespace

template <typename T>
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<std::string>(&input)->required(),
"File path to mesh used to create a convex hull."
)
(
"output,o",
po::value<std::string>(&output)->required(),
"File path to save the generated convex hull as a ply."
)
(
"max_convex_hulls,n",
po::value<unsigned>(&(params.max_convex_hulls))->notifier([](const unsigned& val){check_range(val, 1u, 32u);}),
"Maximum number of convex hulls"
)
(
"resolution,r",
po::value<unsigned>(&(params.resolution))->notifier([](const unsigned& val){check_range(val, 1u, std::numeric_limits<unsigned>::max());}),
"Number of voxels to use to represent the shape"
)
(
"min_volume_percent_error,e",
po::value<double>(&(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<unsigned>(&(params.max_recursion_depth)),
"Maximum recursion depth for convex decomposition improvement"
)
(
"shrinkwrap,s",
po::value<bool>(&(params.shrinkwrap)),
"Shrinkwrap the voxel positions to the source mesh on output"
)
(
"max_num_vertices,v",
po::value<unsigned>(&(params.max_num_vertices_per_ch))->notifier([](const unsigned& val){check_range(val, 4u, std::numeric_limits<unsigned>::max());}),
"Maximum number of vertices per convex hull"
)
(
"min_edge_length,l",
po::value<unsigned>(&(params.min_edge_length))->notifier([](const unsigned& val){check_range(val, 1u, std::numeric_limits<unsigned>::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<bool>(&(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<tesseract_geometry::ConvexMesh::Ptr> 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;
}
3 changes: 2 additions & 1 deletion tesseract_common/include/tesseract_common/macros.h
Original file line number Diff line number Diff line change
@@ -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))