-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
- Loading branch information
1 parent
6cf06bc
commit 9a60a1c
Showing
11 changed files
with
374 additions
and
67 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,323 @@ | ||
| <!DOCTYPE html> | ||
| <html lang="en"> | ||
|
|
||
| <head> | ||
| <meta charset="UTF-8"> | ||
| <meta name="viewport" content="width=device-width, initial-scale=1.0"> | ||
| <title>Projective ICP Visual Odometry</title> | ||
|
|
||
| <link rel="stylesheet" href="https://cdn.jsdelivr.net/npm/katex@0.16.2/dist/katex.min.css"> | ||
| <script defer src="https://cdn.jsdelivr.net/npm/katex@0.16.2/dist/katex.min.js"></script> | ||
| <script defer src="https://cdn.jsdelivr.net/npm/katex@0.16.2/dist/contrib/auto-render.min.js" | ||
| onload="renderMathInElement(document.body);"></script> | ||
|
|
||
| <style> | ||
| .centered-div { | ||
| width: 1000px; /* Set the width of the div */ | ||
| margin: 0 auto; /* This centers the div horizontally */ | ||
| } | ||
| .container { | ||
| display: flex; | ||
| justify-content: center; /* Horizontally center */ | ||
| background-color: rgb(253, 253, 253) | ||
| } | ||
| .space30px { | ||
| margin-bottom: 30px; /* Adjust the value to control the spacing */ | ||
| } | ||
| .space20px { | ||
| margin-bottom: 20px; /* Adjust the value to control the spacing */ | ||
| } | ||
| .space10px { | ||
| margin-bottom: 10px; /* Adjust the value to control the spacing */ | ||
| } | ||
| body { | ||
| font-family: 'Helvetica Neue', Arial, sans-serif; | ||
| } | ||
| h1 { | ||
| color: rgb(43, 43, 43) | ||
| } | ||
| h2 { | ||
| color: rgb(43, 43, 43) | ||
| } | ||
| h3 { | ||
| color: rgb(43, 43, 43) | ||
| } | ||
| h4 { | ||
| color: rgb(43, 43, 43) | ||
| } | ||
| p { | ||
| color: rgb(87, 87, 87) | ||
| } | ||
| li { | ||
| color: rgb(87, 87, 87) | ||
| } | ||
| </style> | ||
| </head> | ||
|
|
||
| <body> | ||
|
|
||
| <div class="container"> | ||
| <div class="centered-div"> | ||
|
|
||
| <h1 style="color: rgb(88, 88, 231);">Visual Odometry</h1> | ||
|
|
||
| <hr style="height:1px;border-width:0;color:rgb(232, 232, 232);background-color:rgb(217, 217, 217, 0.7)"> | ||
|
|
||
| <h1>Projective ICP Visual Odometry</h1> | ||
|
|
||
| <hr style="height:1px;border-width:0;color:rgb(232, 232, 232);background-color:rgb(217, 217, 217, 0.7)"> | ||
|
|
||
| <p align="center"> | ||
| <img src="media/icp.gif" alt="First Estimate" width="900"/> | ||
| </p> | ||
|
|
||
| <h2>Description</h2> | ||
| <hr style="height:1px;border-width:0;color:rgb(232, 232, 232);background-color:rgb(217, 217, 217, 0.7)"> | ||
| <p> | ||
| This project implements a projective ICP based visual odometry, to estimate both the trajectory of a robot and the 3D map of the visual features. | ||
| The robot is equipped with a monocular camera with known intrinsic and extrinsic parameters. | ||
| </p> | ||
|
|
||
| <div class="space30px"></div> | ||
|
|
||
| <h3>Data</h3> | ||
| <p> | ||
| Set of 120 measurements where each measurement contains a variable set of pairs (image_point, appearance), where: | ||
| <ul> | ||
| <li>image point: \([r,c]\) 1x2 vector.</li> | ||
| <li>appearance: \([f_1, ..., f_{10}]\) 1x10 vector (feature descriptor).</li> | ||
| </ul> | ||
| </p> | ||
|
|
||
| <div class="space30px"></div> | ||
| <h3>Algorithm</h3> | ||
|
|
||
| <div class="space20px"></div> | ||
| <h4>Initialization step</h4> | ||
| <p> | ||
| The initial pose (the one relative to the measurement 0) is set to the identity. Then the estimate of the first pose is computed with the following steps: | ||
| </p> | ||
| <ol> | ||
| <li>Match the image points of the measurement 0 with the image points of the measurement 1 using the appearance;</li> | ||
| <li>Estimate the essential matrix relative to the first two camera poses with the set of matched image points;</li> | ||
| <li>Recover the relative position between the two camera poses using the essential matrix and the set of matched image points.</li> | ||
| </ol> | ||
| <p> | ||
| These steps provide a first estimate of the pose of the camera. | ||
| </p> | ||
| <p> | ||
| Then a first estimate of the map is computed by triangulating the image points of the first two measurements, using the estimated pose. | ||
| </p> | ||
|
|
||
| <div class="space20px"></div> | ||
| <h4>Update step</h4> | ||
|
|
||
| <p> | ||
| The update step takes one measurement at a time and performs the projective ICP algorithm between the current measurement and the current estimated 3D map, to recover the relative pose of the camera with these steps: | ||
| </p> | ||
| <ol> | ||
| <li>Match the image points of the current measurement with the 3D points of the map using the appearance;</li> | ||
| <li>Perform one step of the projective ICP algorithm;</li> | ||
| <li>Repeat until the maximum number of iterations is reached or a stopping criterion is met.</li> | ||
| </ol> | ||
| <p> | ||
| Using the new estimated pose of the camera from the projective ICP, a new set of 3D points are triangulated and added to the estimated map. | ||
| </p> | ||
| <p> | ||
| The update step is repeated for each measurement. | ||
| </p> | ||
|
|
||
| <div class="space20px"></div> | ||
| <h4>Projective ICP</h4> | ||
| <p> | ||
| A single step of the projective ICP is divided into two parts: | ||
| </p> | ||
| <ol> | ||
| <li>Linearize the problem;</li> | ||
| <li>Resolve the linearized problem with a least square approach.</li> | ||
| </ol> | ||
| <p> | ||
| The linearization part takes as input the reference image points (from the measurement) and the current world points from the estimated map, already matched, and the current pose of the camera with respect to the world \( {}^wT_{c_0}\). Then, it calculates the matrix \(H\) and the vector \(b\) by computing for each pair of points the error \(e\) and the Jacobian \(J\) in this way: | ||
| </p> | ||
|
|
||
| <p> | ||
| \[ | ||
| \begin{align} | ||
| \text{World point in camera coordinates (hom): } &\hat{p}_{hom} = inv({}^wT_{c_0}) p_{w,hom}\\ | ||
| \text{World point in camera coordinates: } &\hat{p} = p_{w,hom}[:3]/p_{w,hom}[3]\\ | ||
| \text{World point on image plane (hom): } &\hat{p}_{cam}=K p_w\\ | ||
| \end{align} | ||
| \] | ||
| </p> | ||
|
|
||
| <div class="space20px"></div> | ||
| <p> | ||
| \[ | ||
| \begin{align} | ||
| e = p_r-\hat{p}_{cam} | ||
| \end{align} | ||
| \] | ||
| </p> | ||
|
|
||
| <div class="space20px"></div> | ||
| <p> | ||
| \[ | ||
| \begin{align} | ||
| J &= J_{proj}(\hat{p}_{cam}) K J_{icp}\\ | ||
| J_{icp} &= [I_{3\times3} | ⌊-\hat{p}⌋_\times] \\ | ||
| J_{proj}(\hat{p}_{cam}) &= \begin{bmatrix}\frac{1}{z} & 0 & -\frac{x}{z^2} \\ 0 & \frac{1}{z} & \frac{y}{z^2} \end{bmatrix} | ||
| \end{align} | ||
| \] | ||
| </p> | ||
|
|
||
| <p> | ||
| Then the error is used to compute \(chi = e^T \times e\), then: | ||
| </p> | ||
| <ul> | ||
| <li>if \(chi \le kernel\_threshold\) point is an inlier,</li> | ||
| <li>otherwise the points in an outlier.</li> | ||
| </ul> | ||
|
|
||
| <p> | ||
| The errors and Jacobians from the inliers are used to compute \(H\) and \(b\) as: | ||
| </p> | ||
| <p> | ||
| \[ | ||
| \begin{align} | ||
| H &= H + J^T J \\ | ||
| b &= b + J^T e | ||
| \end{align} | ||
| \] | ||
| </p> | ||
|
|
||
| <p> | ||
| Then a 6D vector describing the relative pose of the camera with respect to the previous pose is calculated by solving: | ||
| </p> | ||
| <p> | ||
| \[ | ||
| \begin{align} | ||
| dx \leftarrow slove_{lstq}(H dx = -b) \\ | ||
| {}^wT_{c_1} = v2T(dx) {}^wT_{c_0} | ||
| \end{align} | ||
| \] | ||
| </p> | ||
|
|
||
| <h2>Results</h2> | ||
| <hr style="height:1px;border-width:0;color:rgb(232, 232, 232);background-color:rgb(217, 217, 217, 0.7)"> | ||
|
|
||
| <div class="space20px"></div> | ||
| <h3>Visual results</h3> | ||
| <p align="center"> | ||
| <iframe src="media/3D_plot.html" width="900" height="600"></iframe> | ||
| </p> | ||
|
|
||
|
|
||
| <div class="space20px"></div> | ||
| <h3>Numerical results</h3> | ||
|
|
||
| <table border="1" cellpadding="5" cellspacing="0" align="center"> | ||
| <tr> | ||
| <th>Rotation Errors</th> | ||
| <th></th> | ||
| <th>Translation Errors</th> | ||
| <th></th> | ||
| <th>Translation Errors Ratio</th> | ||
| <th></th> | ||
| </tr> | ||
| <tr> | ||
| <td>Max rotation error</td> | ||
| <td>0.200 [rad] - 11.464 [deg]</td> | ||
| <td>Max translation error</td> | ||
| <td>0.395 [m]</td> | ||
| <td>Max translation error ratio</td> | ||
| <td>5.117</td> | ||
| </tr> | ||
| <tr> | ||
| <td>Min rotation error</td> | ||
| <td>0.000 [rad] - 0.000 [deg]</td> | ||
| <td>Min translation error</td> | ||
| <td>0.004 [m]</td> | ||
| <td>Min translation error ratio</td> | ||
| <td>4.71346</td> | ||
| </tr> | ||
| <tr> | ||
| <td>Mean rotation error</td> | ||
| <td>0.029 [rad] - 1.666 [deg]</td> | ||
| <td>Mean translation error</td> | ||
| <td>0.179 [m]</td> | ||
| <td>Mean translation error ratio</td> | ||
| <td>4.92998</td> | ||
| </tr> | ||
| </table> | ||
|
|
||
| <div class="space20px"></div> | ||
|
|
||
| <table border="1" cellpadding="5" cellspacing="0" align="center"> | ||
| <tr> | ||
| <th>Map Results</th> | ||
| <th></th> | ||
| </tr> | ||
| <tr> | ||
| <td>Number of points estimated map</td> | ||
| <td>314</td> | ||
| </tr> | ||
| <tr> | ||
| <td>RMSE estimated map</td> | ||
| <td>0.14540 [m]</td> | ||
| </tr> | ||
| <tr> | ||
| <td>Scale</td> | ||
| <td>0.20284</td> | ||
| </tr> | ||
| </table> | ||
|
|
||
| <div class="space20px"></div> | ||
|
|
||
| <p align="center"> | ||
| <img src="media/errors.png" alt="Errors" width="900"/> | ||
| </p> | ||
|
|
||
| <table style="width: 100%;" align="center"> | ||
| <tr> | ||
| <td style="width: 40%; vertical-align: top; text-align: left"> | ||
| \[ | ||
| \begin{align} | ||
| T_{rel}^{est} &= {}^{r_i}T_{r_{i+1}}^{est} = inv({}^wT_{r_i})^{est} {}^wT_{r_{i+1}}^{est} \\ | ||
|
|
||
| T_{rel}^{gt} &= {}^{r_i}T_{r_{i+1}}^{gt} = inv({}^wT_{r_i})^{gt} {}^wT_{r_{i+1}}^{gt} \\ | ||
|
|
||
| T_{error} &= {}^{r_{i+1}^{est}}T_{r_{i+1}^{gt}}^{error} = inv(T_{rel}^{est}) T_{rel}^{gt} \\ | ||
|
|
||
| R_{error} &= T_{error}[0:3,0:3] \\ | ||
|
|
||
| t_{rel}^{est} &= T_{rel}^{est}[0:3,3] \\ | ||
|
|
||
| t_{rel}^{gt} &= T_{rel}^{gt}[0:3,3] \\ | ||
| \end{align} | ||
| \] | ||
| </td> | ||
| <td style="width: 60%; vertical-align: top; text-align: left;"> | ||
|
|
||
| \[ | ||
| \begin{align} | ||
| rot\_error \space [rad] &= arccos((trace(R_{error})-1)/2) \\ | ||
|
|
||
| translation\_error\_ratio &= norm(t_{rel}^{est})/norm(t_{rel}^{gt}) \\ | ||
|
|
||
| scale &= 1/translation\_error\_ratio \\ | ||
|
|
||
| translation\_error \space [m] &= norm(scale \cdot t_{rel}^{est}-t_{rel}^{gt}) \\ | ||
| \end{align} | ||
| \] | ||
| </td> | ||
| </tr> | ||
| </table> | ||
|
|
||
|
|
||
| </div> | ||
| </div> | ||
|
|
||
| </body> | ||
| </html> | ||
|
|
||
|
|
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Large diffs are not rendered by default.
Oops, something went wrong.
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Oops, something went wrong.