diff --git a/beluga_vdb/CMakeLists.txt b/beluga_vdb/CMakeLists.txt new file mode 100644 index 000000000..c305ddf67 --- /dev/null +++ b/beluga_vdb/CMakeLists.txt @@ -0,0 +1,131 @@ +# Copyright 2024 Ekumen, Inc. +# +# 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 +# +# 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. + +cmake_minimum_required(VERSION 3.16) + +project(beluga_vdb VERSION 1.0) + +set(CMAKE_EXPORT_COMPILE_COMMANDS ON) + +set(OPENVDB_CMAKE_MODULE_PATH + ${OPENVDB_CMAKE_MODULE_PATH} + CACHE PATH "Path to OpenVDB CMake module") + +if(NOT CMAKE_BUILD_TYPE) + message(STATUS "Setting build type to 'Release' as none was specified.") + set(CMAKE_BUILD_TYPE + "Release" + CACHE STRING "Build type" FORCE) +endif() + +find_package(beluga REQUIRED) + +find_package(Eigen3 REQUIRED NO_MODULE) +find_package(range-v3 REQUIRED) +find_package(Sophus REQUIRED) + +if(NOT OPENVDB_CMAKE_MODULE_PATH) + file( + GLOB_RECURSE + OPENVDB_MODULES + /usr/lib/*/FindOpenVDB.cmake + /usr/local/lib/*/FindOpenVDB.cmake) + list(LENGTH OPENVDB_MODULES NUM_OPENVDB_MODULES) + if(NUM_OPENVDB_MODULES EQUAL 1) + list( + GET + OPENVDB_MODULES + 0 + OPENVDB_MODULE) + get_filename_component(OPENVDB_CMAKE_MODULE_PATH ${OPENVDB_MODULE} + DIRECTORY) + endif() + unset(NUM_OPENVDB_MODULES) + unset(OPENVDB_MODULES) +endif() + +if(OPENVDB_CMAKE_MODULE_PATH) + list(APPEND CMAKE_MODULE_PATH ${OPENVDB_CMAKE_MODULE_PATH}) +endif() + +find_package(OpenVDB REQUIRED) + +if(OPENVDB_CMAKE_MODULE_PATH) + list(POP_BACK CMAKE_MODULE_PATH) +endif() + +add_library(${PROJECT_NAME} INTERFACE) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + target_compile_options( + ${PROJECT_NAME} + INTERFACE -Wall + -Wconversion + -Wextra + -Werror + -Wpedantic) +endif() +if(CMAKE_BUILD_TYPE MATCHES "Debug") + target_compile_options(${PROJECT_NAME} INTERFACE -fno-inline) +endif() + +target_include_directories( + ${PROJECT_NAME} + INTERFACE $ + $) +target_link_libraries(${PROJECT_NAME} INTERFACE beluga::beluga OpenVDB::openvdb) +target_compile_features(${PROJECT_NAME} INTERFACE cxx_std_17) +target_compile_definitions(${PROJECT_NAME} INTERFACE EIGEN_NO_DEBUG + SOPHUS_USE_BASIC_LOGGING) + +add_executable(clang_tidy_findable) +target_sources(clang_tidy_findable PRIVATE src/clang_tidy_findable.cpp) +target_link_libraries(clang_tidy_findable PRIVATE ${PROJECT_NAME}) + +option(BUILD_TESTING "Build the testing tree." ON) +if(BUILD_TESTING) + message(STATUS "Build testing enabled.") + enable_testing() + add_subdirectory(test) +endif() + +install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME}) + +install( + TARGETS ${PROJECT_NAME} + EXPORT ${PROJECT_NAME}Targets + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin) + +set(INSTALL_CMAKEDIR ${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/cmake) + +install( + EXPORT ${PROJECT_NAME}Targets + FILE ${PROJECT_NAME}Targets.cmake + NAMESPACE ${PROJECT_NAME}:: + DESTINATION ${INSTALL_CMAKEDIR}) + +include(CMakePackageConfigHelpers) +configure_package_config_file( + ${CMAKE_CURRENT_SOURCE_DIR}/cmake/Config.cmake.in + "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}Config.cmake" + INSTALL_DESTINATION ${INSTALL_CMAKEDIR}) +write_basic_package_version_file( + ${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}ConfigVersion.cmake + COMPATIBILITY SameMajorVersion) + +install(FILES "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}Config.cmake" + "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}ConfigVersion.cmake" + DESTINATION ${INSTALL_CMAKEDIR}) diff --git a/beluga_vdb/cmake/Config.cmake.in b/beluga_vdb/cmake/Config.cmake.in new file mode 100644 index 000000000..eddfa6d28 --- /dev/null +++ b/beluga_vdb/cmake/Config.cmake.in @@ -0,0 +1,42 @@ +@PACKAGE_INIT@ + +include(CMakeFindDependencyMacro) +find_dependency(Eigen3 REQUIRED NO_MODULE) +find_dependency(range-v3 REQUIRED) +find_dependency(Sophus REQUIRED) +find_dependency(beluga REQUIRED) + +set(OPENVDB_CMAKE_MODULE_PATH + ${OPENVDB_CMAKE_MODULE_PATH} + CACHE PATH "Path to OpenVDB CMake module") + +if(NOT OPENVDB_CMAKE_MODULE_PATH) + file( + GLOB_RECURSE + OPENVDB_MODULES + /usr/lib/*/FindOpenVDB.cmake + /usr/local/lib/*/FindOpenVDB.cmake) + list(LENGTH OPENVDB_MODULES NUM_OPENVDB_MODULES) + if(NUM_OPENVDB_MODULES EQUAL 1) + list(GET OPENVDB_MODULES 0 OPENVDB_MODULE) + get_filename_component(OPENVDB_CMAKE_MODULE_PATH ${OPENVDB_MODULE} DIRECTORY) + endif() + unset(NUM_OPENVDB_MODULES) + unset(OPENVDB_MODULES) +endif() + +if(OPENVDB_CMAKE_MODULE_PATH) + list(APPEND CMAKE_MODULE_PATH ${OPENVDB_CMAKE_MODULE_PATH}) +endif() + +find_dependency(OpenVDB REQUIRED) + +if(OPENVDB_CMAKE_MODULE_PATH) + list(POP_BACK CMAKE_MODULE_PATH) +endif() + +include("${CMAKE_CURRENT_LIST_DIR}/@PROJECT_NAME@Targets.cmake") + +set(@PROJECT_NAME@_LIBRARIES @PROJECT_NAME@::@PROJECT_NAME@) + +check_required_components($@PROJECT_NAME@) diff --git a/beluga_vdb/include/beluga_vdb/sensor/likelihood_field_model3.hpp b/beluga_vdb/include/beluga_vdb/sensor/likelihood_field_model3.hpp new file mode 100644 index 000000000..b841f2a75 --- /dev/null +++ b/beluga_vdb/include/beluga_vdb/sensor/likelihood_field_model3.hpp @@ -0,0 +1,162 @@ +// Copyright 2024 Ekumen, Inc. +// +// 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 +// +// 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. + +#ifndef BELUGA_VDB_SENSOR_LIKELIHOOD_FIELD_MODEL3_HPP +#define BELUGA_VDB_SENSOR_LIKELIHOOD_FIELD_MODEL3_HPP + +#include +#include +#include +#include + +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +/** + * \file + * \brief Implementation of a likelihood field sensor model for 3D Lidars. + */ + +namespace beluga_vdb { + +/// Parameters used to construct a LikelihoodFieldModel3 instance. +/** + * See Probabilistic Robotics \cite thrun2005probabilistic Chapter 6.4, particularly Table 6.3. + */ +struct LikelihoodFieldModel3Param { + /// Maximum distance to obstacle. + /** + * When creating a distance map, if the distance to an obstacle is higher than the value specified here, + * then this value will be used. + */ + double max_obstacle_distance = 100.0; + /// Maximum range of a laser ray. + double max_laser_distance = 2.0; + /// Weight used to combine the probability of hitting an obstacle. + double z_hit = 0.5; + /// Weight used to combine the probability of random noise in perception. + double z_random = 0.5; + /// Standard deviation of a gaussian centered arounds obstacles. + /** + * Used to calculate the probability of the obstacle being hit. + */ + double sigma_hit = 0.2; +}; + +/// Likelihood field sensor model for range finders. +/** + * This model relies on a pre-computed likelihood map of the environment. + * It is less computationally intensive than the beluga::BeamSensorModel + * because no ray-tracing is required, and it can also provide better + * performance in environments with non-smooth occupation maps. See + * Probabilistic Robotics \cite thrun2005probabilistic, Chapter 6.4, + * for further reference. + * + * \note This class satisfies \ref SensorModelPage. + * + * \tparam OpenVDB grid type. + */ +template +class LikelihoodFieldModel3 { + public: + /// State type of a particle. + using state_type = Sophus::SE3d; + + /// Weight type of the particle. + using weight_type = double; + + /// Measurement type given by the interface. + using measurement_type = PointCloud; + + /// Map representation type. + using map_type = GridT; + + /// Parameter type that the constructor uses to configure the likelihood field model. + using param_type = LikelihoodFieldModel3Param; + + /// Constructs a LikelihoodFieldModel3 instance. + /** + * \param params Parameters to configure this instance. + * See beluga::LikelihoodFieldModel3Param for details. + * \param grid Narrow band Level set grid representing the static map that the sensor model + * uses to compute a likelihood field for lidar hits and compute importance weights + * for particle states. + * Currently only supports OpenVDB Level sets. + */ + explicit LikelihoodFieldModel3(const param_type& params, const map_type& grid) + : params_{params}, + grid_{openvdb::gridPtrCast(grid.deepCopyGrid())}, + accessor_{grid_->getConstAccessor()}, + transform_{grid_->transform()}, + background_{grid_->background()}, + two_squared_sigma_{2 * params.sigma_hit * params.sigma_hit}, + amplitude_{params.z_hit / (params.sigma_hit * std::sqrt(2 * Sophus::Constants::pi()))}, + offset_{params.z_random / params.max_laser_distance} { + openvdb::initialize(); + /// Pre-computed parameters + assert(two_squared_sigma_ > 0.0); + assert(amplitude_ > 0.0); + } + + /// Returns a state weighting function conditioned on 3D lidar hits. + /** + * \param measurement 3D lidar measurement containing the hit points and the transform to the origin. + * \return a state weighting function satisfying \ref StateWeightingFunctionPage + * and borrowing a reference to this sensor model (and thus their lifetime are bound). + */ + [[nodiscard]] auto operator()(measurement_type&& measurement) const { + // Transform each point from the sensor frame to the origin frame + auto transformed_points = measurement.points() | ranges::views::transform([&](const auto& point) { + return measurement.origin() * point.template cast(); + }) | + ranges::to(); + + return [this, points = std::move(transformed_points)](const state_type& state) -> weight_type { + return ranges::fold_left( + points | // + ranges::views::transform([this, &state](const auto& point) { + const Eigen::Vector3d point_in_state_frame = state * point; + const openvdb::math::Coord ijk = transform_.worldToIndexCellCentered( + openvdb::math::Vec3d(point_in_state_frame.x(), point_in_state_frame.y(), point_in_state_frame.z())); + const auto distance = accessor_.isValueOn(ijk) ? accessor_.getValue(ijk) : background_; + return amplitude_ * std::exp(-(distance * distance) / two_squared_sigma_) + offset_; + }), + 1.0, std::plus{}); + }; + } + + private: + param_type params_; + const typename map_type::Ptr grid_; + const typename map_type::ConstAccessor accessor_; + const openvdb::math::Transform transform_; + const typename map_type::ValueType background_; + double two_squared_sigma_; + double amplitude_; + double offset_; +}; + +} // namespace beluga_vdb + +#endif diff --git a/beluga_vdb/package.xml b/beluga_vdb/package.xml new file mode 100644 index 000000000..e3aa54baa --- /dev/null +++ b/beluga_vdb/package.xml @@ -0,0 +1,38 @@ + + + + beluga_vdb + 1.0.0 + + A beluga extension to facilitate the use of OpenVDB. + + Gerardo Puga + Ivan Paunovic + Nahuel Espinosa + + Apache License 2.0 + + cmake + + beluga + eigen + libboost-iostreams-dev + libboost-thread-dev + libopenvdb-dev + range-v3 + sophus + tbb + + clang-format + clang-tidy + gtest + libgmock-dev + + doxygen + graphviz + texlive-latex-base + + + cmake + + diff --git a/beluga_vdb/src/clang_tidy_findable.cpp b/beluga_vdb/src/clang_tidy_findable.cpp new file mode 100644 index 000000000..607e856e6 --- /dev/null +++ b/beluga_vdb/src/clang_tidy_findable.cpp @@ -0,0 +1,27 @@ +// Copyright 2022-2023 Ekumen, Inc. +// +// 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 +// +// 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. + +/* + * This file exists in this header-only library as a workaround for + * clang-tidy to find and process the headers, since all the source files + * inside the test folder are ignored. + * It will not be exported or installed with the rest of the targets of this + * library. + */ + +#include + +int main() { + return 0; +} diff --git a/beluga_vdb/test/CMakeLists.txt b/beluga_vdb/test/CMakeLists.txt new file mode 100644 index 000000000..7f972fd5f --- /dev/null +++ b/beluga_vdb/test/CMakeLists.txt @@ -0,0 +1,31 @@ +# Copyright 2024 Ekumen, Inc. +# +# 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 +# +# 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. + +find_package(GTest MODULE) +if(NOT TARGET GTest::gmock_main) + include(FetchContent) + FetchContent_Declare( + googletest + GIT_REPOSITORY https://github.com/google/googletest.git + GIT_TAG 03597a01ee50ed33e9dfd640b249b4be3799d395) + # For Windows: Prevent overriding the parent project's compiler/linker + # settings + set(GTEST_FORCE_SHARED_CRT + ON + CACHE BOOL "" FORCE) + FetchContent_MakeAvailable(googletest) +endif() + +include(CTest) +add_subdirectory(beluga_vdb) diff --git a/beluga_vdb/test/beluga_vdb/CMakeLists.txt b/beluga_vdb/test/beluga_vdb/CMakeLists.txt new file mode 100644 index 000000000..a7d8dbed4 --- /dev/null +++ b/beluga_vdb/test/beluga_vdb/CMakeLists.txt @@ -0,0 +1,23 @@ +# Copyright 2024 Ekumen, Inc. +# +# 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 +# +# 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. + +add_executable(test_beluga_vdb sensor/test_likelihood_3d_field_model.cpp) + +target_link_libraries(test_beluga_vdb PRIVATE ${PROJECT_NAME} beluga_vdb + GTest::gmock_main) +target_include_directories(test_beluga_vdb PRIVATE include) +target_compile_options(test_beluga_vdb PRIVATE -Wno-sign-compare) + +include(GoogleTest) +gtest_discover_tests(test_beluga_vdb) diff --git a/beluga_vdb/test/beluga_vdb/include/beluga_vdb/test/simple_pointcloud_interface.hpp b/beluga_vdb/test/beluga_vdb/include/beluga_vdb/test/simple_pointcloud_interface.hpp new file mode 100644 index 000000000..d51dbbf13 --- /dev/null +++ b/beluga_vdb/test/beluga_vdb/include/beluga_vdb/test/simple_pointcloud_interface.hpp @@ -0,0 +1,45 @@ +// Copyright 2024 Ekumen, Inc. +// +// 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 +// +// 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. + +#ifndef BELUGA_VDB_TEST_SIMPLE_POINTCLOUD_INTERFACE_HPP +#define BELUGA_VDB_TEST_SIMPLE_POINTCLOUD_INTERFACE_HPP + +#include +#include + +namespace beluga_vdb::testing { + +template +class SimpleSparsePointCloud3 { + public: + explicit SimpleSparsePointCloud3(std::vector> points, Sophus::SE3d origin = Sophus::SE3d{}) + : points_(std::move(points)), origin_(std::move(origin)) {} + + /// Get the point cloud frame origin in the filter frame. + [[nodiscard]] const auto& origin() const { return origin_; } + + /// Get the 3D points collection. + [[nodiscard]] const auto& points() const { return points_; } + + private: + std::vector> points_; + Sophus::SE3d origin_; +}; + +using SimpleSparsePointCloud3d = SimpleSparsePointCloud3; +using SimpleSparsePointCloud3f = SimpleSparsePointCloud3; + +} // namespace beluga_vdb::testing + +#endif diff --git a/beluga_vdb/test/beluga_vdb/sensor/test_likelihood_3d_field_model.cpp b/beluga_vdb/test/beluga_vdb/sensor/test_likelihood_3d_field_model.cpp new file mode 100644 index 000000000..ba3deb762 --- /dev/null +++ b/beluga_vdb/test/beluga_vdb/sensor/test_likelihood_3d_field_model.cpp @@ -0,0 +1,170 @@ +// Copyright 2024 Ekumen, Inc. +// +// 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 +// +// 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 +#include + +#include +#include + +#include +#include +#include + +#include +#include +#include + +#include "beluga_vdb/sensor/likelihood_field_model3.hpp" +#include "beluga_vdb/test/simple_pointcloud_interface.hpp" + +namespace { + +template +auto make_map(const double voxel_size, const std::vector& world_points) { + // Parameters + constexpr int kHalfWidth = 3; + constexpr int kClosingSteps = 0; + + // Create the grid + const typename GridT::Ptr grid = openvdb::FloatGrid::create(); + grid->setTransform(openvdb::math::Transform::createLinearTransform(voxel_size)); + + // Get an accessor for coordinate-based access to voxels + typename GridT::Accessor accessor = grid->getAccessor(); + + // Fill the grid + const openvdb::math::Transform& transform = grid->transform(); + for (const auto& point : world_points) { + // Transform to index world + const openvdb::math::Coord ijk = transform.worldToIndexCellCentered(point); + // Set the voxel to 1 + accessor.setValue(ijk, 1.0); + // Activate the voxel + accessor.setActiveState(ijk); + } + + // Check grid + openvdb::tools::CheckLevelSet checker(*grid); + // Check inactive values have a magnitude equal to the background value + assert(checker.checkInactiveValues() == ""); + + // Transform to levelset + return openvdb::tools::topologyToLevelSet(*grid, kHalfWidth, kClosingSteps); +} + +TEST(TestLikelihoodFieldModel3, Point) { + openvdb::initialize(); + // Parameters + constexpr double kVoxelSize = 0.07; + constexpr double kMaxObstacleDistance = 2.0; + constexpr double kMaxLaserDistance = 100.0; + constexpr double kZHit = 0.5; + constexpr double kZRandom = 0.001; + constexpr double kSigmaHit = 0.22; + + // Create Grid + const std::vector world_points{openvdb::math::Vec3s(1.0F, 1.0F, 1.0F)}; + auto map = make_map(kVoxelSize, world_points); + + const auto params = + beluga_vdb::LikelihoodFieldModel3Param{kMaxObstacleDistance, kMaxLaserDistance, kZHit, kZRandom, kSigmaHit}; + auto sensor_model = + beluga_vdb::LikelihoodFieldModel3{ + params, *map}; + + // Exact point + auto pointcloud_measurement_exact = + beluga_vdb::testing::SimpleSparsePointCloud3f{std::vector>{{1.0F, 1.0F, 1.0F}}}; + auto state_weighting_function_exact = sensor_model(std::move(pointcloud_measurement_exact)); + ASSERT_GT(state_weighting_function_exact(Sophus::SE3d{}), 1.90); + + // Close point (Inside the narrow band) + auto pointcloud_measurement_close = + beluga_vdb::testing::SimpleSparsePointCloud3f{std::vector>{{1.035F, 1.0F, 1.0F}}}; + auto state_weighting_function_close = sensor_model(std::move(pointcloud_measurement_close)); + ASSERT_GT(state_weighting_function_close(Sophus::SE3d{}), 1.8); + + // Far point (Outside the narrow band) + auto pointcloud_measurement_far = + beluga_vdb::testing::SimpleSparsePointCloud3f{std::vector>{{1.1F, 1.1F, 1.1F}}}; + auto state_weighting_function_far = sensor_model(std::move(pointcloud_measurement_far)); + ASSERT_LT(state_weighting_function_far(Sophus::SE3d{}), 1.65); +} + +TEST(TestLikelihoodFieldModel3, Cube) { + openvdb::initialize(); + // Parameters + constexpr double kVoxelSize = 0.07; + constexpr double kMaxObstacleDistance = 2.0; + constexpr double kMaxLaserDistance = 100.0; + constexpr double kZHit = 0.5; + constexpr double kZRandom = 0.001; + constexpr double kSigmaHit = 0.22; + + // Create Grid + const std::vector world_points{ + openvdb::math::Vec3s(1.0F, 1.0F, 1.0F), openvdb::math::Vec3s(1.0F, 1.0F, -1.0F), + openvdb::math::Vec3s(1.0F, -1.0F, 1.0F), openvdb::math::Vec3s(1.0F, -1.0F, -1.0F), + openvdb::math::Vec3s(-1.0F, -1.0F, 1.0F), openvdb::math::Vec3s(-1.0F, -1.0F, -1.0F), + openvdb::math::Vec3s(-1.0F, 1.0F, 1.0F), openvdb::math::Vec3s(-1.0F, 1.0F, -1.0F)}; + auto map = make_map(kVoxelSize, world_points); + + const auto params = + beluga_vdb::LikelihoodFieldModel3Param{kMaxObstacleDistance, kMaxLaserDistance, kZHit, kZRandom, kSigmaHit}; + auto sensor_model = + beluga_vdb::LikelihoodFieldModel3{ + params, *map}; + + // Exact point + auto pointcloud_measurement_exact = beluga_vdb::testing::SimpleSparsePointCloud3f{std::vector>{ + {1.0F, 1.0F, 1.0F}, + {1.0F, 1.0F, -1.0F}, + {1.0F, -1.0F, 1.0F}, + {1.0F, -1.0F, -1.0F}, + {-1.0F, -1.0F, 1.0F}, + {-1.0F, -1.0F, -1.0F}, + {-1.0F, 1.0F, 1.0F}, + {-1.0F, 1.0F, -1.0F}}}; + auto state_weighting_function_exact = sensor_model(std::move(pointcloud_measurement_exact)); + ASSERT_GT(state_weighting_function_exact(Sophus::SE3d{}), 8.2); + + // Close point (Inside the narrow band) + auto pointcloud_measurement_close = beluga_vdb::testing::SimpleSparsePointCloud3f{std::vector>{ + {1.035F, 1.0F, 1.0F}, + {1.035F, 1.0F, -1.0F}, + {1.035F, -1.0F, 1.0F}, + {1.035F, -1.0F, -1.0F}, + {-1.0F, -1.0F, 1.0F}, + {-1.0F, -1.0F, -1.0F}, + {-1.0F, 1.0F, 1.0F}, + {-1.0F, 1.0F, -1.0F}}}; + auto state_weighting_function_close = sensor_model(std::move(pointcloud_measurement_close)); + ASSERT_GT(state_weighting_function_close(Sophus::SE3d{}), 7.4); + + // Far point (Outside the narrow band) + auto pointcloud_measurement_far = beluga_vdb::testing::SimpleSparsePointCloud3f{std::vector>{ + {1.1F, 1.1F, 1.1F}, + {1.1F, 1.1F, -1.1F}, + {1.1F, -1.1F, 1.1F}, + {1.1F, -1.1F, -1.1F}, + {-1.1F, -1.1F, 1.1F}, + {-1.1F, -1.1F, -1.1F}, + {-1.1F, 1.1F, 1.1F}, + {-1.1F, 1.1F, -1.1F}}}; + auto state_weighting_function_far = sensor_model(std::move(pointcloud_measurement_far)); + ASSERT_LT(state_weighting_function_far(Sophus::SE3d{}), 6.85); +} + +} // namespace diff --git a/tools/build-and-test.sh b/tools/build-and-test.sh index ce7feaf0b..708ab8952 100755 --- a/tools/build-and-test.sh +++ b/tools/build-and-test.sh @@ -22,16 +22,27 @@ SCRIPT_PATH=$(dirname "$(readlink -f "$0")") set -o errexit +CMAKE_EXTRA_ARGS="" +COLCON_EXTRA_ARGS="" + if [ "${ROS_DISTRO}" != "noetic" ]; then if [ "${ROS_DISTRO}" != "jazzy" ] && [ "${ROS_DISTRO}" != "rolling" ]; then ROS_PACKAGES="beluga beluga_ros beluga_amcl beluga_benchmark beluga_example beluga_system_tests beluga_tools" else ROS_PACKAGES="beluga beluga_ros beluga_amcl beluga_system_tests beluga_tools" + if [ "${ROS_DISTRO}" != "humble" ] && [ "${ROS_DISTRO}" != "iron" ]; then + ROS_PACKAGES="beluga beluga_ros beluga_amcl beluga_system_tests beluga_tools beluga_vdb" + fi fi else ROS_PACKAGES="beluga beluga_ros beluga_amcl beluga_example" fi + +if [ "${CMAKE_EXTRA_ARGS}" != "" ]; then + COLCON_EXTRA_ARGS="${COLCON_EXTRA_ARGS} --cmake-args ${CMAKE_EXTRA_ARGS}" +fi + source /opt/ros/${ROS_DISTRO}/setup.sh echo ::group::Release Build @@ -43,9 +54,11 @@ colcon build \ build-testing-on \ ccache \ release \ - --cmake-force-configure + --cmake-force-configure \ + ${COLCON_EXTRA_ARGS} echo ::endgroup:: + echo ::group::Debug Build colcon build \ --packages-up-to ${ROS_PACKAGES} \ @@ -57,7 +70,8 @@ colcon build \ coverage-gcc \ coverage-pytest \ debug \ - --cmake-force-configure + --cmake-force-configure \ + ${COLCON_EXTRA_ARGS} echo ::endgroup:: LCOV_CONFIG_PATH=${SCRIPT_PATH}/../.lcovrc diff --git a/tools/run-clang-tidy.sh b/tools/run-clang-tidy.sh index 5a96237a8..60ea94a08 100755 --- a/tools/run-clang-tidy.sh +++ b/tools/run-clang-tidy.sh @@ -20,14 +20,24 @@ set -o errexit -o xtrace +CMAKE_EXTRA_ARGS="" +COLCON_EXTRA_ARGS="" + if [ "${ROS_DISTRO}" != "noetic" ]; then ROS_PACKAGES="beluga beluga_ros beluga_amcl beluga_system_tests" + if [ "${ROS_DISTRO}" != "humble" ] && [ "${ROS_DISTRO}" != "iron" ]; then + ROS_PACKAGES="beluga beluga_ros beluga_amcl beluga_system_tests beluga_vdb" + fi else ROS_PACKAGES="beluga beluga_ros beluga_amcl" fi source /opt/ros/${ROS_DISTRO}/setup.sh -colcon build --packages-up-to ${ROS_PACKAGES} --event-handlers=console_cohesion+ --symlink-install --mixin ccache + +if [ "${CMAKE_EXTRA_ARGS}" != "" ]; then + COLCON_EXTRA_ARGS="${COLCON_EXTRA_ARGS} --cmake-args ${CMAKE_EXTRA_ARGS}" +fi +colcon build --packages-up-to ${ROS_PACKAGES} --event-handlers=console_cohesion+ --symlink-install --mixin ccache ${COLCON_EXTRA_ARGS} echo ${ROS_PACKAGES} | xargs -n1 echo | # NOTE: `-Wno-gnu-zero-variadic-macro-arguments` is needed due to