I'm currently taking an introductory course in Autonomous Systems where our project involves implementing the FastSLAM algorithm for an Alphabot2. This project uses a camera and Aruco markers as landmarks. We aim to develop a dynamic covariance matrix for our measurement model that adjusts based on the distance to the camera.

We have managed to figure out a way to calculate a formula for the covariance matrix based on this paper. Specifically, we followed the method outlined in the paper to analyze the reprojection errors and compute the covariance matrix as a function of the distance.

Here are the steps we took:

  1. Calculated the reprojection errors for images in different ranges.
  2. Plotted these errors to observe their behavior with respect to distance.
  3. Derived the measurement noise covariance matrix $R$ as follows:

$$ \begin{bmatrix}(a \cdot \text{range} + b)^2 & 0 \\ 0 & (a \cdot \text{range} + b)^2 \end{bmatrix} $$

The reprojection errors, when plotted, showed a decreasing trend with distance, which is consistent with the results found in the paper. The paper suggests reprojection errors decrease with distance due to the error propagation properties, errors decrease with distance due to the diminishing impact of 3D coordinate estimation errors on the 2D image plane as distance increases. However, I am a bit confused about the relationship between the standard deviation and variance in this context:

  • The standard deviation of the reprojection errors decreases with increasing distance,  due to the diminishing effect of error propagation in 3D space.

  • Simultaneously, the variance of the measurement noise covariance matrix increases with distance, indicating higher uncertainty for distant landmarks. Could you help clarify why there is this apparent contradiction? Specifically, how can the standard deviation of reprojection errors decrease while the overall measurement noise covariance increases with distance?

Additionally, the covariance matrix is calculated in the image plane and is measured in pixels. Our Aruco detector, however, provides the transformations indicating the pose of the Aruco markers relative to the camera (using tvec and rvec), which are measured in meters. Therefore, we need to convert from 2D image plane coordinates to 3D coordinates.

Our professor suggested this would be an interesting topic to develop and provided the following guidance:

Given the pinhole sensor model: $$ x = f_x \frac{X}{Z} + c_x, \quad y = f_y \frac{Y}{Z} + c_y \quad \implies \quad R_{\text{2D}} = J_c R_{\text{3D}} J_c^T. $$

Where $J_c$ is the Jacobian which transforms 3D coordinates to the image plane, $R_{\text{3D}}$ is the covariance matrix in the 3D plane, and $R_{\text{2D}}$ is the covariance matrix we have obtained. We then have an optimization problem with the Frobenius norm: $$ \operatorname{argmin}_{R_{\text{3D}}} \big\|R_{\text{2D}} - J_c R_{\text{3D}} J_c^T\big\|^2_F. $$

Could anyone provide guidance or resources on how to approach this problem? Specifically, how to set up and solve this optimization to find $R_{\text{3D}}$?



Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.