Description

SLAM Solvers
1 2D Linear SLAM
In this problem you will implement your own 2D linear SLAM solver in linear.py. Data are provided in 2d linear.npz, 2d linear loop.npz with loaders. Everything you need to know to complete this problem was covered in class, so please refer to your notes and lecture slides for guidance.
We will be using the least squares formulation of the SLAM problem, which was presented in class:
x∗ = argmin
.
(1)
≈ argminkAx − bk2 ,
x
where zi is the i-th measurement, hi (x) is the corresponding prediction function, and denotes the squared Mahalanobis distance: aTΣ−1a.
In this problem, the state vector x is comprised of the trajectory of robot positions and the landmark positions. Both positions are simply (x,y) coordinates. For a sanity check, we visualize the ground truth trajectory and landmarks in the beginning.
There are two types of measurements: odometry and landmark measurements. Odometry measurements give a relative (∆x,∆y) displacement from the previous position to the next position (in global frame). Landmark measurements also give a relative displacement (∆x,∆y) from the robot position to the landmark (also in global frame).
1.1 Measurement function (10 points)
Given robot poses r and r at time t and t + 1 , write out the measurement function and its Jacobian. (5 points)
ho(rt, rt+1) : R4 → R2,
Ho(rt, rt+1) : R4 → R2×4.
Similarly, given the robot pose at time t and the k-th landmark
l . (5 points)
hl(rt,lk) : R4 → R2,
Hl(rt,lk) : R4 → R2×4.
1.2 Build a linear system (15 points)
Use the derivation above, please complete the function create linear system to construct the linear system as described in Eq. 1. (15 points)
• Note in this setup, you will be filling the blocks in the large linear system that is aimed at batch optimizing the large state vector stacking all the robot and landmark positions. Please carefully select indices for both measurements and states when you fill in Jacobians.
• Use int to convert observation[:,0] and obsevation[:,1] into pose and landmark indices respectively.
• In addition, you will have to add a prior to the first robot pose, otherwise the system will be underconstrained and the state will be subject to an arbitrary global transformation.
• Please refer to the function document for detailed instructions.
1.3 Solvers (20 points)
Given the data and the linear system, you are now ready to solve the 2D linear SLAM problem. You are required to implement 5 solvers to solve Ax = b where A is a sparse non-square matrix.
1.4 Exploit sparsity (30 points + 10 points)
Now we want to exploit sparsity in the linear system in QR and LU factorizations.
1. lucholmod. Change the ordering from the default NATURAL to COLAMD (Column approximate minimum degree permutation) and return x, U. (5 points)
2. (Bonus) Instead of LU’s built-in solver, write your own forward/backward substitution to compute x. Note because of reordering (permutation), you need to manipulate both rows and columns. Please check online documents for more details. (10 points)
3. qrcholmod. Change the ordering from the default NATURAL to COLAMD (Column approximate minimum degree permutation) and return x, R. Note now you have to use E from sparseqr.rz.
sparseqr.permutationvectortomatrix can be useful for permutation. (5 points)
4. Now proceed with 2dlinear.npz, visualize the trajectory and landmarks, and report the efficiency of your method in terms of run time. Attach the visualization and analysis of corresponding factor for qr,qr colamd,lu,lucolamd. What are your observations and their potential reasons (general comparison between QR, LU, and their reordered version)? (10 points)
5. Similarly, process 2dlinearloop.npz. Are there differences in efficiency comparing to 2dlinear.npz? Write down your observations and reasoning. (10 points)
2 2D Nonlinear SLAM
Now you are going to extend the linear SLAM to a nonlinear version in nonlinear.py.
The problem set-up is exactly the same as in the linear problem, except we introduce a nonlinear measurement function that returns a bearing angle θ and range d (in robot’s body frame, notice we assume that this robot always perfectly facing the x-direction of the global frame), which together describe the vector from the robot to the landmark:
. (2)
2.1 Measurement function (10 points)
In your nonlinear algorithm, you’ll need to predict measurements based on the current state estimate.
1. Fill in the functions odometryestimation, and bearing range estimation with corresponding measurement functions. The odometry measurement function is the same linear function we used in the linear SLAM algorithm, while the landmark measurement function is the new nonlinear function introduced in Eq. 2. Please carefully check indices and offsets in the state vector. (5 points)
2. Derive the jacobian of the nonlinear landmark function in your writeup
,
and implement the function compute meas obs jacobian to calculate the jacobian at the provided linearization point. (5 points)
2.2 Build a linear system (15 points)
Use the derivation above, implement create linear system, now in nonlinear.py, to generate the linear system A and b at the current linearization point. (15 points) In addition to the notes for the linear case, please remember to
• Use the provided initialization of x as the linearization point.
• Error per observation is the difference of measurements and estimates because of linearization.
• Use warp2pi to normalize the difference of angles.
2.3 Solver (10 points)
Process 2d nonlinear.npz. Select one solver you have implemented, and visualize the trajectory and landmarks before and after optimization. Briefly summarize the differences between the optimization process of the linear and the non-linear SLAM problems. (10 points)
3 Code Submission
Instructions:
• Use conda to create an environment and run ./installdeps.sh to install dependencies. If you encounter failures, please install SuiteSparse to your system (usually already installed), see dependencies for sparseqr.
• Read documents and source code for the packages (scipy.sparse.linalg and sparseqr). It is a good exercise to learn to use libraries you are not familiar with.
• Use command line arguments. For instance, you can run python linear.py ../data/2d linear.npz –method pinv qr qr colamd lu lu colamd to check the results of all methods altogether on the 2dlinear case.
Please upload your code to gradescope excluding the data folder.