Efficient Lidar Lookup / Lidar updates
Right now the
Lidar ee-component logic is highly inefficient: it iterates over all the edges of all the buildings in the map per lidar trace (8 per vehicle).
It also accesses the vehicle position in a conceptually wrong way.
Its math can also be improved.
Use the grid acceleration structure in
CollisionDetection and traverse it (up to a max distance) starting from the lidar "ray" origin. Then check the buildings referenced in the grid cell.
CollisionDetectionavailable in the BuildContext for the ee-components.
- Implement a cell traversal + have a hash-set to remove duplicate checks on buildings (a single building can be linked in multiple overlapping cells)
- Choose a sensible max distance for the lidar check after which the grid traversal is stopped.
Accessing the position
Right now, the Lidar component listens for
true_position messages. Such messages are intended as "measured position" for use by components that would actually need to get an estimate of the position in a real vehicle. (Such as the Navigation component).
The physics of the Lidar are not dependent on a measured position: they happen in the real world. The Lidar should thus use the position of the dynamic_object of the vehicle.
On the other hand, the distance measured by the lidar could be modeled as imprecise, for example introduction noise.
Bonus Tasks: Clearer lidar math
- IPM for the math: store the vectors in the Lidar object and re-use them using IPM functions.
- Vehicle "model matrix":
- The lidar "rays" (position and direction vectors) can be defined in the local vehicle space once.
- When computing/updating, generate one rotation/translation matrix (in extended coordinate space) for the vehicle
- Use this matrix in the
computeShortestDistance()function to get the lidar ray position and direction in world space
- Overall edge/ray intersection math can be revisited (use simple dot products, edge "normals", ...) + check if the intersection is "in front" of the ray (does not seem to be the case/to be verified).