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.

Sign up for GitHub

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

Jump to bottom

Add likelihood field 3d model version #440

Open
wants to merge 50 commits into
base: main
Choose a base branch
from
Open

Add likelihood field 3d model version #440

wants to merge 50 commits into from

Conversation

pvela2017
Copy link
Contributor

@pvela2017 pvela2017 commented Oct 1, 2024 •
edited
Loading

Proposed changes

Describe the big picture of your changes here to communicate to the maintainers why we should accept this pull request.
If it fixes a bug or resolves a feature request, be sure to link to that issue.

Type of change

  • 🐛 Bugfix (change which fixes an issue)
  • 🚀 Feature (change which adds functionality)
  • 📚 Documentation (change which fixes or extends documentation)

💥 Breaking change! Explain why a non-backwards compatible change is necessary or remove this line entirely if not applicable.

Checklist

Put an x in the boxes that apply. This is simply a reminder of what we will require before merging your code.

  • Lint and unit tests (if any) pass locally with my changes
  • I have added tests that prove my fix is effective or that my feature works
  • I have added necessary documentation (if appropriate)
  • All commits have been signed for DCO

Additional comments

Anything worth mentioning to the reviewers.

@pvela2017
Add pointcloud API contract and ROS interface
d96bab0 
Added a ROS point cloud interface for float data type and it respective testcase

Signed-off-by: Pablo Vela <[email protected]>
@pvela2017
Change PointCloud description for better clarification
251c03b 
Changed description names to be more natural and specified pointcloud requirements

Signed-off-by: Pablo Vela <[email protected]>
@pvela2017
Changed copyright year
370a6fa 
Signed-off-by: Pablo Vela <[email protected]>
@pvela2017
Add newlines
54c4417 
Signed-off-by: Pablo Vela <[email protected]>
@pvela2017
Expand testcase scenarios
7481543 
Added new testcases to cover all the cases

Signed-off-by: Pablo Vela <[email protected]>
@pvela2017
Clarify documentation
104131b 
Signed-off-by: Pablo Vela <[email protected]>
@pvela2017
Change to return Eigen::Map
b39afcd 
Signed-off-by: Pablo Vela <[email protected]>
@pvela2017
Add make pointcloud message function
18cbff5 
Signed-off-by: Pablo Vela <[email protected]>
@pvela2017
Add non-aligned memory pointcloud wrapper
11fc506 
Signed-off-by: Pablo Vela <[email protected]>
@pvela2017
Add non-aligned memory pointcloud wrapper
3111751 
Signed-off-by: Pablo Vela <[email protected]>
@pvela2017
Add more restrictions to check
d53e2d4 
Signed-off-by: Pablo Vela <[email protected]>
@pvela2017
Change to pointfieldtypeastype templatization
73df8e2 
Signed-off-by: Pablo Vela <[email protected]>
@pvela2017
Fix ROS1
674f7c4 
Signed-off-by: Pablo Vela <[email protected]>
@pvela2017
Add eigen lib to test
d4e5767 
Signed-off-by: Pablo Vela <[email protected]>
@pvela2017
Fix ROS1 eigen compatibility
3f5ec82 
Signed-off-by: Pablo Vela <[email protected]>
@pvela2017
Fix clang tidy
cff6c8f 
Signed-off-by: Pablo Vela <[email protected]>
@pvela2017
Rename sparse pointcloud
8f2718a 
Signed-off-by: Pablo Vela <[email protected]>
@pvela2017
Change using Pointfield
cc1a62c 
Signed-off-by: Pablo Vela <[email protected]>
@pvela2017
Change details
c703610 
Signed-off-by: Pablo Vela <[email protected]>
@pvela2017
Change to true types
91606d5 
Signed-off-by: Pablo Vela <[email protected]>
@pvela2017
Add sparse pointcloud API contract
96eab5d 
Signed-off-by: Pablo Vela <[email protected]>
@pvela2017
Improve documentation
3688a6a 
Signed-off-by: Pablo Vela <[email protected]>
@pvela2017
Change name
51ba647 
Signed-off-by: Pablo Vela <[email protected]>
@pvela2017
Fix clang
29e838a 
Signed-off-by: Pablo Vela <[email protected]>
@pvela2017
Fix is_dense bool noetic
8229dcf 
Signed-off-by: Pablo Vela <[email protected]>
@pvela2017
Fix clang jazzy
b7c9c66 
Signed-off-by: Pablo Vela <[email protected]>
@pvela2017
Add openvdb dependencies
4870924 
Signed-off-by: Pablo Vela <[email protected]>
@pvela2017
Add likelihood 3D field model
0f135a1 
Signed-off-by: Pablo Vela <[email protected]>
@pvela2017
Add Openvdb dependencies
580ce80 
Signed-off-by: Pablo Vela <[email protected]>
@pvela2017
Add likelihood 3d field model test
251bbd2 
Signed-off-by: Pablo Vela <[email protected]>
@pvela2017 pvela2017 marked this pull request as ready for review October 21, 2024 16:03
@pvela2017
Fix return data
d5f800d 
Signed-off-by: Pablo Vela <[email protected]>
@pvela2017
Fix cast eigen vector
3be87cd 
Signed-off-by: Pablo Vela <[email protected]>
@pvela2017
Fix openvdb map creation
a6b9e06 
Signed-off-by: Pablo Vela <[email protected]>
@pvela2017
Fix compile with full options
a1a1eac 
Signed-off-by: Pablo Vela <[email protected]>
hidmic
hidmic reviewed Oct 23, 2024
View reviewed changes
Copy link
Collaborator

@hidmic hidmic left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

First pass, nice work Pablo!

beluga/CMakeLists.txt Outdated
@@ -18,6 +18,8 @@ project(beluga VERSION 1.0)

set(CMAKE_EXPORT_COMPILE_COMMANDS ON)

list(APPEND CMAKE_MODULE_PATH "/usr/local/lib/cmake/OpenVDB")
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@pvela2017 consider adding this to the find_package statement, as PATHS. Same elsewhere.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This was delayed.

Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think we don't need this for OpenVDB as distributed by Ubuntu (and Debian).

beluga/include/beluga/sensor/likelihood_3d_field_model.hpp Outdated
* \tparam OpenVDB grid type.
*/
template <typename GridT, typename U>
class Likelihood3DFieldModel {
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@pvela2017 meta: should this be LikelihoodFieldModel3? LikelihoodFieldModel would then become LikelihoodFieldModel2.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Changed in 015f1b7

beluga/include/beluga/sensor/likelihood_3d_field_model.hpp Outdated
*
* \tparam OpenVDB grid type.
*/
template <typename GridT, typename U>
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@pvela2017 consider using meaningful template parameter names (e.g. PointCloud instead of U)

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Changed in 015f1b7

beluga/include/beluga/sensor/likelihood_3d_field_model.hpp Outdated
*/
explicit Likelihood3DFieldModel(const param_type& params, const map_type& grid)
: params_{params},
looker_{openvdb::tools::ClosestSurfacePoint<map_type>::create(grid)},
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@pvela2017 shall we have an update_map() method that takes care of creating the ClosestSurfacePoint instance from a given grid?

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think no needed yet

beluga/include/beluga/sensor/likelihood_3d_field_model.hpp Outdated

// Transform each point to every particle state
std::transform(
points.begin(), points.end(), vdb_points.begin(), [this, pointcloud_size, state](const auto& point) {
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@pvela2017 why do you need pointcloud_size here?

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Changed in 015f1b7

beluga/test/beluga/sensor/test_likelihood_3d_field_model.cpp Outdated
Comment on lines 63 to 65
typename GridT::Ptr grid_levelset = openvdb::tools::topologyToLevelSet(*grid, 1, 0, 0, 0);

return grid_levelset;
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@pvela2017 consider collapsing these lines into one:

Suggested change
typename GridT::Ptr grid_levelset = openvdb::tools::topologyToLevelSet(*grid, 1, 0, 0, 0);
return grid_levelset;
return openvdb::tools::topologyToLevelSet(*grid, 1, 0, 0, 0);

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Changed in ee56b7f

beluga/test/beluga/sensor/test_likelihood_3d_field_model.cpp
}

TEST(Likelihood3DFieldModel, Likelihood3DField) {
openvdb::initialize();
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@pvela2017 hmm, this is implementation specific. Consider initializing openvdb on likelihood field model construction. It's a no-op after the first call, so we are good.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Changed in 015f1b7

beluga/test/beluga/sensor/test_likelihood_3d_field_model.cpp Outdated
TEST(Likelihood3DFieldModel, Likelihood3DField) {
openvdb::initialize();
// Create Grid
const double voxel_size = 0.07;
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@pvela2017 nit: use Hungarian notation for constants e.g. constexpr double kVoxelSize = 0.07;

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Changed in ee56b7f

beluga/test/beluga/sensor/test_likelihood_3d_field_model.cpp Outdated
const std::vector<openvdb::math::Vec3s> world_points{openvdb::math::Vec3s(1.0F, 1.0F, 1.0F)};
auto map = make_map<openvdb::FloatGrid, openvdb::math::Vec3s>(voxel_size, world_points);

const auto params = beluga::Likelihood3DFieldModelParam{voxel_size, 2.0, 20.0, 0.5, 0.5, 0.2};
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@pvela2017 nit: consider using named constants for parameter passing.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Changed in ee56b7f

beluga/test/beluga/sensor/test_likelihood_3d_field_model.cpp
template <typename GridT, typename T>
auto make_map(const double voxel_size, const std::vector<T>& world_points) {
// Create the grid
typename GridT::Ptr grid = openvdb::FloatGrid::create();
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@pvela2017 fyi: this is the kind of situation where auto comes quite handy.

@pvela2017
Fix nit
015f1b7 
Signed-off-by: Pablo Vela <[email protected]>
@pvela2017
Add close and far point test
ee56b7f 
Signed-off-by: Pablo Vela <[email protected]>
@pvela2017
Fix openvdb dependency
01018a3 
Signed-off-by: Pablo Vela <[email protected]>
@pvela2017
Change test parameters
c0b1c45 
Signed-off-by: Pablo Vela <[email protected]>
@pvela2017 pvela2017 requested a review from hidmic October 30, 2024 03:24
@hidmic
Copy link
Collaborator

hidmic commented Nov 4, 2024

@pvela2017 we have a problem. OpenVDB as released with Ubuntu 22.04 appears to be broken (see https://bugs.launchpad.net/ubuntu/+source/openvdb/+bug/1970108). That makes this an Ubuntu 24.04 feature. We'll have to make it optional. That is, find_package(OpenVDB QUIET) and using compile definitions to skip code that depends on OpenVDB.

@pvela2017
Copy link
Contributor Author

@pvela2017 we have a problem. OpenVDB as released with Ubuntu 22.04 appears to be broken (see https://bugs.launchpad.net/ubuntu/+source/openvdb/+bug/1970108). That makes this an Ubuntu 24.04 feature. We'll have to make it optional. That is, find_package(OpenVDB QUIET) and using compile definitions to skip code that depends on OpenVDB.

Changed in 3a98181

hidmic
hidmic reviewed Nov 13, 2024
View reviewed changes
beluga/cmake/Config.cmake.in Outdated
include(CMakeFindDependencyMacro)
find_dependency(Eigen3 REQUIRED NO_MODULE)
find_dependency(range-v3 REQUIRED)
find_dependency(HDF5 COMPONENTS CXX REQUIRED)
find_dependency(Sophus REQUIRED)
find_dependency(TBB REQUIRED)
find_dependency(OpenVDB REQUIRED)
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@pvela2017 this should not be required. It may be missing.

beluga/test/beluga/CMakeLists.txt Outdated
@@ -52,7 +53,6 @@ add_executable(
sensor/test_beam_model.cpp
sensor/test_bearing_sensor_model.cpp
sensor/test_landmark_sensor_model.cpp
sensor/test_lfm_with_unknown_space.cpp
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@pvela2017 why was this removed?

beluga/CMakeLists.txt Outdated
list(APPEND CMAKE_MODULE_PATH "/usr/lib/aarch64-linux-gnu/cmake/OpenVDB")
find_package(OpenVDB QUIET)
if(OpenVDB_FOUND)
add_definitions(-USE_OPENVDB)
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@pvela2017 consider using BELUGA_OPENVDB_SUPPORT for a compile definition. Also, use target_compile_definitions for the ${PROJECT_NAME} target instead of a blanket add_definitions macro.

beluga/CMakeLists.txt Outdated
Comment on lines 51 to 52
list(APPEND CMAKE_MODULE_PATH "/usr/lib/x86_64-linux-gnu/cmake/OpenVDB/")
list(APPEND CMAKE_MODULE_PATH "/usr/lib/aarch64-linux-gnu/cmake/OpenVDB")
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@pvela2017 instead of blanket adding these paths, consider:

  • adding a USE_OPENVDB option that defaults to false
  • using find_file to look for the FindOpenVDB.cmake path under /usr/lib (and /usr/local/lib perhaps)
  • setting an OPENVDB_CMAKE_MODULE_PATH using the parent path of the module path found, allowing the user to override it

You can skip almost everything if USE_OPENVDB is false.

beluga/test/beluga/include/beluga/test/simple_pointcloud_interface.hpp Outdated
[[nodiscard]] const auto& origin() const { return origin_; }

/// Get the 3D points collection.
[[nodiscard]] auto& points() const { return points_; }
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@pvela2017 hmm, shouldn't this be

Suggested change
[[nodiscard]] auto& points() const { return points_; }
[[nodiscard]] const auto& points() const { return points_; }

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Reviewers

@hidmic hidmic hidmic left review comments

At least 1 approving review is required to merge this pull request.

Assignees
No one assigned
Labels
None yet
Projects
None yet
Milestone
No milestone
Development

Successfully merging this pull request may close these issues.
2 participants
@pvela2017 @hidmic