diff --git a/electives/amr/autonomous-mobile-robots-cheatsheet.pdf b/electives/amr/autonomous-mobile-robots-cheatsheet.pdf index 10a7d3b..f9e8b2d 100644 Binary files a/electives/amr/autonomous-mobile-robots-cheatsheet.pdf and b/electives/amr/autonomous-mobile-robots-cheatsheet.pdf differ diff --git a/electives/amr/parts/02_Sensors-Actuators/04_depth-range.tex b/electives/amr/parts/02_Sensors-Actuators/04_depth-range.tex index 9981261..25af9f2 100644 --- a/electives/amr/parts/02_Sensors-Actuators/04_depth-range.tex +++ b/electives/amr/parts/02_Sensors-Actuators/04_depth-range.tex @@ -7,8 +7,10 @@ Typ. derive D.M. with sensor dir param (as vec line eq) \subsubsection{Classic Stereo} -Both images: same plane, focal length, centre, $x$-axis. Given corresponding pixels $[u_l, v]$ and $[u_r, v]$, -$z = \frac{b \cdot f}{u_r - u_l}$ with $u_l = f\cdot \frac{x}{z} + c_u$ and $u_r = f \cdot \frac{x - b}{z} + c_u$ +Both images: same plane, focal length $f$, centre, $x$-axis. Given corresponding pixels $[u_l, v]$ and $[u_r, v]$: disparity (pixel offset) $d = u_r - u_l$; +$z = \frac{b \cdot f}{d}$, then projected into left (right) cam, $u_l = f\cdot \frac{x}{z} + c_u$ or $u_r = f \cdot \frac{x - b}{z} + c_u$, $b$ distance between cameras. +Then often apply pinhole camera projection (to e.g. get 3D point coords, comp ${_C}\vec{r}$, then point $z \cdot {_C}\vec{r}$). +Uncertainty introduced with 1.-order error propag. $\Delta z = \pardiff{d}z \Delta d$, with $\Delta d$ error of $d$ \subsubsection{Time of Flight, Projection} @@ -31,6 +33,7 @@ $z = \frac{b \cdot f}{u_r - u_l}$ with $u_l = f\cdot \frac{x}{z} + c_u$ and $u_r d \vec{x'} \\d \end{bmatrix} }$ +This $x'$ is obtained from pinhole cam projection. \subsubsection{Range Sensors} diff --git a/electives/amr/parts/03_multi-sensor-estimation/02_nonlinear-least-squares.tex b/electives/amr/parts/03_multi-sensor-estimation/02_nonlinear-least-squares.tex index 204c51c..869ee2d 100644 --- a/electives/amr/parts/03_multi-sensor-estimation/02_nonlinear-least-squares.tex +++ b/electives/amr/parts/03_multi-sensor-estimation/02_nonlinear-least-squares.tex @@ -1,9 +1,25 @@ \subsection{Non-Linear Least Squares} Find $\vec{x}^* = \text{argmax}\; \P(\vec{x} | \vec{z}) = \argmin{}(-\log(\P(\vec{x}|\vec{z})))$ -\bi{Gauss-Newton} % TODO: Do we really need these? If so, use from NumCS (much simpler notation) +\bi{Gauss-Newton} Need func $F$ and its Jacobian (or derivative) $D$ +\rmvspace % TODO: Error propagation laws +\begin{minted}[ + breaklines, + breakindentnchars=2, +]{python} +def gauss_newton(x: np.ndarray, F, DF, tol=1e-6): + s = np.linalg.lstsq(DF(x), F(x))[0] # least sq. + x = x-s; k = 1 + while np.linalg.norm(s) > tol * np.linalg.norm(x): + s = np.linalg.lstsq(DF(x), F(x))[0] + x = x-s; k += 1 # k opt for max iter + return x, k +\end{minted} +\rmvspace \bi{Levenberg-Marquardt} +Minimize $||F(x^{(k)}) + DF(x^{(k)})|| + \lambda||s||$. +% TODO: Do we need these? \bi{Local Param.} diff --git a/electives/amr/parts/05_planning-control/01_motion-planning-exploration/02_rrt.tex b/electives/amr/parts/05_planning-control/01_motion-planning-exploration/02_rrt.tex index b5e02db..94a8740 100644 --- a/electives/amr/parts/05_planning-control/01_motion-planning-exploration/02_rrt.tex +++ b/electives/amr/parts/05_planning-control/01_motion-planning-exploration/02_rrt.tex @@ -1,4 +1,3 @@ -\newpage \subsubsection{Rapidly-Exploring Random Tree (RRT)} \begin{algorithm} \begin{algorithmic}[1]