Add beam sensor model #160

serraramiro1 · 2023-04-04T13:30:37Z

Related to #99

Summary

Adds a brasenhaum raycasting function for casting rays on an occupancy grid.
Adds a beam sensor model implementation.
Adds beam sensor model to the runtime selection for the AMCL node.
Adds tests for both the raycasting and the model itself.

Checklist

Read the contributing guidelines.
Configured pre-commit and ran colcon test locally.
Signed all commits for DCO.
Added tests (regression tests for bugs, coverage of new code for features).
Updated documentation (as needed).
Checked that CI is passing.

serraramiro1 · 2023-04-04T13:38:31Z

beluga/include/beluga/algorithm/raycasting.hpp

+
+  int error = 0;
+  // Needed because of the way OccupancyGrid signals an out of bounds index.
+  std::size_t last_ok_index = steep ? grid.index(y * grid.resolution(), x * grid.resolution())


This is weird but needed.
I think it'd be nice to provide an abstaction of index() that signals an out-of-bound cell differently, instead of the current approach that returns size()

Thinking out loud, index() could return std::optional as a poor mans substitute to e.g. abseil::StatusOr.

beluga/include/beluga/sensor/beam_model.hpp

beluga/test/beluga/algorithm/test_raycasting.cpp

beluga_amcl/include/beluga_amcl/occupancy_grid.hpp

beluga/include/beluga/algorithm/raycasting.hpp

beluga_amcl/src/amcl_node.cpp

hidmic

This is a great start. It needs polishing but functionality is pretty much there.

beluga/include/beluga/algorithm/raycasting.hpp

hidmic · 2023-04-12T11:58:55Z

beluga/include/beluga/algorithm/raycasting.hpp

+
+  int error = 0;
+  // Needed because of the way OccupancyGrid signals an out of bounds index.
+  std::size_t last_ok_index = steep ? grid.index(y * grid.resolution(), x * grid.resolution())


Thinking out loud, index() could return std::optional as a poor mans substitute to e.g. abseil::StatusOr.

beluga/include/beluga/algorithm/raycasting.hpp

beluga/test/beluga/algorithm/test_raycasting.cpp

hidmic · 2023-04-12T12:21:30Z

beluga/include/beluga/sensor/beam_model.hpp

+      // TODO(Ramiro): We're converting from range + bearing to cartesian points in the ROS node, but we want range +
+      // bearing here. We might want to make that conversion in the likelihood model instead, and let the measurement
+      // type be range, bearing instead of x, y.


Thinking out loud, measurement_type should perhaps be a proxy object to iterate over points in either cartesian or polar coordinate, which may or may not incur additional overhead depending on underlying data (e.g. in ROS, LaserScan messages use polar coordinates where PointCloud2 messages normally use cartesian coordinates).

beluga/include/beluga/algorithm/raycasting.hpp

hidmic · 2023-04-14T18:23:03Z

I'll give this a shot.

beluga/include/beluga/algorithm/raycasting.hpp

glpuga · 2023-04-15T22:53:33Z

beluga/include/beluga/sensor/beam_model.hpp

@@ -0,0 +1,186 @@
+// Copyright 2022 Ekumen, Inc.


+1 to using 2023

Fixed in 875d382.

beluga/include/beluga/algorithm/raycasting.hpp

glpuga · 2023-04-15T23:20:01Z

beluga/include/beluga/algorithm/raycasting.hpp

+    last_ok_index = index;
+    x += 1;
+    error += delta_y;
+    if (2 * error >= delta_x) {


Do we have a source for this version of the algorithm? Isn't this comparison done before updating the error with delta_y?

beluga/include/beluga/algorithm/raycasting.hpp

Signed-off-by: Ramiro Serra <[email protected]>

Signed-off-by: Michel Hidalgo <[email protected]>

hidmic · 2023-05-03T13:51:56Z

@glpuga @nahueespinosa @serraramiro1 PTAL

Signed-off-by: Michel Hidalgo <[email protected]>

serraramiro1 · 2023-05-03T14:28:17Z

beluga/include/beluga/sensor/likelihood_field_model.hpp

 * - Give possibly const values `x` and `y` of type `double`, `g.index(x, y)` returns a `std::size`,
 *   representing the index of the cell.
 * - Give a possibly const `Eigen::Vector2d` `p`, `g.index(p)` is equivalent to `g.index(p.x(), p.y())`.
+ * - Give possibly const values `x` and `y` of type `int`, `g.index(x, y)` returns a `std::size`,


Suggested change

* - Give possibly const values `x` and `y` of type `int`, `g.index(x, y)` returns a `std::size`,

* - Given possibly const values `x` and `y` of type `int`, `g.index(x, y)` returns a `std::size`,

I thought about this and concluded this is error prone.

Let's say an user defines its own Grid, that defines the double variant of this method, i.e. index(double x , double y) , this will still compile and produce incorrect results.

How do we feel about enforcing this through an abstract interface? Or removing the builtin type variants in favor of Eigen::Vector2 ones that prevent this from happening.

I'd also be happy coming up with a different name for continous vs discrete methods to prevent this from happening.

Let's say an user defines its own Grid, that defines the double variant of this method, i.e. index(double x , double y) , this will still compile and produce incorrect results.

That is possible, yes.

How do we feel about enforcing this through an abstract interface? Or removing the builtin type variants in favor of Eigen::Vector2 ones that prevent this from happening.
I'd also be happy coming up with a different name for continous vs discrete methods to prevent this from happening.

I think the OccupancyGrid concept needs to be reworked, but I didn't want to do it here. Better method naming is perhaps best. I'd suggest deferring it to a follow-up PR though, where we can address semantics and code reuse.

Fixed typos in aa3f86c.

Another aspect of the current changes to the OccupancyGrid that is perhaps more controversial is the use of plain ints.

I understand why std::size_t was used but arithmetic with unsigned numeric types is error prone even when defined, and thus why you normally don't see unsigned scalars in vector types. I'd also argue that the current API is unnecessarily low level. Storage layout details can be abstracted without compromising performance.

beluga_system_tests/test/test_system.cpp

beluga/include/beluga/algorithm/raycasting.hpp

Signed-off-by: Michel Hidalgo <[email protected]>

serraramiro1 · 2023-05-03T17:18:42Z

LGTM

glpuga

Great work!

beluga/include/beluga/algorithm/raycasting.hpp

beluga/test/benchmark/benchmark_raycasting.cpp

beluga_amcl/include/beluga_amcl/occupancy_grid.hpp

beluga_amcl/src/amcl_node.cpp

beluga_example/config/params.yaml

Signed-off-by: Michel Hidalgo <[email protected]>

hidmic · 2023-05-05T11:23:44Z

Anything else here @glpuga @nahueespinosa ?

nahueespinosa

LGTM!

nahueespinosa · 2023-05-05T11:57:39Z

beluga/include/beluga/sensor/beam_model.hpp

@@ -0,0 +1,186 @@
+// Copyright 2022 Ekumen, Inc.


+1 to using 2023

glpuga · 2023-05-05T12:25:25Z

@hidmic Nop, it GO on my side. 🟢🟢🟢

Signed-off-by: Michel Hidalgo <[email protected]>

olmerg · 2023-05-12T19:22:39Z

Which tasks of Make beluga_amcl a drop-in replacement of nav2_amcl was implemented? @hidmic

hidmic · 2023-05-12T19:25:51Z

It would say (in terms of parameters):

lambda_short

z_max

z_short

Beam skip parameters have not been implemented. It's a rather simple variation though.

serraramiro1 commented Apr 4, 2023

View reviewed changes

serraramiro1 requested a review from nahueespinosa April 4, 2023 13:52

nahueespinosa added the do-not-land Do not merge these changes into the main branch label Apr 4, 2023

nahueespinosa changed the title ~~[DO NOT MERGE] Add beam sensor model~~ Add beam sensor model Apr 4, 2023

nahueespinosa added enhancement New feature or request cpp Related to C++ code labels Apr 4, 2023

serraramiro1 force-pushed the ramiro/beam_sensor_model branch from b05bb3f to 7596970 Compare April 4, 2023 14:28

hidmic reviewed Apr 12, 2023

View reviewed changes

hidmic self-assigned this Apr 14, 2023

glpuga reviewed Apr 15, 2023

View reviewed changes

beluga/include/beluga/algorithm/raycasting.hpp Outdated Show resolved Hide resolved

glpuga reviewed Apr 15, 2023

View reviewed changes

beluga/include/beluga/algorithm/raycasting.hpp Outdated Show resolved Hide resolved

glpuga reviewed Apr 15, 2023

View reviewed changes

beluga/include/beluga/algorithm/raycasting.hpp Outdated Show resolved Hide resolved

glpuga reviewed Apr 15, 2023

View reviewed changes

beluga/include/beluga/algorithm/raycasting.hpp Outdated Show resolved Hide resolved

hidmic mentioned this pull request May 2, 2023

[DRAFT] [WON'T MERGE] Beam sensor model rework #168

Closed

6 tasks

serraramiro1 and others added 12 commits May 3, 2023 00:52

Add beam model and raycasting

b594bf2

Signed-off-by: Ramiro Serra <[email protected]>

Lint

f4df081

Signed-off-by: Ramiro Serra <[email protected]>

Small changes, add defaults, variant

1d6576a

Signed-off-by: Ramiro Serra <[email protected]>

Remove debug prints, temporarily set params to use beam model

0ca6cee

Signed-off-by: Ramiro Serra <[email protected]>

Fix tests

d4f9d4c

Signed-off-by: Ramiro Serra <[email protected]>

Fix includes, fix tests comments

d106f84

Signed-off-by: Ramiro Serra <[email protected]>

Fix includes order

f0c06f8

Signed-off-by: Ramiro Serra <[email protected]>

Rework beam sensor model and raycasting

7f8b7ea

Signed-off-by: Michel Hidalgo <[email protected]>

Add beam sensor model to AMCL node

61b3ee2

Signed-off-by: Michel Hidalgo <[email protected]>

Update examples

a0bc68d

Signed-off-by: Michel Hidalgo <[email protected]>

Update system tests

05ec8f2

Signed-off-by: Michel Hidalgo <[email protected]>

Update docs and style

c77b3d2

Signed-off-by: Michel Hidalgo <[email protected]>

hidmic force-pushed the ramiro/beam_sensor_model branch from 31d666c to c77b3d2 Compare May 3, 2023 13:51

hidmic added 3 commits May 3, 2023 14:13

Get documentation to build

c95a260

Signed-off-by: Michel Hidalgo <[email protected]>

Please clang-tidy

48bac19

Signed-off-by: Michel Hidalgo <[email protected]>

Drop debugging headers

8944c9b

Signed-off-by: Michel Hidalgo <[email protected]>

serraramiro1 commented May 3, 2023

View reviewed changes

beluga/include/beluga/algorithm/raycasting.hpp Outdated Show resolved Hide resolved

Address peer review comments

aa3f86c

Signed-off-by: Michel Hidalgo <[email protected]>

hidmic mentioned this pull request May 3, 2023

Research alternative to Bresenham's raytracing algorithm #167

Open

2 tasks

Fix raytracing benchmark

f43c8b4

Signed-off-by: Michel Hidalgo <[email protected]>

hidmic removed the do-not-land Do not merge these changes into the main branch label May 3, 2023

hidmic requested a review from glpuga May 3, 2023 18:30

hidmic mentioned this pull request May 3, 2023

Rework occupancy grids #171

Closed

2 tasks

glpuga approved these changes May 3, 2023

View reviewed changes

Address peer review comments

7120efd

Signed-off-by: Michel Hidalgo <[email protected]>

nahueespinosa approved these changes May 5, 2023

View reviewed changes

Update copyright date

875d382

Signed-off-by: Michel Hidalgo <[email protected]>

hidmic merged commit 966b3ed into main May 5, 2023

hidmic deleted the ramiro/beam_sensor_model branch May 5, 2023 13:25

nahueespinosa mentioned this pull request May 9, 2023

Implement beam sensor model #99

Closed

1 task

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Add beam sensor model #160

Add beam sensor model #160

serraramiro1 commented Apr 4, 2023 •

edited by hidmic

Loading

serraramiro1 Apr 4, 2023

hidmic Apr 12, 2023

hidmic left a comment

hidmic Apr 12, 2023

hidmic Apr 12, 2023

hidmic commented Apr 14, 2023

glpuga Apr 15, 2023

nahueespinosa May 5, 2023 •

edited

Loading

hidmic May 5, 2023

glpuga Apr 15, 2023

hidmic commented May 3, 2023

serraramiro1 May 3, 2023

serraramiro1 May 3, 2023

hidmic May 3, 2023

hidmic May 3, 2023

hidmic May 3, 2023

serraramiro1 commented May 3, 2023

glpuga left a comment

hidmic commented May 5, 2023

nahueespinosa left a comment

nahueespinosa May 5, 2023 •

edited

Loading

glpuga commented May 5, 2023 •

edited

Loading

olmerg commented May 12, 2023

hidmic commented May 12, 2023 •

edited

Loading

	* - Give possibly const values `x` and `y` of type `int`, `g.index(x, y)` returns a `std::size`,
	* - Given possibly const values `x` and `y` of type `int`, `g.index(x, y)` returns a `std::size`,

Add beam sensor model #160

Add beam sensor model #160

Conversation

serraramiro1 commented Apr 4, 2023 • edited by hidmic Loading

Summary

Checklist

Choose a reason for hiding this comment

Choose a reason for hiding this comment

hidmic left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

hidmic commented Apr 14, 2023

Choose a reason for hiding this comment

nahueespinosa May 5, 2023 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

hidmic commented May 3, 2023

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

serraramiro1 commented May 3, 2023

glpuga left a comment

Choose a reason for hiding this comment

hidmic commented May 5, 2023

nahueespinosa left a comment

Choose a reason for hiding this comment

nahueespinosa May 5, 2023 • edited Loading

Choose a reason for hiding this comment

glpuga commented May 5, 2023 • edited Loading

olmerg commented May 12, 2023

hidmic commented May 12, 2023 • edited Loading

serraramiro1 commented Apr 4, 2023 •

edited by hidmic

Loading

nahueespinosa May 5, 2023 •

edited

Loading

nahueespinosa May 5, 2023 •

edited

Loading

glpuga commented May 5, 2023 •

edited

Loading

hidmic commented May 12, 2023 •

edited

Loading