Add MCL to main stack

[wheitman]

• Tune resample rate
• Tune particle count, add adaptive MCL

[wheitman]

In progress. Need to adjust grid to include sidewalks, medians, and other hard, flat surfaces. Should make MCL more generalized to numerous settings.

[wheitman]

[wheitman]

Issue is that the drivable area grid is now in the base_link frame. I need to update the update_weights function within mcl.py to handle this change, perhaps by checking the frame ID of the map grid.

[wheitman]

Pseudocode

Variables

• particles: cloud in map
• map: grid in base_link
• lidar: cloud in base_link

Procedure

1. Transform particles to base_link using result from previous iteration.
2. For each particle:
   a. Get base_link->particle transform
   b. Apply transform to get lidar cloud in particle frame
   c. Assign each lidar point to a grid cell (round x/y to int)
   d. For each point, if point's class matches its cell, alignment++
   e. Weight = normalized alignment

[wheitman]

I believe that the LiDAR cloud is being transformed to the particle frame correctly, though something is still off with update_weights. The filter does not converge to an appropriate state. Resampling rate does seem reasonable, and weights are being updated frequently as well. Will need further tuning.

[wheitman]

Still working on this.

The good news is that the particle motion update is smooth thanks to a transition to speedometer and IMU-based delta calculations. This means that the vehicle is usually not jumpy as with GNSS.

The bad news is that the particles are frequently resampled and then they (frustratingly) converge at either the perfectly-aligned position or one that's totally wrong.

I've verified that the alignments are being correctly calculated by plotting each particle onto the grid.

Tomorrow, I'd like to try a so-bad-it-may-be-good approach:

1. Generate a bunch of particles in a Gaussian distribution around the latest GNSS pose (e.g. +/- 2 meters, +/- 0.2 radians)
2. For each particle, calculate its alignment (as mentioned, I believe this part works)
3. Choose the most aligned particle. Make this our pose.
4. Repeat.

[wheitman]

I'm still working on this. A stable result is frustratingly elusive. I will not give up.

[wheitman]

Yes, I'm still working on this. The voting method works, barely. It's a good foundation for a revised particle filter, though, since I've verified that alignments are properly calculated etc etc.

I also took a detour to colorize the semantic segmentation output, both in 2D and 3D, and to update the documentation with all possible tags from CityScapes.

[wheitman]

Dead reckoning now works. This means that we can afford to be more selective about which short-term observations are incorporated. We can throw out noisy data.

dead_reckoning-2023-02-01_12.56.57.mp4

[wheitman]

Good news: I've added RTrees in the MapManager for both objects (such as speed limit signs, crosswalks) and signals (stop signs, traffic lights). Signals did not even exist in libOpenDRIVE at all, but the logic was similar to the Object's.

This means I'm almost ready to add landmark constraints to the particle filter.

Bad news: There's a significant memory leak with the 2D semantic segmentation node. It's likely upstream with mmsegmentation, so I may need to switch frameworks.

Anyway, see some terminal output of nearby objects:

[wheitman]

Closed with #326 . Works for now. Requires additional constraints and tuning, but the foundation is solid.