A cleaning robot drives along a straight hallway and records its position at n moments, the poses x_0, \dots, x_{n-1} (metres along the hallway). It does not know them exactly. What it has are constraints, each a tuple (i, j, d, var) saying: "x_j - x_i was measured to be d, with error variance var". Most come from wheel odometry between consecutive poses; a loop closure is a constraint between poses far apart in time, for instance when the robot recognises the lift door it passed earlier.
The measurements disagree a little, so no positions satisfy all of them exactly. The best positions are those that make the weighted sum of squared misses smallest,
with x_0 = 0 held fixed (all constraints are differences, so without it the whole set could slide). This is a pose graph: poses are nodes, constraints are edges.
Write pose_graph_1d(n, constraints) that returns the list of the n best positions, starting with 0.0. numpy is available (np.zeros, np.linalg.solve).
Examples
Input: n = 5, constraints = [(0, 1, 1.0, 1.0), (1, 2, 1.0, 1.0), (2, 3, 1.0, 1.0), (3, 4, 1.0, 1.0), (0, 4, 3.6, 1.0)]
Output: [0.0, 0.9199999999999997, 1.8399999999999994, 2.7599999999999993, 3.6799999999999997]
Explanation: odometry puts pose 4 at 4.0, the loop closure at 3.6. All five constraints are equally
trusted, so each takes a fifth of the 0.4 disagreement: every step shrinks to 0.92, and the
closure stretches to 3.68.
Input: n = 5, constraints = [(0, 1, 1.0, 0.1), (1, 2, 1.0, 1.0), (2, 3, 1.0, 0.1), (3, 4, 1.0, 0.1), (0, 4, 3.6, 0.01)]
Output: [0.0, 0.9694656488549619, 1.6641221374045805, 2.6335877862595423, 3.603053435114504]
Explanation: the closure is now very precise and the step from pose 1 to 2 (over a slippery
patch) is ten times less trusted than the others, so that step absorbs most of the error.
Constraints
0 <= i, j < n,i != j,var > 0; every pose is linked to pose 0 through some chain of constraints- answers are compared with a tolerance of
1e-6; numpy arrays or tuples are accepted in place of lists
Goals
- Write a chain of odometry steps and a loop closure as weighted least-squares constraints
- Build and solve the normal equations with one pose held fixed
- See how a loop closure spreads the accumulated drift over the steps, most onto the least trusted ones