A mapping robot carries a laser scanner, but not at its centre: the scanner is bolted somewhere on the body and may even face backwards. Its readings come out as points in the scanner's own frame (x along the scanner's forward direction, y to the scanner's left, metres). To draw a map, every point must end up in the world frame (x to the right, y up).
Three frames are involved, and two poses connect them, each written (x, y, theta) with theta in radians, counter-clockwise:
mount: where the scanner sits in the robot's frame (the robot's frame has x forward and y to the robot's left, origin at its centre): the scanner's position, and the angle from the robot's forward axis to the scanner's forward axis.pose: where the robot is in the world: its centre, and the angle from the world's +x axis to the robot's forward axis.
scan is a list of points (x, y) in the scanner's frame. A beam that got no echo is recorded as None: leave it out of the result.
Write scan_to_world(pose, mount, scan) that returns the list of world points, as (x, y) tuples of floats, in the order of the scan without the None entries. Collect them and press Run with plot(xs, ys, kind="scatter") to see the room.
Examples
Input: pose = (2, 1, 1.5707963267948966), mount = (0.3, 0, 0), scan = [(1, 0), (0, 2), None]
Output: [(2.0, 2.3), (0.0, 1.3)]
Explanation: the scanner is 0.3 m in front of the centre, facing forward. The robot faces +y.
The point 1 m ahead of the scanner is 1.3 m ahead of the robot, so 1.3 m above (2, 1).
Input: pose = (0, 0, 0), mount = (-0.2, 0.1, 3.141592653589793), scan = [(1, 0), (0.5, -0.5)]
Output: [(-1.2, 0.1), (-0.7, 0.6)]
Explanation: this scanner sits at the back and faces backwards, so its "forward" is the
robot's -x and its "right" (negative y) is the robot's left.
Constraints
- answers are compared with a tolerance of
1e-6; do not round
Goals
- Chain two frame changes: sensor to robot, then robot to world
- Describe where a sensor sits on a robot with a pose of its own
- Process a whole scan, skipping beams without an echo