commit
						9e4b4b3f72
					
				|  | @ -1,5 +1,5 @@ | |||
| #LyX 2.0 created this file. For more info see http://www.lyx.org/ | ||||
| \lyxformat 413 | ||||
| #LyX 2.1 created this file. For more info see http://www.lyx.org/ | ||||
| \lyxformat 474 | ||||
| \begin_document | ||||
| \begin_header | ||||
| \textclass article | ||||
|  | @ -15,13 +15,13 @@ theorems-std | |||
| \font_roman times | ||||
| \font_sans default | ||||
| \font_typewriter default | ||||
| \font_math auto | ||||
| \font_default_family rmdefault | ||||
| \use_non_tex_fonts false | ||||
| \font_sc false | ||||
| \font_osf false | ||||
| \font_sf_scale 100 | ||||
| \font_tt_scale 100 | ||||
| 
 | ||||
| \graphics default | ||||
| \default_output_format default | ||||
| \output_sync 0 | ||||
|  | @ -32,15 +32,24 @@ theorems-std | |||
| \use_hyperref false | ||||
| \papersize default | ||||
| \use_geometry true | ||||
| \use_amsmath 1 | ||||
| \use_esint 0 | ||||
| \use_mhchem 1 | ||||
| \use_mathdots 1 | ||||
| \use_package amsmath 1 | ||||
| \use_package amssymb 1 | ||||
| \use_package cancel 1 | ||||
| \use_package esint 0 | ||||
| \use_package mathdots 1 | ||||
| \use_package mathtools 1 | ||||
| \use_package mhchem 1 | ||||
| \use_package stackrel 1 | ||||
| \use_package stmaryrd 1 | ||||
| \use_package undertilde 1 | ||||
| \cite_engine basic | ||||
| \cite_engine_type default | ||||
| \biblio_style plain | ||||
| \use_bibtopic false | ||||
| \use_indices false | ||||
| \paperorientation portrait | ||||
| \suppress_date false | ||||
| \justification true | ||||
| \use_refstyle 0 | ||||
| \index Index | ||||
| \shortcut idx | ||||
|  | @ -96,7 +105,7 @@ We will start with a small example of a robot moving in a plane, parameterized | |||
| 2D pose | ||||
| \emph default | ||||
|   | ||||
| \begin_inset Formula $(x,\, y,\,\theta)$ | ||||
| \begin_inset Formula $(x,\,y,\,\theta)$ | ||||
| \end_inset | ||||
| 
 | ||||
| . | ||||
|  | @ -135,7 +144,7 @@ A similar story holds for translation in the | |||
|  direction, and in fact for translations in general: | ||||
| \begin_inset Formula  | ||||
| \[ | ||||
| (x_{t},\, y_{t},\,\theta_{t})=(x_{0}+v_{x}t,\, y_{0}+v_{y}t,\,\theta_{0}) | ||||
| (x_{t},\,y_{t},\,\theta_{t})=(x_{0}+v_{x}t,\,y_{0}+v_{y}t,\,\theta_{0}) | ||||
| \] | ||||
| 
 | ||||
| \end_inset | ||||
|  | @ -143,7 +152,7 @@ A similar story holds for translation in the | |||
| Similarly for rotation we have  | ||||
| \begin_inset Formula  | ||||
| \[ | ||||
| (x_{t},\, y_{t},\,\theta_{t})=(x_{0},\, y_{0},\,\theta_{0}+\omega t) | ||||
| (x_{t},\,y_{t},\,\theta_{t})=(x_{0},\,y_{0},\,\theta_{0}+\omega t) | ||||
| \] | ||||
| 
 | ||||
| \end_inset | ||||
|  | @ -175,7 +184,7 @@ status collapsed | |||
| \end_inset | ||||
| 
 | ||||
| 
 | ||||
| \begin_inset Caption | ||||
| \begin_inset Caption Standard | ||||
| 
 | ||||
| \begin_layout Plain Layout | ||||
| Robot moving along a circular trajectory. | ||||
|  | @ -196,20 +205,20 @@ However, if we combine translation and rotation, the story breaks down! | |||
|  We cannot write | ||||
| \begin_inset Formula  | ||||
| \[ | ||||
| (x_{t},\, y_{t},\,\theta_{t})=(x_{0}+v_{x}t,\, y_{0}+v_{y}t,\,\theta_{0}+\omega t) | ||||
| (x_{t},\,y_{t},\,\theta_{t})=(x_{0}+v_{x}t,\,y_{0}+v_{y}t,\,\theta_{0}+\omega t) | ||||
| \] | ||||
| 
 | ||||
| \end_inset | ||||
| 
 | ||||
| The reason is that, if we move the robot a tiny bit according to the velocity | ||||
|  vector  | ||||
| \begin_inset Formula $(v_{x},\, v_{y},\,\omega)$ | ||||
| \begin_inset Formula $(v_{x},\,v_{y},\,\omega)$ | ||||
| \end_inset | ||||
| 
 | ||||
| , we have (to first order) | ||||
| \begin_inset Formula  | ||||
| \[ | ||||
| (x_{\delta},\, y_{\delta},\,\theta_{\delta})=(x_{0}+v_{x}\delta,\, y_{0}+v_{y}\delta,\,\theta_{0}+\omega\delta) | ||||
| (x_{\delta},\,y_{\delta},\,\theta_{\delta})=(x_{0}+v_{x}\delta,\,y_{0}+v_{y}\delta,\,\theta_{0}+\omega\delta) | ||||
| \] | ||||
| 
 | ||||
| \end_inset | ||||
|  | @ -255,7 +264,7 @@ status open | |||
| \end_inset | ||||
| 
 | ||||
| 
 | ||||
| \begin_inset Caption | ||||
| \begin_inset Caption Standard | ||||
| 
 | ||||
| \begin_layout Plain Layout | ||||
| \begin_inset CommandInset label | ||||
|  | @ -290,7 +299,7 @@ To make progress, we have to be more precise about how the robot behaves. | |||
|  as | ||||
| \begin_inset Formula  | ||||
| \[ | ||||
| T_{1}T_{2}=(x_{1},\, y_{1},\,\theta_{1})(x_{2},\, y_{2},\,\theta_{2})=(x_{1}+\cos\theta_{1}x_{2}-\sin\theta y_{2},\, y_{1}+\sin\theta_{1}x_{2}+\cos\theta_{1}y_{2},\,\theta_{1}+\theta_{2}) | ||||
| T_{1}T_{2}=(x_{1},\,y_{1},\,\theta_{1})(x_{2},\,y_{2},\,\theta_{2})=(x_{1}+\cos\theta_{1}x_{2}-\sin\theta y_{2},\,y_{1}+\sin\theta_{1}x_{2}+\cos\theta_{1}y_{2},\,\theta_{1}+\theta_{2}) | ||||
| \] | ||||
| 
 | ||||
| \end_inset | ||||
|  | @ -1600,13 +1609,13 @@ Hence, an alternative way of writing down elements of | |||
| \end_inset | ||||
| 
 | ||||
|  is as the ordered pair  | ||||
| \begin_inset Formula $(R,\, t)$ | ||||
| \begin_inset Formula $(R,\,t)$ | ||||
| \end_inset | ||||
| 
 | ||||
| , with composition defined a | ||||
| \begin_inset Formula  | ||||
| \[ | ||||
| (R_{1},\, t_{1})(R_{2},\, t_{2})=(R_{1}R_{2},\, R{}_{1}t_{2}+t_{1}) | ||||
| (R_{1},\,t_{1})(R_{2},\,t_{2})=(R_{1}R_{2},\,R{}_{1}t_{2}+t_{1}) | ||||
| \] | ||||
| 
 | ||||
| \end_inset | ||||
|  | @ -2569,13 +2578,13 @@ where | |||
| \end_inset | ||||
| 
 | ||||
|  is as the ordered pair  | ||||
| \begin_inset Formula $(R,\, t)$ | ||||
| \begin_inset Formula $(R,\,t)$ | ||||
| \end_inset | ||||
| 
 | ||||
| , with composition defined as | ||||
| \begin_inset Formula  | ||||
| \[ | ||||
| (R_{1},\, t_{1})(R_{2},\, t_{2})=(R_{1}R_{2},\, R{}_{1}t_{2}+t_{1}) | ||||
| (R_{1},\,t_{1})(R_{2},\,t_{2})=(R_{1}R_{2},\,R{}_{1}t_{2}+t_{1}) | ||||
| \] | ||||
| 
 | ||||
| \end_inset | ||||
|  | @ -2944,6 +2953,218 @@ p\\ | |||
| \end_inset | ||||
| 
 | ||||
| 
 | ||||
| \end_layout | ||||
| 
 | ||||
| \begin_layout Section | ||||
| 3D Similarity Transformations | ||||
| \end_layout | ||||
| 
 | ||||
| \begin_layout Standard | ||||
| The group of 3D similarity transformations  | ||||
| \begin_inset Formula $Sim(3)$ | ||||
| \end_inset | ||||
| 
 | ||||
|  is the set of  | ||||
| \begin_inset Formula $4\times4$ | ||||
| \end_inset | ||||
| 
 | ||||
|  invertible matrices of the form | ||||
| \begin_inset Formula  | ||||
| \[ | ||||
| T\define\left[\begin{array}{cc} | ||||
| R & t\\ | ||||
| 0 & s^{-1} | ||||
| \end{array}\right] | ||||
| \] | ||||
| 
 | ||||
| \end_inset | ||||
| 
 | ||||
| where  | ||||
| \begin_inset Formula $s$ | ||||
| \end_inset | ||||
| 
 | ||||
|  is a scalar. | ||||
|  There are several different conventions in use for the Lie algebra generators, | ||||
|  but we use | ||||
| \begin_inset Formula  | ||||
| \[ | ||||
| G^{1}=\left(\begin{array}{cccc} | ||||
| 0 & 0 & 0 & 0\\ | ||||
| 0 & 0 & -1 & 0\\ | ||||
| 0 & 1 & 0 & 0\\ | ||||
| 0 & 0 & 0 & 0 | ||||
| \end{array}\right)\mbox{}G^{2}=\left(\begin{array}{cccc} | ||||
| 0 & 0 & 1 & 0\\ | ||||
| 0 & 0 & 0 & 0\\ | ||||
| -1 & 0 & 0 & 0\\ | ||||
| 0 & 0 & 0 & 0 | ||||
| \end{array}\right)\mbox{ }G^{3}=\left(\begin{array}{cccc} | ||||
| 0 & -1 & 0 & 0\\ | ||||
| 1 & 0 & 0 & 0\\ | ||||
| 0 & 0 & 0 & 0\\ | ||||
| 0 & 0 & 0 & 0 | ||||
| \end{array}\right) | ||||
| \] | ||||
| 
 | ||||
| \end_inset | ||||
| 
 | ||||
| 
 | ||||
| \begin_inset Formula  | ||||
| \[ | ||||
| G^{4}=\left(\begin{array}{cccc} | ||||
| 0 & 0 & 0 & 1\\ | ||||
| 0 & 0 & 0 & 0\\ | ||||
| 0 & 0 & 0 & 0\\ | ||||
| 0 & 0 & 0 & 0 | ||||
| \end{array}\right)\mbox{}G^{5}=\left(\begin{array}{cccc} | ||||
| 0 & 0 & 0 & 0\\ | ||||
| 0 & 0 & 0 & 1\\ | ||||
| 0 & 0 & 0 & 0\\ | ||||
| 0 & 0 & 0 & 0 | ||||
| \end{array}\right)\mbox{ }G^{6}=\left(\begin{array}{cccc} | ||||
| 0 & 0 & 0 & 0\\ | ||||
| 0 & 0 & 0 & 0\\ | ||||
| 0 & 0 & 0 & 1\\ | ||||
| 0 & 0 & 0 & 0 | ||||
| \end{array}\right)\mbox{ }G^{7}=\left(\begin{array}{cccc} | ||||
| 0 & 0 & 0 & 0\\ | ||||
| 0 & 0 & 0 & 0\\ | ||||
| 0 & 0 & 0 & 0\\ | ||||
| 0 & 0 & 0 & -1 | ||||
| \end{array}\right) | ||||
| \] | ||||
| 
 | ||||
| \end_inset | ||||
| 
 | ||||
| 
 | ||||
| \end_layout | ||||
| 
 | ||||
| \begin_layout Subsection | ||||
| Actions | ||||
| \end_layout | ||||
| 
 | ||||
| \begin_layout Standard | ||||
| The action of  | ||||
| \begin_inset Formula $\SEthree$ | ||||
| \end_inset | ||||
| 
 | ||||
|  on 3D points is done by embedding the points in  | ||||
| \begin_inset Formula $\mathbb{R}^{4}$ | ||||
| \end_inset | ||||
| 
 | ||||
|  by using homogeneous coordinates | ||||
| \begin_inset Formula  | ||||
| \[ | ||||
| \hat{q}=\left[\begin{array}{c} | ||||
| q\\ | ||||
| s^{-1} | ||||
| \end{array}\right]=\left[\begin{array}{c} | ||||
| Rp+t\\ | ||||
| s^{-1} | ||||
| \end{array}\right]=\left[\begin{array}{cc} | ||||
| R & t\\ | ||||
| 0 & s^{-1} | ||||
| \end{array}\right]\left[\begin{array}{c} | ||||
| p\\ | ||||
| 1 | ||||
| \end{array}\right]=T\hat{p} | ||||
| \] | ||||
| 
 | ||||
| \end_inset | ||||
| 
 | ||||
| The derivative  | ||||
| \begin_inset Formula $D_{1}f(\xi)$ | ||||
| \end_inset | ||||
| 
 | ||||
|  in an incremental change  | ||||
| \begin_inset Formula $\xi$ | ||||
| \end_inset | ||||
| 
 | ||||
|  to  | ||||
| \begin_inset Formula $T$ | ||||
| \end_inset | ||||
| 
 | ||||
|  is given by  | ||||
| \begin_inset Formula $TH(p)$ | ||||
| \end_inset | ||||
| 
 | ||||
|  where  | ||||
| \begin_inset Formula  | ||||
| \[ | ||||
| H(p)=G_{jk}^{i}p^{j}=\left(\begin{array}{ccccccc} | ||||
| 0 & z & -y & 1 & 0 & 0 & 0\\ | ||||
| -z & 0 & x & 0 & 1 & 0 & 0\\ | ||||
| y & -x & 0 & 0 & 0 & 1 & 0\\ | ||||
| 0 & 0 & 0 & 0 & 0 & 0 & -1 | ||||
| \end{array}\right) | ||||
| \] | ||||
| 
 | ||||
| \end_inset | ||||
| 
 | ||||
| In other words | ||||
| \begin_inset Formula  | ||||
| \[ | ||||
| D_{1}f(\xi)=\left[\begin{array}{cc} | ||||
| R & t\\ | ||||
| 0 & s^{-1} | ||||
| \end{array}\right]\left[\begin{array}{ccc} | ||||
| -\left[p\right]_{x} & I_{3} & 0\\ | ||||
| 0 & 0 & -1 | ||||
| \end{array}\right]=\left[\begin{array}{ccc} | ||||
| -R\left[p\right]_{x} & R & -t\\ | ||||
| 0 & 0 & -s^{-1} | ||||
| \end{array}\right] | ||||
| \] | ||||
| 
 | ||||
| \end_inset | ||||
| 
 | ||||
| This is the derivative for the action on homogeneous coordinates. | ||||
|  Switching back to non-homogeneous coordinates is done by | ||||
| \begin_inset Formula  | ||||
| \[ | ||||
| \left[\begin{array}{c} | ||||
| q\\ | ||||
| a | ||||
| \end{array}\right]\rightarrow q/a | ||||
| \] | ||||
| 
 | ||||
| \end_inset | ||||
| 
 | ||||
| with derivative | ||||
| \begin_inset Formula  | ||||
| \[ | ||||
| \left[\begin{array}{cc} | ||||
| a^{-1}I_{3} & -qa^{-2}\end{array}\right] | ||||
| \] | ||||
| 
 | ||||
| \end_inset | ||||
| 
 | ||||
| For  | ||||
| \begin_inset Formula $a=s^{-1}$ | ||||
| \end_inset | ||||
| 
 | ||||
|  we obtain | ||||
| \begin_inset Formula  | ||||
| \[ | ||||
| D_{1}f(\xi)=\left[\begin{array}{cc} | ||||
| sI_{3} & -qs^{2}\end{array}\right]\left[\begin{array}{ccc} | ||||
| -R\left[p\right]_{x} & R & -t\\ | ||||
| 0 & 0 & -s^{-1} | ||||
| \end{array}\right]=\left[\begin{array}{ccc} | ||||
| -sR\left[p\right]_{x} & sR & -st+qs\end{array}\right]=\left[\begin{array}{ccc} | ||||
| -sR\left[p\right]_{x} & sR & sRp\end{array}\right] | ||||
| \] | ||||
| 
 | ||||
| \end_inset | ||||
| 
 | ||||
| 
 | ||||
| \end_layout | ||||
| 
 | ||||
| \begin_layout Standard | ||||
| \begin_inset Newpage pagebreak | ||||
| \end_inset | ||||
| 
 | ||||
| 
 | ||||
| \end_layout | ||||
| 
 | ||||
| \begin_layout Section | ||||
|  |  | |||
										
											Binary file not shown.
										
									
								
							
							
								
								
									
										45
									
								
								doc/math.lyx
								
								
								
								
							
							
						
						
									
										45
									
								
								doc/math.lyx
								
								
								
								
							|  | @ -1,5 +1,5 @@ | |||
| #LyX 2.0 created this file. For more info see http://www.lyx.org/ | ||||
| \lyxformat 413 | ||||
| #LyX 2.1 created this file. For more info see http://www.lyx.org/ | ||||
| \lyxformat 474 | ||||
| \begin_document | ||||
| \begin_header | ||||
| \textclass article | ||||
|  | @ -17,13 +17,13 @@ theorems-ams-bytype | |||
| \font_roman times | ||||
| \font_sans default | ||||
| \font_typewriter default | ||||
| \font_math auto | ||||
| \font_default_family rmdefault | ||||
| \use_non_tex_fonts false | ||||
| \font_sc false | ||||
| \font_osf false | ||||
| \font_sf_scale 100 | ||||
| \font_tt_scale 100 | ||||
| 
 | ||||
| \graphics default | ||||
| \default_output_format default | ||||
| \output_sync 0 | ||||
|  | @ -34,15 +34,24 @@ theorems-ams-bytype | |||
| \use_hyperref false | ||||
| \papersize default | ||||
| \use_geometry true | ||||
| \use_amsmath 1 | ||||
| \use_esint 0 | ||||
| \use_mhchem 1 | ||||
| \use_mathdots 1 | ||||
| \use_package amsmath 1 | ||||
| \use_package amssymb 1 | ||||
| \use_package cancel 1 | ||||
| \use_package esint 0 | ||||
| \use_package mathdots 1 | ||||
| \use_package mathtools 1 | ||||
| \use_package mhchem 1 | ||||
| \use_package stackrel 0 | ||||
| \use_package stmaryrd 1 | ||||
| \use_package undertilde 1 | ||||
| \cite_engine basic | ||||
| \cite_engine_type default | ||||
| \biblio_style plain | ||||
| \use_bibtopic false | ||||
| \use_indices false | ||||
| \paperorientation portrait | ||||
| \suppress_date false | ||||
| \justification true | ||||
| \use_refstyle 0 | ||||
| \index Index | ||||
| \shortcut idx | ||||
|  | @ -1603,11 +1612,7 @@ name "sec:Derivatives-of-Actions" | |||
| \end_layout | ||||
| 
 | ||||
| \begin_layout Standard | ||||
| The (usual) action of an  | ||||
| \begin_inset Formula $n$ | ||||
| \end_inset | ||||
| 
 | ||||
| -dimensional matrix group  | ||||
| The (usual) action of a matrix group  | ||||
| \begin_inset Formula $G$ | ||||
| \end_inset | ||||
| 
 | ||||
|  | @ -1632,7 +1637,7 @@ Since this is a function defined on the product | |||
| \end_inset | ||||
| 
 | ||||
|  the derivative is a linear transformation  | ||||
| \begin_inset Formula $Df:\Multi{2n}n$ | ||||
| \begin_inset Formula $Df:\Multi{m+n}n$ | ||||
| \end_inset | ||||
| 
 | ||||
|  with | ||||
|  | @ -1643,7 +1648,15 @@ Df_{(T,p)}\left(\xi,\delta p\right)=D_{1}f_{(T,p)}\left(\xi\right)+D_{2}f_{(T,p) | |||
| 
 | ||||
| \end_inset | ||||
| 
 | ||||
| where  | ||||
| \begin_inset Formula $m$ | ||||
| \end_inset | ||||
| 
 | ||||
|  is the dimensionality of the manifold  | ||||
| \begin_inset Formula $G$ | ||||
| \end_inset | ||||
| 
 | ||||
| . | ||||
| \end_layout | ||||
| 
 | ||||
| \begin_layout Theorem | ||||
|  | @ -1672,7 +1685,7 @@ H(p) & I_{n}\end{array}\right] | |||
| \end_inset | ||||
| 
 | ||||
| with  | ||||
| \begin_inset Formula $H:\Reals n\rightarrow\Reals{n\times n}$ | ||||
| \begin_inset Formula $H:\Reals m\rightarrow\Reals{n\times m}$ | ||||
| \end_inset | ||||
| 
 | ||||
|  a linear mapping that depends on  | ||||
|  | @ -1799,7 +1812,7 @@ with | |||
| \end_inset | ||||
| 
 | ||||
|  an  | ||||
| \begin_inset Formula $n\times n$ | ||||
| \begin_inset Formula $n\times m$ | ||||
| \end_inset | ||||
| 
 | ||||
|  matrix that depends on  | ||||
|  | @ -3377,7 +3390,7 @@ Retractions | |||
| 
 | ||||
| \begin_layout Standard | ||||
| \begin_inset FormulaMacro | ||||
| \renewcommand{\retract}{\mathcal{R}} | ||||
| \newcommand{\retract}{\mathcal{R}} | ||||
| {\mathcal{R}} | ||||
| \end_inset | ||||
| 
 | ||||
|  |  | |||
							
								
								
									
										
											BIN
										
									
								
								doc/math.pdf
								
								
								
								
							
							
						
						
									
										
											BIN
										
									
								
								doc/math.pdf
								
								
								
								
							
										
											Binary file not shown.
										
									
								
							|  | @ -67,19 +67,19 @@ Similarity3 Similarity3::operator*(const Similarity3& T) const { | |||
| } | ||||
| 
 | ||||
| Similarity3 Similarity3::inverse() const { | ||||
|   Rot3 Rt = R_.inverse(); | ||||
|   Point3 sRt = R_.inverse() * (-s_ * t_); | ||||
|   const Rot3 Rt = R_.inverse(); | ||||
|   const Point3 sRt = Rt * (-s_ * t_); | ||||
|   return Similarity3(Rt, sRt, 1.0 / s_); | ||||
| } | ||||
| 
 | ||||
| Point3 Similarity3::transform_from(const Point3& p, //
 | ||||
|     OptionalJacobian<3, 7> H1, OptionalJacobian<3, 3> H2) const { | ||||
|   Point3 q = R_ * p + t_; | ||||
|   const Point3 q = R_ * p + t_; | ||||
|   if (H1) { | ||||
|     const Matrix3 R = R_.matrix(); | ||||
|     Matrix3 DR = s_ * R * skewSymmetric(-p.x(), -p.y(), -p.z()); | ||||
|     // TODO(frank): explain the derivative in lambda
 | ||||
|     *H1 << DR, s_ * R, s_ * p.vector(); | ||||
|     // For this derivative, see LieGroups.pdf
 | ||||
|     const Matrix3 sR = s_ * R_.matrix(); | ||||
|     const Matrix3 DR = sR * skewSymmetric(-p.x(), -p.y(), -p.z()); | ||||
|     *H1 << DR, sR, sR * p.vector(); | ||||
|   } | ||||
|   if (H2) | ||||
|     *H2 = s_ * R_.matrix(); // just 3*3 sub-block of matrix()
 | ||||
|  | @ -94,7 +94,7 @@ Matrix4 Similarity3::wedge(const Vector7& xi) { | |||
|   // http://www.ethaneade.org/latex2html/lie/node29.html
 | ||||
|   const auto w = xi.head<3>(); | ||||
|   const auto u = xi.segment<3>(3); | ||||
|   double lambda = xi[6]; | ||||
|   const double lambda = xi[6]; | ||||
|   Matrix4 W; | ||||
|   W << skewSymmetric(w), u, 0, 0, 0, -lambda; | ||||
|   return W; | ||||
|  | @ -106,81 +106,43 @@ Matrix7 Similarity3::AdjointMap() const { | |||
|   const Vector3 t = t_.vector(); | ||||
|   const Matrix3 A = s_ * skewSymmetric(t) * R; | ||||
|   Matrix7 adj; | ||||
|   adj << | ||||
|       R, Z_3x3, Matrix31::Zero(), // 3*7
 | ||||
|       A, s_ * R, -s_ * t, // 3*7
 | ||||
|       Matrix16::Zero(), 1; // 1*7
 | ||||
|   adj << R, Z_3x3, Matrix31::Zero(), // 3*7
 | ||||
|   A, s_ * R, -s_ * t, // 3*7
 | ||||
|   Matrix16::Zero(), 1; // 1*7
 | ||||
|   return adj; | ||||
| } | ||||
| 
 | ||||
| Matrix3 Similarity3::GetV(Vector3 w, double lambda) { | ||||
|   // http://www.ethaneade.org/latex2html/lie/node29.html
 | ||||
|   double lambda2 = lambda * lambda; | ||||
|   double theta2 = w.transpose() * w; | ||||
|   double theta = sqrt(theta2); | ||||
|   double A, B, C; | ||||
|   // TODO(frank): eliminate copy/paste
 | ||||
|   if (theta2 > 1e-9 && lambda2 > 1e-9) { | ||||
|   const double theta2 = w.transpose() * w; | ||||
|   double Y, Z, W; | ||||
|   if (theta2 > 1e-9) { | ||||
|     const double theta = sqrt(theta2); | ||||
|     const double X = sin(theta) / theta; | ||||
|     const double Y = (1 - cos(theta)) / theta2; | ||||
|     const double Z = (1 - X) / theta2; | ||||
|     const double W = (0.5 - Y) / theta2; | ||||
|     const double alpha = lambda2 / (lambda2 + theta2); | ||||
|     const double beta = (exp(-lambda) - 1 + lambda) / lambda2; | ||||
|     const double gamma = Y - (lambda * Z); | ||||
|     const double mu = (1 - lambda + (0.5 * lambda2) - exp(-lambda)) | ||||
|         / (lambda2 * lambda); | ||||
|     const double upsilon = Z - (lambda * W); | ||||
|     A = (1 - exp(-lambda)) / lambda; | ||||
|     B = alpha * (beta - gamma) + gamma; | ||||
|     C = alpha * (mu - upsilon) + upsilon; | ||||
|   } else if (theta2 <= 1e-9 && lambda2 > 1e-9) { | ||||
|     //Taylor series expansions
 | ||||
|     const double Y = 0.5 - theta2 / 24.0; | ||||
|     const double Z = 1.0 / 6.0 - theta2 / 120.0; | ||||
|     const double W = 1.0 / 24.0 - theta2 / 720.0; | ||||
|     const double alpha = lambda2 / (lambda2 + theta2); | ||||
|     const double beta = (exp(-lambda) - 1 + lambda) / lambda2; | ||||
|     const double gamma = Y - (lambda * Z); | ||||
|     const double mu = (1 - lambda + (0.5 * lambda2) - exp(-lambda)) | ||||
|         / (lambda2 * lambda); | ||||
|     const double upsilon = Z - (lambda * W); | ||||
|     A = (1 - exp(-lambda)) / lambda; | ||||
|     B = alpha * (beta - gamma) + gamma; | ||||
|     C = alpha * (mu - upsilon) + upsilon; | ||||
|   } else if (theta2 > 1e-9 && lambda2 <= 1e-9) { | ||||
|     const double X = sin(theta) / theta; | ||||
|     const double Y = (1 - cos(theta)) / theta2; | ||||
|     const double Z = (1 - X) / theta2; | ||||
|     const double W = (0.5 - Y) / theta2; | ||||
|     const double alpha = lambda2 / (lambda2 + theta2); | ||||
|     const double beta = 0.5 - lambda / 6.0 + lambda2 / 24.0 | ||||
|         - (lambda * lambda2) / 120; | ||||
|     const double gamma = Y - (lambda * Z); | ||||
|     const double mu = 1.0 / 6.0 - lambda / 24 + lambda2 / 120 | ||||
|         - (lambda * lambda2) / 720; | ||||
|     const double upsilon = Z - (lambda * W); | ||||
|     if (lambda < 1e-9) { | ||||
|       A = 1 - lambda / 2.0 + lambda2 / 6.0; | ||||
|     } else { | ||||
|       A = (1 - exp(-lambda)) / lambda; | ||||
|     } | ||||
|     B = alpha * (beta - gamma) + gamma; | ||||
|     C = alpha * (mu - upsilon) + upsilon; | ||||
|     Y = (1 - cos(theta)) / theta2; | ||||
|     Z = (1 - X) / theta2; | ||||
|     W = (0.5 - Y) / theta2; | ||||
|   } else { | ||||
|     const double Y = 0.5 - theta2 / 24.0; | ||||
|     const double Z = 1.0 / 6.0 - theta2 / 120.0; | ||||
|     const double W = 1.0 / 24.0 - theta2 / 720.0; | ||||
|     const double gamma = Y - (lambda * Z); | ||||
|     const double upsilon = Z - (lambda * W); | ||||
|     if (lambda < 1e-9) { | ||||
|       A = 1 - lambda / 2.0 + lambda2 / 6.0; | ||||
|     } else { | ||||
|       A = (1 - exp(-lambda)) / lambda; | ||||
|     } | ||||
|     B = gamma; | ||||
|     C = upsilon; | ||||
|     // Taylor series expansion for theta=0, X not needed (as is 1)
 | ||||
|     Y = 0.5 - theta2 / 24.0; | ||||
|     Z = 1.0 / 6.0 - theta2 / 120.0; | ||||
|     W = 1.0 / 24.0 - theta2 / 720.0; | ||||
|   } | ||||
|   const double lambda2 = lambda * lambda, lambda3 = lambda2 * lambda; | ||||
|   double A, alpha = 0.0, beta, mu; | ||||
|   if (lambda2 > 1e-9) { | ||||
|     A = (1.0 - exp(-lambda)) / lambda; | ||||
|     alpha = 1.0 / (1.0 + theta2 / lambda2); | ||||
|     beta = (exp(-lambda) - 1 + lambda) / lambda2; | ||||
|     mu = (1 - lambda + (0.5 * lambda2) - exp(-lambda)) / lambda3; | ||||
|   } else { | ||||
|     A = 1.0 - lambda / 2.0 + lambda2 / 6.0; | ||||
|     beta = 0.5 - lambda / 6.0 + lambda2 / 24.0 - lambda3 / 120.0; | ||||
|     mu = 1.0 / 6.0 - lambda / 24.0 + lambda2 / 120.0 - lambda3 / 720.0; | ||||
|   } | ||||
|   const double gamma = Y - (lambda * Z), upsilon = Z - (lambda * W); | ||||
|   const double B = alpha * (beta - gamma) + gamma; | ||||
|   const double C = alpha * (mu - upsilon) + upsilon; | ||||
|   const Matrix3 Wx = skewSymmetric(w[0], w[1], w[2]); | ||||
|   return A * I_3x3 + B * Wx + C * Wx * Wx; | ||||
| } | ||||
|  | @ -218,7 +180,7 @@ std::ostream &operator<<(std::ostream &os, const Similarity3& p) { | |||
| const Matrix4 Similarity3::matrix() const { | ||||
|   Matrix4 T; | ||||
|   T.topRows<3>() << R_.matrix(), t_.vector(); | ||||
|   T.bottomRows<1>() << 0, 0, 0, 1.0/s_; | ||||
|   T.bottomRows<1>() << 0, 0, 0, 1.0 / s_; | ||||
|   return T; | ||||
| } | ||||
| 
 | ||||
|  | @ -226,4 +188,4 @@ Similarity3::operator Pose3() const { | |||
|   return Pose3(R_, s_ * t_); | ||||
| } | ||||
| 
 | ||||
| } | ||||
| } // namespace gtsam
 | ||||
|  |  | |||
|  | @ -45,7 +45,8 @@ static const Rot3 R = Rot3::Rodrigues(0.3, 0, 0); | |||
| static const double s = 4; | ||||
| static const Similarity3 id; | ||||
| static const Similarity3 T1(R, Point3(3.5, -8.2, 4.2), 1); | ||||
| static const Similarity3 T2(Rot3::Rodrigues(0.3, 0.2, 0.1), Point3(3.5, -8.2, 4.2), 1); | ||||
| static const Similarity3 T2(Rot3::Rodrigues(0.3, 0.2, 0.1), | ||||
|     Point3(3.5, -8.2, 4.2), 1); | ||||
| static const Similarity3 T3(Rot3::Rodrigues(-90, 0, 0), Point3(1, 2, 3), 1); | ||||
| static const Similarity3 T4(R, P, s); | ||||
| static const Similarity3 T5(R, P, 10); | ||||
|  | @ -187,11 +188,7 @@ TEST(Similarity3, manifold_first_order) { | |||
| // Return as a 4*4 Matrix
 | ||||
| TEST(Similarity3, Matrix) { | ||||
|   Matrix4 expected; | ||||
|   expected << | ||||
|       1, 0, 0, 1, | ||||
|       0, 1, 0, 1, | ||||
|       0, 0, 1, 0, | ||||
|       0, 0, 0, 0.5; | ||||
|   expected << 1, 0, 0, 1, 0, 1, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0.5; | ||||
|   Matrix4 actual = T6.matrix(); | ||||
|   EXPECT(assert_equal(expected, actual)); | ||||
| } | ||||
|  | @ -200,12 +197,12 @@ TEST(Similarity3, Matrix) { | |||
| // Exponential and log maps
 | ||||
| TEST(Similarity3, ExpLogMap) { | ||||
|   Vector7 delta; | ||||
|   delta << 0.1,0.2,0.3,0.4,0.5,0.6,0.7; | ||||
|   delta << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7; | ||||
|   Vector7 actual = Similarity3::Logmap(Similarity3::Expmap(delta)); | ||||
|   EXPECT(assert_equal(delta, actual)); | ||||
| 
 | ||||
|   Vector7 zeros; | ||||
|   zeros << 0,0,0,0,0,0,0; | ||||
|   zeros << 0, 0, 0, 0, 0, 0, 0; | ||||
|   Vector7 logIdentity = Similarity3::Logmap(Similarity3::identity()); | ||||
|   EXPECT(assert_equal(zeros, logIdentity)); | ||||
| 
 | ||||
|  | @ -214,7 +211,8 @@ TEST(Similarity3, ExpLogMap) { | |||
|   EXPECT(assert_equal(expZero, ident)); | ||||
| 
 | ||||
|   // Compare to matrix exponential, using expm in Lie.h
 | ||||
|   EXPECT(assert_equal(expm<Similarity3>(delta), Similarity3::Expmap(delta), 1e-3)); | ||||
|   EXPECT( | ||||
|       assert_equal(expm<Similarity3>(delta), Similarity3::Expmap(delta), 1e-3)); | ||||
| } | ||||
| 
 | ||||
| //******************************************************************************
 | ||||
|  | @ -237,8 +235,8 @@ TEST(Similarity3, GroupAction) { | |||
|   EXPECT(assert_equal(Point3(2, 2, 3), Ta.transform_from(pa))); | ||||
|   EXPECT(assert_equal(Point3(4, 4, 6), Tb.transform_from(pa))); | ||||
| 
 | ||||
|   Similarity3 Tc(Rot3::Rz(M_PI/2.0), Point3(1, 2, 3), 1.0); | ||||
|   Similarity3 Td(Rot3::Rz(M_PI/2.0), Point3(1, 2, 3), 2.0); | ||||
|   Similarity3 Tc(Rot3::Rz(M_PI / 2.0), Point3(1, 2, 3), 1.0); | ||||
|   Similarity3 Td(Rot3::Rz(M_PI / 2.0), Point3(1, 2, 3), 2.0); | ||||
|   EXPECT(assert_equal(Point3(1, 3, 3), Tc.transform_from(pa))); | ||||
|   EXPECT(assert_equal(Point3(2, 6, 6), Td.transform_from(pa))); | ||||
| 
 | ||||
|  | @ -249,10 +247,10 @@ TEST(Similarity3, GroupAction) { | |||
|   Point3 q(1, 2, 3); | ||||
|   for (const auto T : { T1, T2, T3, T4, T5, T6 }) { | ||||
|     Point3 q(1, 0, 0); | ||||
|     Matrix H1 = numericalDerivative21<Point3, Similarity3, Point3>(f, T1, q); | ||||
|     Matrix H2 = numericalDerivative22<Point3, Similarity3, Point3>(f, T1, q); | ||||
|     Matrix H1 = numericalDerivative21<Point3, Similarity3, Point3>(f, T, q); | ||||
|     Matrix H2 = numericalDerivative22<Point3, Similarity3, Point3>(f, T, q); | ||||
|     Matrix actualH1, actualH2; | ||||
|     T1.transform_from(q, actualH1, actualH2); | ||||
|     T.transform_from(q, actualH1, actualH2); | ||||
|     EXPECT(assert_equal(H1, actualH1)); | ||||
|     EXPECT(assert_equal(H2, actualH2)); | ||||
|   } | ||||
|  | @ -387,25 +385,25 @@ TEST(Similarity3, AlignScaledPointClouds) { | |||
| TEST(Similarity3 , Invariants) { | ||||
|   Similarity3 id; | ||||
| 
 | ||||
|   EXPECT(check_group_invariants(id,id)); | ||||
|   EXPECT(check_group_invariants(id,T3)); | ||||
|   EXPECT(check_group_invariants(T2,id)); | ||||
|   EXPECT(check_group_invariants(T2,T3)); | ||||
|   EXPECT(check_group_invariants(id, id)); | ||||
|   EXPECT(check_group_invariants(id, T3)); | ||||
|   EXPECT(check_group_invariants(T2, id)); | ||||
|   EXPECT(check_group_invariants(T2, T3)); | ||||
| 
 | ||||
|   EXPECT(check_manifold_invariants(id,id)); | ||||
|   EXPECT(check_manifold_invariants(id,T3)); | ||||
|   EXPECT(check_manifold_invariants(T2,id)); | ||||
|   EXPECT(check_manifold_invariants(T2,T3)); | ||||
|   EXPECT(check_manifold_invariants(id, id)); | ||||
|   EXPECT(check_manifold_invariants(id, T3)); | ||||
|   EXPECT(check_manifold_invariants(T2, id)); | ||||
|   EXPECT(check_manifold_invariants(T2, T3)); | ||||
| } | ||||
| 
 | ||||
| //******************************************************************************
 | ||||
| TEST(Similarity3 , LieGroupDerivatives) { | ||||
|   Similarity3 id; | ||||
| 
 | ||||
|   CHECK_LIE_GROUP_DERIVATIVES(id,id); | ||||
|   CHECK_LIE_GROUP_DERIVATIVES(id,T2); | ||||
|   CHECK_LIE_GROUP_DERIVATIVES(T2,id); | ||||
|   CHECK_LIE_GROUP_DERIVATIVES(T2,T3); | ||||
|   CHECK_LIE_GROUP_DERIVATIVES(id, id); | ||||
|   CHECK_LIE_GROUP_DERIVATIVES(id, T2); | ||||
|   CHECK_LIE_GROUP_DERIVATIVES(T2, id); | ||||
|   CHECK_LIE_GROUP_DERIVATIVES(T2, T3); | ||||
| } | ||||
| 
 | ||||
| //******************************************************************************
 | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue