diff --git a/.gitmodules b/.gitmodules index 9ab6b77be..e69de29bb 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,3 +0,0 @@ -[submodule "tools/pybind11"] - path = tools/pybind11 - url = https://github.com/pybind/pybind11.git diff --git a/tools/pybind11 b/tools/pybind11 deleted file mode 160000 index 60526d463..000000000 --- a/tools/pybind11 +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 60526d463650487ca6aa8b6e15d0522df03a4835 diff --git a/voxblox/CMakeLists.txt b/voxblox/CMakeLists.txt index ed55f2dea..0366dea19 100644 --- a/voxblox/CMakeLists.txt +++ b/voxblox/CMakeLists.txt @@ -7,11 +7,6 @@ catkin_simple() set(CMAKE_MACOSX_RPATH 0) add_definitions(-std=c++11 -Wall -Wextra) -if (EXISTS "${CMAKE_CURRENT_LIST_DIR}/../tools/pybind11/CMakeLists.txt") - set(HAVE_PYBIND11 TRUE) - message(STATUS "Found pybind11; generating Python bindings") -endif() - ############ # PROTOBUF # ############ @@ -90,35 +85,6 @@ cs_add_library(${PROJECT_NAME} ) target_link_libraries(${PROJECT_NAME} ${PROJECT_NAME}_proto ${PROTOBUF_LIBRARIES}) -################### -# PYTHON BINDINGS # -################### -if(HAVE_PYBIND11) - catkin_python_setup() - - add_subdirectory(../tools/pybind11 pybind11) - message("Building Python bindings for voxblox") - pybind11_add_module(voxbloxpy - ${${PROJECT_NAME}_SRCS} - src/pybind11/esdf_map_bind.cc - src/pybind11/tsdf_map_bind.cc - src/pybind11/esdf_integrator_bind.cc - src/pybind11/tsdf_integrator_bind.cc - src/pybind11/voxel_bind.cc - src/pybind11/layer_bind.cc - src/pybind11/block_bind.cc - src/pybind11/layer_io_bind.cc - src/pybind11/planning_utils_bind.cc - src/pybind11/bind.cc) - set_target_properties(voxbloxpy PROPERTIES LINKER_LANGUAGE CXX) - target_link_libraries(voxbloxpy PUBLIC ${PROJECT_NAME}_proto - ${PROTOBUF_LIBRARIES}) - - set_target_properties(voxbloxpy - PROPERTIES LIBRARY_OUTPUT_DIRECTORY - ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_PYTHON_DESTINATION}/../) -endif() - ############ # BINARIES # ############ diff --git a/voxblox/include/voxblox/integrator/tsdf_integrator.h b/voxblox/include/voxblox/integrator/tsdf_integrator.h index d99096030..ec46e93a6 100644 --- a/voxblox/include/voxblox/integrator/tsdf_integrator.h +++ b/voxblox/include/voxblox/integrator/tsdf_integrator.h @@ -25,6 +25,19 @@ namespace voxblox { +enum class TsdfIntegratorType : int { + kSimple = 1, + kMerged = 2, + kFast = 3, +}; + +static constexpr size_t kNumTsdfIntegratorTypes = 3u; + +const std::array + kTsdfIntegratorTypeNames = {{/*kSimple*/ "simple", + /*kMerged*/ "merged", + /*kFast*/ "fast"}}; + // Note most functions state if they are thread safe. Unless explicitly stated // otherwise, this thread safety is based on the assumption that any pointers // passed to the functions point to objects that are guaranteed to not be @@ -32,6 +45,7 @@ namespace voxblox { class TsdfIntegratorBase { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW + typedef std::shared_ptr Ptr; struct Config { EIGEN_MAKE_ALIGNED_OPERATOR_NEW @@ -134,6 +148,16 @@ class TsdfIntegratorBase { ApproxHashArray<12, std::mutex, GlobalIndex, LongIndexHash> mutexes_; }; +class TsdfIntegratorFactory { + public: + static TsdfIntegratorBase::Ptr create( + const std::string& integrator_type_name, + const TsdfIntegratorBase::Config& config, Layer* layer); + static TsdfIntegratorBase::Ptr create( + const TsdfIntegratorType integrator_type, + const TsdfIntegratorBase::Config& config, Layer* layer); +}; + class SimpleTsdfIntegrator : public TsdfIntegratorBase { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW diff --git a/voxblox/include/voxblox/utils/neighbor_tools.h b/voxblox/include/voxblox/utils/neighbor_tools.h index 5a45c1a57..73a1984e7 100644 --- a/voxblox/include/voxblox/utils/neighbor_tools.h +++ b/voxblox/include/voxblox/utils/neighbor_tools.h @@ -16,11 +16,21 @@ enum Connectivity : unsigned int { class NeighborhoodLookupTables { public: typedef Eigen::Matrix - IndexOffsets; + LongIndexOffsets; + typedef Eigen::Matrix IndexOffsets; typedef Eigen::Matrix Distances; + // Stores the distances to the 6, 18, and 26 neighborhood, in that order. + // These distances need to be scaled by the voxel distance to get metric + // distances. static const Distances kDistances; + + // Lookup table for the offsets between a index and its 6, 18, and 26 + // neighborhood, in that order. These two offset tables are the same except + // for the type, this saves casting the offset when used with either global + // index (long) or local index (int) in the neighborhood lookup. static const IndexOffsets kOffsets; + static const LongIndexOffsets kLongOffsets; }; template @@ -28,7 +38,20 @@ class Neighborhood : public NeighborhoodLookupTables { public: typedef Eigen::Matrix IndexMatrix; - static void getHierarchicalIndexFromDirection( + // Get the global index of all (6, 18, or 26) neighbors of the input index. + static void getFromGlobalIndex(const GlobalIndex& global_index, + IndexMatrix* neighbors) { + CHECK_NOTNULL(neighbors); + for (unsigned int i = 0u; i < kConnectivity; ++i) { + neighbors->col(i) = global_index + kLongOffsets.col(i); + } + } + + // Get the hierarchical index (block idx, local voxel index) a voxel described + // by its hierarchical index and a direction. The main purpose of this + // function is to solve the cross-block indexing that happens when looking up + // neighbors at the block boundaries. + static void getFromHierarchicalIndexAndDirection( const BlockIndex& block_index, const VoxelIndex& voxel_index, const SignedIndex& direction, const size_t voxels_per_side, BlockIndex* neighbor_block_index, VoxelIndex* neighbor_voxel_index) { @@ -52,6 +75,10 @@ class Neighborhood : public NeighborhoodLookupTables { } } + // Get the hierarchical indices (block idx, local voxel index) for all + // neighbors (6, 18, or 26 neighborhood) of a hierarcical index. This function + // solves the cross-block indexing that happens when looking up neighbors at + // the block boundary. static void getFromHierarchicalIndex(const BlockIndex& block_index, const VoxelIndex& voxel_index, const size_t voxels_per_side, @@ -61,17 +88,9 @@ class Neighborhood : public NeighborhoodLookupTables { AlignedVector& neighbors = *neighbors_ptr; for (unsigned int i = 0u; i < kConnectivity; ++i) { VoxelKey& neighbor = neighbors[i]; - getHierarchicalIndexFromDirection(block_index, voxel_index, - kOffsets.col(i), voxels_per_side, - &neighbor.first, &neighbor.second); - } - } - - static void getFromGlobalIndex(const GlobalIndex& global_index, - IndexMatrix* neighbors) { - CHECK_NOTNULL(neighbors); - for (unsigned int i = 0u; i < kConnectivity; ++i) { - neighbors->col(i) = global_index + kOffsets.col(i); + getFromHierarchicalIndexAndDirection(block_index, voxel_index, + kOffsets.col(i), voxels_per_side, + &neighbor.first, &neighbor.second); } } }; diff --git a/voxblox/python/voxblox/__init__.py b/voxblox/python/voxblox/__init__.py deleted file mode 100644 index bb7788c30..000000000 --- a/voxblox/python/voxblox/__init__.py +++ /dev/null @@ -1,3 +0,0 @@ -# TODO(mereweth@jpl.nasa.gov) - this is a bit convoluted but it works -# we are importing all from the shared library that we built using pybind11 -from voxbloxpy import * diff --git a/voxblox/setup.py b/voxblox/setup.py deleted file mode 100644 index bd7e58a6c..000000000 --- a/voxblox/setup.py +++ /dev/null @@ -1,12 +0,0 @@ -## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD - -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup - -# fetch values from package.xml -setup_args = generate_distutils_setup( - packages=['voxblox'], - package_dir={'': 'python'}, -) - -setup(**setup_args) diff --git a/voxblox/src/integrator/esdf_integrator.cc b/voxblox/src/integrator/esdf_integrator.cc index a2b3a4cb3..cd911ee16 100644 --- a/voxblox/src/integrator/esdf_integrator.cc +++ b/voxblox/src/integrator/esdf_integrator.cc @@ -372,6 +372,10 @@ void EsdfIntegrator::processOpenSet() { // Go through the neighbors and see if we can update any of them. for (unsigned int idx = 0u; idx < neigbor_indices.cols(); ++idx) { const GlobalIndex& neighbor_index = neigbor_indices.col(idx); + const SignedIndex& direction = + NeighborhoodLookupTables::kOffsets.col(idx); + FloatingPoint distance = + NeighborhoodLookupTables::kDistances[idx] * voxel_size_; EsdfVoxel* neighbor_voxel = esdf_layer_->getVoxelPtrByGlobalIndex(neighbor_index); @@ -385,10 +389,7 @@ void EsdfIntegrator::processOpenSet() { continue; } - SignedIndex direction = (neighbor_index - global_index).cast(); SignedIndex new_parent = -direction; - FloatingPoint distance = - direction.cast().norm() * voxel_size_; if (config_.full_euclidean_distance) { // In this case, the new parent is is actually the parent of the // current voxel. @@ -396,6 +397,7 @@ void EsdfIntegrator::processOpenSet() { new_parent = voxel->parent - direction; distance = voxel_size_ * (new_parent.cast().norm() - voxel->parent.cast().norm()); + if (distance < 0.0) { continue; } @@ -482,6 +484,8 @@ bool EsdfIntegrator::updateVoxelFromNeighbors(const GlobalIndex& global_index) { // Go through the neighbors and see if we can update any of them. for (unsigned int idx = 0u; idx < neigbor_indices.cols(); ++idx) { const GlobalIndex& neighbor_index = neigbor_indices.col(idx); + const FloatingPoint distance = + Neighborhood::kDistances[idx]; EsdfVoxel* neighbor_voxel = esdf_layer_->getVoxelPtrByGlobalIndex(neighbor_index); diff --git a/voxblox/src/integrator/tsdf_integrator.cc b/voxblox/src/integrator/tsdf_integrator.cc index dfbf32525..515531755 100644 --- a/voxblox/src/integrator/tsdf_integrator.cc +++ b/voxblox/src/integrator/tsdf_integrator.cc @@ -1,9 +1,48 @@ -#include "voxblox/integrator/tsdf_integrator.h" #include #include +#include "voxblox/integrator/tsdf_integrator.h" namespace voxblox { +TsdfIntegratorBase::Ptr TsdfIntegratorFactory::create( + const std::string& integrator_type_name, + const TsdfIntegratorBase::Config& config, Layer* layer) { + CHECK(!integrator_type_name.empty()); + + int integrator_type = 1; + for (const std::string& valid_integrator_type_name : + kTsdfIntegratorTypeNames) { + if (integrator_type_name == valid_integrator_type_name) { + return create(static_cast(integrator_type), config, + layer); + } + ++integrator_type; + } + LOG(FATAL) << "Unknown TSDF integrator type: " << integrator_type_name; +} + +TsdfIntegratorBase::Ptr TsdfIntegratorFactory::create( + const TsdfIntegratorType integrator_type, + const TsdfIntegratorBase::Config& config, Layer* layer) { + CHECK_NOTNULL(layer); + switch (integrator_type) { + case TsdfIntegratorType::kSimple: + return TsdfIntegratorBase::Ptr(new SimpleTsdfIntegrator(config, layer)); + break; + case TsdfIntegratorType::kMerged: + return TsdfIntegratorBase::Ptr(new MergedTsdfIntegrator(config, layer)); + break; + case TsdfIntegratorType::kFast: + return TsdfIntegratorBase::Ptr(new FastTsdfIntegrator(config, layer)); + break; + default: + LOG(FATAL) << "Unknown TSDF integrator type: " + << static_cast(integrator_type); + break; + } + return TsdfIntegratorBase::Ptr(); +} + // Note many functions state if they are thread safe. Unless explicitly stated // otherwise, this thread safety is based on the assumption that any pointers // passed to the functions point to objects that are guaranteed to not be diff --git a/voxblox/src/pybind11/bind.cc b/voxblox/src/pybind11/bind.cc deleted file mode 100644 index 1a12e7598..000000000 --- a/voxblox/src/pybind11/bind.cc +++ /dev/null @@ -1,30 +0,0 @@ -#include -namespace py = pybind11; - -void layer_io_bind(py::module &); -void planning_utils_bind(py::module &); -void voxel_bind(py::module &); -void block_bind(py::module &); -void layer_bind(py::module &); -void esdf_map_bind(py::module &); -void tsdf_map_bind(py::module &); -void esdf_integrator_bind(py::module &); -void tsdf_integrator_bind(py::module &); - -#include "voxblox/io/layer_io.h" -using voxblox::TsdfVoxel; -using voxblox::EsdfVoxel; -using TsdfLayer = voxblox::Layer; -using EsdfLayer = voxblox::Layer; - -PYBIND11_MODULE(voxbloxpy, m) { - voxel_bind(m); - layer_bind(m); - block_bind(m); - tsdf_map_bind(m); - esdf_map_bind(m); - tsdf_integrator_bind(m); - esdf_integrator_bind(m); - layer_io_bind(m); - planning_utils_bind(m); -} diff --git a/voxblox/src/pybind11/block_bind.cc b/voxblox/src/pybind11/block_bind.cc deleted file mode 100644 index 53c5f06e2..000000000 --- a/voxblox/src/pybind11/block_bind.cc +++ /dev/null @@ -1,61 +0,0 @@ -#include "voxblox/core/block.h" -#include "voxblox/core/voxel.h" - -#include -#include -namespace py = pybind11; - -using voxblox::TsdfVoxel; -using voxblox::EsdfVoxel; -using voxblox::FloatingPoint; -using voxblox::Point; - -using TsdfBlock = voxblox::Block; -using EsdfBlock = voxblox::Block; - -void block_bind(py::module& m) { - /* TODO(mereweth@jpl.nasa.gov) - use shared_ptr instead of default unique_ptr - * for Python reference counting? - */ - py::class_ >(m, "TsdfBlock") - .def(py::init()) - - .def_property_readonly("block_size", &TsdfBlock::block_size) - .def_property_readonly("num_voxels", &TsdfBlock::num_voxels) - .def_property_readonly("voxel_size", &TsdfBlock::voxel_size) - .def_property_readonly("voxels_per_side", &TsdfBlock::voxels_per_side) - .def_property_readonly("origin", &TsdfBlock::origin) - .def_property_readonly( - "updated", - (bool (TsdfBlock::*)(void) const) &TsdfBlock::updated) - .def_property_readonly("has_data", - (bool (TsdfBlock::*)(void) const) &TsdfBlock::has_data) - - .def("set_updated", &TsdfBlock::set_updated) - .def("set_has_data", &TsdfBlock::set_has_data) - .def("getVoxelByCoordinates", - (TsdfVoxel * (TsdfBlock::*)(const Point& coords)) & - TsdfBlock::getVoxelPtrByCoordinates, - py::return_value_policy::reference_internal); - - py::class_ >(m, "EsdfBlock") - .def(py::init()) - - .def_property_readonly("block_size", &EsdfBlock::block_size) - .def_property_readonly("num_voxels", &EsdfBlock::num_voxels) - .def_property_readonly("voxel_size", &EsdfBlock::voxel_size) - .def_property_readonly("voxels_per_side", &EsdfBlock::voxels_per_side) - .def_property_readonly("origin", &EsdfBlock::origin) - .def_property_readonly( - "updated", - (bool (EsdfBlock::*)(void) const) &EsdfBlock::updated) - .def_property_readonly("has_data", - (bool (EsdfBlock::*)(void) const) &EsdfBlock::has_data) - - .def("set_updated", &EsdfBlock::set_updated) - .def("set_has_data", &EsdfBlock::set_has_data) - .def("getVoxelByCoordinates", - (EsdfVoxel * (EsdfBlock::*)(const Point& coords)) & - EsdfBlock::getVoxelPtrByCoordinates, - py::return_value_policy::reference_internal); -} diff --git a/voxblox/src/pybind11/esdf_integrator_bind.cc b/voxblox/src/pybind11/esdf_integrator_bind.cc deleted file mode 100644 index e00ad7bcd..000000000 --- a/voxblox/src/pybind11/esdf_integrator_bind.cc +++ /dev/null @@ -1,35 +0,0 @@ -#include "voxblox/core/layer.h" -#include "voxblox/integrator/esdf_integrator.h" - -#include -#include -namespace py = pybind11; - -using voxblox::EsdfIntegrator; - -using voxblox::TsdfVoxel; -using voxblox::EsdfVoxel; -using TsdfLayer = voxblox::Layer; -using EsdfLayer = voxblox::Layer; - -void esdf_integrator_bind(py::module &m) { // NOLINT - py::class_(m, "EsdfIntegratorConfig") - .def(py::init<>()) - .def_readwrite("max_distance_m", &EsdfIntegrator::Config::max_distance_m) - .def_readwrite("min_distance_m", &EsdfIntegrator::Config::min_distance_m) - .def_readwrite("default_distance_m", - &EsdfIntegrator::Config::default_distance_m) - .def_readwrite("min_weight", &EsdfIntegrator::Config::min_weight) - .def_readwrite("num_buckets", &EsdfIntegrator::Config::num_buckets) - .def_readwrite("clear_sphere_radius", - &EsdfIntegrator::Config::clear_sphere_radius) - .def_readwrite("occupied_sphere_radius", - &EsdfIntegrator::Config::occupied_sphere_radius); - - py::class_(m, "EsdfIntegrator") - .def(py::init()) - - .def("updateFromTsdfLayerBatch", - &EsdfIntegrator::updateFromTsdfLayerBatch) - .def("updateFromTsdfLayer", &EsdfIntegrator::updateFromTsdfLayer); -} diff --git a/voxblox/src/pybind11/esdf_map_bind.cc b/voxblox/src/pybind11/esdf_map_bind.cc deleted file mode 100644 index a0f2d56ce..000000000 --- a/voxblox/src/pybind11/esdf_map_bind.cc +++ /dev/null @@ -1,41 +0,0 @@ -#include "voxblox/core/esdf_map.h" -#include "voxblox/core/layer.h" - -#include -#include -namespace py = pybind11; - -using voxblox::EsdfMap; -using voxblox::EsdfVoxel; -using EsdfLayer = voxblox::Layer; - -void esdf_map_bind(py::module &m) { - /* TODO(mereweth@jpl.nasa.gov) - use shared_ptr or default unique_ptr for - * Python reference counting? - */ - - py::class_(m, "EsdfMapConfig") - .def(py::init<>()) - .def_readwrite("voxel_size", &EsdfMap::Config::esdf_voxel_size) - .def_readwrite("voxels_per_side", &EsdfMap::Config::esdf_voxels_per_side); - - py::class_ >(m, "EsdfMap") - .def(py::init()) - .def(py::init()) - .def_property_readonly("block_size", &EsdfMap::block_size) - .def_property_readonly("voxel_size", &EsdfMap::voxel_size) - - .def("getEsdfLayer", &EsdfMap::getEsdfLayer) - - .def("getDistanceAtPosition", &EsdfMap::batchGetDistanceAtPosition) - .def("getDistanceAndGradientAtPosition", - &EsdfMap::batchGetDistanceAndGradientAtPosition) - .def("isObserved", &EsdfMap::batchIsObserved) - - .def("coordPlaneSliceGetDistance", &EsdfMap::coordPlaneSliceGetDistance, - "Evaluate distances at all allocated voxels along an axis-aligned " - "slice", - py::arg("free_plane_index"), py::arg("free_plane_val"), - py::arg("positions"), py::arg("distances"), - py::arg("max_points") = 100000u); -} diff --git a/voxblox/src/pybind11/layer_bind.cc b/voxblox/src/pybind11/layer_bind.cc deleted file mode 100644 index 916ad2302..000000000 --- a/voxblox/src/pybind11/layer_bind.cc +++ /dev/null @@ -1,44 +0,0 @@ -#include "voxblox/core/layer.h" - -#include -#include -namespace py = pybind11; - -using voxblox::TsdfVoxel; -using voxblox::EsdfVoxel; -using voxblox::FloatingPoint; - -using TsdfLayer = voxblox::Layer; -using EsdfLayer = voxblox::Layer; - -void layer_bind(py::module &m) { - /* TODO(mereweth@jpl.nasa.gov) - use shared_ptr instead of default unique_ptr - * for Python reference counting? - */ - py::class_ >(m, "TsdfLayer") - .def(py::init()) - - .def_property_readonly("block_size", &TsdfLayer::block_size) - .def_property_readonly("voxel_size", &TsdfLayer::voxel_size) - .def_property_readonly("voxels_per_side", &TsdfLayer::voxels_per_side) - - .def("saveToFile", (bool (TsdfLayer::*)(const std::string &) const) & - TsdfLayer::saveToFile) - - .def("allocateBlockPtrByCoordinates", - &TsdfLayer::allocateBlockPtrByCoordinates) - .def("removeBlockByCoordinates", &TsdfLayer::removeBlockByCoordinates); - - py::class_ >(m, "EsdfLayer") - .def(py::init()) - - .def_property_readonly("block_size", &EsdfLayer::block_size) - .def_property_readonly("voxel_size", &EsdfLayer::voxel_size) - .def_property_readonly("voxels_per_side", &EsdfLayer::voxels_per_side) - - .def("saveToFile", (bool (EsdfLayer::*)(const std::string &) const) & - EsdfLayer::saveToFile) - .def("allocateBlockPtrByCoordinates", - &EsdfLayer::allocateBlockPtrByCoordinates) - .def("removeBlockByCoordinates", &EsdfLayer::removeBlockByCoordinates); -} diff --git a/voxblox/src/pybind11/layer_io_bind.cc b/voxblox/src/pybind11/layer_io_bind.cc deleted file mode 100644 index 15ffe0860..000000000 --- a/voxblox/src/pybind11/layer_io_bind.cc +++ /dev/null @@ -1,35 +0,0 @@ -#include "voxblox/io/layer_io.h" - -#include -namespace py = pybind11; - -using voxblox::TsdfVoxel; -using voxblox::EsdfVoxel; -using TsdfLayer = voxblox::Layer; -using EsdfLayer = voxblox::Layer; - -void layer_io_bind(py::module &m) { - m.def("loadTsdfLayer", [](const std::string &file) { - if (file.empty()) { - throw std::runtime_error(std::string("Empty file path: ") + file); - } - TsdfLayer::Ptr layer_from_file; - if (!voxblox::io::LoadLayer(file, &layer_from_file)) { - throw std::runtime_error(std::string("Could not load layer from: ") + - file); - } - return layer_from_file; - }); - - m.def("loadEsdfLayer", [](const std::string &file) { - if (file.empty()) { - throw std::runtime_error(std::string("Empty file path: ") + file); - } - EsdfLayer::Ptr layer_from_file; - if (!voxblox::io::LoadLayer(file, &layer_from_file)) { - throw std::runtime_error(std::string("Could not load layer from: ") + - file); - } - return layer_from_file; - }); -} diff --git a/voxblox/src/pybind11/planning_utils_bind.cc b/voxblox/src/pybind11/planning_utils_bind.cc deleted file mode 100644 index 6e3e155c4..000000000 --- a/voxblox/src/pybind11/planning_utils_bind.cc +++ /dev/null @@ -1,31 +0,0 @@ -#include "voxblox/utils/planning_utils.h" - -#include -#include -namespace py = pybind11; - -using voxblox::EsdfVoxel; -using voxblox::FloatingPoint; -using voxblox::Point; -using EsdfLayer = voxblox::Layer; -using voxblox::utils::fillSphereAroundPoint; -using voxblox::utils::clearSphereAroundPoint; - -void planning_utils_bind(py::module& m) { - m.def("fillSphereAroundPoint", - [](const Point& center, const FloatingPoint radius, - const float max_distance_m, EsdfLayer* layer) { - if (!layer) { - throw std::runtime_error(std::string("Empty layer")); - } - fillSphereAroundPoint(center, radius, max_distance_m, layer); - }); - m.def("clearSphereAroundPoint", - [](const Point& center, const float max_distance_m, - const FloatingPoint radius, EsdfLayer* layer) { - if (!layer) { - throw std::runtime_error(std::string("Empty layer")); - } - clearSphereAroundPoint(center, radius, max_distance_m, layer); - }); -} diff --git a/voxblox/src/pybind11/tsdf_integrator_bind.cc b/voxblox/src/pybind11/tsdf_integrator_bind.cc deleted file mode 100644 index 26ae6e3fd..000000000 --- a/voxblox/src/pybind11/tsdf_integrator_bind.cc +++ /dev/null @@ -1,20 +0,0 @@ -#include "voxblox/core/layer.h" -#include "voxblox/integrator/tsdf_integrator.h" - -#include -#include -namespace py = pybind11; - -using voxblox::TsdfIntegratorBase; -using voxblox::SimpleTsdfIntegrator; - -using voxblox::TsdfVoxel; -using TsdfLayer = voxblox::Layer; - -void tsdf_integrator_bind(py::module &m) { - py::class_(m, "TsdfIntegratorConfig") - .def(py::init<>()); - - py::class_(m, "SimpleTsdfIntegrator") - .def(py::init()); -} diff --git a/voxblox/src/pybind11/tsdf_map_bind.cc b/voxblox/src/pybind11/tsdf_map_bind.cc deleted file mode 100644 index 84e3b8b50..000000000 --- a/voxblox/src/pybind11/tsdf_map_bind.cc +++ /dev/null @@ -1,37 +0,0 @@ -#include "voxblox/core/layer.h" -#include "voxblox/core/tsdf_map.h" - -#include -#include -namespace py = pybind11; - -using voxblox::TsdfMap; -using voxblox::TsdfVoxel; -using TsdfLayer = voxblox::Layer; - -void tsdf_map_bind(py::module &m) { - /* TODO(mereweth@jpl.nasa.gov) - use shared_ptr or default unique_ptr for - * Python reference counting? - */ - - py::class_(m, "TsdfMapConfig") - .def(py::init<>()) - .def_readwrite("voxel_size", &TsdfMap::Config::tsdf_voxel_size) - .def_readwrite("voxels_per_side", &TsdfMap::Config::tsdf_voxels_per_side); - - py::class_ >(m, "TsdfMap") - .def(py::init()) - .def(py::init()) - .def_property_readonly("block_size", &TsdfMap::block_size) - .def_property_readonly("voxel_size", &TsdfMap::voxel_size) - - .def("getTsdfLayer", &TsdfMap::getTsdfLayer) - - .def("coordPlaneSliceGetDistanceWeight", - &TsdfMap::coordPlaneSliceGetDistanceWeight, - "Evaluate distances at all allocated voxels along an axis-aligned " - "slice", - py::arg("free_plane_index"), py::arg("free_plane_val"), - py::arg("positions"), py::arg("distances"), py::arg("weights"), - py::arg("max_points") = 100000u); -} diff --git a/voxblox/src/pybind11/voxel_bind.cc b/voxblox/src/pybind11/voxel_bind.cc deleted file mode 100644 index 4a5db09a9..000000000 --- a/voxblox/src/pybind11/voxel_bind.cc +++ /dev/null @@ -1,28 +0,0 @@ -#include "voxblox/core/voxel.h" - -#include -#include -namespace py = pybind11; - -using voxblox::TsdfVoxel; -using voxblox::EsdfVoxel; - -void voxel_bind(py::module &m) { - /* TODO(mereweth@jpl.nasa.gov) - use shared_ptr instead of default unique_ptr - * for Python reference counting? - */ - py::class_(m, "TsdfVoxel") - .def(py::init<>()) - - .def_readwrite("distance", &TsdfVoxel::distance) - .def_readwrite("weight", &TsdfVoxel::weight); - - py::class_(m, "EsdfVoxel") - .def(py::init<>()) - - .def_readwrite("distance", &EsdfVoxel::distance) - .def_readwrite("observed", &EsdfVoxel::observed) - .def_readwrite("in_queue", &EsdfVoxel::in_queue) - .def_readwrite("fixed", &EsdfVoxel::fixed) - .def_readwrite("parent", &EsdfVoxel::parent); -} diff --git a/voxblox/src/utils/neighbor_tools.cc b/voxblox/src/utils/neighbor_tools.cc index d0738a2b4..e28d311a3 100644 --- a/voxblox/src/utils/neighbor_tools.cc +++ b/voxblox/src/utils/neighbor_tools.cc @@ -26,6 +26,10 @@ const NeighborhoodLookupTables::IndexOffsets NeighborhoodLookupTables::kOffsets 0, 0, 0, 0, -1, 1, 0, 0, 0, 0, -1, 1, -1, 1, -1, -1, 1, 1, -1, 1, -1, 1, -1, 1, -1, 1; return directions_matrix; }(); + +const NeighborhoodLookupTables::LongIndexOffsets NeighborhoodLookupTables::kLongOffsets = [] { + return NeighborhoodLookupTables::kOffsets.cast(); +}(); // clang-format on } // namespace voxblox diff --git a/voxblox/test/test_sdf_integrators.cc b/voxblox/test/test_sdf_integrators.cc index 8b894025c..fef8dd76a 100644 --- a/voxblox/test/test_sdf_integrators.cc +++ b/voxblox/test/test_sdf_integrators.cc @@ -246,6 +246,7 @@ TEST_P(SdfIntegratorsTest, EsdfIntegrators) { // Figure out some metrics to compare against, based on voxel size. constexpr FloatingPoint kFloatingPointToleranceHigh = 1e-4; constexpr FloatingPoint kKindaSimilar = 1e-2; + constexpr FloatingPoint kCloseEnough = 1.0; // Make sure they're all reasonable. EXPECT_NEAR(incremental_result.min_error, 0.0, kFloatingPointToleranceHigh); @@ -268,7 +269,7 @@ TEST_P(SdfIntegratorsTest, EsdfIntegrators) { batch_full_euclidean_result.num_overlapping_voxels); EXPECT_NEAR(incremental_result.rmse, batch_result.rmse, kKindaSimilar); EXPECT_NEAR(incremental_result.max_error, batch_result.max_error, - kKindaSimilar); + kCloseEnough); // Output for debugging. io::SaveLayer(tsdf_layer, "esdf_euclidean_test.voxblox", true); diff --git a/voxblox/test/test_voxbloxpy.py b/voxblox/test/test_voxbloxpy.py deleted file mode 100644 index 42f2a516a..000000000 --- a/voxblox/test/test_voxbloxpy.py +++ /dev/null @@ -1,116 +0,0 @@ -#!/usr/bin/env python - -import numpy as np - -import timeit - -import sys -sys.path.append( - '/Users/mereweth/snappy/tsdf_catkin_ws/devel/.private/voxblox/lib/') - -import voxblox -dir(voxblox.EsdfMap) - -try: - layer = voxblox.loadEsdfLayer('THIS_DOES_NOT_EXIST') -except RuntimeError as e: - print(e) - -# make sure this throws an exception rather than segfaulting -# try: - # voxblox_tango_interfacepy.tangoLoadLayer("/Users/mereweth/Desktop/cow_and_lady/cow_and_lady.tsdf.proto") -# except RuntimeError as e: - # print(e) - -# convert Tango TSDF to TSDF and serialize -#ntl = voxblox_tango_interfacepy.tangoLoadLayer("/Users/mereweth/Desktop/map.ntsdf.proto") -# ntl.saveToFile("/Users/mereweth/Desktop/map.tsdf.proto") - -# make sure this throws an exception rather than segfaulting -try: - voxblox.loadTsdfLayer("/Users/mereweth/Desktop/map.ntsdf.proto") -except RuntimeError as e: - print(e) - -tl = voxblox.loadTsdfLayer( - "/Users/mereweth/Desktop/environments/bldg_344_huge/map_cleared.tsdf.proto") -el = voxblox.EsdfLayer(tl.voxel_size, tl.voxels_per_side) -m = voxblox.EsdfMap(el) -ei = voxblox.EsdfIntegrator(voxblox.EsdfIntegratorConfig(), tl, el) -ei.updateFromTsdfLayerBatch() - -b = tl.allocateBlockPtrByCoordinates(np.array([0, 0, 0.5], dtype='double')) -v = b.getVoxelByCoordinates(np.array([0, 0, 0.5], dtype='double')) - -v.distance = 0.3 -b.set_updated(True) - -max_distance_m = 2.0 -voxblox.clearSphereAroundPoint( - np.array([0, 0, 0.5], dtype='double'), 0.5, max_distance_m, el) -voxblox.fillSphereAroundPoint( - np.array([0, 0, 0.5], dtype='double'), 0.5, max_distance_m, el) - -el.saveToFile("/Users/mereweth/Desktop/_test_cow_and_lady.esdf.proto") - -layer = voxblox.loadEsdfLayer( - '/Users/mereweth/Desktop/environments/bldg_344_huge/map_cleared.esdf.proto') -assert(layer is not None) -map = voxblox.EsdfMap(layer) - -print map.voxel_size -print map.block_size - -import numpy as np - -x_ = np.linspace(0.0, 0.2, 100) -y_ = np.linspace(0.0, 0.2, 100) -z_ = np.linspace(0.0, 0.2, 100) -x, y, z = np.meshgrid(x_, y_, z_) -query = np.matrix(np.c_[x.flatten(), y.flatten(), z.flatten()]).T - -# query = np.matrix([[0,0,0.1], -# [0.1,0,0], -# [0.1,0.1,0], -# [0,0.1,0]], dtype='double').T - -grad = np.matrix(np.zeros(np.shape(query), dtype='double')) - -dist = np.matrix(np.zeros((np.shape(query)[1], 1), dtype='double')) - -obs = np.matrix(np.zeros((np.shape(query)[1], 1), dtype='int32')) - -map.isObserved(query, obs) -map.getDistanceAtPosition(query, dist, obs) - - -def interp_fun(): - map.getDistanceAndGradientAtPosition(query, dist, grad, obs) - - -interp_duration = timeit.timeit(interp_fun, number=10) / 10.0 -print(interp_duration) - -num_pts = 1000 * 1000 -slice_pos = np.matrix(np.zeros((3, num_pts))) -slice_dist = np.matrix(np.zeros((np.shape(slice_pos)[1], 1), dtype='double')) - - -def no_interp_fun(): - map.coordPlaneSliceGetDistance(2, # xy plane - 0.5, # z - slice_pos, - slice_dist, - num_pts) - - -no_interp_duration = timeit.timeit(no_interp_fun, number=10) / 10.0 -print(no_interp_duration) - -try: - import IPython - IPython.embed() - -except BaseException: - import code - code.interact(local=dict(globals(), **locals())) diff --git a/voxblox_ros/include/voxblox_ros/tsdf_server.h b/voxblox_ros/include/voxblox_ros/tsdf_server.h index 720d1ee90..6d9163d2e 100644 --- a/voxblox_ros/include/voxblox_ros/tsdf_server.h +++ b/voxblox_ros/include/voxblox_ros/tsdf_server.h @@ -96,6 +96,16 @@ class TsdfServer { double getSliceLevel() const { return slice_level_; } void setSliceLevel(double slice_level) { slice_level_ = slice_level; } + bool setPublishSlices() const { return publish_slices_; } + void setPublishSlices(const bool publish_slices) { + publish_slices_ = publish_slices; + } + + void setWorldFrame(const std::string& world_frame) { + world_frame_ = world_frame; + } + std::string getWorldFrame() const { return world_frame_; } + // CLEARS THE ENTIRE MAP! virtual void clear(); diff --git a/voxblox_ros/src/tsdf_server.cc b/voxblox_ros/src/tsdf_server.cc index becc84ad3..74a8e796d 100644 --- a/voxblox_ros/src/tsdf_server.cc +++ b/voxblox_ros/src/tsdf_server.cc @@ -430,6 +430,7 @@ void TsdfServer::integratePointcloud(const Transformation& T_G_C, const Pointcloud& ptcloud_C, const Colors& colors, const bool is_freespace_pointcloud) { + CHECK_EQ(ptcloud_C.size(), colors.size()); tsdf_integrator_->integratePointCloud(T_G_C, ptcloud_C, colors, is_freespace_pointcloud); } diff --git a/voxblox_tango_interface/CMakeLists.txt b/voxblox_tango_interface/CMakeLists.txt deleted file mode 100644 index 71c087642..000000000 --- a/voxblox_tango_interface/CMakeLists.txt +++ /dev/null @@ -1,122 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(voxblox_tango_interface) - -find_package(catkin_simple REQUIRED) -catkin_simple() - -set(CMAKE_MACOSX_RPATH 0) -add_definitions(-std=c++11 -Wall -Wextra) - -if (EXISTS "${CMAKE_CURRENT_LIST_DIR}/../tools/pybind11/CMakeLists.txt") - set(HAVE_PYBIND11 TRUE) - message(STATUS "Found pybind11; generating Python bindings") -endif() - -############ -# PROTOBUF # -############ -# General idea: first check if we have protobuf catkin, then use that. -# Otherwise use system protobuf. -set(PROTO_DEFNS proto/tsdf2/MapHeader.proto - proto/tsdf2/Volume.proto) -set(ADDITIONAL_LIBRARIES "") - -find_package(protobuf_catkin QUIET) -if (protobuf_catkin_FOUND) - message(STATUS "Using protobuf_catkin") - list(APPEND catkin_INCLUDE_DIRS ${protobuf_catkin_INCLUDE_DIRS}) - list(APPEND catkin_LIBRARIES ${protobuf_catkin_LIBRARIES}) - include_directories(${CMAKE_CURRENT_BINARY_DIR}) - - PROTOBUF_CATKIN_GENERATE_CPP(PROTO_SRCS PROTO_HDRS ${PROTO_DEFNS}) - set(ADDITIONAL_LIBRARIES ${protobuf_catkin_LIBRARIES}) -else() - message(STATUS "Using system protobuf") - find_package(Protobuf REQUIRED) - include_directories(${PROTOBUF_INCLUDE_DIRS}) - include_directories(${CMAKE_CURRENT_BINARY_DIR}) - - PROTOBUF_GENERATE_CPP(PROTO_SRCS PROTO_HDRS ${PROTO_DEFNS}) - set(ADDITIONAL_LIBRARIES ${PROTOBUF_LIBRARY}) -endif() - -#################### -# SET SOURCE FILES # -#################### - -set("${PROJECT_NAME}_SRCS" - src/core/tango_block_interface.cc -) - -############# -# LIBRARIES # -############# -# NOTE(mereweth@jpl.nasa.gov) - Be careful when compiling Proto cc files. It is -# best to do so only once (make a shared library for each set of Proto files. -# Otherwise, at some point, you will get errors from double-adding protobuf -# formats, of the following form: -# [libprotobuf ERROR google/protobuf/descriptor_database.cc:57] -# File already exists in database: Block.proto - -# Avoid having multiple compiled copies of the same .pb.cc -# file sharing a single copy of libprotobuf.so -cs_add_library(${PROJECT_NAME}_proto - ${PROTO_SRCS} -) -target_link_libraries(${PROJECT_NAME}_proto ${PROTOBUF_LIBRARIES}) - -cs_add_library(${PROJECT_NAME} - ${${PROJECT_NAME}_SRCS} -) -target_link_libraries(${PROJECT_NAME} ${PROJECT_NAME}_proto ${PROTOBUF_LIBRARIES} pthread) - -################### -# PYTHON BINDINGS # -################### - -if(HAVE_PYBIND11) - catkin_python_setup() - - add_subdirectory(../tools/pybind11 pybind11) - message("Building Python bindings for voxblox") - pybind11_add_module(voxblox_tango_interfacepy - ${${PROJECT_NAME}_SRCS} - src/pybind11/bind.cc - ../voxblox/src/pybind11/layer_bind.cc - src/pybind11/tango_layer_interface_bind.cc - src/pybind11/tango_layer_io_bind.cc) - set_target_properties(voxblox_tango_interfacepy PROPERTIES LINKER_LANGUAGE CXX) - target_link_libraries(voxblox_tango_interfacepy PUBLIC ${PROJECT_NAME}_proto - ${PROTOBUF_LIBRARIES}) - - set_target_properties(voxblox_tango_interfacepy - PROPERTIES LIBRARY_OUTPUT_DIRECTORY - ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_PYTHON_DESTINATION}/../ - ) -endif() - -############ -# BINARIES # -############ - -add_executable(mesh_tango_tsdf - test/mesh_tango_tsdf.cc -) -target_link_libraries(mesh_tango_tsdf ${PROJECT_NAME}) - -add_executable(tango_tsdf_resize_to_tsdf - test/tango_tsdf_resize_to_tsdf.cc -) -target_link_libraries(tango_tsdf_resize_to_tsdf ${PROJECT_NAME}) - -add_executable(tango_tsdf_to_esdf - test/tango_tsdf_to_esdf.cc -) -target_link_libraries(tango_tsdf_to_esdf ${PROJECT_NAME}) - -########## -# EXPORT # -########## -cs_install() -cs_export(INCLUDE_DIRS include ${CMAKE_CURRENT_BINARY_DIR} - LIBRARIES ${ADDITIONAL_LIBRARIES}) diff --git a/voxblox_tango_interface/include/voxblox_tango_interface/core/ntsdf_voxel.h b/voxblox_tango_interface/include/voxblox_tango_interface/core/ntsdf_voxel.h deleted file mode 100644 index 7242d39c2..000000000 --- a/voxblox_tango_interface/include/voxblox_tango_interface/core/ntsdf_voxel.h +++ /dev/null @@ -1,40 +0,0 @@ -#ifndef VOXBLOX_TANGO_INTERFACE_CORE_VOXEL_H_ -#define VOXBLOX_TANGO_INTERFACE_CORE_VOXEL_H_ - -#include -#include - -#include "voxblox/core/common.h" -#include "voxblox/core/color.h" -#include "voxblox/core/voxel.h" - -namespace voxblox { - -/* NOTE(mereweth@jpl.nasa.gov) - This voxel type is not used for now. - * The only implemented functionality is to load the Tango serialized NTSDF into - * a Layer - */ -struct NtsdfVoxel { - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - uint32_t ntsdf = 0; - uint32_t color = 0; - - /* TODO(mereweth@jpl.nasa.gov) - should we use 16-bit fields - * with attribute packed instead? - */ -}; - -// Used for serialization only. -namespace voxel_types { - const std::string kNtsdf = "ntsdf"; -} // namespace voxel_types - -template <> -inline std::string getVoxelType() { - return voxel_types::kNtsdf; -} - -} // namespace voxblox - -#endif // VOXBLOX_TANGO_INTERFACE_CORE_VOXEL_H_ diff --git a/voxblox_tango_interface/include/voxblox_tango_interface/core/tango_block_interface.h b/voxblox_tango_interface/include/voxblox_tango_interface/core/tango_block_interface.h deleted file mode 100644 index 31b3c984b..000000000 --- a/voxblox_tango_interface/include/voxblox_tango_interface/core/tango_block_interface.h +++ /dev/null @@ -1,45 +0,0 @@ -#ifndef VOXBLOX_TANGO_INTERFACE_CORE_BLOCK_H_ -#define VOXBLOX_TANGO_INTERFACE_CORE_BLOCK_H_ - -#include "voxblox/core/block.h" -#include "voxblox/core/voxel.h" - -#include "./Volume.pb.h" - -namespace voxblox { - -class TangoBlockInterface : public Block { - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - TangoBlockInterface(const size_t voxels_per_side, - const FloatingPoint voxel_size, - const Point& origin, - const unsigned int max_ntsdf_voxel_weight, - const FloatingPoint meters_to_ntsdf) - : Block(voxels_per_side, voxel_size, origin), - max_ntsdf_voxel_weight_(max_ntsdf_voxel_weight), - meters_to_ntsdf_(meters_to_ntsdf) {} - - /* The audit flag allows converting a Tango NTSDF dump to TSDF without - * scaling the distances or weights. This is a debugging feature. - */ - TangoBlockInterface(const tsdf2::VolumeProto& proto, - const unsigned int max_ntsdf_voxel_weight, - const FloatingPoint meters_to_ntsdf, - const bool audit = false); - - private: - void deserializeProto(const tsdf2::VolumeProto& proto); - void deserializeFromIntegers(const AlignedVector& data, - const bool audit = false); - - unsigned int max_ntsdf_voxel_weight_; - FloatingPoint meters_to_ntsdf_; -}; - -} // namespace voxblox - -#include "voxblox_tango_interface/core/tango_block_interface_inl.h" - -#endif // VOXBLOX_TANGO_INTERFACE_CORE_BLOCK_H_ diff --git a/voxblox_tango_interface/include/voxblox_tango_interface/core/tango_block_interface_inl.h b/voxblox_tango_interface/include/voxblox_tango_interface/core/tango_block_interface_inl.h deleted file mode 100644 index 857722828..000000000 --- a/voxblox_tango_interface/include/voxblox_tango_interface/core/tango_block_interface_inl.h +++ /dev/null @@ -1,58 +0,0 @@ -#ifndef VOXBLOX_TANGO_INTERFACE_CORE_BLOCK_INL_H_ -#define VOXBLOX_TANGO_INTERFACE_CORE_BLOCK_INL_H_ - -#include "./Volume.pb.h" - -/* TODO(mereweth@jpl.nasa.gov) - this is not a template class, so no need to - * have these definitions here - */ - -namespace voxblox { - -inline TangoBlockInterface::TangoBlockInterface( - const tsdf2::VolumeProto& proto, const unsigned int max_ntsdf_voxel_weight, - const FloatingPoint meters_to_ntsdf, const bool audit) - : TangoBlockInterface(proto.voxels_per_side(), proto.voxel_size(), - proto.has_origin() - ? Point(proto.origin().x(), proto.origin().y(), - proto.origin().z()) - : - /* NOTE(mereweth@jpl.nasa.gov) - origin field - * seems to be deprecated - * as of 2017/06/15 - * Without it, loading of old TSDF2 dumps is - * broken, so we - * pass it if present in the protobuf dump - */ - Point(proto.index().x() * proto.voxel_size() * - proto.voxels_per_side(), - proto.index().y() * proto.voxel_size() * - proto.voxels_per_side(), - proto.index().z() * proto.voxel_size() * - proto.voxels_per_side()), - max_ntsdf_voxel_weight, meters_to_ntsdf) { - has_data_ = proto.has_data(); - - // Convert the data into a vector of integers. - AlignedVector data; - data.reserve(proto.ntsdf_voxels_size()); - - auto ntsdf_word = proto.ntsdf_voxels().begin(); - auto color_word = proto.color_voxels().begin(); - for (; ntsdf_word != proto.ntsdf_voxels().end() && - color_word != proto.color_voxels().end(); - ++ntsdf_word, ++color_word) { - data.push_back(*ntsdf_word); - data.push_back(*color_word); - } - if (ntsdf_word != proto.ntsdf_voxels().end() || - color_word != proto.color_voxels().end()) { - LOG(FATAL) << "Unequal number of tsdf and color voxels in Tango TSDF"; - } - - deserializeFromIntegers(data, audit); -} - -} // namespace voxblox - -#endif // VOXBLOX_TANGO_INTERFACE_CORE_BLOCK_INL_H_ diff --git a/voxblox_tango_interface/include/voxblox_tango_interface/core/tango_layer_interface.h b/voxblox_tango_interface/include/voxblox_tango_interface/core/tango_layer_interface.h deleted file mode 100644 index 90c75c54f..000000000 --- a/voxblox_tango_interface/include/voxblox_tango_interface/core/tango_layer_interface.h +++ /dev/null @@ -1,49 +0,0 @@ -#ifndef VOXBLOX_TANGO_INTERFACE_CORE_LAYER_H_ -#define VOXBLOX_TANGO_INTERFACE_CORE_LAYER_H_ - -#include "voxblox/core/layer.h" - -#include "./MapHeader.pb.h" -#include "./Volume.pb.h" - -#include "voxblox_tango_interface/core/tango_block_interface.h" - -namespace voxblox { - -class TangoLayerInterface : public Layer { - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - // NOTE(mereweth@jpl.nasa.gov) - need this typedef - typedef std::shared_ptr Ptr; - - explicit TangoLayerInterface(const tsdf2::MapHeaderProto& proto); - - explicit TangoLayerInterface(const FloatingPoint voxel_size, - const size_t voxels_per_side, - const unsigned int max_ntsdf_voxel_weight, - const FloatingPoint meters_to_ntsdf) - : Layer(voxel_size, voxels_per_side), - max_ntsdf_voxel_weight_(max_ntsdf_voxel_weight), - meters_to_ntsdf_(meters_to_ntsdf) {} - - bool isCompatible(const tsdf2::MapHeaderProto& layer_proto) const; - bool isCompatible(const tsdf2::VolumeProto& block_proto) const; - - /* The audit flag allows converting a Tango NTSDF dump to TSDF without - * scaling the distances or weights. This is a debugging feature. - */ - bool addBlockFromProto(const tsdf2::VolumeProto& block_proto, - const TangoLayerInterface::BlockMergingStrategy strategy, - const bool audit = false); - - private: - unsigned int max_ntsdf_voxel_weight_; - FloatingPoint meters_to_ntsdf_; -}; - -} // namespace voxblox - -#include "voxblox_tango_interface/core/tango_layer_interface_inl.h" - -#endif // VOXBLOX_TANGO_INTERFACE_CORE_LAYER_H_ diff --git a/voxblox_tango_interface/include/voxblox_tango_interface/core/tango_layer_interface_inl.h b/voxblox_tango_interface/include/voxblox_tango_interface/core/tango_layer_interface_inl.h deleted file mode 100644 index 2154e5258..000000000 --- a/voxblox_tango_interface/include/voxblox_tango_interface/core/tango_layer_interface_inl.h +++ /dev/null @@ -1,124 +0,0 @@ -#ifndef VOXBLOX_TANGO_INTERFACE_CORE_LAYER_INL_H_ -#define VOXBLOX_TANGO_INTERFACE_CORE_LAYER_INL_H_ - -#include "./MapHeader.pb.h" -#include "./Volume.pb.h" - -#include -#include - -#define VOXBLOX_TANGO_LAYER_INTERFACE_VOXEL_EPS (1e-8f) - -namespace voxblox { - -inline TangoLayerInterface::TangoLayerInterface( - const tsdf2::MapHeaderProto& proto) - : Layer(proto.voxel_size(), proto.voxels_per_volume_side()), - max_ntsdf_voxel_weight_(proto.max_ntsdf_voxel_weight()), - meters_to_ntsdf_(proto.meters_to_ntsdf()) { - // Derived config parameter. - block_size_ = voxel_size_ * voxels_per_side_; - block_size_inv_ = 1.0 / block_size_; - - LOG(INFO) << "Meters to NTSDF: " << meters_to_ntsdf_ << "\t" - << "Max NTSDF voxel weight: " << max_ntsdf_voxel_weight_ << "\n"; - - CHECK_GT(proto.voxel_size(), 0.0); - CHECK_GT(proto.voxels_per_volume_side(), 0u); -} - -inline bool TangoLayerInterface::addBlockFromProto( - const tsdf2::VolumeProto& block_proto, - const TangoLayerInterface::BlockMergingStrategy strategy, - const bool audit) { - CHECK_EQ(getType().compare(voxel_types::kTsdf), 0) - << "The voxel type of this layer is not TsdfVoxel!"; - - if (isCompatible(block_proto)) { - TangoBlockInterface::Ptr block_ptr(new TangoBlockInterface( - block_proto, max_ntsdf_voxel_weight_, meters_to_ntsdf_, audit)); - - const BlockIndex block_index = getGridIndexFromOriginPoint( - block_ptr->origin(), block_size_inv_); - switch (strategy) { - case TangoLayerInterface::BlockMergingStrategy::kProhibit: - CHECK_EQ(block_map_.count(block_index), 0u) - << "Block collision at index: " << block_index; - block_map_[block_index] = block_ptr; - break; - case TangoLayerInterface::BlockMergingStrategy::kReplace: - block_map_[block_index] = block_ptr; - break; - case TangoLayerInterface::BlockMergingStrategy::kDiscard: - block_map_.insert(std::make_pair(block_index, block_ptr)); - break; - case TangoLayerInterface::BlockMergingStrategy::kMerge: { - typename BlockHashMap::iterator it = block_map_.find(block_index); - if (it == block_map_.end()) { - block_map_[block_index] = block_ptr; - } else { - it->second->mergeBlock(*block_ptr); - } - } break; - default: - LOG(FATAL) << "Unknown BlockMergingStrategy: " - << static_cast(strategy); - return false; - } - // Mark that this block has been updated. - block_map_[block_index]->updated() = true; - } else { - LOG(ERROR) - << "The blocks from this protobuf are not compatible with this layer!"; - return false; - } - return true; -} - -inline bool TangoLayerInterface::isCompatible( - const tsdf2::MapHeaderProto& layer_proto) const { - bool compatible = true; - compatible &= (fabs(layer_proto.voxel_size() - voxel_size_) <= - VOXBLOX_TANGO_LAYER_INTERFACE_VOXEL_EPS); - // NOTE(mereweth@jpl.nasa.gov) - MapHeader voxels_per_volume_side is a double - compatible &= (fabs(layer_proto.voxels_per_volume_side() - - static_cast(voxels_per_side_)) <= - VOXBLOX_TANGO_LAYER_INTERFACE_VOXEL_EPS); - - if (!compatible) { - LOG(INFO) << std::setprecision(10) - << "TangoLayerInterface voxel_size: " << voxel_size_ - << ", voxels_per_side: " << voxels_per_side_; - LOG(INFO) << std::setprecision(10) - << "MapHeaderProto voxel_size: " << layer_proto.voxel_size() - << ", voxels_per_volume_side: " - << layer_proto.voxels_per_volume_side(); - } - - return compatible; -} - -inline bool TangoLayerInterface::isCompatible( - const tsdf2::VolumeProto& block_proto) const { - bool compatible = true; - compatible &= (fabs(block_proto.voxel_size() - voxel_size_) <= - VOXBLOX_TANGO_LAYER_INTERFACE_VOXEL_EPS); - // NOTE(mereweth@jpl.nasa.gov) - Volume voxels_per_side is an int32 - compatible &= - (block_proto.voxels_per_side() == static_cast(voxels_per_side_)); - - if (!compatible) { - LOG(INFO) << std::setprecision(10) - << "TangoLayerInterface voxel_size: " << voxel_size_ - << ", voxels_per_side: " << voxels_per_side_; - LOG(INFO) << std::setprecision(10) - << "VolumeProto voxel_size: " << block_proto.voxel_size() - << ", voxels_per_side: " << block_proto.voxels_per_side(); - } - - return compatible; -} - -} // namespace voxblox - -#endif // VOXBLOX_TANGO_INTERFACE_CORE_LAYER_INL_H_ diff --git a/voxblox_tango_interface/include/voxblox_tango_interface/io/tango_layer_io.h b/voxblox_tango_interface/include/voxblox_tango_interface/io/tango_layer_io.h deleted file mode 100644 index 6aeecb06e..000000000 --- a/voxblox_tango_interface/include/voxblox_tango_interface/io/tango_layer_io.h +++ /dev/null @@ -1,170 +0,0 @@ -#ifndef VOXBLOX_TANGO_INTERFACE_CORE_IO_TANGO_LAYER_IO_H_ -#define VOXBLOX_TANGO_INTERFACE_CORE_IO_TANGO_LAYER_IO_H_ - -#include - -#include - -#include "./Volume.pb.h" -#include "./MapHeader.pb.h" -#include "voxblox/core/common.h" -#include "voxblox/utils/protobuf_utils.h" - -#include "voxblox_tango_interface/core/tango_layer_interface.h" - -namespace voxblox { -namespace io { - -/* NOTE(mereweth@jpl.nasa.gov) - when adding functions here, inline them - * or split into hpp/cpp - */ - -/* TODO(mereweth@jpl.nasa.gov) - if we load into NTSDF voxel later, what to call - * those functions? - */ - -inline bool TangoLoadLayer(const std::string& file_path, - TangoLayerInterface::Ptr* layer_ptr, - const bool audit = false) { - CHECK_NOTNULL(layer_ptr); - - // Open and check the file - std::fstream proto_file; - proto_file.open(file_path, std::fstream::in); - if (!proto_file.is_open()) { - LOG(ERROR) << "Could not open protobuf file to load layer: " << file_path; - return false; - } - - // Unused byte offset result. - uint32_t tmp_byte_offset; - - // Get number of messages - uint32_t num_protos; - if (!utils::readProtoMsgCountToStream(&proto_file, &num_protos, - &tmp_byte_offset)) { - LOG(ERROR) << "Could not read number of messages."; - return false; - } - - if (num_protos == 0u) { - LOG(WARNING) << "Empty protobuf file!"; - return false; - } - - // TODO(mereweth@jpl.nasa.gov) - how to check if compatible? - - // Get header and create the layer if compatible - tsdf2::MapHeaderProto layer_proto; - if (!utils::readProtoMsgFromStream(&proto_file, &layer_proto, - &tmp_byte_offset)) { - LOG(ERROR) << "Could not read layer protobuf message."; - return false; - } - - if ((layer_proto.voxel_size() <= 0.0f) || - (layer_proto.voxels_per_volume_side() <= 0.0f) || - (layer_proto.max_ntsdf_voxel_weight() <= 0u) || - (layer_proto.meters_to_ntsdf() <= 0.0f)) { - LOG(ERROR) - << "Invalid parameter in layer protobuf message. Check the format."; - return false; - } - - *layer_ptr = aligned_shared(layer_proto); - CHECK(*layer_ptr); - - // Read all blocks and add them to the layer. - const size_t num_blocks = num_protos - 1; - for (uint32_t block_idx = 0u; block_idx < num_blocks; ++block_idx) { - tsdf2::VolumeProto block_proto; - if (!utils::readProtoMsgFromStream(&proto_file, &block_proto, - &tmp_byte_offset)) { - LOG(ERROR) << "Could not read block protobuf message number " - << block_idx; - return false; - } - - // TODO(mereweth@jpl.nasa.gov) - how to check if compatible? - - if (block_proto.has_data()) { - if (!(*layer_ptr) - ->addBlockFromProto( - block_proto, - Layer::BlockMergingStrategy::kProhibit, audit)) { - LOG(ERROR) << "Could not add the block protobuf message to the layer!"; - return false; - } - } - } - - return true; -} - -inline bool TangoLoadBlocksFromFile( - const std::string& file_path, - const Layer::BlockMergingStrategy strategy, - TangoLayerInterface* layer_ptr, const bool audit = false) { - CHECK_NOTNULL(layer_ptr); - - // Open and check the file - std::fstream proto_file; - proto_file.open(file_path, std::fstream::in); - if (!proto_file.is_open()) { - LOG(ERROR) << "Could not open protobuf file to load layer: " << file_path; - return false; - } - - // Unused byte offset result. - uint32_t tmp_byte_offset; - - // Get number of messages - uint32_t num_protos; - if (!utils::readProtoMsgCountToStream(&proto_file, &num_protos, - &tmp_byte_offset)) { - LOG(ERROR) << "Could not read number of messages."; - return false; - } - - if (num_protos == 0u) { - LOG(WARNING) << "Empty protobuf file!"; - return false; - } - - // Get header and check if it is compatible with existing layer. - tsdf2::MapHeaderProto layer_proto; - if (!utils::readProtoMsgFromStream(&proto_file, &layer_proto, - &tmp_byte_offset)) { - LOG(ERROR) << "Could not read layer protobuf message."; - return false; - } - if (!layer_ptr->isCompatible(layer_proto)) { - LOG(ERROR) << "The layer information read from file is not compatible with " - "the current layer!"; - return false; - } - - // Read all blocks and add them to the layer. - const size_t num_blocks = num_protos - 1; - for (uint32_t block_idx = 0u; block_idx < num_blocks; ++block_idx) { - tsdf2::VolumeProto block_proto; - if (!utils::readProtoMsgFromStream(&proto_file, &block_proto, - &tmp_byte_offset)) { - LOG(ERROR) << "Could not read block protobuf message number " - << block_idx; - return false; - } - - if (!layer_ptr->addBlockFromProto(block_proto, strategy, audit)) { - LOG(ERROR) << "Could not add the block protobuf message to the layer!"; - return false; - } - } - return true; -} - -} // namespace io - -} // namespace voxblox - -#endif // VOXBLOX_TANGO_INTERFACE_CORE_IO_TANGO_LAYER_IO_H_ diff --git a/voxblox_tango_interface/package.xml b/voxblox_tango_interface/package.xml deleted file mode 100644 index 109091677..000000000 --- a/voxblox_tango_interface/package.xml +++ /dev/null @@ -1,18 +0,0 @@ - - - voxblox_tango_interface - 0.0.0 - Voxblox Tango interface - - Gene Merewether - - Gene Merewether - - BSD - - catkin - catkin_simple - - gflags_catkin - voxblox - diff --git a/voxblox_tango_interface/proto/tsdf2/MapHeader.proto b/voxblox_tango_interface/proto/tsdf2/MapHeader.proto deleted file mode 100644 index 20df45694..000000000 --- a/voxblox_tango_interface/proto/tsdf2/MapHeader.proto +++ /dev/null @@ -1,10 +0,0 @@ -package tsdf2; - -message MapHeaderProto { - optional double voxel_size = 1; - optional double voxels_per_volume_side = 2; - optional uint32 max_ntsdf_voxel_weight = 3; - optional bool use_color = 4; - optional double meters_to_ntsdf = 5; - optional uint32 num_volumes = 6; -} diff --git a/voxblox_tango_interface/proto/tsdf2/Volume.proto b/voxblox_tango_interface/proto/tsdf2/Volume.proto deleted file mode 100644 index ff7f33253..000000000 --- a/voxblox_tango_interface/proto/tsdf2/Volume.proto +++ /dev/null @@ -1,49 +0,0 @@ -package tsdf2; - -// TODO(mereweth@jpl.nasa.gov) - include this or verify ok to omit -//import "engine_data_proto/Vector3d.proto"; - -message VolumeProto { - message GridIndex { - required int32 x = 1; - required int32 y = 2; - required int32 z = 3; - }; - - // TODO(mereweth@jpl.nasa.gov) - include this or verify ok to omit - message Vector3dProto { - required double x = 1; - required double y = 2; - required double z = 3; - } - - // Number of voxels per each dimension. - optional int32 voxels_per_side = 1; - - // Size of each voxel, in meters. - optional double voxel_size = 2; - - // Maximum voxel weight - optional uint32 max_ntsdf_voxel_weight = 3; - - // TODO(mereweth@jpl.nasa.gov) - include this or verify ok to omit - // DEPRECATED: The origin of the volume, in meters. - //optional engine_data_proto.Vector3dProto origin = 4 [deprecated=true]; - optional Vector3dProto origin = 4 [deprecated=true]; - - // The grid index of the volume. - optional GridIndex index = 8; - - // If the volume has received at least one update. - optional bool has_data = 5; - - // Voxel data is in x -> y -> z major order. - // The first 16 bits of the voxel are its signed distance function, the - // second 16 bits are its weight. - repeated uint32 ntsdf_voxels = 6 [packed=true]; - - // Color data is in x -> y -> z major order. - // The first 24 bits are red, green and blue bytes. The last 8 bits are a - // weight. - repeated uint32 color_voxels = 7 [packed=true]; -} diff --git a/voxblox_tango_interface/python/voxblox_tango_interface/__init__.py b/voxblox_tango_interface/python/voxblox_tango_interface/__init__.py deleted file mode 100644 index 4a0528638..000000000 --- a/voxblox_tango_interface/python/voxblox_tango_interface/__init__.py +++ /dev/null @@ -1 +0,0 @@ -from voxblox_tango_interfacepy import * diff --git a/voxblox_tango_interface/setup.py b/voxblox_tango_interface/setup.py deleted file mode 100644 index 7c916e9cc..000000000 --- a/voxblox_tango_interface/setup.py +++ /dev/null @@ -1,12 +0,0 @@ -## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD - -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup - -# fetch values from package.xml -setup_args = generate_distutils_setup( - packages=['voxblox_tango_interface'], - package_dir={'': 'python'}, -) - -setup(**setup_args) diff --git a/voxblox_tango_interface/src/core/tango_block_interface.cc b/voxblox_tango_interface/src/core/tango_block_interface.cc deleted file mode 100644 index 6090fc965..000000000 --- a/voxblox_tango_interface/src/core/tango_block_interface.cc +++ /dev/null @@ -1,41 +0,0 @@ -#include "voxblox_tango_interface/core/tango_block_interface.h" - -#include - -namespace voxblox { - -void TangoBlockInterface::deserializeFromIntegers( - const AlignedVector& data, const bool audit) { - constexpr size_t kNumDataPacketsPerVoxel = 2u; - const size_t num_data_packets = data.size(); - - CHECK_EQ(num_voxels_ * kNumDataPacketsPerVoxel, num_data_packets); - for (size_t voxel_idx = 0u, data_idx = 0u; - voxel_idx < num_voxels_ && data_idx < num_data_packets; - ++voxel_idx, data_idx += kNumDataPacketsPerVoxel) { - const uint32_t bytes_1 = data[data_idx]; - const uint32_t bytes_2 = data[data_idx + 1u]; - - TsdfVoxel& voxel = voxels_[voxel_idx]; - - // TODO(mereweth@jpl.nasa.gov) - is this the best way to unpack NTSDF? - - if (audit) { - voxel.distance = static_cast(bytes_1 >> 16); - voxel.weight = static_cast(bytes_1 & 0x0000FFFF); - } else { - voxel.distance = static_cast(bytes_1 >> 16) / meters_to_ntsdf_; - - voxel.weight = static_cast(max_ntsdf_voxel_weight_) / - static_cast(UINT16_MAX) * - static_cast(bytes_1 & 0x0000FFFF); - } - - voxel.color.r = static_cast(bytes_2 >> 24); - voxel.color.g = static_cast((bytes_2 & 0x00FF0000) >> 16); - voxel.color.b = static_cast((bytes_2 & 0x0000FF00) >> 8); - voxel.color.a = static_cast(bytes_2 & 0x000000FF); - } -} - -} // namespace voxblox diff --git a/voxblox_tango_interface/src/pybind11/bind.cc b/voxblox_tango_interface/src/pybind11/bind.cc deleted file mode 100644 index 21d5da056..000000000 --- a/voxblox_tango_interface/src/pybind11/bind.cc +++ /dev/null @@ -1,14 +0,0 @@ -#include -namespace py = pybind11; - -void layer_bind(py::module &); -void tango_layer_interface_bind(py::module &); -void tango_layer_io_bind(py::module &); - -PYBIND11_MODULE(voxblox_tango_interfacepy, m) { - // NOTE(mereweth@jpl.nasa.gov) - this could come from voxblox - layer_bind(m); - - tango_layer_interface_bind(m); - tango_layer_io_bind(m); -} diff --git a/voxblox_tango_interface/src/pybind11/tango_layer_interface_bind.cc b/voxblox_tango_interface/src/pybind11/tango_layer_interface_bind.cc deleted file mode 100644 index 810076677..000000000 --- a/voxblox_tango_interface/src/pybind11/tango_layer_interface_bind.cc +++ /dev/null @@ -1,29 +0,0 @@ -#include "voxblox/core/layer.h" -#include "voxblox_tango_interface/core/tango_layer_interface.h" - -#include -namespace py = pybind11; - -// NOTE(mereweth@jpl.nasa.gov) - this is to get the base class functions -using voxblox::TsdfVoxel; -using TsdfLayer = voxblox::Layer; -using voxblox::FloatingPoint; - -using voxblox::TangoLayerInterface; - -void tango_layer_interface_bind(py::module& m) { - // NOTE(mereweth@jpl.nasa.gov) - second template arg is to get the base class - // methods - py::class_ >(m, "TangoLayerInterface") - .def(py::init()) - - .def_property_readonly("block_size", &TangoLayerInterface::block_size) - .def_property_readonly("voxel_size", &TangoLayerInterface::voxel_size) - .def_property_readonly("voxels_per_side", - &TangoLayerInterface::voxels_per_side) - - .def("saveToFile", - (bool (TangoLayerInterface::*)(const std::string&) const) & - TangoLayerInterface::saveToFile); -} diff --git a/voxblox_tango_interface/src/pybind11/tango_layer_io_bind.cc b/voxblox_tango_interface/src/pybind11/tango_layer_io_bind.cc deleted file mode 100644 index 07628d715..000000000 --- a/voxblox_tango_interface/src/pybind11/tango_layer_io_bind.cc +++ /dev/null @@ -1,23 +0,0 @@ -#include "voxblox_tango_interface/io/tango_layer_io.h" - -#include -namespace py = pybind11; - -using voxblox::TangoLayerInterface; -using voxblox::io::TangoLoadLayer; - -void tango_layer_io_bind(py::module &m) { - m.def("loadTangoLayer", [](const std::string &file, const bool audit) { - if (file.empty()) { - throw std::runtime_error(std::string("Empty file path: ") + file); - } - TangoLayerInterface::Ptr layer_from_file; - if (!TangoLoadLayer(file, &layer_from_file, audit)) { - throw std::runtime_error(std::string("Could not load layer from: ") + - file); - } - return *layer_from_file; - }, - "Load Tango NTSDF layer from protobuf; convert to TSDF layer", - py::arg("file_path"), py::arg("audit") = false); -} diff --git a/voxblox_tango_interface/test/mesh_tango_tsdf.cc b/voxblox_tango_interface/test/mesh_tango_tsdf.cc deleted file mode 100644 index 6996ea8f0..000000000 --- a/voxblox_tango_interface/test/mesh_tango_tsdf.cc +++ /dev/null @@ -1,49 +0,0 @@ -#include // NOLINT - -#include -#include - -#include "voxblox_tango_interface/core/tango_layer_interface.h" -#include "voxblox_tango_interface/io/tango_layer_io.h" - -using namespace voxblox; // NOLINT - -int main(int argc, char** argv) { - google::InitGoogleLogging(argv[0]); - - if (argc != 3) { - throw std::runtime_error( - "Args: filename to load, followed by filename to save to"); - } - - const std::string file = argv[1]; - - TangoLayerInterface::Ptr layer_from_file; - io::TangoLoadLayer(file, &layer_from_file); - - LOG(INFO) << "Layer memory size: " << layer_from_file->getMemorySize(); - LOG(INFO) << "Layer voxel size: " << layer_from_file->voxel_size(); - LOG(INFO) << "Layer voxels per side: " << layer_from_file->voxels_per_side(); - - // Mesh accessories. - MeshIntegratorConfig mesh_config; - std::shared_ptr mesh_layer_; - std::unique_ptr > mesh_integrator_; - - mesh_layer_.reset(new MeshLayer(layer_from_file->block_size())); - mesh_integrator_.reset(new MeshIntegrator( - mesh_config, layer_from_file.get(), mesh_layer_.get())); - - constexpr bool only_mesh_updated_blocks = false; - constexpr bool clear_updated_flag = true; - mesh_integrator_->generateMesh(only_mesh_updated_blocks, clear_updated_flag); - LOG(INFO) << "Number of meshes: " - << mesh_layer_->getNumberOfAllocatedMeshes(); - const bool mesh_success = outputMeshLayerAsPly(argv[2], *mesh_layer_); - - if (mesh_success == false) { - throw std::runtime_error("Failed to save mesh"); - } - - return 0; -} diff --git a/voxblox_tango_interface/test/tango_tsdf_resize_to_tsdf.cc b/voxblox_tango_interface/test/tango_tsdf_resize_to_tsdf.cc deleted file mode 100644 index 84acb1b2f..000000000 --- a/voxblox_tango_interface/test/tango_tsdf_resize_to_tsdf.cc +++ /dev/null @@ -1,55 +0,0 @@ -#include // NOLINT -#include // NOLINT - -#include - -#include "voxblox/io/layer_io.h" -#include "voxblox_tango_interface/core/tango_layer_interface.h" -#include "voxblox_tango_interface/io/tango_layer_io.h" - -using namespace voxblox; // NOLINT - -int main(int argc, char** argv) { - google::InitGoogleLogging(argv[0]); - - if ((argc != 5) && (argc != 3)) { - throw std::runtime_error( - std::string("Args: filename to load, filename to save to\n") + - "Optional args: new voxel size, new # voxels per block side"); - } - - const std::string file = argv[1]; - bool tsdf_success = false; - - TangoLayerInterface::Ptr layer_from_file; - io::TangoLoadLayer(file, &layer_from_file); - - LOG(INFO) << "Layer memory size: " << layer_from_file->getMemorySize(); - LOG(INFO) << "Old layer voxel size: " << layer_from_file->voxel_size(); - LOG(INFO) << "Old layer voxels per side: " - << layer_from_file->voxels_per_side(); - - if (argc == 5) { - const FloatingPoint voxel_size = std::stof(argv[3]); - const size_t voxels_per_side = std::stoi(argv[4]); - - Layer::Ptr resized_layer = - std::make_shared >(voxel_size, voxels_per_side); - LOG(INFO) << "New layer voxel size: " << resized_layer->voxel_size(); - LOG(INFO) << "New layer voxels per side: " - << resized_layer->voxels_per_side(); - mergeLayerAintoLayerB(*layer_from_file, resized_layer.get()); - LOG(INFO) << "New layer memory size after merge: " - << resized_layer->getMemorySize(); - - tsdf_success = io::SaveLayer(*resized_layer, argv[2]); - } else { - tsdf_success = io::SaveLayer(*layer_from_file, argv[2]); - } - - if (tsdf_success == false) { - throw std::runtime_error("Failed to save TSDF"); - } - - return 0; -} diff --git a/voxblox_tango_interface/test/tango_tsdf_to_esdf.cc b/voxblox_tango_interface/test/tango_tsdf_to_esdf.cc deleted file mode 100644 index d811fe272..000000000 --- a/voxblox_tango_interface/test/tango_tsdf_to_esdf.cc +++ /dev/null @@ -1,60 +0,0 @@ -#include // NOLINT - -#include -#include - -#include "voxblox_tango_interface/io/tango_layer_io.h" -#include "voxblox_tango_interface/core/tango_layer_interface.h" - -using namespace voxblox; // NOLINT - -int main(int argc, char** argv) { - google::InitGoogleLogging(argv[0]); - - if (argc != 7) { - throw std::runtime_error( - std::string("Args: filename to load, filename to save to") + - ", min weight, min fixed distance" + - ", max esdf distance, default esdf distance"); - } - - const std::string file = argv[1]; - const FloatingPoint min_weight = std::stof(argv[3]); - const FloatingPoint min_distance_m = std::stof(argv[4]); - const FloatingPoint max_distance_m = std::stof(argv[5]); - const FloatingPoint default_distance_m = std::stof(argv[6]); - - TangoLayerInterface::Ptr layer_from_file; - io::TangoLoadLayer(file, &layer_from_file); - - LOG(INFO) << "Layer memory size: " << layer_from_file->getMemorySize(); - LOG(INFO) << "Layer voxel size: " << layer_from_file->voxel_size(); - LOG(INFO) << "Layer voxels per side: " << layer_from_file->voxels_per_side(); - - // ESDF maps. - EsdfMap::Config esdf_config; - // Same number of voxels per side for ESDF as with TSDF - esdf_config.esdf_voxels_per_side = layer_from_file->voxels_per_side(); - // Same voxel size for ESDF as with TSDF - esdf_config.esdf_voxel_size = layer_from_file->voxel_size(); - - EsdfIntegrator::Config esdf_integrator_config; - esdf_integrator_config.min_weight = min_weight; - esdf_integrator_config.min_distance_m = min_distance_m; - esdf_integrator_config.max_distance_m = max_distance_m; - esdf_integrator_config.default_distance_m = default_distance_m; - - EsdfMap esdf_map(esdf_config); - EsdfIntegrator esdf_integrator(esdf_integrator_config, layer_from_file.get(), - esdf_map.getEsdfLayerPtr()); - - esdf_integrator.updateFromTsdfLayerBatchFullEuclidean(); - - const bool esdf_success = io::SaveLayer(esdf_map.getEsdfLayer(), argv[2]); - - if (esdf_success == false) { - throw std::runtime_error("Failed to save ESDF"); - } - - return 0; -}