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

hidmic · 2023-05-02T13:31:52Z

Summary

I'm opening this up for visibility and because I'm running out of ideas. This isn't PR material, it's incomplete, and it must be split before it can be reviewed and merged. That said, it aggregates all functionality developed to date and rebase against main to get the beam sensor model up and running.

To reproduce:

ROSDISTRO=rolling docker/run.sh --build
colcon build --packages-up-to beluga_example
ros2 launch beluga_example example_rosbag_launch.py amcl.laser_model_type:=beam amcl.set_initial_pose:=True amcl.initial_pose.y:=2.0  # test Beluga implementation
ros2 launch beluga_example example_rosbag_launch.py package:=nav2_amcl node:=amcl amcl.laser_model_type:=beam  amcl.set_initial_pose:=True amcl.initial_pose.y:=2.0  # test Nav2 AMCL implementation

What you'll see is Beluga underperfoming in terms of accuracy and consistency through time compared to Nav2 AMCL.

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.

Signed-off-by: Ramiro Serra <serraramiro@ekumenlabs.com>

Signed-off-by: Michel Hidalgo <michel@ekumenlabs.com>

hidmic · 2023-05-02T13:48:58Z

Observations, tests, and hypothesis to date:

Initial particle distribution in state space for Beluga appears to be much wider than for Nav2 AMCL, even though both are configured the same.
Drawing pose estimates with no regards for weights does not cope well with wide or multi-modal state distributions. Add beam sensor model #160 was developed before Add WeightedStateEstimator2d mixin #161, so I went that down that rabbit hole again, unnecessarily. Still, we are not on par with Nav2 AMCL, which considers particle clusters. I suspect this may be part of the problem here.
Weights are normalized for pose estimation, but not during operation. I've seen them up in the 10e+32s. Renormalizing post resampling doesn't fix issues anyways.
Ray tracing bogs down the beam sensor model substantially. I spent some time benchmarking and profiling the integer arithmetic Bresenham algorithm implementation, to get times down, but I've been unable to lower it further. In our example, importance sampling can take up to 40ms for 2000 particles and 100 beams per scan. Reducing Eigen usage may help, but I suspect multi-resolution ray tracing is in order. Not for now.
Our perfect_odometry bagfile appears to contain LaserScan messages with negative ranges.
tf2_ros::Buffer::lookupTransform will sometimes yield an identity transform even when /tf data suggests otherwise. Querying for the last available time instead of an specific time (guaranteed to be available via tf2_ros::message_filters) seems to fix the glitch.
Reversing polar to cartesian coordinate conversion for laser scan points, as performed in Add beam sensor model #160, with no regards for base to laser transform yields skewed points. Thought that was the culprit at a time. Thus, LaserScan and RotatingBeamScan classes.

hidmic · 2023-05-02T13:55:50Z

beluga_amcl/src/amcl_node.cpp

      odom_to_base_transform);
  } catch (const tf2::TransformException & error) {
    RCLCPP_ERROR(get_logger(), "Could not transform from odom to base: %s", error.what());
    return;
  }

-  auto base_to_laser_transform = Sophus::SE3d{};
+  auto base_to_laser_transform = Sophus::SE2d{};


I figured I would ask. Here we lookup SE(2) odometry but use SE(3) transforms to get laser scans in the laser frame to the base frame. What's the rationale here? In what circumstances would a system with a non planar transform between base and laser frames be able to use planar AMCL?

In what circumstances would a system with a non planar transform between base and laser frames be able to use planar AMCL?

Fair question. I think the answer would be lasers that are mounted upside-down. I remember getting the idea from nav2_amcl/src/amcl_node.cpp#L780-L784.

Ah good point. That's super common, though I'd argue that working in 3D just for that case is unnecessary overhead. Let's take that discussion to #170.

hidmic · 2023-05-02T14:57:30Z

Things to try out as per team discussion:

Lower bagfile playback rate
Force zero initial pose covariance
Force constant resampling
Revisit scan pose transform

ivanpauno

partial review ...

ivanpauno · 2023-05-04T13:21:21Z

beluga/include/beluga/algorithm/raycasting.hpp

+/// Leverages a Brasenham raycasting technique to cast a ray on the occupancy grid, \n
+/// from the laser frame and with the provided bearing until \n
+///  it hits an unknown/occupied cell or reaches `max_beam_range`.
+/// Unknown cells are treated as occupied.
+/**
+ * \tparam OccupancyGrid Type that satisfies \ref OccupancyGridPage.
+ * \param grid Grid to cast the ray on.
+ * \param starting_position Position of the laser in map frame.
+ * \param bearing_in_laser_frame An SO2 rotation that represents the bearing of the beam in laser frame.
+ * \param max_beam_range Maximum range of the sensor in meters.
+ * \return double Length in meters of the casted ray.
+ */
+


nit: what is this documenting?

ivanpauno · 2023-05-04T13:21:50Z

beluga/include/beluga/algorithm/raycasting.hpp

+namespace beluga {
+
+/// Bresenham's 2D line drawing algorithm, optimized for integer arithmetic.
+struct bresenham2i {


note to self: review raycasting in detail later

hidmic · 2023-05-09T13:14:56Z

As this won't be merged, I'll close it but keep the branch around for #170.

Related to #170. Completes the implementation of the laser scan class proposed in #168. Signed-off-by: Nahuel Espinosa <nespinosa@ekumenlabs.com> Co-authored-by: Gerardo Puga <glpuga@ekumenlabs.com>

serraramiro1 and others added 8 commits May 2, 2023 10:07

Add beam model and raycasting

0062489

Signed-off-by: Ramiro Serra <serraramiro@ekumenlabs.com>

Lint

325ca7b

Signed-off-by: Ramiro Serra <serraramiro@ekumenlabs.com>

Small changes, add defaults, variant

0674f92

Signed-off-by: Ramiro Serra <serraramiro@ekumenlabs.com>

Remove debug prints, temporarily set params to use beam model

d4bf462

Signed-off-by: Ramiro Serra <serraramiro@ekumenlabs.com>

Fix tests

8e81fc4

Signed-off-by: Ramiro Serra <serraramiro@ekumenlabs.com>

Fix includes, fix tests comments

987aa3f

Signed-off-by: Ramiro Serra <serraramiro@ekumenlabs.com>

Fix includes order

c1147cb

Signed-off-by: Ramiro Serra <serraramiro@ekumenlabs.com>

Beam sensor model still misbehaves

8e63faa

Signed-off-by: Michel Hidalgo <michel@ekumenlabs.com>

hidmic commented May 2, 2023

View reviewed changes

This was referenced May 3, 2023

Generalize planar laser scan support #170

Closed

Add beam sensor model #160

Merged

Extend development containers #173

Merged

ivanpauno reviewed May 9, 2023

View reviewed changes

hidmic closed this May 9, 2023

nahueespinosa mentioned this pull request Dec 10, 2023

Generalize planar laser scan support #271

Merged

7 tasks

nahueespinosa deleted the hidmic/beam_sensor_model branch January 7, 2024 15:04

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

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

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

hidmic commented May 2, 2023 •

edited

Loading

hidmic commented May 2, 2023 •

edited

Loading

hidmic May 2, 2023

nahueespinosa May 2, 2023

hidmic May 3, 2023

hidmic commented May 2, 2023 •

edited

Loading

ivanpauno left a comment

ivanpauno May 4, 2023

ivanpauno May 4, 2023

hidmic commented May 9, 2023

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

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

Conversation

hidmic commented May 2, 2023 • edited Loading

Summary

Checklist

hidmic commented May 2, 2023 • edited Loading

hidmic May 2, 2023

Choose a reason for hiding this comment

nahueespinosa May 2, 2023

Choose a reason for hiding this comment

hidmic May 3, 2023

Choose a reason for hiding this comment

hidmic commented May 2, 2023 • edited Loading

ivanpauno left a comment

Choose a reason for hiding this comment

ivanpauno May 4, 2023

Choose a reason for hiding this comment

ivanpauno May 4, 2023

Choose a reason for hiding this comment

hidmic commented May 9, 2023

hidmic commented May 2, 2023 •

edited

Loading

hidmic commented May 2, 2023 •

edited

Loading

hidmic commented May 2, 2023 •

edited

Loading