路标点位姿估计
问1:假设机器人站在原地不动，不妨设为\(\tau = \begin{bmatrix} \tau_x & \tau_y & \tau_{\theta} \end{bmatrix}^T\)， 通过相机持续观测一个路标，第\(i\)次观测得到路标相对于机器人的位姿为\({}_i\!Z = \begin{bmatrix} {}_i\!Z_x & {}_i\!Z_y & {}_i\!Z_{\theta} \end{bmatrix}^T\)。 现在需要估计路标在世界中的位姿\(X = \begin{bmatrix} x & y & \theta \end{bmatrix}^T\)。
正向来说，在没有任何测量误差的情况下，我们可以写出路标的位姿关系：
$$ \begin{equation} \begin{bmatrix} \cos(\theta) & -\sin(\theta) & x \\ \sin(\theta) & \cos(\theta) & y \\ 0 & 0 & 1 \\ \end{bmatrix} = \begin{bmatrix} \cos(\tau_{\theta}) & -\sin(\tau_{\theta}) & \tau_x \\ \sin(\tau_{\theta}) & \cos(\tau_{\theta}) & \tau_y \\ 0 & 0 & 1 \\ \end{bmatrix}\begin{bmatrix} \cos(Z_{\theta}) & -\sin(Z_{\theta}) & Z_x \\ \sin(Z_{\theta}) & \cos(Z_{\theta}) & Z_y \\ 0 & 0 & 1 \\ \end{bmatrix} \end{equation} $$ $$ \begin{equation} \begin{bmatrix} x \\ y \\ \theta \end{bmatrix} = \begin{bmatrix} \cos(\tau_{\theta})Z_x - \sin(\tau_{\theta})Z_y + \tau_x \\ \sin(\tau_{\theta})Z_x + \cos(\tau_{\theta})Z_y + \tau_y \\ \tau_{\theta} + Z_{\theta} \end{bmatrix} \end{equation} $$根据路标在世界中的位姿估计\(X = \begin{bmatrix} x & y & \theta \end{bmatrix}^T\)， 可以预测相机的观测值为\(\hat{Z} = \begin{bmatrix} \hat{Z_x} & \hat{Z_y} & \hat{Z_{\theta}} \end{bmatrix}^T\)。
$$ \begin{equation} \begin{bmatrix} \cos(\hat{Z_{\theta}}) & -\sin(\hat{Z_{\theta}}) & \hat{Z_x} \\ \sin(\hat{Z_{\theta}}) & \cos(\hat{Z_{\theta}}) & \hat{Z_y} \\ 0 & 0 & 1 \\ \end{bmatrix} = \begin{bmatrix} \cos(\tau_{\theta}) & \sin(\tau_{\theta}) & \tau_x^{-1} \\ -\sin(\tau_{\theta}) & \cos(\tau_{\theta}) & \tau_y^{-1} \\ 0 & 0 & 1 \\ \end{bmatrix} \begin{bmatrix} \cos(\theta) & -\sin(\theta) & x \\ \sin(\theta) & \cos(\theta) & y \\ 0 & 0 & 1 \\ \end{bmatrix} \end{equation} $$ $$ \begin{align} \tau_x^{-1} & = -\cos(\tau_{\theta})\tau_x - \sin(\tau_{\theta})\tau_y \\ \tau_y^{-1} & = \sin(\tau_{\theta})\tau_x - \cos(\tau_{\theta})\tau_y \end{align} $$ $$ \begin{equation} \begin{bmatrix} \hat{Z_x} \\ \hat{Z_y} \\ \hat{Z_{\theta}} \end{bmatrix} = \begin{bmatrix} \cos(\tau_{\theta})x + \sin(\tau_{\theta})y + \tau_x^{-1} \\ -\sin(\tau_{\theta})x + \cos(\tau_{\theta})y + \tau_y^{-1} \\ \theta - \tau_{\theta} \end{bmatrix} \end{equation} $$那么预测值与观测值之间的误差可以用\({}_i\!e = \hat{Z} - {}_i\!Z\)来表示。假设误差服从高斯分布\({}_i\!e \sim N(\boldsymbol{0}, {}_i\!\Sigma)\)，那么有非线性最小二乘问题：
$$ \min F(x) = \min \Sigma_i\left( {}_i\!e^T {}_i\!\Sigma^{-1} {}_i\!e^T\right) $$