Add likelihood field 3d model version

[pvela2017]

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.

[hidmic]

Great work @pvela2017! As I said offline, we are likely moving OpenVDB out of Beluga core (solely because OpenVDB packaging is terrible). I'll take it from here.

In the meantime, CC'ing @nahueespinosa for another take.

[hidmic]

@pvela2017 nit: we can fetch pointcloud_size as points.size().

[pvela2017]

I think points type changed to Eigen after transforming the points to base_link frame.

But now reserve was removed according to this so no need for this variable.

Changed in 2aabb60

[nahueespinosa]

Awesome!

[nahueespinosa]

nit: ranges::views::all should not be needed. The transform view will do the conversion.

Suggested change

	auto transformed_points = ranges::views::all(measurement.points()) |
	auto transformed_points = measurement.points() |

[pvela2017]

Changed in 2aabb60

[nahueespinosa]

Suggested change

	* - beluga::Likelihood3DFieldModel
	* - beluga::LikelihoodFieldModel3

[pvela2017]

Changed in 2aabb60

[nahueespinosa]

meta: It doesn't seem like we need to dynamically allocate the distances since we will iterate over them only once. Non-blocking though.

[pvela2017]

Changed in 2aabb60

edit: I was thinking, if we dont allocate memory for the distances, wouldn't it loose performance since everytime the vector is getting full it would have to allocate more memory? Since there are a lot of points this would happen several time or would this be negligible?

[nahueespinosa]

There are other instances that need replacement.

Suggested change

	/// Constructs a Likelihood3DFieldModel instance.
	/// Constructs a LikelihoodFieldModel3 instance.

[pvela2017]

Changed in 2aabb60

[hidmic]

Ready to go once pvela2017#2 goes in. FYI @glpuga @nahueespinosa.

[pvela2017]

@hidmic

I forgot that setting the checker as const generate problems when running the build-and-test.sh script, but not when just using colcon build.

What should we do? It is nice to have the checker but I don't think it is mandatory.

/ws/src/beluga/beluga_vdb/test/beluga_vdb/sensor/test_likelihood_3d_field_model.cpp:79:64: required from here /ws/src/beluga/beluga_vdb/test/beluga_vdb/sensor/test_likelihood_3d_field_model.cpp:61:37: error: passing ‘const openvdb::v10_0::tools::CheckLevelSet<openvdb::v10_0::Grid<openvdb::v10_0::tree::Tree<openvdb::v10_0::tree::RootNode<openvdb::v10_0::tree::InternalNode<openvdb::v10_0::tree::InternalNode<openvdb::v10_0::tree::LeafNode<float, 3>, 4>, 5> > > > >’ as ‘this’ argument discards qualifiers [-fpermissive] 61 | assert(checker.checkInactiveValues() == "");

PD: I commented this before, but it seems it didn't generate a notification

[glpuga]

@hidmic

I forgot that setting the checker as const generate problems when running the build-and-test.sh script, but not when just using colcon build.

What should we do? It is nice to have the checker but I don't think it is mandatory.

/ws/src/beluga/beluga_vdb/test/beluga_vdb/sensor/test_likelihood_3d_field_model.cpp:79:64: required from here /ws/src/beluga/beluga_vdb/test/beluga_vdb/sensor/test_likelihood_3d_field_model.cpp:61:37: error: passing ‘const openvdb::v10_0::tools::CheckLevelSet<openvdb::v10_0::Grid<openvdb::v10_0::tree::Tree<openvdb::v10_0::tree::RootNode<openvdb::v10_0::tree::InternalNode<openvdb::v10_0::tree::InternalNode<openvdb::v10_0::tree::LeafNode<float, 3>, 4>, 5> > > > >’ as ‘this’ argument discards qualifiers [-fpermissive] 61 | assert(checker.checkInactiveValues() == "");

PD: I commented this before, but it seems it didn't generate a notification

@pvela2017 Michel will be out on PTO for a couple of weeks. If you give us more context maybe @nahueespinosa a me can help you.

[pvela2017]

@hidmic
I forgot that setting the checker as const generate problems when running the build-and-test.sh script, but not when just using colcon build.
What should we do? It is nice to have the checker but I don't think it is mandatory.
/ws/src/beluga/beluga_vdb/test/beluga_vdb/sensor/test_likelihood_3d_field_model.cpp:79:64: required from here /ws/src/beluga/beluga_vdb/test/beluga_vdb/sensor/test_likelihood_3d_field_model.cpp:61:37: error: passing ‘const openvdb::v10_0::tools::CheckLevelSet<openvdb::v10_0::Grid<openvdb::v10_0::tree::Tree<openvdb::v10_0::tree::RootNode<openvdb::v10_0::tree::InternalNode<openvdb::v10_0::tree::InternalNode<openvdb::v10_0::tree::LeafNode<float, 3>, 4>, 5> > > > >’ as ‘this’ argument discards qualifiers [-fpermissive] 61 | assert(checker.checkInactiveValues() == "");
PD: I commented this before, but it seems it didn't generate a notification

@pvela2017 Michel will be out on PTO for a couple of weeks. If you give us more context maybe @nahueespinosa a me can help you.

Whops! I hope he doesnt get the notifications then.

Sure, in the testcase for beluga_vdb we use an OpenVDB object to check if the background of the level set is right.
const openvdb::tools::CheckLevelSet<GridT> checker(*grid);

Originally, it was without const, just openvdb::tools::CheckLevelSet<GridT> checker(*grid); but Michel suggest to changed to const, which does make sense, since we are not changing it. But this way seems to have problems and doesn't compile when using the build-and-test.sh script.

So, what would be better remove this checking or just leave it without const?

Thank you!

[glpuga]

Sure, in the testcase for beluga_vdb we use an OpenVDB object to check if the background of the level set is right. const openvdb::tools::CheckLevelSet<GridT> checker(*grid);

Originally, it was without const, just openvdb::tools::CheckLevelSet<GridT> checker(*grid); but Michel suggest to changed to const, which does make sense, since we are not changing it. But this way seems to have problems and doesn't compile when using the build-and-test.sh script.

So, what would be better remove this checking or just leave it without const?

The problem seems to be that that checker has noconst version of the checkInactiveValues() method, so you can't call it if you create the instance as const. The method internally seems to modify state in within the instance (a counter and maybe some other stuff).

Leave it as non-const.

[pvela2017]

Sure, in the testcase for beluga_vdb we use an OpenVDB object to check if the background of the level set is right. const openvdb::tools::CheckLevelSet<GridT> checker(*grid);
Originally, it was without const, just openvdb::tools::CheckLevelSet<GridT> checker(*grid); but Michel suggest to changed to const, which does make sense, since we are not changing it. But this way seems to have problems and doesn't compile when using the build-and-test.sh script.
So, what would be better remove this checking or just leave it without const?

The problem seems to be that that checker has noconst version of the checkInactiveValues() method, so you can't call it if you create the instance as const. The method internally seems to modify state in within the instance (a counter and maybe some other stuff).

Leave it as non-const.

Ok, I pushed the changes!